关于目前ICP算法的理解和总结

基本概念之配准

有两个点集，source和target，target不变，source经过旋转（Rotation）和平移（Translation）甚至加上尺度（Scale）变换，使得变换后的source点集尽量和target点集重合，这个变换的过程就叫点集配准。

两个点集的对应，输出通常是一个4×4刚性变换矩阵：代表旋转和平移，它应用于源数据集，结果是完全与目标数据集匹配。下图是“双对应”算法中一次迭代的步骤：

image.png

对两个数据源a，b匹配运算步骤如下：

1. 从其中一个数据源a出发，分析其最能代表两个数据源场景共同点的关键点k
2. 在每个关键点ki处，算出一个特征描述子fi
3. 从这组特征描述子{fi}和他们在a和b中的XYZ坐标位置，基于fi和xyz的相似度，找出一组对应
4. 由于实际数据源是有噪的，所以不是所有的对应都有效，这就需要一步一步排除对匹配起负作用的对应
   从剩下的较好对应中，估计出一个变换

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;
}