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 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 
#include 
#include 
#include 

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

    // 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 icp;
    icp.setInputCloud(cloud_in);
    icp.setInputTarget(cloud_out);

    //Creates a pcl::PointCloud to which the IterativeClosestPoint can save the resultant cloud after applying the algorithm
    pcl::PointCloud 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: " < 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


转载:http://www.cnblogs.com/21207-iHome/p/6034462.html

你可能感兴趣的:(PCL)