registration.h
Go to the documentation of this file.
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


pcl
Author(s): Open Perception
autogenerated on Mon Oct 6 2014 03:17:37