preface
Continue PCL - ICP code reading (III) - Registration initialization , this article mainly introduces the getfitnesscore function of the Registration category. This function is used to calculate the mse (mean squared error) between source and target. PCL provides two versions of getfitnesscore, which are applicable to two vectors or point clouds respectively.
getFitnessScore - for two vectors
The two vectors passed in represent the square of the distance. This function is used to calculate the mse between them.
// Don't you understand? // Calculate the mse of two vectors? template <typename PointSource, typename PointTarget, typename Scalar> inline double Registration<PointSource, PointTarget, Scalar>::getFitnessScore( const std::vector<float>& distances_a, const std::vector<float>& distances_b) {
Eigen::VectorXf::Map creates a mapping of Eigen::VectorXf based on existing vectors or arrays. The first parameter of its constructor represents the address of the existing vector, and the second parameter represents the vector with row number to be established.
So the meaning of this code is: take the first NR of two vectors respectively_ Elem elements are map_a and map_b.
unsigned int nr_elem = static_cast<unsigned int>(std::min(distances_a.size(), distances_b.size())); Eigen::VectorXf map_a = Eigen::VectorXf::Map(&distances_a[0], nr_elem); Eigen::VectorXf map_b = Eigen::VectorXf::Map(&distances_b[0], nr_elem);
Return the average value of the square of the distance between two vectors:
return (static_cast<double>((map_a - map_b).sum()) / static_cast<double>(nr_elem)); }
Note: because the two vectors passed in represent the square of the distance, it is normal that there is no square in the code. But will the subtraction of two distance squares be equal to the distance square? Should we open the root sign first, subtract and then square it?
getFitnessScore - for two point clouds
This function is used to calculate the final_transform_ Converted input_ And target_ mean square error between. This is where ICP routine Functions used in.
// Distance exceeds max_range will not be counted template <typename PointSource, typename PointTarget, typename Scalar> inline double Registration<PointSource, PointTarget, Scalar>::getFitnessScore(double max_range) { // Calculate the mean squared error between the converted input point cloud and the target point cloud double fitness_score = 0.0;
Correction algorithm (i.e The introduced align function) will estimate a 4 * 4 transformation matrix final_transformation_， Use it for input_ Convert the point cloud to get the input_transformed_:
// Transform the input dataset using the final transformation PointCloudSource input_transformed; transformPointCloud(*input_, input_transformed, final_transformation_);
Just find the nearest neighbor:
pcl::Indices nn_indices(1); std::vector<float> nn_dists(1); // For each point in the source dataset int nr = 0;
Traverse each point of the converted input point cloud and find the nearest point in the target point cloud:
for (const auto& point : input_transformed) { // Find its nearest neighbor in the target tree_->nearestKSearch(point, 1, nn_indices, nn_dists);
Point pairs with too large distance are excluded, and those who meet the conditions are included in fitness_ In score:
(note that the element in nn_dists returned by nearestKSearch represents the square of the distance)
// Deal with occlusions (incomplete targets) // Exclude point pairs with too large distance if (nn_dists[0] <= max_range) { // Add to the fitness score fitness_score += nn_dists[0]; nr++; } }
Before that, fitness_score is sum of squared error. After averaging here, fitness_ Change score to mse (mean squared error):
if (nr > 0) return (fitness_score / nr);
If the number of valid pairs is 0, the maximum value of double type is returned:
return (std::numeric_limits<double>::max()); }
Note: the smaller the fitness score, the better. Therefore, the maximum value returned indicates that the two point clouds are not aligned at all