PCL点云库：ICP算法

　　ICP（Iterative Closest Point迭代最近点）算法是一种点集对点集配准方法。在VTK、PCL、MRPT、MeshLab等C++库或软件中都有实现，可以参见维基百科中的ICP Algorithm Implementations.

　　ICP算法采用最小二乘估计计算变换矩阵，原理简单且具有较好的精度，但是由于采用了迭代计算，导致算法计算速度较慢，而且采用ICP进行配准计算时，其对待配准点云的初始位置有一定要求，若所选初始位置不合理，则会导致算法陷入局部最优。PCL点云库已经实现了多种点云配准算法：

• pcl::GeneralizedIterativeClosestPoint< PointSource, PointTarget > Class Template Reference

• pcl::IterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference

• pcl::IterativeClosestPointWithNormals< PointSource, PointTarget, Scalar > Class Template Reference

• pcl::IterativeClosestPointNonLinear< PointSource, PointTarget, Scalar > Class Template Reference

• pcl::JointIterativeClosestPoint< PointSource, PointTarget, Scalar > Class Template Reference

• pcl::registration::IncrementalICP< PointT, Scalar > Class Template Reference

　　IterativeClosestPoint类提供了标准ICP算法的实现（The transformation is estimated based on SVD），算法迭代结束条件有如下几个:

1. 最大迭代次数：Number of iterations has reached the maximum user imposed number of iterations (via setMaximumIterations)
2. 两次变化矩阵之间的差值：The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via setTransformationEpsilon)
3. 均方误差（MSE）：The sum of Euclidean squared errors is smaller than a user defined threshold (via setEuclideanFitnessEpsilon)

　　基本用法如下：

IterativeClosestPoint<PointXYZ, PointXYZ> icp;

// Set the input source and target
icp.setInputCloud (cloud_source);
icp.setInputTarget (cloud_target);

// Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
icp.setMaxCorrespondenceDistance (0.05);
// Set the maximum number of iterations (criterion 1)
icp.setMaximumIterations (50);
// Set the transformation epsilon (criterion 2)
icp.setTransformationEpsilon (1e-8);
// Set the euclidean distance difference epsilon (criterion 3)
icp.setEuclideanFitnessEpsilon (1);

// Perform the alignment
icp.align (cloud_source_registered);
// Obtain the transformation that aligned cloud_source to cloud_source_registered
Eigen::Matrix4f transformation = icp.getFinalTransformation ();

　　下面是一个完整的例子：

#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>

int main (int argc, char** argv)
{
    //Creates two pcl::PointCloud<pcl::PointXYZ> boost shared pointers and initializes them
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);

    // Fill in the CloudIn data
    cloud_in->width    = 5;
    cloud_in->height   = 1;
    cloud_in->is_dense = false;
    cloud_in->points.resize (cloud_in->width * cloud_in->height);
    for (size_t i = 0; i < cloud_in->points.size (); ++i)
    {
        cloud_in->points[i].x = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud_in->points[i].y = 1024 * rand () / (RAND_MAX + 1.0f);
        cloud_in->points[i].z = 1024 * rand () / (RAND_MAX + 1.0f);
    }


    *cloud_out = *cloud_in;

    //performs a simple rigid transform on the point cloud
    for (size_t i = 0; i < cloud_in->points.size (); ++i)
        cloud_out->points[i].x = cloud_in->points[i].x + 1.5f;

    //creates an instance of an IterativeClosestPoint and gives it some useful information
    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    icp.setInputCloud(cloud_in);
    icp.setInputTarget(cloud_out);

    //Creates a pcl::PointCloud<pcl::PointXYZ> to which the IterativeClosestPoint can save the resultant cloud after applying the algorithm
    pcl::PointCloud<pcl::PointXYZ> Final;

    //Call the registration algorithm which estimates the transformation and returns the transformed source (input) as output.
    icp.align(Final);

    //Return the state of convergence after the last align run. 
    //If the two PointClouds align correctly then icp.hasConverged() = 1 (true). 
    std::cout << "has converged: " << icp.hasConverged() <<std::endl;

    //Obtain the Euclidean fitness score (e.g., sum of squared distances from the source to the target) 
    std::cout << "score: " <<icp.getFitnessScore() << std::endl; 
    std::cout << "----------------------------------------------------------"<< std::endl;

    //Get the final transformation matrix estimated by the registration method. 
    std::cout << icp.getFinalTransformation() << std::endl;

    return (0);
}

　　结果如下，ICP算法计算出了正确的变换

　　在PCL官方的tutorial中还有个ICP算法交互的例子（Interactive Iterative Closest Point，网站上该例子的源代码编译时有一点问题需要修改...），该程序中按一次空格ICP迭代计算一次。可以看出，随着迭代进行，两块点云逐渐重合在一起。

 

参考：

How to use iterative closest point

http://pointclouds.org/documentation/tutorials/iterative_closest_point.php#iterative-closest-point

Interactive Iterative Closest Point

http://pointclouds.org/documentation/tutorials/interactive_icp.php#interactive-icp

PCL之ICP算法实现

https://segmentfault.com/a/1190000005930422

PCL学习笔记二：Registration (ICP算法)

http://blog.csdn.net/u010696366/article/details/8941938