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