ICP算法原理
这部分在很多博客中有说明，这里不加以复述，提供几个博客链接，有兴趣者请自行研究，本文主要介绍如何使用PCL中的ICP算法。不了解PCL库的请看链接。
参考资料1：关于配准问题
参考资料2：ICP基本原理
PCL中ICP算法使用详解
pcl中实现了ICP算法类 IterativeClosestPoint，也提供了关于该类的使用教程DEMO，但不够具体，下面对其做写补充：
几个主要的设置：
1. setInputCloud (cloud_source) 设置原始点云
2. setInputTarget (cloud_target) 设置目标点云
3. setMaxCorrespondenceDistance（） 设置最大对应点的欧式距离，只有对应点之间的距离小于该设置值的对应点才作为ICP计算的点对。默认值为：1.7976931348623157e+308，基本上对所有点都计算了匹配点。
4. 三个迭代终止条件设置：
1) setMaximumIterations（） 设置最大的迭代次数。迭代停止条件之一
2) setTransformationEpsilon（） 设置前后两次迭代的转换矩阵的最大容差（epsilion），一旦两次迭代小于这个最大容差，则认为已经收敛到最优解，迭代停止。迭代停止条件之二，默认值为：0
3) setEuclideanFitnessEpsilon（） 设置前后两次迭代的点对的欧式距离均值的最大容差，迭代终止条件之三，默认值为：-std::numeric_limits::max ()
迭代满足上述任一条件，终止迭代。原文如下：
* ConvergenceCriteria, and implements the following criteria for registration loop
* evaluation:
*
* * a maximum number of iterations has been reached
* * the transformation (R, t) cannot be further updated (the difference between current and previous is smaller than a threshold)
* * the Mean Squared Error (MSE) between the current set of correspondences and the previous one is smaller than some threshold (both relative and absolute tests)
*
* \note Convergence is considered reached if ANY of the above criteria are met.5.getFinalTransformation () 获取最终的配准的转化矩阵，即原始点云到目标点云的刚体变换，返回Matrix4数据类型，该数据类型采用了另一个专门用于矩阵计算的开源c++库eigen。
6. align (PointCloudSource &output) 进行ICP配准，输出变换后点云
7. hasConverged（） 获取收敛状态，注意，只要迭代过程符合上述三个终止条件之一，该函数返回true。
8. min_number_correspondences_ 最小匹配点对数量，默认值为3，即由空间中的非共线的三点就能确定刚体变换，建议将该值设置大点，否则容易出现错误匹配。
9. 最终迭代终止的原因可从convergence_criteria_变量中获取，
- getFitnessScore（）用于获取迭代结束后目标点云和配准后的点云的最近点之间距离的均值。
值得注意的是，在PCL中，通常以两点之间的欧式距离的平方作为两点之间的距离，之所以这样设置，是为了省掉开根号计算，开根号计算是很费时的操作。
使用案例
main.cpp
#include "PointCloudProcess.hpp"
#include <pcl/console/time.h>
int main(int argc, char *argv[])
{
//icp匹配实验
#if 1
pcl::console::TicToc time;
resolution = 0.0009338;
cv::Scalar color = cv::Scalar(0, 255, 0);
pcl::visualization::PCLVisualizer viewer("ICP demo");
pcl::PointCloud<PointType>::Ptr pcloud_ori(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr pcloud_source(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr pcloud_target(new pcl::PointCloud<PointType>());
pcl::PointCloud<PointType>::Ptr pcloud_icp(new pcl::PointCloud<PointType>());
cout << "-------------------ICP test-------------------" << endl;
//***读入点云文件
readPointCloud<PointType>("../datafile/for ZYK/model.ply", *pcloud_ori);
cout << "读入点云数：" << pcloud_ori->size() << endl;
//***点云下采样（减小点云密度，提高ICP配准速度）
time.tic();
downSamplePointCloud(pcloud_ori, resolution * 5, pcloud_source, 1);
cout << "下采样耗时：" << time.toc() << endl;
cout << "采样后的点数：" << pcloud_source->size() << endl;
addPointCloud<PointType>(viewer, pcloud_source, "pcloud_source", color, 2, false);
//***生成目标点云
// Defining a rotation matrix and translation vector
Eigen::Matrix4d transformation_matrix = Eigen::Matrix4d::Identity();
// A rotation matrix
double theta = M_PI / 12; // The angle of rotation in radians
transformation_matrix (0, 0) = cos (theta);
transformation_matrix (0, 1) = -sin (theta);
transformation_matrix (1, 0) = sin (theta);
transformation_matrix (1, 1) = cos (theta);
// A translation on Z axis (0.4 meters)
transformation_matrix (2, 3) = 0.04;
// Display in terminal the transformation matrix
std::cout << "Applying this rigid transformation to: cloud_in -> cloud_icp" << std::endl;
print4x4Matrix (transformation_matrix);
// Executing the transformation
time.tic();
pcl::transformPointCloud(*pcloud_source, *pcloud_target, transformation_matrix);
cout << "变换点云耗时：" << time.toc() << endl;
color = cv::Scalar(255, 0, 0);
addPointCloud<PointType>(viewer, pcloud_target, "pcloud_target", color, 2, true);
//***进行ICP配准循环，单次循环执行一次迭代
for (int i = 0; i < 15; i++)
{
time.tic();
pcl::IterativeClosestPoint<PointType, PointType> icp;
icp.setMaximumIterations(1);
icp.setInputSource(pcloud_source);
icp.setInputTarget(pcloud_target);
icp.align(*pcloud_icp);
icp.setMaximumIterations(1); // We set this variable to 1 for the next time we will call .align () function
std::cout << "运行第 " << i << "次ICP迭代的时间" << time.toc() << " ms" << std::endl;
if (icp.hasConverged())
{
std::cout << "\nICP has converged, score is " << icp.getFitnessScore() << std::endl;
std::cout << "\nICP transformation " << i << " : cloud_icp -> cloud_in" << std::endl;
transformation_matrix = icp.getFinalTransformation().cast<double>();
print4x4Matrix(transformation_matrix);
//显示配准结果
viewer.removePointCloud("pcloud_source");
color = cv::Scalar(0, 255, 0);
addPointCloud<PointType>(viewer, pcloud_icp, "pcloud_source", color,2, true);
viewer.spinOnce(1);
pcloud_source = pcloud_icp;
}
else
{
PCL_ERROR("\nICP has not converged.\n");
return (-1);
}
}
#endif
return 1;
}PointCloudProcess.hpp 文件代码如下
#pragma once
#include <pcl/io/pcd_io.h>
#include <pcl/io/ply_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/impl/point_types.hpp> //pcl中的点云类型
#include <pcl/filters/statistical_outlier_removal.h>//过滤离散点
#include <pcl/visualization/pcl_visualizer.h>
#include <string>
#include "cv.h"
#include <iostream>
#include <sstream>
using namespace pcl;
template <class PointT>
bool readPointCloud(const std::string &fileName, pcl::PointCloud<PointT> &outPointCloud)
{
string _format = fileName.substr(fileName.length() - 3, 3);
if (_format == "ply")
{
if (pcl::io::loadPLYFile<PointT>(fileName, outPointCloud) < 0)
{
pcl::console::print_error("Error loading pointCloud file!\n");
return false;
}
}
else if (_format == "pcd")
{
if (pcl::io::loadPCDFile(fileName, outPointCloud) < 0)
{
pcl::console::print_error("Error loading pointCloud file!\n");
return false;
}
}
else
{
cout << "file format error!" << endl;
return false;
}
return true;
}
//打印4*4变换矩阵
//print 4*4 matrix
inline void print4x4Matrix(const Eigen::Matrix4d & matrix)
{
printf("Rotation matrix :\n");
printf(" | %6.3f %6.3f %6.3f | \n", matrix(0, 0), matrix(0, 1), matrix(0, 2));
printf("R = | %6.3f %6.3f %6.3f | \n", matrix(1, 0), matrix(1, 1), matrix(1, 2));
printf(" | %6.3f %6.3f %6.3f | \n", matrix(2, 0), matrix(2, 1), matrix(2, 2));
printf("Translation vector :\n");
printf("t = < %6.3f, %6.3f, %6.3f >\n\n", matrix(0, 3), matrix(1, 3), matrix(2, 3));
return;
}
template <class PointT>
bool pointCloudICP(pcl::PointCloud<PointT> PointSource, pcl::PointCloud<PointT> PointTarget, pcl::PointCloud<PointT> &outPointCloud)
{
return false;
}
template <class PointT>
bool addPointCloud(pcl::visualization::PCLVisualizer &viewer, typename pcl::PointCloud<PointT>::Ptr pointCloud, const std::string pcloudID, cv::Scalar &color, int size, bool holdon) //为什么加了typename就可以了？？？？？
{
if (!holdon)
{
viewer.removeAllPointClouds();
}
pcl::visualization::PointCloudColorHandlerCustom<PointT> cloud_in_color_h(pointCloud, (int)color[0], (int)color[1], (int)color[2]);
viewer.addPointCloud(pointCloud, cloud_in_color_h, pcloudID);
viewer.setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, size, pcloudID);
//viewer.updatePointCloud(pointCloud, pcloudID);//加上这句后什么都没有
//viewer.spin();
return true;
}
//利用统计分析过滤离群点
template <class PointT>
void filterOutlier(typename PointCloud<PointT>::Ptr &srcPointCloud)
{
//基本原理，对点云所有点计算与最近点的K个点的距离的均值，对所有的点的距离均值求平均值和标准差，确定全局均值和标准差并以
//mean + std_mul_ * stddev 作为距离阈值distance_threshold，其中 std_mul_为标准差的放大倍数。将大于distance_threshold判定为离散点。
pcl::StatisticalOutlierRemoval<PointT> sor;
sor.setInputCloud(srcPointCloud);
sor.setMeanK(20);//Set the number of nearest neighbors to use for mean distance estimation.
sor.setStddevMulThresh(1.0);
sor.filter(*srcPointCloud);
return;
}
//计算点云的分辨率，对于存在较多离散点的场景，应先对离散点剔除。
template <class PointT>
double computeCloudResolution(typename PointCloud<PointT>::ConstPtr srcPointCloud)
{
double res = 0.0;
int n_points = 0;
int nres;
std::vector<int> indices(2);
std::vector<float> sqr_distances(2);
pcl::search::KdTree<PointT> tree;
tree.setInputCloud(srcPointCloud);
for (size_t i = 0; i < srcPointCloud->size(); ++i)
{
if (!pcl_isfinite((*srcPointCloud)[i].x))
{
continue;
}
//Considering the second neighbor since the first is the point itself.
nres = tree.nearestKSearch(i, 2, indices, sqr_distances);
//函数说明：1. 为所要查询的点的索引； 2.为K领域的个数，=1时表示查询点自己；3.为搜索完的领域点对应的索引； 4.为领域点到查询点的欧式距离
if (nres == 2)
{
res += sqrt(sqr_distances[1]);
++n_points;
}
}
if (n_points != 0)
{
res /= n_points;
}
return res;
}
//对点云进行下采样
template <class PointT>
bool downSamplePointCloud(typename pcl::PointCloud<PointT>::Ptr &srcPointCloud, const double relSamplingDistance,
typename pcl::PointCloud<PointT>::Ptr &outCloud, const int method)
{
//用uniformSampling对点云进行下采样
if (method == 1)
{
pcl::UniformSampling<PointT> uniform_sampling;
uniform_sampling.setInputCloud(srcPointCloud);
uniform_sampling.setRadiusSearch(relSamplingDistance);
uniform_sampling.filter(*outCloud);
return true;
}
//用voxelGraid进行下采样
if (method == 2)
{
pcl::VoxelGrid<PointT> sor;
sor.setInputCloud(srcPointCloud);
sor.setLeafSize(relSamplingDistance, relSamplingDistance, relSamplingDistance);
sor.filter(*outCloud);
return true;
}
//通过实验发现，方法一，是对原样本点进行采样，而方法二则是通过体素计算重心法进行采样，采样效率上，方法2更快，大概是方法一的6倍
return true;
}程序运行效果：
主要运用场景
一. 多视角点云的拼接
二. 3D物体识别时的位姿矫正
值得探讨的问题：
3D物体识别时的位姿矫正中，ICP是粗配准后的精配准常用的方式，精配准之后还需用HV方式对错误匹配进行剔除，那么问题来了：可否对icp的配准结果进行分析，判断是否正确配准呢？答案是可以的。
这个问题有两种情况：
1）如果两点云集都只是模型点云的一部分，且只有部分相似，这种情况下，无法用icp的结果判断是否正确配准。
2）对于其中有一个点云集是完整点云集的情况下，可以通过最近点距离小于分辨率的点对，占模型所有点的比例，得到一个分值，设定分值阈值确定是否是正确配准。
另外，其实ICP算法能运动的场景很有局限性，以下几种情况就比较难于获得较好的配准效果：
情况一：用于做点云拼接时，点云的重叠部分不大时，拼接不准。
情况二：点云密度较为稀疏时，很难达到较高的配准精度，其配准精度受限于点云的密度及点云采样一致性。提高点云密度，同时也提高配准时间，这在很多场景中都是不允许的。