Functions
Matching

Low-level functions for matching the map pair. More...

Functions

 map_merge_3d::ENUM_CLASS (EstimationMethod, MATCHING, SAC_IA)
 
Eigen::Matrix4f map_merge_3d::estimateTransform (const PointCloudPtr &source_points, const PointCloudPtr &source_keypoints, const LocalDescriptorsPtr &source_descriptors, const PointCloudPtr &target_points, const PointCloudPtr &target_keypoints, const LocalDescriptorsPtr &target_descriptors, EstimationMethod method, bool refine, double inlier_threshold, double max_correspondence_distance, int max_iterations, size_t matching_k, double transform_epsilon)
 Estimate transformation between two pointclouds. More...
 
Eigen::Matrix4f map_merge_3d::estimateTransformFromCorrespondences (const PointCloudPtr &source_keypoints, const PointCloudPtr &target_keypoints, const CorrespondencesPtr &correspondences, CorrespondencesPtr &inliers, double inlier_threshold)
 Estimates transformation between source and target pointcloud based on correspondences. More...
 
Eigen::Matrix4f map_merge_3d::estimateTransformFromDescriptorsSets (const PointCloudPtr &source_keypoints, const LocalDescriptorsPtr &source_descriptors, const PointCloudPtr &target_keypoints, const LocalDescriptorsPtr &target_descriptors, double min_sample_distance, double max_correspondence_distance, int max_iterations)
 Estimates transformation between source and target pointcloud based on descriptors. More...
 
Eigen::Matrix4f map_merge_3d::estimateTransformICP (const PointCloudPtr &source_points, const PointCloudPtr &target_points, const Eigen::Matrix4f &initial_guess, double max_correspondence_distance, double outlier_rejection_threshold, int max_iterations=100, double transformation_epsilon=0.0)
 Use ICP to estimate transform between grids. More...
 
CorrespondencesPtr map_merge_3d::findFeatureCorrespondences (const LocalDescriptorsPtr &source_descriptors, const LocalDescriptorsPtr &target_descriptors, size_t k=5)
 Finds correspondences between two sets of feature descriptors. More...
 
double map_merge_3d::transformScore (const PointCloudPtr &source_points, const PointCloudPtr &target_points, const Eigen::Matrix4f &transform, double max_distance)
 Computes euclidean distance between two pointclouds. More...
 

Detailed Description

Low-level functions for matching the map pair.

Low-level functions to find correspondences between two pointclouds and match them together. These functions operate on two pointclouds.

Function Documentation

◆ ENUM_CLASS()

map_merge_3d::ENUM_CLASS ( EstimationMethod  ,
MATCHING  ,
SAC_IA   
)

◆ estimateTransform()

Eigen::Matrix4f map_merge_3d::estimateTransform ( const PointCloudPtr source_points,
const PointCloudPtr source_keypoints,
const LocalDescriptorsPtr source_descriptors,
const PointCloudPtr target_points,
const PointCloudPtr target_keypoints,
const LocalDescriptorsPtr target_descriptors,
EstimationMethod  method,
bool  refine,
double  inlier_threshold,
double  max_correspondence_distance,
int  max_iterations,
size_t  matching_k,
double  transform_epsilon 
)

Estimate transformation between two pointclouds.

Uses extracted features to estimate rigid transformation. First the initial transformation is estimated using selected method, then the transformation may be selectively refined using ICP.

Parameters
source_pointsSource pointcloud
source_keypointsKeypoints of source pointcloud
source_descriptorsDescriptors for keypoints of source pointcloud
target_pointsTarget pointcloud
target_keypointsKeypoints of target pointcloud
target_descriptorsDescriptors for keypoints of target pointcloud
methodMethod for estimating initial transformation
refineWhether to refine initial transformation with ICP.
inlier_thresholdThreshold for inliers in RANSAC during initial estimation.
max_correspondence_distanceMaximum distance for a matched points to be considered the same point
max_iterationsmaximum iterations for RANSAC
matching_knumber of nearest descriptors to consider for matching
transform_epsilonthe smallest change allowed until ICP convergence.
Returns
estimated rigid transform between source and target pointclouds or zero matrix if the transformation could not be estimated

◆ estimateTransformFromCorrespondences()

Eigen::Matrix4f map_merge_3d::estimateTransformFromCorrespondences ( const PointCloudPtr source_keypoints,
const PointCloudPtr target_keypoints,
const CorrespondencesPtr &  correspondences,
CorrespondencesPtr &  inliers,
double  inlier_threshold 
)

Estimates transformation between source and target pointcloud based on correspondences.

Uses RANSAC to find inliers fitting a rigid transformation model. Final transformation is found using SVD on inliers set.

Parameters
source_keypointsKeypoints of source pointcloud
target_keypointsKeypoints of target pointcloud
correspondencesCorrespondences between keypoints
[out]inliersEstimated inliers from RANSAC
inlier_thresholdthreshold for considering a point as inlier in RANSAC
Returns
estimated rigid transformation between source and target or zero matrix if transformation could not be estimated

◆ estimateTransformFromDescriptorsSets()

Eigen::Matrix4f map_merge_3d::estimateTransformFromDescriptorsSets ( const PointCloudPtr source_keypoints,
const LocalDescriptorsPtr source_descriptors,
const PointCloudPtr target_keypoints,
const LocalDescriptorsPtr target_descriptors,
double  min_sample_distance,
double  max_correspondence_distance,
int  max_iterations 
)

Estimates transformation between source and target pointcloud based on descriptors.

Use SampleConsensusInitialAlignment to find a rough alignment from the source to the target

Parameters
source_keypointsKeypoints of source pointcloud
source_descriptorsdescriptors for source_keypoints
target_keypointsKeypoints of target pointcloud
target_descriptorsdescriptors for target_keypoints
min_sample_distanceThe minimum distance between any two random samples
max_correspondence_distanceMaximum distance for a matched points to be considered the same point
max_iterationsThe number of RANSAC iterations to perform
Returns
estimated rigid transformation between source and target or zero matrix

◆ estimateTransformICP()

Eigen::Matrix4f map_merge_3d::estimateTransformICP ( const PointCloudPtr source_points,
const PointCloudPtr target_points,
const Eigen::Matrix4f &  initial_guess,
double  max_correspondence_distance,
double  outlier_rejection_threshold,
int  max_iterations = 100,
double  transformation_epsilon = 0.0 
)

Use ICP to estimate transform between grids.

Parameters
source_pointssource point cloud
target_pointstarget point cloud.
initial_guessInitial transformation to start with
max_correspondence_distanceA 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_thresholdA threshold used to define outliers during RANSAC
max_iterationsmaximum iterations for RANSAC
transformation_epsilonThe smallest iterative transformation allowed before the algorithm is considered to have converged
Returns
estimated rigid transformation between source and target pointclouds

◆ findFeatureCorrespondences()

CorrespondencesPtr map_merge_3d::findFeatureCorrespondences ( const LocalDescriptorsPtr source_descriptors,
const LocalDescriptorsPtr target_descriptors,
size_t  k = 5 
)

Finds correspondences between two sets of feature descriptors.

Uses cross-matching algorithm with first-k selection

Parameters
source_descriptorsFeature descriptors of source pointcloud
target_descriptorsFeature descriptors of target pointcloud
knumber of nearest descriptors to consider for matching
Returns
correspondences source -> target

◆ transformScore()

double map_merge_3d::transformScore ( const PointCloudPtr source_points,
const PointCloudPtr target_points,
const Eigen::Matrix4f &  transform,
double  max_distance 
)

Computes euclidean distance between two pointclouds.

Computes a euclidean score for an estimated transformation. Because we expect only some parts of the maps overlapping, only points closer than max_distance will be include in the score.

Parameters
source_pointsSource pointcloud
target_pointsTarget pointcloud
transformestimated transformation between source and target
max_distanceMaximum distance between two points to be included in the score.
Returns
transformation euclidean score


map_merge_3d
Author(s): Jiri Horner
autogenerated on Mon Feb 28 2022 22:47:17