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 

运行结果:

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

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

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

pcl点云库(1):迭代最近点算法(icp)算法实践及解析_第2张图片

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时

pcl点云库(1):迭代最近点算法(icp)算法实践及解析_第3张图片

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

pcl点云库(1):迭代最近点算法(icp)算法实践及解析_第4张图片

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

pcl点云库(1):迭代最近点算法(icp)算法实践及解析_第5张图片

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

pcl点云库(1):迭代最近点算法(icp)算法实践及解析_第6张图片

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

你可能感兴趣的:(ros学习,c++,pcl,算法,ros,pcl,点云)