Point Cloud Library (PCL) 1.15.0
Loading...
Searching...
No Matches
crh_alignment.h
1/*
2 * crh_recognition.h
3 *
4 * Created on: Mar 30, 2012
5 * Author: aitor
6 */
7
8#pragma once
9
10#include <pcl/common/common.h>
11#include <pcl/features/crh.h>
12#include <pcl/common/fft/kiss_fftr.h>
13#include <pcl/common/transforms.h>
14
15namespace pcl
16{
17
18 /** \brief CRHAlignment uses two Camera Roll Histograms (CRH) to find the
19 * roll rotation that aligns both views. See:
20 * - CAD-Model Recognition and 6 DOF Pose Estimation
21 * A. Aldoma, N. Blodow, D. Gossow, S. Gedikli, R.B. Rusu, M. Vincze and G. Bradski
22 * ICCV 2011, 3D Representation and Recognition (3dRR11) workshop
23 * Barcelona, Spain, (2011)
24 *
25 * \author Aitor Aldoma
26 * \ingroup recognition
27 */
28
29 template<typename PointT, int nbins_>
30 class PCL_EXPORTS CRHAlignment
31 {
32 private:
33
34 /** \brief Sorts peaks */
35 struct peaks_ordering
36 {
37 bool
38 operator() (std::pair<float, int> const& a, std::pair<float, int> const& b)
39 {
40 return a.first > b.first;
41 }
42 };
43
44 using PointTPtr = typename pcl::PointCloud<PointT>::Ptr;
45
46 /** \brief View of the model to be aligned to input_view_ */
47 PointTPtr target_view_;
48 /** \brief View of the input */
49 PointTPtr input_view_;
50 /** \brief Centroid of the model_view_ */
51 Eigen::Vector3f centroid_target_;
52 /** \brief Centroid of the input_view_ */
53 Eigen::Vector3f centroid_input_;
54 /** \brief transforms from model view to input view */
55 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_;
56 /** \brief Allowed maximum number of peaks */
57 int max_peaks_;
58 /** \brief Quantile of peaks after sorting to be checked */
59 float quantile_;
60 /** \brief Threshold for a peak to be accepted.
61 * If peak_i >= (max_peak * accept_threhsold_) => peak is accepted
62 */
63 float accept_threshold_;
64
65 /** \brief computes the transformation to the z-axis
66 * \param[in] centroid
67 * \param[out] transformation to z-axis
68 */
69 void
70 computeTransformToZAxes (Eigen::Vector3f & centroid, Eigen::Affine3f & transform)
71 {
72 Eigen::Vector3f plane_normal;
73 plane_normal[0] = -centroid[0];
74 plane_normal[1] = -centroid[1];
75 plane_normal[2] = -centroid[2];
76 Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
77 plane_normal.normalize ();
78 Eigen::Vector3f axis = plane_normal.cross (z_vector);
79 double rotation = -asin (axis.norm ());
80 axis.normalize ();
81 transform = Eigen::Affine3f (Eigen::AngleAxisf (static_cast<float>(rotation), axis));
82 }
83
84 /** \brief computes the roll transformation
85 * \param[in] centroid input
86 * \param[in] centroid view
87 * \param[in] roll_angle
88 * \param[out] roll transformation
89 */
90 void
91 computeRollTransform (Eigen::Vector3f & centroidInput, Eigen::Vector3f & centroidResult, double roll_angle, Eigen::Affine3f & final_trans)
92 {
93 Eigen::Affine3f transformInputToZ;
94 computeTransformToZAxes (centroidInput, transformInputToZ);
95
96 transformInputToZ = transformInputToZ.inverse ();
97 Eigen::Affine3f transformRoll (Eigen::AngleAxisf (-static_cast<float>(roll_angle * M_PI / 180), Eigen::Vector3f::UnitZ ()));
98 Eigen::Affine3f transformDBResultToZ;
99 computeTransformToZAxes (centroidResult, transformDBResultToZ);
100
101 final_trans = transformInputToZ * transformRoll * transformDBResultToZ;
102 }
103 public:
104
105 /** \brief Constructor. */
107 max_peaks_ = 5;
108 quantile_ = 0.2f;
109 accept_threshold_ = 0.8f;
110 }
111
112 /** \brief returns the computed transformations
113 * \param[out] transforms transformations
114 */
115 void getTransforms(std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transforms) {
116 transforms = transforms_;
117 }
118
119 /** \brief sets model and input views
120 * \param[in] input_view
121 * \param[in] target_view
122 */
123 void
124 setInputAndTargetView (PointTPtr & input_view, PointTPtr & target_view)
125 {
126 target_view_ = target_view;
127 input_view_ = input_view;
128 }
129
130 /** \brief sets model and input centroids
131 * \param[in] c1 model view centroid
132 * \param[in] c2 input view centroid
133 */
134 void
135 setInputAndTargetCentroids (Eigen::Vector3f & c1, Eigen::Vector3f & c2)
136 {
137 centroid_target_ = c2;
138 centroid_input_ = c1;
139 }
140
141 /** \brief Computes the transformation aligning model to input
142 * \param[in] input_ftt CRH histogram of the input cloud
143 * \param[in] target_ftt CRH histogram of the target cloud
144 */
145 void
147 {
148
149 transforms_.clear(); //clear from last round...
150
151 std::vector<float> peaks;
152 computeRollAngle (input_ftt, target_ftt, peaks);
153
154 //if the number of peaks is too big, we should try to reduce using siluette matching
155
156 for (const float &peak : peaks)
157 {
158 Eigen::Affine3f rollToRot;
159 computeRollTransform (centroid_input_, centroid_target_, peak, rollToRot);
160
161 Eigen::Matrix4f rollHomMatrix = Eigen::Matrix4f ();
162 rollHomMatrix.setIdentity (4, 4);
163 rollHomMatrix = rollToRot.matrix ();
164
165 Eigen::Matrix4f translation2;
166 translation2.setIdentity (4, 4);
167 Eigen::Vector3f centr = rollToRot * centroid_target_;
168 translation2 (0, 3) = centroid_input_[0] - centr[0];
169 translation2 (1, 3) = centroid_input_[1] - centr[1];
170 translation2 (2, 3) = centroid_input_[2] - centr[2];
171
172 Eigen::Matrix4f resultHom (translation2 * rollHomMatrix);
173 transforms_.push_back(resultHom.inverse());
174 }
175
176 }
177
178 /** \brief Computes the roll angle that aligns input to model.
179 * \param[in] input_ftt CRH histogram of the input cloud
180 * \param[in] target_ftt CRH histogram of the target cloud
181 * \param[out] peaks Vector containing angles where the histograms correlate
182 */
183 void
185 std::vector<float> & peaks)
186 {
187
188 pcl::PointCloud<pcl::Histogram<nbins_> > input_ftt_negate (input_ftt);
189
190 for (int i = 2; i < (nbins_); i += 2)
191 input_ftt_negate[0].histogram[i] = -input_ftt_negate[0].histogram[i];
192
193 int nr_bins_after_padding = 180;
194 int peak_distance = 5;
195 int cutoff = nbins_ - 1;
196
197 kiss_fft_cpx * multAB = new kiss_fft_cpx[nr_bins_after_padding];
198 for (int i = 0; i < nr_bins_after_padding; i++)
199 multAB[i].r = multAB[i].i = 0.f;
200
201 int k = 0;
202 multAB[k].r = input_ftt_negate[0].histogram[0] * target_ftt[0].histogram[0];
203 k++;
204
205 float a, b, c, d;
206 for (int i = 1; i < cutoff; i += 2, k++)
207 {
208 a = input_ftt_negate[0].histogram[i];
209 b = input_ftt_negate[0].histogram[i + 1];
210 c = target_ftt[0].histogram[i];
211 d = target_ftt[0].histogram[i + 1];
212 multAB[k].r = a * c - b * d;
213 multAB[k].i = b * c + a * d;
214
215 float tmp = std::sqrt (multAB[k].r * multAB[k].r + multAB[k].i * multAB[k].i);
216
217 multAB[k].r /= tmp;
218 multAB[k].i /= tmp;
219 }
220
221 multAB[nbins_ - 1].r = input_ftt_negate[0].histogram[nbins_ - 1] * target_ftt[0].histogram[nbins_ - 1];
222
223 kiss_fft_cfg mycfg = kiss_fft_alloc (nr_bins_after_padding, 1, nullptr, nullptr);
224 kiss_fft_cpx * invAB = new kiss_fft_cpx[nr_bins_after_padding];
225 kiss_fft (mycfg, multAB, invAB);
226
227 std::vector < std::pair<float, int> > scored_peaks (nr_bins_after_padding);
228 for (int i = 0; i < nr_bins_after_padding; i++)
229 scored_peaks[i] = std::make_pair (invAB[i].r, i);
230
231 std::sort (scored_peaks.begin (), scored_peaks.end (), peaks_ordering ());
232
233 std::vector<int> peaks_indices;
234 std::vector<float> peaks_values;
235
236 // we look at the upper quantile_
237 float quantile = quantile_;
238 int max_inserted= max_peaks_;
239
240 int inserted=0;
241 bool stop=false;
242 for (int i = 0; (i < static_cast<int> (quantile * static_cast<float> (nr_bins_after_padding))) && !stop; i++)
243 {
244 if (scored_peaks[i].first >= scored_peaks[0].first * accept_threshold_)
245 {
246 bool insert = true;
247
248 for (const int &peaks_index : peaks_indices)
249 { //check inserted peaks, first pick always inserted
250 if ((std::abs (peaks_index - scored_peaks[i].second) <= peak_distance) ||
251 (std::abs (peaks_index - (scored_peaks[i].second - nr_bins_after_padding)) <= peak_distance))
252 {
253 insert = false;
254 break;
255 }
256 }
257
258 if (insert)
259 {
260 peaks_indices.push_back (scored_peaks[i].second);
261 peaks_values.push_back (scored_peaks[i].first);
262 peaks.push_back (static_cast<float> (scored_peaks[i].second * (360 / nr_bins_after_padding)));
263 inserted++;
264 if(inserted >= max_inserted)
265 stop = true;
266 }
267 }
268 }
269 }
270 };
271}
CRHAlignment uses two Camera Roll Histograms (CRH) to find the roll rotation that aligns both views.
void setInputAndTargetCentroids(Eigen::Vector3f &c1, Eigen::Vector3f &c2)
sets model and input centroids
void align(pcl::PointCloud< pcl::Histogram< nbins_ > > &input_ftt, pcl::PointCloud< pcl::Histogram< nbins_ > > &target_ftt)
Computes the transformation aligning model to input.
void computeRollAngle(pcl::PointCloud< pcl::Histogram< nbins_ > > &input_ftt, pcl::PointCloud< pcl::Histogram< nbins_ > > &target_ftt, std::vector< float > &peaks)
Computes the roll angle that aligns input to model.
CRHAlignment()
Constructor.
void setInputAndTargetView(PointTPtr &input_view, PointTPtr &target_view)
sets model and input views
void getTransforms(std::vector< Eigen::Matrix4f, Eigen::aligned_allocator< Eigen::Matrix4f > > &transforms)
returns the computed transformations
PointCloud represents the base class in PCL for storing collections of 3D points.
shared_ptr< PointCloud< PointT > > Ptr
Define standard C methods and C++ classes that are common to all methods.
#define M_PI
Definition pcl_macros.h:203
kiss_fft_scalar r
Definition kiss_fft.h:53
kiss_fft_scalar i
Definition kiss_fft.h:54
A point structure representing an N-D histogram.