5#include <pcl/registration/ia_ransac.h>
6#include <pcl/registration/icp.h>
28computeInitialAlignment (
const PointCloudPtr & source_points,
const LocalDescriptorsPtr & source_descriptors,
29 const PointCloudPtr & target_points,
const LocalDescriptorsPtr & target_descriptors,
30 float min_sample_distance,
float max_correspondence_distance,
int nr_iterations)
33 sac_ia.setMinSampleDistance (min_sample_distance);
34 sac_ia.setMaxCorrespondenceDistance (max_correspondence_distance);
35 sac_ia.setMaximumIterations (nr_iterations);
37 sac_ia.setInputSource (source_points);
38 sac_ia.setSourceFeatures (source_descriptors);
40 sac_ia.setInputTarget (target_points);
41 sac_ia.setTargetFeatures (target_descriptors);
44 sac_ia.align (registration_output);
46 return (sac_ia.getFinalTransformation ());
70refineAlignment (
const PointCloudPtr & source_points,
const PointCloudPtr & target_points,
71 const Eigen::Matrix4f & initial_alignment,
float max_correspondence_distance,
72 float outlier_rejection_threshold,
float transformation_epsilon,
float max_iterations)
76 icp.setMaxCorrespondenceDistance (max_correspondence_distance);
77 icp.setRANSACOutlierRejectionThreshold (outlier_rejection_threshold);
78 icp.setTransformationEpsilon (transformation_epsilon);
79 icp.setMaximumIterations (max_iterations);
81 PointCloudPtr source_points_transformed (
new PointCloud);
84 icp.setInputSource (source_points_transformed);
85 icp.setInputTarget (target_points);
88 icp.align (registration_output);
90 return (icp.getFinalTransformation () * initial_alignment);
Iterator class for point clouds with or without given indices.
PointCloud represents the base class in PCL for storing collections of 3D points.
void transformPointCloud(const pcl::PointCloud< PointT > &cloud_in, pcl::PointCloud< PointT > &cloud_out, const Eigen::Matrix< Scalar, 4, 4 > &transform, bool copy_all_fields)
Apply a rigid transform defined by a 4x4 matrix.