Comment on page
Iterative Closest Point
The following shows an example implementation of ICP algorithm. It can have exactly the same performance as the ICP algorithm in PCL.
// Implementation by Hanzhe Teng, Feb 2022
// refer to pcl/registration/impl/icp.hpp and transformation_estimation_svd.hpp
Eigen::Matrix4f ClassicICPRegistration (const PointCloudPtr& source, const PointCloudPtr& target) {
// initialization
int iteration = 100;
float distance_threshold = 0.05; // icp.setMaxCorrespondenceDistance
int cloud_size = static_cast<int> (source->size());
Eigen::Matrix4f transformation = Eigen::Matrix4f::Identity();
Eigen::Matrix4f final_transformation = Eigen::Matrix4f::Identity();
pcl::PointCloud<PointT>::Ptr source_trans (new pcl::PointCloud<PointT>);
// build K-d tree for target cloud
pcl::search::KdTree<PointT>::Ptr kdtree (new pcl::search::KdTree<PointT>);
kdtree->setInputCloud(target);
std::vector<int> indices (1); // for nearestKSearch
std::vector<float> sq_dist (1); // for nearestKSearch
// repeat until convergence
for (int t = 0; t < iteration; ++t) {
// transform source using estimated transformation
pcl::transformPointCloud<PointT> (*source, *source_trans, final_transformation);
// visualize source_trans in each step if needed
// find correspondences in target
std::vector<std::pair<int, int>> correspondences;
for (int i = 0; i < cloud_size; ++i) {
kdtree->nearestKSearch(source_trans->points[i], 1, indices, sq_dist);
if (sq_dist[0] > distance_threshold * distance_threshold) // skip if too far
continue;
correspondences.push_back({i, indices[0]});
}
// convert to Eigen format
int idx = 0;
Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_src (3, correspondences.size());
Eigen::Matrix<float, 3, Eigen::Dynamic> cloud_tgt (3, correspondences.size());
for (const auto& corres : correspondences) {
cloud_src (0, idx) = source_trans->points[corres.first].x;
cloud_src (1, idx) = source_trans->points[corres.first].y;
cloud_src (2, idx) = source_trans->points[corres.first].z;
cloud_tgt (0, idx) = target->points[corres.second].x;
cloud_tgt (1, idx) = target->points[corres.second].y;
cloud_tgt (2, idx) = target->points[corres.second].z;
++idx;
}
// skip a few steps here for simplicity, such as
// check convergence (if trans update < required epsilon)
// check if cloud_src and cloud_tgt are valid (>0 or >3?)
// solve using Umeyama's algorithm (SVD)
transformation = Eigen::umeyama (cloud_src, cloud_tgt, false);
final_transformation = transformation * final_transformation;
std::cout << "it = " << t << "; cloud size = " << cloud_size << "; idx = " << idx << std::endl;
std::cout << "current transformation estimation" << std::endl << final_transformation << std::endl;
}
return final_transformation;
}
Last modified 11d ago