求解带缩放的点云icp算法

概述

 点云配准通常会需要用到两个点云数据。第一类点云数据称为原始点云，用S（source）来表示。第二类点云数据称为目标点云，用T（Target）来表示。

        点云配准是让原始点云S在目标点云T的坐标上进行显示。我们可以通过找到点云中具有相似特征的点云来确定坐标的变换关系。例如，同一个物体的点云同时出现在原始点云和目标点云中，并且在两个点云中有特征相似的部分点云，根据这些相似的点云信息来计算出变换关系。

        假设原始点云到目标点云发生的是刚体变换，即原始点云通过旋转和平移即可得到目标点云。这里的旋转和平移过程用旋转变换矩阵R和平移变换矩阵T来表示。我们用P(S)表示原始点云中的点，P(T)表示原始点云在目标点云坐标系中的点。那么这种变换关系可以表示为：

        因此，点云配准的主要任务是计算出旋转矩阵R和平移矩阵T。

迭代最近点算法(Iterative Closest Point, ICP)
        第一步：初始化R、T矩阵，根据R、T矩阵可以得到P(T)，即原始点云在目标点云坐标系下的坐标。

        第二步：在目标点云中寻找与P(T)最近的点，并且距离小于规定的阈值，这个阈值可以自己定义。

        第三步：对第二步中匹配到的点计算欧式距离误差，并且通过最小二乘法来优化R、T矩阵。

        第四步：将第三步优化后的R、T矩阵带回第一步中