Point Cloud Library (PCL)
1.7.0
|
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 00019 /** \brief CRHAlignment uses two Camera Roll Histograms (CRH) to find the 00020 * roll rotation that aligns both views. See: 00021 * - CAD-Model Recognition and 6 DOF Pose Estimation 00022 * A. Aldoma, N. Blodow, D. Gossow, S. Gedikli, R.B. Rusu, M. Vincze and G. Bradski 00023 * ICCV 2011, 3D Representation and Recognition (3dRR11) workshop 00024 * Barcelona, Spain, (2011) 00025 * 00026 * \author Aitor Aldoma 00027 * \ingroup recognition 00028 */ 00029 00030 template<typename PointT, int nbins_> 00031 class PCL_EXPORTS CRHAlignment 00032 { 00033 private: 00034 00035 /** \brief Sorts peaks */ 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 00047 /** \brief View of the model to be aligned to input_view_ */ 00048 PointTPtr target_view_; 00049 /** \brief View of the input */ 00050 PointTPtr input_view_; 00051 /** \brief Centroid of the model_view_ */ 00052 Eigen::Vector3f centroid_target_; 00053 /** \brief Centroid of the input_view_ */ 00054 Eigen::Vector3f centroid_input_; 00055 /** \brief transforms from model view to input view */ 00056 std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > transforms_; 00057 /** \brief Allowed maximum number of peaks */ 00058 int max_peaks_; 00059 /** \brief Quantile of peaks after sorting to be checked */ 00060 float quantile_; 00061 /** \brief Threshold for a peak to be accepted. 00062 * If peak_i >= (max_peak * accept_threhsold_) => peak is accepted 00063 */ 00064 float accept_threshold_; 00065 00066 /** \brief computes the transformation to the z-axis 00067 * \param[in] centroid 00068 * \param[out] trasnformation to z-axis 00069 */ 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 00085 /** \brief computes the roll transformation 00086 * \param[in] centroid input 00087 * \param[in] centroid view 00088 * \param[in] roll_angle 00089 * \param[out] roll transformation 00090 */ 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 00106 /** \brief Constructor. */ 00107 CRHAlignment() { 00108 max_peaks_ = 5; 00109 quantile_ = 0.2f; 00110 accept_threshold_ = 0.8f; 00111 } 00112 00113 /** \brief returns the computed transformations 00114 * \param[out] transformations 00115 */ 00116 void getTransforms(std::vector<Eigen::Matrix4f, Eigen::aligned_allocator<Eigen::Matrix4f> > & transforms) { 00117 transforms = transforms_; 00118 } 00119 00120 /** \brief sets model and input views 00121 * \param[in] model view 00122 * \param[in] input_view 00123 */ 00124 void 00125 setInputAndTargetView (PointTPtr & input_view, PointTPtr & target_view) 00126 { 00127 target_view_ = target_view; 00128 input_view_ = input_view; 00129 } 00130 00131 /** \brief sets model and input centroids 00132 * \param[in] c1 model view centroid 00133 * \param[in] c2 input view centroid 00134 */ 00135 void 00136 setInputAndTargetCentroids (Eigen::Vector3f & c1, Eigen::Vector3f & c2) 00137 { 00138 centroid_target_ = c2; 00139 centroid_input_ = c1; 00140 } 00141 00142 /** \brief Computes the transformation aligning model to input 00143 * \param[in] input_ftt CRH histogram of the input cloud 00144 * \param[in] target_ftt CRH histogram of the target cloud 00145 */ 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 00179 /** \brief Computes the roll angle that aligns input to modle. 00180 * \param[in] CRH histogram of the input cloud 00181 * \param[in] CRH histogram of the target cloud 00182 * \param[out] Vector containing angles where the histograms correlate 00183 */ 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_ */