registration.h
#pragma once
#include "typedefs.h"
#include <pcl/registration/ia_ransac.h>
#include <pcl/registration/icp.h>
/* Use SampleConsensusInitialAlignment to find a rough alignment from the source cloud to the target cloud by finding
* correspondences between two sets of local features
* Inputs:
* source_points
* The "source" points, i.e., the points that must be transformed to align with the target point cloud
* source_descriptors
* The local descriptors for each source point
* target_points
* The "target" points, i.e., the points to which the source point cloud will be aligned
* target_descriptors
* The local descriptors for each target point
* min_sample_distance
* The minimum distance between any two random samples
* max_correspondence_distance
* The
* nr_iterations
* The number of RANSAC iterations to perform
* Return: A transformation matrix that will roughly align the points in source to the points in target
*/
Eigen::Matrix4f
computeInitialAlignment (const PointCloudPtr & source_points, const LocalDescriptorsPtr & source_descriptors,
const PointCloudPtr & target_points, const LocalDescriptorsPtr & target_descriptors,
float min_sample_distance, float max_correspondence_distance, int nr_iterations)
{
pcl::SampleConsensusInitialAlignment<PointT, PointT, LocalDescriptorT> sac_ia;
sac_ia.setMinSampleDistance (min_sample_distance);
sac_ia.setMaxCorrespondenceDistance (max_correspondence_distance);
sac_ia.setMaximumIterations (nr_iterations);
sac_ia.setInputSource (source_points);
sac_ia.setSourceFeatures (source_descriptors);
sac_ia.setInputTarget (target_points);
sac_ia.setTargetFeatures (target_descriptors);
PointCloud registration_output;
sac_ia.align (registration_output);
return (sac_ia.getFinalTransformation ());
}
/* Use IterativeClosestPoint to find a precise alignment from the source cloud to the target cloud,
* starting with an initial guess
* Inputs:
* source_points
* The "source" points, i.e., the points that must be transformed to align with the target point cloud
* target_points
* The "target" points, i.e., the points to which the source point cloud will be aligned
* initial_alignment
* An initial estimate of the transformation matrix that aligns the source points to the target points
* max_correspondence_distance
* A threshold on the distance between any two corresponding points. Any corresponding points that are further
* apart than this threshold will be ignored when computing the source-to-target transformation
* outlier_rejection_threshold
* A threshold used to define outliers during RANSAC outlier rejection
* transformation_epsilon
* The smallest iterative transformation allowed before the algorithm is considered to have converged
* max_iterations
* The maximum number of ICP iterations to perform
* Return: A transformation matrix that will precisely align the points in source to the points in target
*/
Eigen::Matrix4f
refineAlignment (const PointCloudPtr & source_points, const PointCloudPtr & target_points,
const Eigen::Matrix4f &initial_alignment, float max_correspondence_distance,
float outlier_rejection_threshold, float transformation_epsilon, float max_iterations)
{
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setMaxCorrespondenceDistance (max_correspondence_distance);
icp.setRANSACOutlierRejectionThreshold (outlier_rejection_threshold);
icp.setTransformationEpsilon (transformation_epsilon);
icp.setMaximumIterations (max_iterations);
PointCloudPtr source_points_transformed (new PointCloud);
pcl::transformPointCloud (*source_points, *source_points_transformed, initial_alignment);
icp.setInputSource (source_points_transformed);
icp.setInputTarget (target_points);
PointCloud registration_output;
icp.align (registration_output);
return (icp.getFinalTransformation () * initial_alignment);
}
pcl::IterativeClosestPoint
IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm.
Definition: icp.h:97
pcl::IterativeClosestPoint::setInputTarget
void setInputTarget(const PointCloudTargetConstPtr &cloud) override
Provide a pointer to the input target (e.g., the point cloud that we want to align to the target)
Definition: icp.h:240
pcl::IterativeClosestPoint::setInputSource
void setInputSource(const PointCloudSourceConstPtr &cloud) override
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition: icp.h:207
pcl::PointCloud
PointCloud represents the base class in PCL for storing collections of 3D points.
Definition: point_cloud.h:173
pcl::Registration::setInputSource
virtual void setInputSource(const PointCloudSourceConstPtr &cloud)
Provide a pointer to the input source (e.g., the point cloud that we want to align to the target)
Definition: registration.hpp:47
pcl::Registration::getFinalTransformation
Matrix4 getFinalTransformation()
Get the final transformation matrix estimated by the registration method.
Definition: registration.h:271
pcl::Registration::setMaximumIterations
void setMaximumIterations(int nr_iterations)
Set the maximum number of iterations the internal optimization should run for.
Definition: registration.h:289
pcl::Registration::setInputTarget
virtual void setInputTarget(const PointCloudTargetConstPtr &cloud)
Provide a pointer to the input target (e.g., the point cloud that we want to align the input source t...
Definition: registration.hpp:61
pcl::Registration::setRANSACOutlierRejectionThreshold
void setRANSACOutlierRejectionThreshold(double inlier_threshold)
Set the inlier distance threshold for the internal RANSAC outlier rejection loop.
Definition: registration.h:328
pcl::Registration::setMaxCorrespondenceDistance
void setMaxCorrespondenceDistance(double distance_threshold)
Set the maximum distance threshold between two correspondent points in source <-> target.
Definition: registration.h:348
pcl::Registration::align
void align(PointCloudSource &output)
Call the registration algorithm which estimates the transformation and returns the transformed source...
Definition: registration.hpp:166
pcl::Registration::setTransformationEpsilon
void setTransformationEpsilon(double epsilon)
Set the transformation epsilon (maximum allowable translation squared difference between two consecut...
Definition: registration.h:370
pcl::SampleConsensusInitialAlignment
SampleConsensusInitialAlignment is an implementation of the initial alignment algorithm described in ...
Definition: ia_ransac.h:54
pcl::SampleConsensusInitialAlignment::setMinSampleDistance
void setMinSampleDistance(float min_sample_distance)
Set the minimum distances between samples.
Definition: ia_ransac.h:191
pcl::SampleConsensusInitialAlignment::setTargetFeatures
void setTargetFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the target point cloud's feature descriptors.
Definition: ia_ransac.hpp:64
pcl::SampleConsensusInitialAlignment::setSourceFeatures
void setSourceFeatures(const FeatureCloudConstPtr &features)
Provide a shared pointer to the source point cloud's feature descriptors.
Definition: ia_ransac.hpp:50
pcl::transformPointCloud
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.
Definition: transforms.hpp:221