PCL之——点云配准算法ICP
一、ICP介绍：
1.ICP用途
ICP原名IterativeClosestPoint（迭代最近点算法），主要用于点云配准，找到两部分点云之间的刚体变换关系。可用于SLAM、位姿估计等
2.ICP原理
刚体变换不理解先看：
https://blog.csdn.net/kksc1099054857/article/details/80280964
ICP算法的基本原理是： 分别在带匹配的目标点云P和源点云Q中，按照一定的约束条件，找到最邻近点（pi，qi），然后计算出最优匹配参数R和t，使得误差函数最小。误差函数为E（R，t）为：
其中n为最邻近点对的个数，pi为目标点云 P 中的一点，qi 为源点云 Q 中与pi对应的最近点，R 为旋转矩阵，t为平移向量。
ICP算法的基本步骤是：
（1）在目标点云P中取点集pi∈P；
（2）找出源点云Q中的对应点集qi∈Q，使得||qi-pi||=min；
（3）计算旋转矩阵R和平移矩阵t，使得误差函数最小；
（4）对pi使用上一步求得的旋转矩阵R和平移矩阵t进行旋转和平移变换，的到新的
对应点集pi’={pi’=Rpi+t,pi∈P}；
（5）计算pi’与对应点集qi的平均距离；
（6）如果d小于某一给定的阈值或者大于预设的最大迭代次数，则停止迭代计算。
否则返回第2步，直到满足收敛条件为止。
ICP算法参数：
（1）优化的目标函数：点到点的距离，点到线的距离，点到面的距离
（2）点云采样方法：
（3）迭代停止条件：mse小于某个值，两次mse差值小于某个值，迭代次数
（4）对应点关系查找：KD树数据结构
3.PCL的ICP实现
PCL doc中Module registration
http://docs.pointclouds.org/1.8.1/group__registration.html
差不多五种实现：
（1）pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget >：
广义迭代最近点算法
（2）pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar >
经典迭代最近点算法
（3）pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar >：
带法向的迭代最近点算法
（4）pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar >
非线性优化的迭代最近点算法
（5）pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar >
角度空间的迭代最近点算法
二、ICP使用：
1.ICP配准步骤
但是一般教程里都没有关键点提取和描述这一步，都是直接匹配的
2.官方例程——采坑
官方例程：采用的猴子模型，我这里用来做位姿估计，使用的我自己的模型
http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp
ICP输入：源点云和目标点云，初始位姿（可以没有，默认为单位阵）。
坑：
在ICP默认代码中，是源点云配目标点云，求取的变换矩阵是源点云——>目标点云的变换矩阵，
官方的源点云是cloud_icp，目标点云是cloud_in，ICP找的是cloud_icp->cloud_in的变换；然而，输入的点云只有cloud_in；cloud_icp是通过已知的变换得到的，如以下代码；
即：cloud_icp=transformation_matrix *cloud_in（矩阵左乘）；
按照道理，ICP的输出应该是transformation_matrix的逆，
但是在ICP中却采用将变换矩阵连续右乘，很明显输出的结果不是transformation_matrix的逆
// Defining a rotation matrix and translation vector
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity ();
// A rotation matrix (see https://en.wikipedia.org/wiki/Rotation_matrix)
double theta = M_PI / 8; // The angle of rotation in radians
transformation_matrix (0, 0) = std::cos (theta);
transformation_matrix (0, 1) = -sin (theta);
transformation_matrix (1, 0) = sin (theta);
transformation_matrix (1, 1) = std::cos (theta);
// A translation on Z axis (0.4 meters)
transformation_matrix (2, 3) = 0.4;
// Display in terminal the transformation matrix
std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
print4x4Matrix (transformation_matrix);
transformation_matrix重新赋值
if (icp.hasConverged ())
{
std::cout << "\nICP has converged, score is " << icp.getFitnessScore () << std::endl;
std::cout << "\nICP transformation " << iterations << " : cloud_icp -> cloud_in" << std::endl;
transformation_matrix = icp.getFinalTransformation ().cast<double>();
print4x4Matrix (transformation_matrix);
}
transformation_matrix连续左乘
while (!viewer.wasStopped ())
{
viewer.spinOnce ();
// The user pressed "space" :
if (next_iteration)
{
// The Iterative Closest Point algorithm
time.tic ();
icp.align (*cloud_icp);
std::cout << "Applied 1 ICP iteration in " << time.toc () << " ms" << std::endl;
if (icp.hasConverged ())
{
printf ("\033[11A"); // Go up 11 lines in terminal output.
printf ("\nICP has converged, score is %+.0e\n", icp.getFitnessScore ());
std::cout << "\nICP transformation " << ++iterations << " : cloud_icp -> cloud_in" << std::endl;
transformation_matrix *= icp.getFinalTransformation ().cast<double>(); // WARNING /!\ This is not accurate! For "educational" purpose only!
print4x4Matrix (transformation_matrix); // Print the transformation between original pose and current pose
我在输出每次ICP后还剩余的误差时，一直不对劲，直到把左乘改为右乘。
3.修改ICP参数
（1）经典ICP基础参数
设置迭代停止条件：最大迭代次数、mse变化大小，
typedef pcl::PointXYZ PointT;
typedef pcl::PointCloud<PointT> PointCloud;
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_model(new pcl::PointCloud<pcl::PointXYZ>);
pcl::IterativeClosestPoint<PointT, PointT> icp;
icp.setMaximumIterations (iterations)
icp.setInputSource (cloud_view);
icp.setInputTarget (cloud_model);
icp.align(*cloud_view);
icp.getFinalTransformation()
icp.getFitnessScore()
（2）非线性优化ICP
只需修改第一句定义即可
pcl::IterativeClosestPointNonlinear<PointT, PointT> icp;
发现非线性优化非常耗时（本例子中约6倍耗时）
（3）修改距离指标为点到面
- 点到面距离
作者论文：Low K L. Linear least-squares optimization for point-to-plane icp surface registration[J]. Chapel Hill, University of North Carolina, 2004, 4(10): 1-3
优化指标为：
- 点云库中如何使用
首先要计算点云的法向，然后把法线信息和xyz信息放在一起，然后使用icp.setTransformationEstimation(point_to_plane)设置距离指标；
经过对比，发现对于本实验目标有较好的效果，几次迭代就收敛到1mm以下
//法向量计算
pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> n;
pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
//建立kdtree来进行近邻点集搜索
pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>);
//为kdtree添加点云数据
tree->setInputCloud(init_trans_cloud);
n.setInputCloud(init_trans_cloud);
n.setSearchMethod(tree);
//点云法向计算时，需要搜索的近邻点大小
n.setKSearch(20);
//开始进行法向计算
n.compute(*normals);
//* normals should not contain the point normals + surface curvatures
//将点云数据与法向信息拼接
pcl::PointCloud<pcl::PointNormal>::Ptr init_trans_cloud_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*init_trans_cloud, *normals, *init_trans_cloud_normals);
pcl::PointCloud<pcl::Normal>::Ptr normals1(new pcl::PointCloud<pcl::Normal>);
tree->setInputCloud(cloud_model);
n.setInputCloud(cloud_model);
n.setSearchMethod(tree);
n.compute(*normals1);
pcl::PointCloud<pcl::PointNormal>::Ptr cloud_model_normals(new pcl::PointCloud<pcl::PointNormal>);
pcl::concatenateFields(*cloud_model, *normals1, *cloud_model_normals);
//点到面的距离函数
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
typedef pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> PointToPlane;
boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
icp.setTransformationEstimation(point_to_plane);
icp.setInputSource (init_trans_cloud_normals);
icp.setInputTarget (cloud_model_normals);
结果：
（4）修改距离指标为对称版本点到面距离指标
PCL1.10版本最新发布了一种新的指标： A symmetric objective function of point to plane，感谢开源
作者论文：Rusinkiewicz S. A symmetric objective function for ICP[J]. ACM Transactions on Graphics (TOG), 2019, 38(4): 1-7.
- 简单介绍
发布了两个版本的目标函数，旋转法线版本和简化版本
作者表示小范围内收敛效果要比点到面好
- 使用
集成的比较好，只需要在点到面的基础上修改
pcl::IterativeClosestPoint<pcl::PointNormal, pcl::PointNormal> icp;
typedef pcl::registration::TransformationEstimationPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> PointToPlane;
typedef pcl::registration::TransformationEstimationSymmetricPointToPlaneLLS<pcl::PointNormal, pcl::PointNormal> SymmPointToPlane;
boost::shared_ptr<SymmPointToPlane> point_to_plane(new SymmPointToPlane);
//boost::shared_ptr<PointToPlane> point_to_plane(new PointToPlane);
icp.setTransformationEstimation(point_to_plane);
（5）icp.getFitnessScore()方法无法在配准前计算mse
每次配准虽然可以获得欧拉适应度得分（即mse），但是如果想看初始情况的mse；即在icp.align(*cloud_view);前使用则会报错，原因是有些初始化工作需要icp.align中的一些代码来完成，主要是kdtree的东西
参考
http://www.pcl-users.org/ICP-GICP-getFitnessScore-td4039822.html
PCL原始的getFitnessScore()方法：
template <typename PointSource, typename PointTarget, typename Scalar> inline double
pcl::Registration<PointSource, PointTarget, Scalar>::getFitnessScore (double max_range)
{
double fitness_score = 0.0;
// Transform the input dataset using the final transformation
PointCloudSource input_transformed;
transformPointCloud (*input_, input_transformed, final_transformation_);
std::vector<int> nn_indices (1);
std::vector<float> nn_dists (1);
// For each point in the source dataset
int nr = 0;
for (size_t i = 0; i < input_transformed.points.size (); ++i)
{
// Find its nearest neighbor in the target
tree_->nearestKSearch (input_transformed.points[i], 1, nn_indices, nn_dists);
// Deal with occlusions (incomplete targets)
if (nn_dists[0] <= max_range)
{
// Add to the fitness score
fitness_score += nn_dists[0];
nr++;
}
}
if (nr > 0)
return (fitness_score / nr);
else
return (std::numeric_limits<double>::max ());
}
修改的计算mse的函数：
double getmse(PointCloud::Ptr modelPointCloud, PointCloud::Ptr queryPointCloud) {
pcl::KdTreeFLANN <PointT, flann::L2_Simple<float> > modelTree;
modelTree.setInputCloud(modelPointCloud->makeShared());
std::vector<int> nn_indices(1);
std::vector<float> nn_dists(1);
double fitness_score = 0.0;
int count = 0;
//For each point in the source PointCloud
for (int i = 0; i < queryPointCloud->points.size(); i++) {
//Find nearest neighbor in the target (query)
modelTree.nearestKSearch(queryPointCloud->points[i], 1, nn_indices, nn_dists);
count++;
fitness_score += nn_dists[0];
}
if (count > 0)
return (fitness_score / count);
else
return (std::numeric_limits<double >::max());
}
- 注意：如果采用不同的目标函数来衡量最近点之间的距离，如点到点，点到面等，获得的距离衡量指标是不同的，就不能简单的用上面的程序来计算mse了，这个是最简单的点对点距离的。
扫一扫
36万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
