pcl点云库(1):迭代最近点算法(icp)算法实践及解析

目录

1 代码实践

1.1 文件目录：

1.2 编译运行

2 icp匹配相关函数解析

2.1 getFitnessScore()函数

2.2 setMaxCorrespondenceDistance()函数

2.3 setMaximumIterations()函数

2.4 setTransformationEpsilon()函数

2.5 setEuclideanFitnessEpsilon()函数

参考链接：

---

1 代码实践

（注：基于ubuntu、ros的 c++代码）

        github代码：https://github.com/menghxz/pcl_icp_test

1.1 文件目录：

meng@meng:~/ideas/pcl_ws$ tree
.
└── src
    └── pcl_icp_test
        ├── CMakeLists.txt
        ├── include
        │   └── pcl_icp_test #空文件夹
        ├── package.xml
        └── src
            └── pcl_icp_test.cpp

pcl_icp_test.cpp

#include "ros/ros.h"
#include 
#include 
#include 
#include 
#include 
#include 
using namespace std; //hxz

pcl::PointCloud::Ptr cloud_in (new pcl::PointCloud);//输入点云
pcl::PointCloud cloud_in_after_icp ;//输入点云进过icp匹配后的点云
pcl::PointCloud::Ptr cloud_out (new pcl::PointCloud);//目标点云
sensor_msgs::PointCloud2 targetCloudMsg,sourceCloudMsg,aftericpCloudMsg;//
ros::Publisher  pubTargetCloud,pubSourceCloud,pubAfterIcpCloud;


// 生成点云  函数
void create_cloud(){
    // 输入点云-------------------------------------------------------------------------------------------------------
    cloud_in->width    = 50;
    cloud_in->height   = 1;
    cloud_in->points.resize (cloud_in->width * cloud_in->height);
    for (int 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_in->points[i].z = 0;//放在一个平面上，方便观察
    }
    pcl::toROSMsg(*cloud_in, sourceCloudMsg);
    sourceCloudMsg.header.stamp = ros::Time::now();
    sourceCloudMsg.header.frame_id = "map";
    std::cout<<"输入点云创建成功"<points.resize (cloud_in->width * cloud_in->height);
    for (int i = 0; i < cloud_in->points.size (); ++i){
        cloud_out->points[i].x = cloud_in->points[i].x + 2.0f;//简单的刚体变换，每个x都加了5
        cloud_out->points[i].y = cloud_in->points[i].y + 2.0f;//简单的刚体变换，每个x都加了5
        // cloud_out->points[i].z = cloud_in->points[i].z + 0.5f;//简单的刚体变换，每个x都加了0.5
        cloud_out->points[i].z = 0;放在一个平面上，方便观察
    }
    pcl::toROSMsg(*cloud_out, targetCloudMsg);
    targetCloudMsg.header.stamp = ros::Time::now();
    targetCloudMsg.header.frame_id = "map";
    std::cout<<"目标点云转换成功"< icp;
    icp.setMaxCorrespondenceDistance(100);
    icp.setMaximumIterations(100);//迭代次数已达到用户施加的最大迭代次数
    icp.setTransformationEpsilon(1e-6);//先前转换和当前估计转换（即两次位姿转换）之间的 epsilon（差异）小于用户施加的值
    icp.setEuclideanFitnessEpsilon(1e-6);//欧几里得平方误差的总和小于用户定义的阈值
    icp.setRANSACIterations(0);// 设置RANSAC运行次数    
    icp.setInputCloud(cloud_in);
    icp.setInputTarget(cloud_out);
    //cloud_in_after_icp用来存储应用ICP算法之后的结果
    icp.align(cloud_in_after_icp);

    pcl::toROSMsg(cloud_in_after_icp, aftericpCloudMsg);
    aftericpCloudMsg.header.stamp = ros::Time::now();
    aftericpCloudMsg.header.frame_id = "map";
    std::cout<<"发布icp转换后点云"<("/source_cloud", 1);//
    pubTargetCloud = nh.advertise("/target_cloud", 1);//
    pubAfterIcpCloud = nh.advertise("/after_icp_cloud", 1);//

    ros::Rate rate_10hz(10);  
    while(ros::ok()){
        pubSourceCloud.publish(sourceCloudMsg);
        pubTargetCloud.publish(targetCloudMsg);
        pubAfterIcpCloud.publish(aftericpCloudMsg);
        
        rate_10hz.sleep();
    }

    ros::shutdown();
    return (0);
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.0.2)
project(pcl_icp_test)

find_package(catkin REQUIRED COMPONENTS
  roscpp
  rospy
  std_msgs
  sensor_msgs
  pcl_conversions
)

set(PCL_DIR "/usr/share/pcl-1.8")#设置pcl库目录
find_package(PCL REQUIRED)


include_directories(
  include
	${catkin_INCLUDE_DIRS} 
	${PCL_INCLUDE_DIRS})

catkin_package(
  CATKIN_DEPENDS   roscpp rospy std_msgs pcl_conversions
  DEPENDS  PCL 
  INCLUDE_DIRS include
)

# 单独调试pcl_icp
add_executable(pcl_icp_test src/pcl_icp_test.cpp)
target_link_libraries(pcl_icp_test ${catkin_LIBRARIES} ${PCL_LIBRARIES})

package.xml

  pcl_icp_test
  0.0.0
  The pcl_icp_test package
  meng
  TODO

  catkin
  roscpp
  rospy
  std_msgs
  pcl_conversions
  roscpp
  rospy
  std_msgs
  pcl_conversions
  roscpp
  rospy
  std_msgs
  pcl_conversions

  

  

1.2 编译运行

#开启roscore
roscore

#另一个终端
cd xxx/pcl_ws
catkin_make
source devel/setup.bash
rosrun pcl_icp_test pcl_icp_test 

运行结果：

彩色是输入点云即待匹配点云、白色是目标点云、红色是进过icp匹配后的点云

本次icp匹配效果不好，主要因为icp是一种贪心算法，这里可能陷入局部最优----解决办法一般是先进行比较好的粗匹配

2 icp匹配相关函数解析

2.1 getFitnessScore()函数

        获得欧几里得匹配/合适/适应度分数（例如，从源到目标的距离平方的平均值）。这里用于评估icp匹配效果，对icp匹配进行打分，这个打分是匹配后点云中对应点的距离差的平均值，值越小，匹配精度越高。

        getFitnessScore()函数定义有两种形式, 这里使用的是下面的形式,注意参数max_range，其默认值为std::numeric_limits::max()即double类型的最大值。

template
double pcl::Registration< PointSource, PointTarget, Scalar >::getFitnessScore 	\
( 	double  	max_range = std::numeric_limits::max()	) 	

具体定义如下

 template 
 inline double
 Registration::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_);
  
   pcl::Indices nn_indices(1);
   std::vector nn_dists(1);
  
   // For each point in the source dataset 遍历源数据集
   int nr = 0;
   for (const auto& point : input_transformed) {
     // Find its nearest neighbor in the target // 在目标数据集中找到最近的邻点
     tree_->nearestKSearch(point, 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);//距离平均
   return (std::numeric_limits::max());
 }

下面使用实际实验验证一下：

（1）当目标点云比源点云在x、y方向各平移2时

得出结果如下， 可以从rviz和终端中的平移距离看出红色目标点云与白色icp匹配后的点距离差较大，getFitnessScore函数返回值为0.0201301

（2）当目标点云比源点云在x、y方向各平移5时 

 可以从rviz和终端中的平移距离看出红色目标点云与白色icp匹配后的点距离差较小，getFitnessScore函数返回值为3.99723e-12非常小

2.2 setMaxCorrespondenceDistance()函数

        设置源 <-> 目标中两个对应点之间的最大距离阈值。如果距离大于此阈值，则在对齐过程中将忽略这些点。

c++定义：

template
void pcl::Registration< PointSource, PointTarget, Scalar >::setMaxCorrespondenceDistance 	( 	double  	distance_threshold	) 	

参数：distance_threshold

        一个点与其最近邻对应点之间的最大距离阈值，以便在对齐过程中考虑

2.3 setMaximumIterations()函数

        设置内部优化应该运行的最大迭代次数。

c++定义：

template
void pcl::Registration< PointSource, PointTarget, Scalar >::setMaximumIterations 	( 	int  	nr_iterations	) 	

参数：nr_iterations

        内部优化应该运行的最大迭代次数

2.4 setTransformationEpsilon()函数

        设置转换 epsilon（两个连续转换之间的最大允许平移平方差），以便将优化视为已收敛到最终解决方案。

c++定义：

template
void pcl::Registration< PointSource, PointTarget, Scalar >::setTransformationEpsilon 	( 	double  	epsilon	) 	

参数：epsilon

        转换 epsilon，以便将优化视为已收敛到最终解决方案。

2.5 setEuclideanFitnessEpsilon()函数

        在认为算法已经收敛之前，设置 ICP 循环中两个连续步骤之间允许的最大欧几里得误差。误差被估计为欧几里得意义上的对应之间的差异之和，除以对应的数量（即为欧几里得距离平均值）。

c++定义：

template
void pcl::Registration< PointSource, PointTarget, Scalar >::setEuclideanFitnessEpsilon 	( 	double  	epsilon	) 	

参数：epsilon

        算法收敛前的最大允许距离误差

参考链接：

getFitnessScore()函数定义

Point Cloud Library (PCL): pcl::Registration< PointSource, PointTarget, Scalar > Class Template Reference

getFitnessScore()函数具体定义：122行和134行（本次使用134行的）

Point Cloud Library (PCL): pcl/registration/impl/registration.hpp Source File

setMaxCorrespondenceDistance()函数：

Point Cloud Library (PCL): pcl::Registration< PointSource, PointTarget, Scalar > Class Template ReferencesetMaximumIterations()函数Point Cloud Library (PCL): pcl::Registration< PointSource, PointTarget, Scalar > Class Template Reference:

Point Cloud Library (PCL): pcl::Registration< PointSource, PointTarget, Scalar > Class Template Reference setTransformationEpsilon()函数：

Point Cloud Library (PCL): pcl::Registration< PointSource, PointTarget, Scalar > Class Template Reference

setEuclideanFitnessEpsilon()函数：

Point Cloud Library (PCL): pcl::Registration< PointSource, PointTarget, Scalar > Class Template Reference