当前位置: 代码迷 >> 综合 >> ICP 算法(Iterative Closest Point迭代最近点)
  详细解决方案

ICP 算法(Iterative Closest Point迭代最近点)

热度:42   发布时间:2023-10-22 03:57:05.0

为了让机器人能抓取物体,通常要求算法具有两个功能:物体识别+位姿估计

  • 物体识别:采用颜色直方图、局部特征点(SIFT等)、BoF(Bag-of-Features 原理做的图像物体定位算法)等传统方法;特征点提取,聚类,PNP或者close form找到3D的relative pose。深度学习方法:fast-RCNN,yolo2,RCNN,RNN ;
  • 位姿估计:多采用点云配准(可以直接用ICP算法将这片点云与物体的3D模型对齐,求两个点云之间的旋转平移矩阵rigid transform or euclidean transform 刚性变换或欧式变换)或 bounding box 提取等的传统简单方法;深度学习方法,采用卷积神经网络CNN训练一小片点云的可抓取性

ICP是解决三维点集配准问题的一个应用较为广泛的算法,此外在SLAM等移动机器人导航等领域也有着很大的用武之地。三维点集配准是一个非常重要的中间步骤,它在表面重建、三维物体识别、相机定位等问题中有着极其重要的应用。对于三维点集配准问题,研究者提出了很多解决方案但是应用最广泛的,但是应用最广泛,影响最大的还是由Besl和Mckay在1992年提出的迭代最近点算法(Iterative Closest Point,ICP),它是基于纯粹几何模型的三维物体对准算法,由于它的强大功能以及高的精确度,很快就成为了曲面配准中的主流算法。ICP算法有以下特点:

  • 可以获得非常精确的配准效果;
  • 可以处理三维点集、参数曲面等多种形式表达的曲面,也就是说该算法对曲面表示 方法独立;
  • 不必对待处理的点集进行分割和特征提取;
  • 在较好的初值情况下,可以得到很好的算法收敛性。
  • 该算法在搜索对应点的过程中,计算代价非常的大;

具体的实现步骤可以参考:http://blog.csdn.net/yang_q_x/article/details/52326953?locationNum=7  

具体原理性质的东西:http://www.cnblogs.com/liuyuan-pal/p/6444684.html

PCL中实现ICP与相关论文中一致,最终的变换矩阵都是基于SVD(奇异值分解)。由基类Registration派生,直接实例化对象即可:

Pcl::IterativeClosestPoint<SourcePoint,TargetPoint> icp

成员函数的组成:
    1. inline void inline void setSearchMethodTarget(const KdTreePtr &tree)   
           kdtree加速搜索,还有一个Target的函数,用法与之一致,普通的匹配可以不用使用。
    2. inline void setInputSource (const PointCloudSource ConstPtr &cloud)     
           源点云。指将要被旋转和平移的点云。
    3. inline void setInputTarget (const PointCloudTarget ConstPtr &cloud)       
           目标点云。从Source到Target匹配,将源点云变换到目标点云的相对位置。
    4. inline void setMaximumIterations (int nr_iterations)                              
           迭代次数,几十上百甚至上千都可能出现(默认为10),可以用来终止迭代。
    5. inline void setTransformationEpsilon (double epsilon)     
           上次转换与当前转换的差值,这个值一般设为1e-6或者1e-10或者更小,可以用来终止迭代次数。
    6. inline void setEuclideanFitnessEpsilon (double epsilon)      
           前后两次迭代方差的差值,可以用来终止迭代次数。
    7. inline void setMaxCorrespondenceDistance (double distance_threshold) 
           如果两个点云距离大于此值,则被忽略(PCL默认距离单位是m),
           一般根据两个点云之间的距离估计,这个值对点云匹配的结果影响较大。
    8. inline void align (PointCloudSource &output)    output为配准后点云。
    9. inline Matrix4 getFinalTransformation ()    获取最终的转换矩阵。

注:  
    1.如果由于设置相关参数过于苛刻,而造成没有匹配成功法,icp.hasConverged() 会提示匹配失败,
并且  icp.getFitnessScore()  为0,相关的转换矩阵为单位矩阵。
    2.输入的点云最好经过预处理,过于复杂和过多噪声的点与将会出现“Invalid (NaN, Inf) point coordinates given to nearestKSearch!”的错误

icp点云配准案例:http://www.cnblogs.com/21207-iHome/p/6034462.html

typedef pcl::PointXYZRGB PointT;//点类型
pcl::IterativeClosestPoint<PointT, PointT> icp;//创建icp对象
icp.setInputSource(cloud_pr);  //原点云
icp.setInputTarget(cloud_hole_pr);//目标点云
pcl::PointCloud<PointT> final_cloud;//配准后点云
//icp 参数设置
icp.setMaximumIterations(1000);  //最大迭代次数
icp.setEuclideanFitnessEpsilon(0.5);//前后两次迭代误差的差值
icp.setTransformationEpsilon(1e-10); //上次转换与当前转换的差值;
icp.setMaxCorrespondenceDistance(0.7); //忽略在此距离之外的点,对配准影响较大
//保存配准后的点
icp.align(final_cloud);
//icp匹配后的转换矩阵及得分
cout << "has converged: " << icp.hasConverged() << endl
<< "score: " << icp.getFitnessScore() << endl;
cout << icp.getFinalTransformation() << endl; 
#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 thempcl::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 datacloud_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 cloudfor (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 informationpcl::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 algorithmpcl::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);
}

 

  相关解决方案