目录
2.2 setMaxCorrespondenceDistance()函数
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.cpppcl_icp_test.cpp
#include "ros/ros.h"
#include <sensor_msgs/PointCloud2.h>
#include <iostream>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h>
#include <pcl_conversions/pcl_conversions.h>
using namespace std; //hxz
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in (new pcl::PointCloud<pcl::PointXYZ>);//输入点云
pcl::PointCloud<pcl::PointXYZ> cloud_in_after_icp ;//输入点云进过icp匹配后的点云
pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_out (new pcl::PointCloud<pcl::PointXYZ>);//目标点云
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<<"输入点云创建成功"<<std::endl;
// 目标点云------------------------------------------------------
cloud_out->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<<"目标点云转换成功"<<std::endl;
}
// icp匹配函数
void icp_match(){
//创建一个IterativeClosestPoint实例，使用的奇异值分解
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> 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转换后点云"<<std::endl;
//如果变换前后点云正确Align的话（即变换点云通过刚性变换之后几乎和变换后点云完全重合）
//则 icp.hasConverged() = 1 (true)，然后输出fitness得分和其他一些相关信息。
std::cout << "has converged:" << icp.hasConverged() << " score: " << icp.getFitnessScore() << std::endl;
std::cout << icp.getFinalTransformation() << std::endl;//获得最后的变换矩阵
}
int main (int argc, char** argv)
{
ros::init(argc, argv, "icp_match");
ros::NodeHandle nh("~");
create_cloud();
icp_match();
pubSourceCloud = nh.advertise<sensor_msgs::PointCloud2>("/source_cloud", 1);//
pubTargetCloud = nh.advertise<sensor_msgs::PointCloud2>("/target_cloud", 1);//
pubAfterIcpCloud = nh.advertise<sensor_msgs::PointCloud2>("/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
<?xml version="1.0"?>
<package format="2">
<name>pcl_icp_test</name>
<version>0.0.0</version>
<description>The pcl_icp_test package</description>
<maintainer email="meng@todo.todo">meng</maintainer>
<license>TODO</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp</build_depend>
<build_depend>rospy</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>pcl_conversions</build_depend>
<build_export_depend>roscpp</build_export_depend>
<build_export_depend>rospy</build_export_depend>
<build_export_depend>std_msgs</build_export_depend>
<build_export_depend>pcl_conversions</build_export_depend>
<exec_depend>roscpp</exec_depend>
<exec_depend>rospy</exec_depend>
<exec_depend>std_msgs</exec_depend>
<exec_depend>pcl_conversions</exec_depend>
<export>
</export>
</package>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<double>::max()即double类型的最大值。
template<typename PointSource , typename PointTarget , typename Scalar >
double pcl::Registration< PointSource, PointTarget, Scalar >::getFitnessScore \
( double max_range = std::numeric_limits<double>::max() ) 具体定义如下
template <typename PointSource, typename PointTarget, typename Scalar>
inline double
Registration<PointSource, PointTarget, Scalar>::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<float> 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<double>::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<typename PointSource , typename PointTarget , typename Scalar = float>
void pcl::Registration< PointSource, PointTarget, Scalar >::setMaxCorrespondenceDistance ( double distance_threshold ) 参数：distance_threshold
一个点与其最近邻对应点之间的最大距离阈值，以便在对齐过程中考虑
2.3 setMaximumIterations()函数
设置内部优化应该运行的最大迭代次数。
c++定义：
template<typename PointSource , typename PointTarget , typename Scalar = float>
void pcl::Registration< PointSource, PointTarget, Scalar >::setMaximumIterations ( int nr_iterations ) 参数：nr_iterations
内部优化应该运行的最大迭代次数
2.4 setTransformationEpsilon()函数
设置转换 epsilon（两个连续转换之间的最大允许平移平方差），以便将优化视为已收敛到最终解决方案。
c++定义：
template<typename PointSource , typename PointTarget , typename Scalar = float>
void pcl::Registration< PointSource, PointTarget, Scalar >::setTransformationEpsilon ( double epsilon ) 参数：epsilon
转换 epsilon，以便将优化视为已收敛到最终解决方案。
2.5 setEuclideanFitnessEpsilon()函数
在认为算法已经收敛之前，设置 ICP 循环中两个连续步骤之间允许的最大欧几里得误差。误差被估计为欧几里得意义上的对应之间的差异之和，除以对应的数量（即为欧几里得距离平均值）。
c++定义：
template<typename PointSource , typename PointTarget , typename Scalar = float>
void pcl::Registration< PointSource, PointTarget, Scalar >::setEuclideanFitnessEpsilon ( double epsilon ) 参数：epsilon
算法收敛前的最大允许距离误差
参考链接：
getFitnessScore()函数定义
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()函数：
setEuclideanFitnessEpsilon()函数：
评论（0）
您还未登录，请登录后发表或查看评论