Point Cloud Library (PCL)
1.7.0
|
00001 #ifndef REGISTRATION_H 00002 #define REGISTRATION_H 00003 00004 #include "typedefs.h" 00005 00006 #include <pcl/registration/ia_ransac.h> 00007 #include <pcl/registration/icp.h> 00008 00009 /* Use SampleConsensusInitialAlignment to find a rough alignment from the source cloud to the target cloud by finding 00010 * correspondences between two sets of local features 00011 * Inputs: 00012 * source_points 00013 * The "source" points, i.e., the points that must be transformed to align with the target point cloud 00014 * source_descriptors 00015 * The local descriptors for each source point 00016 * target_points 00017 * The "target" points, i.e., the points to which the source point cloud will be aligned 00018 * target_descriptors 00019 * The local descriptors for each target point 00020 * min_sample_distance 00021 * The minimum distance between any two random samples 00022 * max_correspondence_distance 00023 * The 00024 * nr_interations 00025 * The number of RANSAC iterations to perform 00026 * Return: A transformation matrix that will roughly align the points in source to the points in target 00027 */ 00028 Eigen::Matrix4f 00029 computeInitialAlignment (const PointCloudPtr & source_points, const LocalDescriptorsPtr & source_descriptors, 00030 const PointCloudPtr & target_points, const LocalDescriptorsPtr & target_descriptors, 00031 float min_sample_distance, float max_correspondence_distance, int nr_iterations) 00032 { 00033 pcl::SampleConsensusInitialAlignment<PointT, PointT, LocalDescriptorT> sac_ia; 00034 sac_ia.setMinSampleDistance (min_sample_distance); 00035 sac_ia.setMaxCorrespondenceDistance (max_correspondence_distance); 00036 sac_ia.setMaximumIterations (nr_iterations); 00037 00038 sac_ia.setInputCloud (source_points); 00039 sac_ia.setSourceFeatures (source_descriptors); 00040 00041 sac_ia.setInputTarget (target_points); 00042 sac_ia.setTargetFeatures (target_descriptors); 00043 00044 PointCloud registration_output; 00045 sac_ia.align (registration_output); 00046 00047 return (sac_ia.getFinalTransformation ()); 00048 } 00049 00050 /* Use IterativeClosestPoint to find a precise alignment from the source cloud to the target cloud, 00051 * starting with an intial guess 00052 * Inputs: 00053 * source_points 00054 * The "source" points, i.e., the points that must be transformed to align with the target point cloud 00055 * target_points 00056 * The "target" points, i.e., the points to which the source point cloud will be aligned 00057 * intial_alignment 00058 * An initial estimate of the transformation matrix that aligns the source points to the target points 00059 * max_correspondence_distance 00060 * A threshold on the distance between any two corresponding points. Any corresponding points that are further 00061 * apart than this threshold will be ignored when computing the source-to-target transformation 00062 * outlier_rejection_threshold 00063 * A threshold used to define outliers during RANSAC outlier rejection 00064 * transformation_epsilon 00065 * The smallest iterative transformation allowed before the algorithm is considered to have converged 00066 * max_iterations 00067 * The maximum number of ICP iterations to perform 00068 * Return: A transformation matrix that will precisely align the points in source to the points in target 00069 */ 00070 Eigen::Matrix4f 00071 refineAlignment (const PointCloudPtr & source_points, const PointCloudPtr & target_points, 00072 const Eigen::Matrix4f & initial_alignment, float max_correspondence_distance, 00073 float outlier_rejection_threshold, float transformation_epsilon, float max_iterations) 00074 { 00075 00076 pcl::IterativeClosestPoint<PointT, PointT> icp; 00077 icp.setMaxCorrespondenceDistance (max_correspondence_distance); 00078 icp.setRANSACOutlierRejectionThreshold (outlier_rejection_threshold); 00079 icp.setTransformationEpsilon (transformation_epsilon); 00080 icp.setMaximumIterations (max_iterations); 00081 00082 PointCloudPtr source_points_transformed (new PointCloud); 00083 pcl::transformPointCloud (*source_points, *source_points_transformed, initial_alignment); 00084 00085 icp.setInputCloud (source_points_transformed); 00086 icp.setInputTarget (target_points); 00087 00088 PointCloud registration_output; 00089 icp.align (registration_output); 00090 00091 return (icp.getFinalTransformation () * initial_alignment); 00092 } 00093 00094 #endif