迭代最近点 ICP 详细推导（C++实现）

Alan Lan

已于 2022-03-30 17:10:39 修改

阅读量1.1w

点赞数 14

分类专栏： PCL 文章标签： c++ 线性代数算法

于 2020-07-20 19:39:46 首次发布

本文链接：https://blog.csdn.net/A_L_A_N/article/details/81329728

版权

PCL 专栏收录该内容

12 篇文章 21 订阅

订阅专栏

一、简介

迭代最近点算法（Iterated Closest Points, ICP），顾名思义，就是采用迭代优化的思想以空间距离作为匹配点的选择依据，通过不断调整点云的位姿使得匹配点之间距离累计最小。假设有两组点云，其中一个目标点云A另一个为参考点云B，ICP算法的目的是为了算出一个最优的旋转矩阵R和平移向量t使得变换后的点云A能与B达到最精确的匹配。ICP算法因为其思想简单，精度高等特点成为了局部配准的主流算法。

ICP需要反复执行以下两步直至收敛：第一步，计算A点集与B点集之间的匹配点对；第二步，根据前一步得到的匹配点对计算出A点集与B点集之间的转换矩阵，并对A点集进行相应的转换。在取得了点集A与B的匹配点基础上，点集之间的转换矩阵求解也分为两种方式：利用线性代数的求解（主要是SVD），以及利用非线性优化方式的求解（类似于Bundle Adjustment）。其中奇异值分解法（SVD）因为准确、稳定以及高效等特点被广泛运用，本文主要介绍利用SVD进行转换矩阵的求解。

最近点对查找：对应点的计算是整个配准过程中耗费时间最长的步骤，查找最近点，利用 kd tree提高查找速度。kd tree 法建立点的拓扑关系是基于二叉树的坐标轴分割，构造 kd tree 的过程就是按照二叉树法则生成。首先按 X 轴寻找分割线，即计算所有点的x值的平均值，以最接近这个平均值的点的x值将空间分成两部分，然后在分成的子空间中按 Y 轴寻找分割线，将其各分成两部分，分割好的子空间在按X轴分割……依此类推，最后直到分割的区域内只有一个点。这样的分割过程就对应于一个二叉树，二叉树的分节点就对应一条分割线，而二叉树的每个叶子节点就对应一个点，这样点的拓扑关系就建立了。

ICP方法的配准步骤如下：假设给两个三维点集 X1 和 X2，

第一步，计算X2中的每一个点在X1 点集中的对应近点；

第二步，求得使上述对应点对平均距离最小的刚体变换，求得平移参数和旋转参数；

第三步，对X2使用上一步求得的平移和旋转参数，得到新的变换点集；

第四步，如果新的变换点集与参考点集满足两点集的平均距离小于某一给定阈值，则停止迭代计算，否则新的变换点集作为新的X2继续迭代，直到达到目标函数的要求。

二、详细推导

问题描述：

假设存在两个点云集合 $\{p\}$ 和 $\{p^{'}\}$ ，求：一个欧式变换 R, t 使得

求解问题：

假设误差项为：

$e_{i}=p_{i}-(Rp_{i}^{'}+t)$

那么问题转化为优化问题：

$\min_{R,t}J=\frac{1}{2}\sum_{i=1}^{n}\left \| (p_{i}-(Rp_{i}{'}+t)) \right \|^{2}$

定义质心为：

$p=\frac{1}{n}\sum_{i=1}^{n}(p_{i}),p^{'}=\frac{1}{n}\sum_{i=1}^{n}(p_{i}^{'})$

那么有：

$\frac{1}{2}\sum_{i=1}^{n}\left\| p_{i}-(Rp_{i}{'}+t) \right \|^{2} \\=\frac{1}{2}\sum_{i=1}^{n}\left \|p_{i}-Rp_{i}^{'}-t-p+Rp^{'}+p-Rp^{'}\right \|^{2} \\=\frac{1}{2}\sum_{i=1}^{n}\left(\left \|p_{i}-p-R(p_{i}^{'}-p^{'})\right \|^{2}+\left \| p-Rp^{'}-t \right \|^{2}+2(p_{i}-p-R(p_{i}^{'}-p^{'}))^{T}(p-Rp^{'}-t)\right)$

因为

所以问题转化为：

$\min_{R,t}J=\frac{1}{2}\sum_{i=1}^{n}\left (\left \|p_{i}-p-R(p_{i}^{'}-p^{'})\right \|^{2}+\left \| p-Rp^{'}-t \right \|^{2} \right )$

左右两项都大于零，左边只和旋转矩阵R相关，而右边既有R也有t，但只和质心相关。可以先求出R，再令第二项为零就能得到t。

1、计算两组质心位置 $p,p^{'}$ ，然后计算每个点的去质心坐标：
$q_{i}=p_{i}-p,q_{i}^{'}=p_{i}^{'}-p^{'}$
2、根据以下优化问题计算旋转矩阵：
$R^{*}=\arg \min_{R}\frac{1}{2}\sum_{i=1}^{n} \left \| q_{i}-Rq_{i}^{'} \right \|^{2}$
3、根据第2步的R计算t：
$t^{*}=p-Rp^{'}$

展开关于R的误差项有：

$\frac{1}{2}\sum_{i=1}^{n} \left \| q_{i}-Rq_{i}^{'} \right \|^{2} \\=\frac{1}{2}\sum_{i=1}^{n}\left ( q_{i}^{T}q_{i}+q_{i}^{'T}R^{T}Rq_{i}^{'} -2q_{i}^{T}Rq_{i}^{'} \right )$

因为第一项于R无关，第二项由于 $R^{T}R=I$ 与R也无关，那么问题转化为：

$\sum_{i=1}^{n}-q_{i}^{T}Rq_{i}^{'}=\sum_{i=1}^{n}-tr(q_{i}^{T}Rq_{i}^{'})=\sum_{i=1}^{n}-tr(Rq_{i}^{'}q_{i}^{T})=-tr(\sum_{i=1}^{n}Rq_{i}^{'}q_{i}^{T})=-tr(R\sum_{i=1}^{n}q_{i}^{'}q_{i}^{T})$

令： $H=\sum_{i=1}^{n}q_{i}^{'}q_{i}^{T}$

因为问题是求解： $\min_{R}.-tr(RH)$

即： $\max_{R}.tr(RH)$

假设最优解为 $R^{*}$

那么 $tr(R^{*}H)\geq tr(RH)=tr(BR^{*}H)$ ，（因为R是正交矩阵）

对H进行SVD分解 $H=U\Sigma V^{T}$

（H为3X3的矩阵， $\Sigma$ 为奇异值组成的对焦矩阵，对角线元素从大到小排列，U和V为对角矩阵）

当H满秩时，可以得到 $R^{*}=V U^{T}$

那么 $R^{*}H=V U^{T}U\Sigma V^{T}=V \Sigma V^{T}$

令 $A=V\Sigma^{\frac{1}{2}}$

因为 $tr(R^{*}H)=tr(AA^{T})\geq tr(BAA^{T}),(BB^{T}=I)$

所以 $R^{*}=V U^{T}$ 是 $\max_{R}.tr(RH)$ 的最优解。

现在只要证明 $tr(AA^{T})\geq tr(BAA^{T}),(BB^{T}=I)$

证明：

设 $a_{i}$ 为A的第i列，因为 $tr(AB)=tr(BA)$ ，那么有 $tr(BAA^{T})=tr(A^{T}BA)=\sum a_{i}^{T}(Ba_{i})$

根据Schwarz不等式 $a_{i}^{T}(Ba_{i})\leq \sqrt{(a_{i}^{T}a_{i})(a_{i}^{T}B^{T}Ba_{i})}=a_{i}^{T}a_{i}$

即 $tr(BAA^{T})=tr(A^{T}BA)\leq \sum a_{i}^{T}a_{i}=tr(A)$

注意：

1、这个计算需要H是满秩，即 $\{p^{'}\}$ 上的点不共面

2、若 $\{p^{'}\}$ 上的点共面，可以对H求出的解的为0特征值的特征向量计算取反，使得 $det\left | H \right |=1$

3、若 $\{p^{'}\}$ 上的点共线，不能用SVD求解

三、C++实现

icp_test1.cc

#include <fstream>
#include <sstream>
#include <iostream>
#include <vector>
#include <Eigen/Eigen>

void ICP(const std::vector<Eigen::Vector3f>& p1, const std::vector<Eigen::Vector3f>& p2,
         Eigen::Matrix3f& R_12, Eigen::Vector3f& t_12) {
    assert(p1.size() == p2.size());

    // center of mass
    size_t N = p1.size();
    Eigen::Vector3f p1_center, p2_center;
    for (int i = 0; i < N; ++i) {
        p1_center += p1.at(i);
        p2_center += p2.at(i);
    }
    p1_center /= N;
    p2_center /= N;

    // remove the center
    std::vector<Eigen::Vector3f> q1(N), q2(N);
    for (int i = 0; i < N; ++i) {
        q1[i] = p1.at(i) - p1_center;
        q2[i] = p2.at(i) - p2_center;
    }

    // compute q2*q1^T
    Eigen::Matrix3f H = Eigen::Matrix3f::Zero();
    for (int i = 0; i < N; ++i) {
        H += q2.at(i) * q1.at(i).transpose();
    }

    // SVD on H
    Eigen::JacobiSVD<Eigen::Matrix3f> svd(H, Eigen::ComputeFullU | Eigen::ComputeFullV);
    Eigen::Matrix3f U = svd.matrixU();
    Eigen::Matrix3f V = svd.matrixV();

    R_12 = V * U.transpose();
    t_12 = p1_center - R_12 * p2_center;
}

int main(int argc, char** argv) {
    Eigen::AngleAxisf angle_axis(M_PI / 3, Eigen::Vector3f(0, 0, 1));
    Eigen::Matrix3f R_12_ = angle_axis.matrix();
    Eigen::Vector3f t_12_(1, 2, 3);
    std::cout << "except R_12:\n" << R_12_ << std::endl;
    std::cout << "except t_12:\n" << t_12_.transpose() << std::endl;

    std::vector<Eigen::Vector3f> p1, p2;
    Eigen::Vector3f point;

    std::ifstream fin("/tmp/bunny.txt");
    std::string line;
    while (getline(fin, line)) {
        std::stringstream ss(line);
        ss >> point.x();
        ss >> point.y();
        ss >> point.z();
        p2.push_back(point);
        p1.push_back(R_12_ * point + t_12_);
    }
    fin.close();

    Eigen::Matrix3f R_12;
    Eigen::Vector3f t_12;
    ICP(p1, p2, R_12, t_12);

    std::cout << "result R_12:\n" << R_12_ << std::endl;
    std::cout << "result t_12:\n" << t_12.transpose() << std::endl;

    return 0;
}

输出：

except R_12:
      0.5 -0.866025         0
 0.866025       0.5         0
        0         0         1
except t_12:
1 2 3
result R_12:
      0.5 -0.866025         0
 0.866025       0.5         0
        0         0         1
result t_12:
1 2 3

利用pcl库：

icp_test2.cc

  /** \brief @b IterativeClosestPoint provides a base implementation of the Iterative Closest Point algorithm. 
    * The transformation is estimated based on Singular Value Decomposition (SVD).
    *
    * The algorithm has several termination criteria:
    *
    * <ol>
    * <li>Number of iterations has reached the maximum user imposed number of iterations (via \ref setMaximumIterations)</li>
    * <li>The epsilon (difference) between the previous transformation and the current estimated transformation is smaller than an user imposed value (via \ref setTransformationEpsilon)</li>
    * <li>The sum of Euclidean squared errors is smaller than a user defined threshold (via \ref setEuclideanFitnessEpsilon)</li>
    * </ol>
    *
    *
    * Usage example:
    * \code
    * IterativeClosestPoint<PointXYZ, PointXYZ> icp;
    * // Set the input source and target
    * icp.setInputCloud (cloud_source);
    * icp.setInputTarget (cloud_target);
    *
    * // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
    * icp.setMaxCorrespondenceDistance (0.05);
    * // Set the maximum number of iterations (criterion 1)
    * icp.setMaximumIterations (50);
    * // Set the transformation epsilon (criterion 2)
    * icp.setTransformationEpsilon (1e-8);
    * // Set the euclidean distance difference epsilon (criterion 3)
    * icp.setEuclideanFitnessEpsilon (1);
    *
    * // Perform the alignment
    * icp.align (cloud_source_registered);
    *
    * // Obtain the transformation that aligned cloud_source to cloud_source_registered
    * Eigen::Matrix4f transformation = icp.getFinalTransformation ();
    * \endcode
    *
    * \author Radu B. Rusu, Michael Dixon
    * \ingroup registration
    */
#include <fstream>
#include <sstream>
#include <iostream>
#include <pcl/point_types.h>
#include <pcl/registration/icp.h> 

int main(int argc, char** argv) {
    Eigen::AngleAxisf angle_axis(M_PI / 3, Eigen::Vector3f(0, 0, 1));
    Eigen::Matrix3f R_12_ = angle_axis.matrix();
    Eigen::Vector3f t_12_(1, 2, 3);
    std::cout << "except R_12:\n" << R_12_ << std::endl;
    std::cout << "except t_12:\n" << t_12_.transpose() << std::endl;

    pcl::PointCloud<pcl::PointXYZ>::Ptr source_cloud(new pcl::PointCloud<pcl::PointXYZ>); 
    pcl::PointCloud<pcl::PointXYZ>::Ptr target_cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointXYZ point;
 
    std::ifstream fin("/tmp/bunny.txt");
    std::string line;
    while (getline(fin, line)) {
        std::stringstream ss(line);
        ss >> point.x;
        ss >> point.y;
        ss >> point.z;
        source_cloud->push_back(point);
    }
    fin.close();

    Eigen::Matrix4f T_12_;
    T_12_ << R_12_, t_12_, 0, 0, 0, 1;
    pcl::transformPointCloud(*source_cloud, *target_cloud, T_12_);

    pcl::IterativeClosestPoint<pcl::PointXYZ, pcl::PointXYZ> icp;
    // icp.setMaxCorrespondenceDistance(0.05); // Set the max correspondence distance to 5cm (e.g., correspondences with higher distances will be ignored)
    icp.setTransformationEpsilon(1e-8); // 前一个变换矩阵和当前变换矩阵的差异小于阈值时，就认为已经收敛了，是一条收敛条件；// Set the transformation epsilon (criterion 2)
    icp.setEuclideanFitnessEpsilon(1); // 还有一条收敛条件是均方误差和小于阈值，停止迭代。// Set the euclidean distance difference epsilon (criterion 3)
    icp.setMaximumIterations(50); // Set the maximum number of iterations (criterion 1)
    icp.setInputSource(source_cloud); // 设置输入点云
    icp.setInputTarget(target_cloud); // 设置目标点云（输入点云进行仿射变换，得到目标点云）
    pcl::PointCloud<pcl::PointXYZ> Final; // 存储结果								 
    icp.align(Final); // 进行配准，结果存储在Final中
    // 输出ICP配准的信息（是否收敛，拟合度）
    std::cout << "has converged: " << (icp.hasConverged()? "True" : "False") << std::endl;
    std::cout << "score: " << icp.getFitnessScore() << std::endl;
    // 输出最终的变换矩阵（4x4）
    std::cout << "result transformation:\n" << icp.getFinalTransformation() << std::endl;
 
    return 0;
}

输出：

except R_12:
      0.5 -0.866025         0
 0.866025       0.5         0
        0         0         1
except t_12:
1 2 3
has converged: True
score: 7.09191e-13
result transformation:
         0.5    -0.866026  8.05312e-07            1
    0.866027          0.5 -8.71574e-07            2
-1.38908e-07  1.09029e-06            1            3
           0            0            0            1

CMakeLists.txt

cmake_minimum_required(VERSION 2.8.3)
project(test)

set(CMAKE_CXX_STANDARD 11)
set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)

find_package(Eigen3)
INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})

find_package(PCL 1.2 REQUIRED)
include_directories(${PCL_INCLUDE_DIRS})

add_executable(icp_test1 icp_test1.cc)
target_link_libraries(icp_test1 ${Eigen_LIBS})

add_executable(icp_test2 icp_test2.cc)
target_link_libraries(icp_test2 ${Eigen_LIBS} ${PCL_LIBRARIES})

在PCL官方的tutorial中还有个ICP算法交互的例子 Interactive Iterative Closest Point，该程序中按一次空格ICP迭代计算一次，可以看出，随着迭代进行，两块点云逐渐重合在一起。

点云素材:(bunny.txt)

0.0054216 0.11349 0.040749
-0.0017447 0.11425 0.041273
-0.010661 0.11338 0.040916
0.026422 0.11499 0.032623
0.024545 0.12284 0.024255
0.034137 0.11316 0.02507
0.02886 0.11773 0.027037
0.02675 0.12234 0.017605
0.03575 0.1123 0.019109
0.015982 0.12307 0.031279
0.0079813 0.12438 0.032798
0.018101 0.11674 0.035493
0.0086687 0.11758 0.037538
0.01808 0.12536 0.026132
0.0080861 0.12866 0.02619
0.02275 0.12146 0.029671
-0.0018689 0.12456 0.033184
-0.011168 0.12376 0.032519
-0.0020063 0.11937 0.038104
-0.01232 0.11816 0.037427
-0.0016659 0.12879 0.026782
-0.011971 0.12723 0.026219
0.016484 0.12828 0.01928
0.0070921 0.13103 0.018415
0.0014615 0.13134 0.017095
-0.013821 0.12886 0.019265
-0.01725 0.11202 0.040077
-0.074556 0.13415 0.051046
-0.065971 0.14396 0.04109
-0.071925 0.14545 0.043266
-0.06551 0.13624 0.042195
-0.071112 0.13767 0.047518
-0.079528 0.13416 0.051194
-0.080421 0.14428 0.042793
-0.082672 0.1378 0.046806
-0.08813 0.13514 0.042222
-0.066325 0.12347 0.050729
-0.072399 0.12662 0.052364
-0.066091 0.11973 0.050881
-0.072012 0.11811 0.052295
-0.062433 0.12627 0.043831
-0.068326 0.12998 0.048875
-0.063094 0.11811 0.044399
-0.071301 0.11322 0.04841
-0.080515 0.12741 0.052034
-0.078179 0.1191 0.051116
-0.085216 0.12609 0.049001
-0.089538 0.12621 0.044589
-0.082659 0.11661 0.04797
-0.089536 0.11784 0.04457
-0.0565 0.15248 0.030132
-0.055517 0.15313 0.026915
-0.03625 0.17198 0.00017688
-0.03775 0.17198 0.00022189
-0.03625 0.16935 0.00051958
-0.033176 0.15711 0.0018682
-0.051913 0.1545 0.011273
-0.041707 0.16642 0.0030522
-0.049468 0.16414 0.0041988
-0.041892 0.15669 0.0054879
-0.051224 0.15878 0.0080283
-0.062417 0.15317 0.033161
-0.07167 0.15319 0.033701
-0.062543 0.15524 0.027405
-0.07211 0.1555 0.027645
-0.078663 0.15269 0.032268
-0.081569 0.15374 0.026085
-0.08725 0.1523 0.022135
-0.05725 0.15568 0.010325
-0.057888 0.1575 0.0073225
-0.0885 0.15223 0.019215
-0.056129 0.14616 0.03085
-0.054705 0.13555 0.032127
-0.054144 0.14714 0.026275
-0.046625 0.13234 0.021909
-0.05139 0.13694 0.025787
-0.018278 0.12238 0.030773
-0.021656 0.11643 0.035209
-0.031921 0.11566 0.032851
-0.021348 0.12421 0.024562
-0.03241 0.12349 0.023293
-0.024869 0.12094 0.028745
-0.031747 0.12039 0.028229
-0.052912 0.12686 0.034968
-0.041672 0.11564 0.032998
-0.052037 0.1168 0.034582
-0.042495 0.12488 0.024082
-0.047946 0.12736 0.028108
-0.042421 0.12035 0.028633
-0.047661 0.12024 0.028871
-0.035964 0.1513 0.0005395
-0.050598 0.1474 0.013881
-0.046375 0.13293 0.018289
-0.049125 0.13856 0.016269
-0.042976 0.14915 0.0054003
-0.047965 0.14659 0.0086783
-0.022926 0.1263 0.018077
-0.031583 0.1259 0.017804
-0.041733 0.12796 0.01665
-0.061482 0.14698 0.036168
-0.071729 0.15026 0.038328
-0.060526 0.1368 0.035999
-0.082619 0.14823 0.035955
-0.087824 0.14449 0.033779
-0.089 0.13828 0.037774
-0.085662 0.15095 0.028208
-0.089601 0.14725 0.025869
-0.090681 0.13748 0.02369
-0.058722 0.12924 0.038992
-0.060075 0.11512 0.037685
-0.091812 0.12767 0.038703
-0.091727 0.11657 0.039619
-0.093164 0.12721 0.025211
-0.093938 0.12067 0.024399
-0.091583 0.14522 0.01986
-0.090929 0.13667 0.019817
-0.093094 0.11635 0.018959
0.024948 0.10286 0.041418
0.0336 0.092627 0.040463
0.02742 0.096386 0.043312
0.03392 0.086911 0.041034
0.028156 0.086837 0.045084
0.03381 0.078604 0.040854
0.028125 0.076874 0.045059
0.0145 0.093279 0.05088
0.0074817 0.09473 0.052315
0.017407 0.10535 0.043139
0.0079536 0.10633 0.042968
0.018511 0.097194 0.047253
0.0086436 0.099323 0.048079
-0.0020197 0.095698 0.053906
-0.011446 0.095169 0.053862
-0.001875 0.10691 0.043455
-0.011875 0.10688 0.043019
-0.0017622 0.10071 0.046648
-0.012498 0.10008 0.045916
0.016381 0.085894 0.051642
0.0081167 0.08691 0.055228
0.017644 0.076955 0.052372
0.008125 0.076853 0.055536
0.020575 0.088169 0.049006
0.022445 0.075721 0.049563
-0.0017931 0.086849 0.056843
-0.011943 0.086771 0.057009
-0.0019567 0.076863 0.057803
-0.011875 0.076964 0.057022
0.03325 0.067541 0.040033
0.028149 0.066829 0.042953
0.026761 0.057829 0.042588
0.023571 0.04746 0.040428
0.015832 0.067418 0.051639
0.0080431 0.066902 0.055006
0.013984 0.058886 0.050416
0.0080973 0.056888 0.05295
0.020566 0.065958 0.0483
0.018594 0.056539 0.047879
0.012875 0.052652 0.049689
-0.0017852 0.066712 0.056503
-0.011785 0.066885 0.055015
-0.001875 0.056597 0.05441
-0.01184 0.057054 0.052714
-0.015688 0.052469 0.049615
0.0066154 0.04993 0.051259
0.018088 0.046655 0.043321
0.008841 0.045437 0.046623
0.017688 0.039719 0.043084
0.008125 0.039516 0.045374
-0.0016111 0.049844 0.05172
-0.01245 0.046773 0.050903
-0.013851 0.039778 0.051036
-0.0020294 0.044874 0.047587
-0.011653 0.04686 0.048661
-0.0018611 0.039606 0.047339
-0.0091545 0.03958 0.049415
0.043661 0.094028 0.02252
0.034642 0.10473 0.031831
0.028343 0.1072 0.036339
0.036339 0.096552 0.034843
0.031733 0.099372 0.038505
0.036998 0.10668 0.026781
0.032875 0.11108 0.02959
0.040938 0.097132 0.026663
0.044153 0.086466 0.024241
0.05375 0.072221 0.020429
0.04516 0.076574 0.023594
0.038036 0.086663 0.035459
0.037861 0.076625 0.035658
0.042216 0.087237 0.028254
0.042355 0.076747 0.02858
0.043875 0.096228 0.015269
0.044375 0.096797 0.0086445
0.039545 0.1061 0.017655
0.042313 0.10009 0.017237
0.045406 0.087417 0.015604
0.055118 0.072639 0.017944
0.048722 0.07376 0.017434
0.045917 0.086298 0.0094211
0.019433 0.1096 0.039063
0.01097 0.11058 0.039648
0.046657 0.057153 0.031337
0.056079 0.066335 0.024122
0.048168 0.06701 0.026298
0.056055 0.057253 0.024902
0.051163 0.056662 0.029137
0.036914 0.067032 0.036122
0.033 0.06472 0.039903
0.038004 0.056507 0.033119
0.030629 0.054915 0.038484
0.041875 0.066383 0.028357
0.041434 0.06088 0.029632
0.044921 0.049904 0.031243
0.054635 0.050167 0.022044
0.04828 0.04737 0.025845
0.037973 0.048347 0.031456
0.028053 0.047061 0.035991
0.025595 0.040346 0.03415
0.038455 0.043509 0.028278
0.032031 0.043278 0.029253
0.036581 0.040335 0.025144
0.03019 0.039321 0.026847
0.059333 0.067891 0.017361
0.0465 0.071452 0.01971
0.059562 0.057747 0.01834
0.055636 0.049199 0.019173
0.0505 0.045064 0.019181
0.023 0.047803 0.039776
0.022389 0.03886 0.038795
-0.019545 0.0939 0.052205
-0.021462 0.10618 0.042059
-0.031027 0.10395 0.041228
-0.022521 0.097723 0.045194
-0.031858 0.097026 0.043878
-0.043262 0.10412 0.040891
-0.052154 0.10404 0.040972
-0.041875 0.096944 0.042424
-0.051919 0.096967 0.043563
-0.021489 0.086672 0.054767
-0.027 0.083087 0.050284
-0.02107 0.077249 0.054365
-0.026011 0.089634 0.048981
-0.031893 0.087035 0.044169
-0.025625 0.074892 0.047102
-0.03197 0.0769 0.042177
-0.041824 0.086954 0.043295
-0.051825 0.086844 0.044933
-0.041918 0.076728 0.042564
-0.051849 0.076877 0.042992
-0.061339 0.10393 0.041164
-0.072672 0.10976 0.044294
-0.061784 0.096825 0.043327
-0.070058 0.096203 0.041397
-0.080439 0.11091 0.044343
-0.061927 0.086724 0.04452
-0.070344 0.087352 0.041908
-0.06141 0.077489 0.042178
-0.068579 0.080144 0.041024
-0.019045 0.067732 0.052388
-0.017742 0.058909 0.050809
-0.023548 0.066382 0.045226
-0.03399 0.067795 0.040929
-0.02169 0.056549 0.045164
-0.036111 0.060706 0.040407
-0.041231 0.066951 0.041392
-0.048588 0.070956 0.040357
-0.0403 0.059465 0.040446
-0.02192 0.044965 0.052258
-0.029187 0.043585 0.051088
-0.021919 0.039826 0.053521
-0.030331 0.039749 0.052133
-0.021998 0.049847 0.046725
-0.031911 0.046848 0.045187
-0.035276 0.039753 0.047529
-0.042016 0.044823 0.041594
-0.05194 0.044707 0.043498
-0.041928 0.039327 0.043582
-0.051857 0.039252 0.046212
-0.059453 0.04424 0.042862
-0.060765 0.039087 0.044363
-0.024273 0.11038 0.039129
-0.032379 0.10878 0.037952
-0.041152 0.10853 0.037969
-0.051698 0.10906 0.038258
-0.062091 0.10877 0.038274
-0.071655 0.10596 0.037516
-0.074634 0.097746 0.038347
-0.07912 0.10508 0.032308
-0.080203 0.096758 0.033592
-0.08378 0.10568 0.025985
-0.087292 0.10314 0.020825
-0.08521 0.097079 0.02781
-0.088082 0.096456 0.022985
-0.07516 0.08604 0.038816
-0.064577 0.073455 0.03897
-0.072279 0.076416 0.036413
-0.076375 0.072563 0.02873
-0.080031 0.087076 0.03429
-0.078919 0.079371 0.032477
-0.084834 0.086686 0.026974
-0.087891 0.089233 0.022611
-0.081048 0.077169 0.025829
-0.086393 0.10784 0.018635
-0.087672 0.10492 0.017264
-0.089333 0.098483 0.01761
-0.086375 0.083067 0.018607
-0.089179 0.089186 0.018947
-0.082879 0.076109 0.017794
-0.0825 0.074674 0.0071175
-0.026437 0.064141 0.039321
-0.030035 0.06613 0.038942
-0.026131 0.056531 0.038882
-0.031664 0.056657 0.037742
-0.045716 0.064541 0.039166
-0.051959 0.066869 0.036733
-0.042557 0.055545 0.039026
-0.049406 0.056892 0.034344
-0.0555 0.062391 0.029498
-0.05375 0.058574 0.026313
-0.03406 0.050137 0.038577
-0.041741 0.04959 0.03929
-0.050975 0.049435 0.036965
-0.053 0.051065 0.029209
-0.054145 0.054568 0.012257
-0.055848 0.05417 0.0083272
-0.054844 0.049295 0.011462
-0.05615 0.050619 0.0092929
-0.061451 0.068257 0.035376
-0.069725 0.069958 0.032788
-0.062823 0.063322 0.026886
-0.071037 0.066787 0.025228
-0.060857 0.060568 0.022643
-0.067 0.061558 0.020109
-0.0782 0.071279 0.021032
-0.062116 0.045145 0.037802
-0.065473 0.039513 0.037964
-0.06725 0.03742 0.033413
-0.072702 0.065008 0.018701
-0.06145 0.059165 0.018731
-0.0675 0.061479 0.019221
-0.057411 0.054114 0.0038257
-0.079222 0.070654 0.017735
-0.062473 0.04468 0.01111
-0.06725 0.042258 0.010414
-0.066389 0.040515 0.01316
-0.068359 0.038502 0.011958
-0.061381 0.04748 0.007607
-0.068559 0.043549 0.0081576
-0.070929 0.03983 0.0085888
-0.016625 0.18375 -0.019735
-0.015198 0.17471 -0.018868
-0.015944 0.16264 -0.0091037
-0.015977 0.1607 -0.0088072
-0.013251 0.16708 -0.015264
-0.014292 0.16098 -0.011252
-0.013986 0.184 -0.023739
-0.011633 0.17699 -0.023349
-0.0091029 0.16988 -0.021457
-0.025562 0.18273 -0.0096247
-0.02725 0.18254 -0.0094384
-0.025736 0.17948 -0.0089653
-0.031216 0.17589 -0.0051154
-0.020399 0.1845 -0.014943
-0.021339 0.17645 -0.014566
-0.027125 0.17234 -0.010156
-0.03939 0.1733 -0.0023575
-0.022876 0.16406 -0.0078103
-0.031597 0.16651 -0.0049292
-0.0226 0.15912 -0.003799
-0.030372 0.15767 -0.0012672
-0.021158 0.16849 -0.012383
-0.027 0.1712 -0.01022
-0.041719 0.16813 -0.00074958
-0.04825 0.16748 -0.00015191
-0.03725 0.16147 -7.2628e-05
-0.066429 0.15783 -0.0085673
-0.071284 0.15839 -0.005998
-0.065979 0.16288 -0.017792
-0.071623 0.16384 -0.01576
-0.066068 0.16051 -0.013567
-0.073307 0.16049 -0.011832
-0.077 0.16204 -0.019241
-0.077179 0.15851 -0.01495
-0.073691 0.17286 -0.037944
-0.07755 0.17221 -0.039175
-0.065921 0.16586 -0.025022
-0.072095 0.16784 -0.024725
-0.066 0.16808 -0.030916
-0.073448 0.17051 -0.032045
-0.07777 0.16434 -0.025938
-0.077893 0.16039 -0.021299
-0.078211 0.169 -0.034566
-0.034667 0.15131 -0.00071029
-0.066117 0.17353 -0.047453
-0.071986 0.17612 -0.045384
-0.06925 0.182 -0.055026
-0.064992 0.17802 -0.054645
-0.069935 0.17983 -0.051988
-0.07793 0.17516 -0.0444

参考：

在win10系统和VS2013环境配置PCL1.7.2点云库及常见问题解答（附带各种版本安装包下载）_baolinq的博客-CSDN博客

VS 2013+Pcl 1.7.2 安装配置及常见问题_nameix的博客-CSDN博客

ICP迭代最近点优化

利用SVD的方法求解ICP（详细推导）

Alan Lan

关注

14
点赞
踩
138

收藏

觉得还不错? 一键收藏
10
评论
迭代最近点 ICP 详细推导（C++实现）

一、简介迭代最近点算法（Iterated Closest Points, ICP），顾名思义，就是采用迭代优化的思想以空间距离作为匹配点的选择依据，通过不断调整点云的位姿使得匹配点之间距离累计最小。假设有两组点云，其中一个目标点云A另一个为参考点云B，ICP算法的目的是为了算出一个最优的旋转矩阵R和平移向量t使得变换后的点云A能与B达到最精确的匹配。ICP算法因为其思想简单，精度高等特点成为了局部配准的主流算法。 ICP需要反复执行以下两步直至收敛：第一步，计算A点集与...
复制链接

扫一扫