关于目前ICP算法的理解和总结
基本概念之配准
有两个点集，source和target，target不变，source经过旋转（Rotation）和平移（Translation）甚至加上尺度（Scale）变换，使得变换后的source点集尽量和target点集重合，这个变换的过程就叫点集配准。
两个点集的对应，输出通常是一个4×4刚性变换矩阵：代表旋转和平移，它应用于源数据集，结果是完全与目标数据集匹配。下图是“双对应”算法中一次迭代的步骤：
image.png
对两个数据源a，b匹配运算步骤如下：
- 从其中一个数据源a出发，分析其最能代表两个数据源场景共同点的关键点k
- 在每个关键点ki处，算出一个特征描述子fi
- 从这组特征描述子{fi}和他们在a和b中的XYZ坐标位置，基于fi和xyz的相似度，找出一组对应
- 由于实际数据源是有噪的，所以不是所有的对应都有效，这就需要一步一步排除对匹配起负作用的对应
从剩下的较好对应中，估计出一个变换
icp优化思路
首先icp是一步一步迭代得到较好的结果，解的过程其实是一个优化问题，并不能达到绝对正解。这个过程中求两个点云之间变换矩阵是最重要的，PCL里是用奇异值分解SVD实现的
从上面的icp流程中我们可以看到的可以用不同的算法对1、2、3这三个步骤进行优化
关于PLICP调研和优化方案，后续给出
目前我们的代码中的重要参数如下，需要我们设置的参数为加粗部分
inline void inline void setSearchMethodTarget(const KdTreePtr &tree) kdtree
加速搜索，还有一个Target的函数，用法与之一致。
inline void setInputSource (constPointCloudSourceConstPtr &cloud)
需要匹配的点云。
inline void setInputTarget (constPointCloudTargetConstPtr &cloud)
基准点云，也就是从Source到Target的匹配。
inline void setMaxCorrespondenceDistance (doubledistance_threshold)
忽略在此距离之外的点，如果两个点云距离较大，这个值要设的大一些（PCL默认距离单位是m）。目前我们的launch文件中mcpd设置的大小为0.05
inline void setTransformationEpsilon (doubleepsilon)
第2个约束，这个值一般设为1e-6或者更小。（目前在launch文件中并没有设置，代码1e-6，这个值可以更小）
inline void setEuclideanFitnessEpsilon (doubleepsilon)
第3个约束，前后两次迭代误差的差值。（目前在launch文件中并没有设置，代码1e-6，太小，可优化）
inline void setMaximumIterations (intnr_iterations)
第1个约束，迭代次数，几十上百都可能出现。目前在launch文件中maxIterations为250
inline void align (PointCloudSource &output)
输出配准后点云。
inline Matrix4 getFinalTransformation ()
获取最终的转换矩阵。
bool ScanMatch::icp(const pcl::PointCloud<pcl::PointXYZ>::Ptr &scan_cloud,
const pcl::PointCloud<pcl::PointXYZ>::Ptr &map_cloud, tf2::Transform &pose_corrections_out) {
pose_corrections_out = tf2::Transform::getIdentity();
pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> reg;
//第2个约束，这个值一般设为1e-6或者更小
reg.setTransformationEpsilon(1e-6);
//第3个约束，前后两次迭代误差的差值
reg.setEuclideanFitnessEpsilon(1e-6);
//忽略在此距离之外的点，如果两个点云距离较大，这个值要设的大一些
reg.setMaxCorrespondenceDistance(0.02);//0.02
// 第1个约束，迭代次数，几十上百都可能出现
reg.setMaximumIterations(maxIterations_);
//进行RANSAC迭代
reg.setRANSACIterations(maxIterations_);
//剔除错误估计，可用 RANSAC 算法，或减少数量
reg.setRANSACOutlierRejectionThreshold(0.05);
//设置查找近邻时是否双向搜索
reg.setUseReciprocalCorrespondences(true);
{
pcl::search::KdTree<pcl::PointXYZ>::Ptr target_pointcloud_search_method(
new pcl::search::KdTree<pcl::PointXYZ>());
target_pointcloud_search_method->setInputCloud(map_cloud);
reg.setInputTarget(map_cloud);
reg.setSearchMethodTarget(target_pointcloud_search_method, true);
}
pcl::search::KdTree<pcl::PointXYZ>::Ptr reference_pointcloud_search_method(new pcl::search::KdTree<pcl::PointXYZ>());
reference_pointcloud_search_method->setInputCloud(map_cloud);
{
pcl::search::KdTmree<pcl::PointXYZ>::Ptr source_pointcloud_search_method(new pcl::search::KdTree<pcl::PointXYZ>());
source_pointcloud_search_method->setInputCloud(scan_cloud);
reg.setSearchMethodSource(source_pointcloud_search_method, true);
reg.setInputSource(scan_cloud);
}
pcl::PointCloud<pcl::PointXYZ>::Ptr unused(new pcl::PointCloud<pcl::PointXYZ>());
//调用该函数开配准
reg.align(*unused);
const Eigen::Matrix4f &transf = reg.getFinalTransformation();
tf::Transform tf;
matrixAsTransfrom(transf, tf);
if (reg.hasConverged()) {
tfToTF2(tf, pose_corrections_out);
}
return false;
}
