00001 #include <limits>
00002 #include <fstream>
00003 #include <vector>
00004 #include <Eigen/Core>
00005 #include "pcl/point_types.h"
00006 #include "pcl/point_cloud.h"
00007 #include "pcl/io/pcd_io.h"
00008 #include "pcl/kdtree/kdtree_flann.h"
00009 #include "pcl/filters/passthrough.h"
00010 #include "pcl/filters/voxel_grid.h"
00011 #include "pcl/features/normal_3d.h"
00012 #include "pcl/features/fpfh.h"
00013 #include "pcl/registration/ia_ransac.h"
00014 #include "pcl/registration/icp.h"
00015 #include <pcl/surface/mls.h>
00016
00017 #include <NDTMatcherF2F.hh>
00018 #include <PointCloudUtils.hh>
00019
00020 class FeatureCloud
00021 {
00022 public:
00023
00024 typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;
00025 typedef pcl::PointCloud<pcl::Normal> SurfaceNormals;
00026 typedef pcl::PointCloud<pcl::FPFHSignature33> LocalFeatures;
00027
00028
00029
00030 FeatureCloud () :
00031
00032 normal_radius_ (0.05),
00033 feature_radius_ (0.05)
00034 {
00035 }
00036
00037 ~FeatureCloud () {}
00038
00039
00040 void
00041 setInputCloud (PointCloud::Ptr xyz)
00042 {
00043 xyz_ = xyz;
00044 processInput ();
00045 }
00046
00047
00048 void
00049 loadInputCloud (const std::string &pcd_file)
00050 {
00051 xyz_ = PointCloud::Ptr (new PointCloud);
00052 pcl::io::loadPCDFile (pcd_file, *xyz_);
00053 processInput ();
00054 }
00055
00056
00057 PointCloud::Ptr
00058 getPointCloud () const
00059 {
00060 return (xyz_);
00061 }
00062
00063
00064 SurfaceNormals::Ptr
00065 getSurfaceNormals () const
00066 {
00067 return (normals_);
00068 }
00069
00070
00071 LocalFeatures::Ptr
00072 getLocalFeatures () const
00073 {
00074 return (features_);
00075 }
00076
00077 protected:
00078
00079 void
00080 processInput ()
00081 {
00082 computeSurfaceNormals ();
00083 computeLocalFeatures ();
00084 }
00085
00086
00087 void
00088 computeSurfaceNormals ()
00089 {
00090 normals_ = SurfaceNormals::Ptr (new SurfaceNormals);
00091
00092 pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> norm_est;
00093 norm_est.setInputCloud (xyz_);
00094 norm_est.setSearchMethod (search_method_xyz_);
00095 norm_est.setRadiusSearch (normal_radius_);
00096 norm_est.compute (*normals_);
00097 }
00098
00099
00100 void
00101 computeLocalFeatures ()
00102 {
00103 features_ = LocalFeatures::Ptr (new LocalFeatures);
00104
00105 pcl::FPFHEstimation<pcl::PointXYZ, pcl::Normal, pcl::FPFHSignature33> fpfh_est;
00106 fpfh_est.setInputCloud (xyz_);
00107 fpfh_est.setInputNormals (normals_);
00108 fpfh_est.setSearchMethod (search_method_xyz_);
00109 fpfh_est.setRadiusSearch (feature_radius_);
00110 fpfh_est.compute (*features_);
00111 }
00112
00113 private:
00114
00115 PointCloud::Ptr xyz_;
00116 SurfaceNormals::Ptr normals_;
00117 LocalFeatures::Ptr features_;
00118
00119 pcl::Feature<pcl::PointXYZ, pcl::Normal>::KdTreePtr search_method_xyz_;
00120
00121
00122 float normal_radius_;
00123 float feature_radius_;
00124 };
00125
00126 class TemplateRegistration
00127 {
00128 public:
00129
00130
00131
00132 typedef Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> Result;
00133
00134 TemplateRegistration () :
00135 min_sample_distance_ (0.05),
00136 max_correspondence_distance_ (0.01*0.01),
00137 nr_iterations_ (500)
00138 {
00139
00140 sac_ia_.setMinSampleDistance (min_sample_distance_);
00141 sac_ia_.setMaxCorrespondenceDistance (max_correspondence_distance_);
00142 sac_ia_.setMaximumIterations (nr_iterations_);
00143 sac_ia_.setNumberOfSamples (10);
00144 }
00145
00146 ~TemplateRegistration () {}
00147
00148
00149 void
00150 setTargetCloud (FeatureCloud &target_cloud)
00151 {
00152 target_ = target_cloud;
00153 sac_ia_.setInputTarget (target_cloud.getPointCloud ());
00154 sac_ia_.setTargetFeatures (target_cloud.getLocalFeatures ());
00155 }
00156
00157
00158 void
00159 align (FeatureCloud &moving_cloud, TemplateRegistration::Result &result)
00160 {
00161 sac_ia_.setInputCloud (moving_cloud.getPointCloud ());
00162 sac_ia_.setSourceFeatures (moving_cloud.getLocalFeatures ());
00163
00164 pcl::PointCloud<pcl::PointXYZ> registration_output;
00165 sac_ia_.align (registration_output);
00166
00167 result = sac_ia_.getFinalTransformation ().cast<double>();
00168 }
00169
00170 private:
00171
00172 FeatureCloud target_;
00173
00174
00175 pcl::SampleConsensusInitialAlignment<pcl::PointXYZ, pcl::PointXYZ, pcl::FPFHSignature33> sac_ia_;
00176 float min_sample_distance_;
00177 float max_correspondence_distance_;
00178 float nr_iterations_;
00179 };
00180
00181 bool matchICP(pcl::PointCloud<pcl::PointXYZ> &fixed, pcl::PointCloud<pcl::PointXYZ> &moving,
00182 Eigen::Transform<double,3,Eigen::Affine,Eigen::ColMajor> &Tout)
00183 {
00184
00185 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
00186 pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);
00187 pcl::PointCloud<pcl::PointXYZ>::ConstPtr f (new pcl::PointCloud<pcl::PointXYZ>(fixed) );
00188 pcl::PointCloud<pcl::PointXYZ>::ConstPtr m (new pcl::PointCloud<pcl::PointXYZ>(moving) );
00189
00190 pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
00191 gr1.setLeafSize(0.1,0.1,0.1);
00192 gr2.setLeafSize(0.1,0.1,0.1);
00193
00194 gr1.setInputCloud(m);
00195 gr2.setInputCloud(f);
00196
00197 cloud_in->height = 1;
00198 cloud_in->width = cloud_in->points.size();
00199 cloud_out->height = 1;
00200 cloud_out->width = cloud_out->points.size();
00201 cloud_in->is_dense = false;
00202 cloud_out->is_dense = false;
00203
00204 gr1.filter(*cloud_in);
00205 gr2.filter(*cloud_out);
00206
00207
00208
00209
00210 pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
00211
00212 icp.setMaximumIterations(10000);
00213 std::cout<<"max itr are "<<icp.getMaximumIterations()<<std::endl;
00214 icp.setInputCloud(cloud_in);
00215 icp.setInputTarget(cloud_out);
00216
00217 icp.setRANSACOutlierRejectionThreshold (2);
00218 icp.setMaxCorrespondenceDistance(10);
00219 icp.setTransformationEpsilon(0.00001);
00220
00221
00222
00223 pcl::PointCloud<pcl::PointXYZ> Final;
00224 icp.align(Final);
00225
00226
00227
00228
00229
00230
00231
00232 Tout = (icp.getFinalTransformation()).cast<double>();
00233
00234
00235
00236
00237
00238
00239
00240
00241
00242
00243 return icp.hasConverged();
00244
00245 }
00246
00247
00248 int
00249 main (int argc, char **argv)
00250 {
00251 if (argc < 3)
00252 {
00253 printf ("No targets given!\n");
00254 return (-1);
00255 }
00256
00257 struct timeval tv_start, tv_end1, tv_end2;
00258 TemplateRegistration::Result ToutFPFH, ToutICP, ToutNDT, Tout;
00259
00260
00261 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudM (new pcl::PointCloud<pcl::PointXYZ>);
00262 pcl::PointCloud<pcl::PointXYZ>::Ptr cloudF (new pcl::PointCloud<pcl::PointXYZ>);
00263 pcl::PointCloud<pcl::PointXYZ> Final, Final0, Final1;
00264
00265 *cloudM = lslgeneric::readVRML(argv[1]);
00266 *cloudF = lslgeneric::readVRML(argv[2]);
00267
00268 double __res[] = {0.2, 0.4, 1, 2};
00269 std::vector<double> resolutions (__res, __res+sizeof(__res)/sizeof(double));
00270 lslgeneric::NDTMatcherF2F matcherF2F(false, false, false, resolutions);
00271 bool ret = matcherF2F.match(*cloudF,*cloudM,ToutNDT);
00272 Final1 = lslgeneric::transformPointCloud(ToutNDT,*cloudM);
00273
00274
00275
00276
00277
00278
00279
00280
00281
00282
00283
00284
00285 gettimeofday(&tv_start,NULL);
00286
00287 pcl::VoxelGrid<pcl::PointXYZ> gr1,gr2;
00288 gr1.setLeafSize(0.05,0.05,0.05);
00289 gr2.setLeafSize(0.05,0.05,0.05);
00290
00291 gr1.setInputCloud(cloudM);
00292 gr2.setInputCloud(cloudF);
00293
00294 gr1.filter(*cloudM);
00295 gr2.filter(*cloudF);
00296
00297 cloudM->height = 1;
00298 cloudM->width = cloudM->points.size();
00299 cloudF->height = 1;
00300 cloudF->width = cloudF->points.size();
00301 cloudM->is_dense = false;
00302 cloudF->is_dense = false;
00303
00304
00305 FeatureCloud target_cloud, moving_cloud;
00306 target_cloud.setInputCloud (cloudF);
00307 moving_cloud.setInputCloud (cloudM);
00308
00309 TemplateRegistration templateReg;
00310 templateReg.setTargetCloud (target_cloud);
00311
00312
00313 templateReg.align(moving_cloud,ToutFPFH);
00314
00315 gettimeofday(&tv_end1,NULL);
00316
00317 std::cout<<"ToutFPFH: "<<ToutFPFH.translation().transpose()<<"\n"<<ToutFPFH.rotation()<<std::endl;
00318 Final0 = lslgeneric::transformPointCloud(ToutFPFH,*cloudM);
00319
00320 bool converged = matchICP(*cloudF,Final0,ToutICP);
00321 Final = lslgeneric::transformPointCloud(ToutICP,Final0);
00322
00323 gettimeofday(&tv_end2,NULL);
00324
00325
00326 Tout = ToutFPFH*(ToutNDT.inverse());
00327 std::cout<<"FPFH\n";
00328 std::cout<<"E translation "<<Tout.translation().transpose()
00329 <<" (norm) "<<Tout.translation().norm()<<std::endl;
00330 std::cout<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
00331 <<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00332 std::cout<<" TIME: "<<
00333 (tv_end1.tv_sec-tv_start.tv_sec)*1000.+(tv_end1.tv_usec-tv_start.tv_usec)/1000.<<std::endl;
00334
00335 Tout = ToutICP*ToutFPFH*(ToutNDT.inverse());
00336 std::cout<<"FPFH+ICP\n";
00337 std::cout<<"E translation "<<Tout.translation().transpose()
00338 <<" (norm) "<<Tout.translation().norm()<<std::endl;
00339 std::cout<<"E rotation "<<Tout.rotation().eulerAngles(0,1,2).transpose()
00340 <<" (norm) "<<Tout.rotation().eulerAngles(0,1,2).norm()<<std::endl;
00341 std::cout<<" TIME: "<<
00342 (tv_end2.tv_sec-tv_start.tv_sec)*1000.+(tv_end2.tv_usec-tv_start.tv_usec)/1000.<<std::endl;
00343
00344 char fname[50];
00345 snprintf(fname,49,"/home/tsv/ndt_tmp/c2_offset.wrl");
00346 FILE *fout = fopen(fname,"w");
00347 fprintf(fout,"#VRML V2.0 utf8\n");
00348 lslgeneric::writeToVRML(fout,*cloudM,Eigen::Vector3d(1,0,0));
00349 lslgeneric::writeToVRML(fout,Final0,Eigen::Vector3d(0,0,1));
00350 lslgeneric::writeToVRML(fout,Final1,Eigen::Vector3d(0,1,1));
00351 lslgeneric::writeToVRML(fout,Final,Eigen::Vector3d(0,1,0));
00352 lslgeneric::writeToVRML(fout,*cloudF,Eigen::Vector3d(1,1,1));
00353 fclose(fout);
00354
00355
00356
00357 return (0);
00358 }