目录
ICP流程分为4个主要步骤
1、对原始点云数据进行采样,得到初始的点云集合;
2、确定初始对应点集,模板点集在待匹配点集的位置,对结果有至关重要的影响;
3、去除错误对应点对,通过最小二乘法迭代获取正确的匹配点;
4、坐标变换求解,得到旋转矩阵R和平移矢量t;
完成高精度的变换矩阵估计;
使用一点感受
初始点集合,非常非常非常之重要,对最后结果有至关重要的作用;
迭代次数多了之后,或者点云大了之后,运算速度确实慢;
是一种精度比较高的变换矩阵估计,但是精度多高还得结合实际情况,如果需要精度特别高的话还得测试;
实现代码
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
int main(int argc, char** argv)
{
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>(5, 1));
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out(new pcl::PointCloud<pcl::PointXYZ>);
// Fill in the CloudIn data
for (auto& point : *cloud_in)
{
point.x = 1024 * rand() / (RAND_MAX + 1.0f);
point.y = 1024 * rand() / (RAND_MAX + 1.0f);
point.z = 1024 * rand() / (RAND_MAX + 1.0f);
}
std::cout << "Saved " << cloud_in->size() << " data points to input:" << std::endl;
for (auto& point : *cloud_in)
std::cout << point << std::endl;
*cloud_out = *cloud_in;
std::cout << "size:" << cloud_out->size() << std::endl;
for (auto& point : *cloud_out)
point.x += 0.7f;
std::cout << "Transformed " << cloud_in->size() << " data points:" << std::endl;
for (auto& point : *cloud_out)
std::cout << point << std::endl;
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
icp.setInputSource(cloud_in);
icp.setInputTarget(cloud_out);
pcl::PointCloud<pcl::PointXYZ> Final;
icp.align(Final);
std::cout << "has converged:" << icp.hasConverged() << " score: " <<
icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;
return (0);
}
《点云库PCL从入门到精通》