crh_alignment.h
Go to the documentation of this file.
00001 /*
00002  * crh_recognition.h
00003  *
00004  *  Created on: Mar 30, 2012
00005  *      Author: aitor
00006  */
00007 
00008 #ifndef CRH_ALIGNMENT_H_
00009 #define CRH_ALIGNMENT_H_
00010 
00011 #include <pcl/common/common.h>
00012 #include <pcl/features/crh.h>
00013 #include <pcl/common/fft/kiss_fftr.h>
00014 #include <pcl/common/transforms.h>
00015 
00016 namespace pcl
00017 {
00018 
00030   template<typename PointT, int nbins_>
00031     class PCL_EXPORTS CRHAlignment
00032     {
00033     private:
00034 
00036       typedef struct
00037       {
00038         bool
00039         operator() (std::pair<float, int> const& a, std::pair<float, int> const& b)
00040         {
00041           return a.first > b.first;
00042         }
00043       } peaks_ordering;
00044 
00045       typedef typename pcl::PointCloud<PointT>::Ptr PointTPtr;
00046 
00048       PointTPtr target_view_;
00050       PointTPtr input_view_;
00052       Eigen::Vector3f centroid_target_;
00054       Eigen::Vector3f centroid_input_;
00056       std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_;
00058       int max_peaks_;
00060       float quantile_;
00064       float accept_threshold_;
00065 
00070       void
00071       computeTransformToZAxes (Eigen::Vector3f & centroid, Eigen::Affine3f & transform)
00072       {
00073         Eigen::Vector3f plane_normal;
00074         plane_normal[0] = -centroid[0];
00075         plane_normal[1] = -centroid[1];
00076         plane_normal[2] = -centroid[2];
00077         Eigen::Vector3f z_vector = Eigen::Vector3f::UnitZ ();
00078         plane_normal.normalize ();
00079         Eigen::Vector3f axis = plane_normal.cross (z_vector);
00080         double rotation = -asin (axis.norm ());
00081         axis.normalize ();
00082         transform = Eigen::Affine3f (Eigen::AngleAxisf (static_cast<float>(rotation), axis));
00083       }
00084 
00091       void
00092       computeRollTransform (Eigen::Vector3f & centroidInput, Eigen::Vector3f & centroidResult, double roll_angle, Eigen::Affine3f & final_trans)
00093       {
00094         Eigen::Affine3f transformInputToZ;
00095         computeTransformToZAxes (centroidInput, transformInputToZ);
00096 
00097         transformInputToZ = transformInputToZ.inverse ();
00098         Eigen::Affine3f transformRoll (Eigen::AngleAxisf (-static_cast<float>(roll_angle * M_PI / 180), Eigen::Vector3f::UnitZ ()));
00099         Eigen::Affine3f transformDBResultToZ;
00100         computeTransformToZAxes (centroidResult, transformDBResultToZ);
00101 
00102         final_trans = transformInputToZ * transformRoll * transformDBResultToZ;
00103       }
00104     public:
00105 
00107       CRHAlignment() {
00108         max_peaks_ = 5;
00109         quantile_ = 0.2f;
00110         accept_threshold_ = 0.8f;
00111       }
00112 
00116       void getTransforms(std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transforms) {
00117         transforms = transforms_;
00118       }
00119 
00124       void
00125       setInputAndTargetView (PointTPtr & input_view, PointTPtr & target_view)
00126       {
00127         target_view_ = target_view;
00128         input_view_ = input_view;
00129       }
00130 
00135       void
00136       setInputAndTargetCentroids (Eigen::Vector3f & c1, Eigen::Vector3f & c2)
00137       {
00138         centroid_target_ = c2;
00139         centroid_input_ = c1;
00140       }
00141 
00146       void
00147       align (pcl::PointCloud<pcl::Histogram<nbins_> > & input_ftt, pcl::PointCloud<pcl::Histogram<nbins_> > & target_ftt)
00148       {
00149 
00150         transforms_.clear(); //clear from last round...
00151 
00152         std::vector<float> peaks;
00153         computeRollAngle (input_ftt, target_ftt, peaks);
00154 
00155         //if the number of peaks is too big, we should try to reduce using siluette matching
00156 
00157         for (size_t i = 0; i < peaks.size(); i++)
00158         {
00159           Eigen::Affine3f rollToRot;
00160           computeRollTransform (centroid_input_, centroid_target_, peaks[i], rollToRot);
00161 
00162           Eigen::Matrix4f rollHomMatrix = Eigen::Matrix4f ();
00163           rollHomMatrix.setIdentity (4, 4);
00164           rollHomMatrix = rollToRot.matrix ();
00165 
00166           Eigen::Matrix4f translation2;
00167           translation2.setIdentity (4, 4);
00168           Eigen::Vector3f centr = rollToRot * centroid_target_;
00169           translation2 (0, 3) = centroid_input_[0] - centr[0];
00170           translation2 (1, 3) = centroid_input_[1] - centr[1];
00171           translation2 (2, 3) = centroid_input_[2] - centr[2];
00172 
00173           Eigen::Matrix4f resultHom (translation2 * rollHomMatrix);
00174           transforms_.push_back(resultHom.inverse());
00175         }
00176 
00177       }
00178 
00184       void
00185       computeRollAngle (pcl::PointCloud<pcl::Histogram<nbins_> > & input_ftt, pcl::PointCloud<pcl::Histogram<nbins_> > & target_ftt,
00186                         std::vector<float> & peaks)
00187       {
00188 
00189         pcl::PointCloud<pcl::Histogram<nbins_> > input_ftt_negate (input_ftt);
00190 
00191         for (int i = 2; i < (nbins_); i += 2)
00192           input_ftt_negate.points[0].histogram[i] = -input_ftt_negate.points[0].histogram[i];
00193 
00194         int nr_bins_after_padding = 180;
00195         int peak_distance = 5;
00196         int cutoff = nbins_ - 1;
00197 
00198         kiss_fft_cpx * multAB = new kiss_fft_cpx[nr_bins_after_padding];
00199         for (int i = 0; i < nr_bins_after_padding; i++)
00200           multAB[i].r = multAB[i].i = 0.f;
00201 
00202         int k = 0;
00203         multAB[k].r = input_ftt_negate.points[0].histogram[0] * target_ftt.points[0].histogram[0];
00204         k++;
00205 
00206         float a, b, c, d;
00207         for (int i = 1; i < cutoff; i += 2, k++)
00208         {
00209           a = input_ftt_negate.points[0].histogram[i];
00210           b = input_ftt_negate.points[0].histogram[i + 1];
00211           c = target_ftt.points[0].histogram[i];
00212           d = target_ftt.points[0].histogram[i + 1];
00213           multAB[k].r = a * c - b * d;
00214           multAB[k].i = b * c + a * d;
00215 
00216           float tmp = sqrtf (multAB[k].r * multAB[k].r + multAB[k].i * multAB[k].i);
00217 
00218           multAB[k].r /= tmp;
00219           multAB[k].i /= tmp;
00220         }
00221 
00222         multAB[nbins_ - 1].r = input_ftt_negate.points[0].histogram[nbins_ - 1] * target_ftt.points[0].histogram[nbins_ - 1];
00223 
00224         kiss_fft_cfg mycfg = kiss_fft_alloc (nr_bins_after_padding, 1, NULL, NULL);
00225         kiss_fft_cpx * invAB = new kiss_fft_cpx[nr_bins_after_padding];
00226         kiss_fft (mycfg, multAB, invAB);
00227 
00228         std::vector < std::pair<float, int> > scored_peaks (nr_bins_after_padding);
00229         for (int i = 0; i < nr_bins_after_padding; i++)
00230           scored_peaks[i] = std::make_pair<float, int> (invAB[i].r, i);
00231 
00232         std::sort (scored_peaks.begin (), scored_peaks.end (), peaks_ordering ());
00233 
00234         std::vector<int> peaks_indices;
00235         std::vector<float> peaks_values;
00236 
00237         // we look at the upper quantile_
00238         float quantile = quantile_;
00239         int max_inserted= max_peaks_;
00240 
00241         int inserted=0;
00242         bool stop=false;
00243         for (int i = 0; (i < static_cast<int> (quantile * static_cast<float> (nr_bins_after_padding))) && !stop; i++)
00244         {
00245           if (scored_peaks[i].first >= scored_peaks[0].first * accept_threshold_)
00246           {
00247             bool insert = true;
00248 
00249             for (size_t j = 0; j < peaks_indices.size (); j++)
00250             { //check inserted peaks, first pick always inserted
00251               if (std::abs (peaks_indices[j] - scored_peaks[i].second) <= peak_distance || std::abs (
00252                                                                                              peaks_indices[j] - (scored_peaks[i].second
00253                                                                                                  - nr_bins_after_padding)) <= peak_distance)
00254               {
00255                 insert = false;
00256                 break;
00257               }
00258             }
00259 
00260             if (insert)
00261             {
00262               peaks_indices.push_back (scored_peaks[i].second);
00263               peaks_values.push_back (scored_peaks[i].first);
00264               peaks.push_back (static_cast<float> (scored_peaks[i].second * (360 / nr_bins_after_padding)));
00265               inserted++;
00266               if(inserted >= max_inserted)
00267                 stop = true;
00268             }
00269           }
00270         }
00271       }
00272     };
00273 }
00274 
00275 #endif /* CRH_ALIGNMENT_H_ */


pcl
Author(s): Open Perception
autogenerated on Wed Aug 26 2015 15:23:14