00001 #include <fstream>
00002 #include <pcl/common/angles.h>
00003 #include <pcl/console/parse.h>
00004 #include <pcl/point_types.h>
00005 #include <pcl/point_cloud.h>
00006 #include <pcl/point_representation.h>
00007
00008 #include <pcl/io/pcd_io.h>
00009 #include <pcl/console/time.h>
00010 #include <pcl/keypoints/uniform_sampling.h>
00011 #include <pcl/features/normal_3d.h>
00012 #include <pcl/features/fpfh.h>
00013 #include <pcl/registration/correspondence_estimation.h>
00014 #include <pcl/registration/correspondence_estimation_normal_shooting.h>
00015 #include <pcl/registration/correspondence_estimation_backprojection.h>
00016 #include <pcl/registration/correspondence_rejection_median_distance.h>
00017 #include <pcl/registration/correspondence_rejection_surface_normal.h>
00018 #include <pcl/registration/transformation_estimation_point_to_plane_lls.h>
00019 #include <pcl/registration/default_convergence_criteria.h>
00020
00021 #include <pcl/visualization/pcl_visualizer.h>
00022
00023 using namespace std;
00024 using namespace pcl;
00025 using namespace pcl::io;
00026 using namespace pcl::console;
00027 using namespace pcl::registration;
00028 using namespace pcl::visualization;
00029
00030 typedef PointNormal PointT;
00031 typedef PointCloud<PointT> Cloud;
00032 typedef Cloud::Ptr CloudPtr;
00033 typedef Cloud::ConstPtr CloudConstPtr;
00034
00035 CloudPtr src, tgt;
00036
00037 bool rejection = true;
00038 bool visualize = false;
00039
00040 boost::shared_ptr<PCLVisualizer> vis;
00041
00043 void
00044 findCorrespondences (const CloudPtr &src,
00045 const CloudPtr &tgt,
00046 Correspondences &all_correspondences)
00047 {
00048
00049
00050 CorrespondenceEstimationBackProjection<PointT, PointT, PointT> est;
00051 est.setInputSource (src);
00052 est.setInputTarget (tgt);
00053
00054 est.setSourceNormals (src);
00055 est.setTargetNormals (tgt);
00056 est.setKSearch (10);
00057 est.determineCorrespondences (all_correspondences);
00058
00059 }
00060
00062 void
00063 rejectBadCorrespondences (const CorrespondencesPtr &all_correspondences,
00064 const CloudPtr &src,
00065 const CloudPtr &tgt,
00066 Correspondences &remaining_correspondences)
00067 {
00068 CorrespondenceRejectorMedianDistance rej;
00069 rej.setMedianFactor (8.79241104);
00070 rej.setInputCorrespondences (all_correspondences);
00071
00072 rej.getCorrespondences (remaining_correspondences);
00073 return;
00074
00075 CorrespondencesPtr remaining_correspondences_temp (new Correspondences);
00076 rej.getCorrespondences (*remaining_correspondences_temp);
00077 PCL_DEBUG ("[rejectBadCorrespondences] Number of correspondences remaining after rejection: %d\n", remaining_correspondences_temp->size ());
00078
00079
00080 CorrespondenceRejectorSurfaceNormal rej_normals;
00081 rej_normals.setThreshold (acos (deg2rad (45.0)));
00082 rej_normals.initializeDataContainer<PointT, PointT> ();
00083 rej_normals.setInputCloud<PointT> (src);
00084 rej_normals.setInputNormals<PointT, PointT> (src);
00085 rej_normals.setInputTarget<PointT> (tgt);
00086 rej_normals.setTargetNormals<PointT, PointT> (tgt);
00087 rej_normals.setInputCorrespondences (remaining_correspondences_temp);
00088 rej_normals.getCorrespondences (remaining_correspondences);
00089 }
00090
00092 void
00093 findTransformation (const CloudPtr &src,
00094 const CloudPtr &tgt,
00095 const CorrespondencesPtr &correspondences,
00096 Eigen::Matrix4d &transform)
00097 {
00098 TransformationEstimationPointToPlaneLLS<PointT, PointT, double> trans_est;
00099 trans_est.estimateRigidTransformation (*src, *tgt, *correspondences, transform);
00100 }
00101
00103 void
00104 view (const CloudConstPtr &src, const CloudConstPtr &tgt, const CorrespondencesPtr &correspondences)
00105 {
00106 if (!visualize || !vis) return;
00107 PointCloudColorHandlerCustom<PointT> green (tgt, 0, 255, 0);
00108 if (!vis->updatePointCloud<PointT> (src, "source"))
00109 {
00110 vis->addPointCloud<PointT> (src, "source");
00111 vis->resetCameraViewpoint ("source");
00112 }
00113 if (!vis->updatePointCloud<PointT> (tgt, green, "target")) vis->addPointCloud<PointT> (tgt, green, "target");
00114 vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.5, "source");
00115 vis->setPointCloudRenderingProperties (PCL_VISUALIZER_OPACITY, 0.7, "target");
00116 vis->setPointCloudRenderingProperties (PCL_VISUALIZER_POINT_SIZE, 6, "source");
00117 pcl::console::TicToc tt;
00118 tt.tic ();
00119 if (!vis->updateCorrespondences<PointT> (src, tgt, *correspondences, 1))
00120 vis->addCorrespondences<PointT> (src, tgt, *correspondences, 1, "correspondences");
00121 tt.toc_print ();
00122 vis->setShapeRenderingProperties (PCL_VISUALIZER_LINE_WIDTH, 5, "correspondences");
00123
00124 vis->spin ();
00125 }
00126
00128 void
00129 icp (const PointCloud<PointT>::Ptr &src,
00130 const PointCloud<PointT>::Ptr &tgt,
00131 Eigen::Matrix4d &transform)
00132 {
00133 CorrespondencesPtr all_correspondences (new Correspondences),
00134 good_correspondences (new Correspondences);
00135
00136 PointCloud<PointT>::Ptr output (new PointCloud<PointT>);
00137 *output = *src;
00138
00139 Eigen::Matrix4d final_transform (Eigen::Matrix4d::Identity ());
00140
00141 int iterations = 0;
00142 DefaultConvergenceCriteria<double> converged (iterations, transform, *good_correspondences);
00143
00144
00145 do
00146 {
00147
00148 findCorrespondences (output, tgt, *all_correspondences);
00149 PCL_DEBUG ("Number of correspondences found: %d\n", all_correspondences->size ());
00150
00151 if (rejection)
00152 {
00153
00154 rejectBadCorrespondences (all_correspondences, output, tgt, *good_correspondences);
00155 PCL_DEBUG ("Number of correspondences remaining after rejection: %d\n", good_correspondences->size ());
00156 }
00157 else
00158 *good_correspondences = *all_correspondences;
00159
00160
00161 findTransformation (output, tgt, good_correspondences, transform);
00162
00163
00164 final_transform = transform * final_transform;
00165
00166
00167 transformPointCloudWithNormals (*src, *output, final_transform.cast<float> ());
00168
00169
00170 ++iterations;
00171
00172
00173 view (output, tgt, good_correspondences);
00174 }
00175 while (!converged);
00176 transform = final_transform;
00177 }
00178
00180 void
00181 saveTransform (const std::string &file, const Eigen::Matrix4d &transform)
00182 {
00183 ofstream ofs;
00184 ofs.open (file.c_str (), ios::trunc | ios::binary);
00185 for (int i = 0; i < 4; ++i)
00186 for (int j = 0; j < 4; ++j)
00187 ofs.write (reinterpret_cast<const char*>(&transform (i, j)), sizeof (double));
00188 ofs.close ();
00189 }
00190
00191
00192 int
00193 main (int argc, char** argv)
00194 {
00195
00196 bool debug = false;
00197 parse_argument (argc, argv, "-debug", debug);
00198 if (debug)
00199 setVerbosityLevel (L_DEBUG);
00200
00201 parse_argument (argc, argv, "-rejection", rejection);
00202 parse_argument (argc, argv, "-visualization", visualize);
00203 if (visualize)
00204 vis.reset (new PCLVisualizer ("Registration example"));
00205
00206
00207 std::vector<int> p_pcd_file_indices, p_tr_file_indices;
00208 p_pcd_file_indices = parse_file_extension_argument (argc, argv, ".pcd");
00209 if (p_pcd_file_indices.size () != 2)
00210 {
00211 print_error ("Need one input source PCD file and one input target PCD file to continue.\n");
00212 print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
00213 return (-1);
00214 }
00215 p_tr_file_indices = parse_file_extension_argument (argc, argv, ".transform");
00216 if (p_tr_file_indices.size () != 1)
00217 {
00218 print_error ("Need one output transform file to continue.\n");
00219 print_error ("Example: %s source.pcd target.pcd output.transform\n", argv[0]);
00220 return (-1);
00221 }
00222
00223
00224 print_info ("Loading %s as source and %s as target...\n", argv[p_pcd_file_indices[0]], argv[p_pcd_file_indices[1]]);
00225 src.reset (new PointCloud<PointT>);
00226 tgt.reset (new PointCloud<PointT>);
00227 if (loadPCDFile (argv[p_pcd_file_indices[0]], *src) == -1 || loadPCDFile (argv[p_pcd_file_indices[1]], *tgt) == -1)
00228 {
00229 print_error ("Error reading the input files!\n");
00230 return (-1);
00231 }
00232
00233
00234 Eigen::Matrix4d transform;
00235 icp (src, tgt, transform);
00236
00237 saveTransform (argv[p_tr_file_indices[0]], transform);
00238
00239 cerr.precision (15);
00240 std::cerr << transform << std::endl;
00241 }
00242