Point Cloud Library (PCL)  1.14.1-dev
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 
15 namespace 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_>
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.
Definition: crh_alignment.h:31
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.
Definition: point_cloud.h:173
shared_ptr< PointCloud< PointT > > Ptr
Definition: point_cloud.h:413
Define standard C methods and C++ classes that are common to all methods.
#define PCL_EXPORTS
Definition: pcl_macros.h:323
#define M_PI
Definition: pcl_macros.h:201
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.