ICP(Iterative Closest Point)算法和Umeyama算法

ICP算法介绍

ICP算法是基于EM(Expectation-maximization algorithm)思想的方法，采用交替迭代法优化得到最优值。即ICP分为两步迭代优化，优化点云匹配及优化运动估计。

点云匹配是将两帧点云数据在同一个坐标系下，一帧数据中的点找到另一帧数据最近的点，就作为一对匹配点。

运动估计就是根据得到得两帧点云得匹配情况，构建最小二乘方程，求解。假设已知两帧点云图之间的匹配关系，那么求解两帧点云之间的位姿就是求最小二乘方程的解，即以下方程的解$R$与$t$。

其中，$R$是3*3矩阵，$t$是3*1向量。$N_p$表示点数。

求解得到运动估计，ICP采用Umeyama算法通过SVD分解求的运动估计得最优值。

除了使用逐步迭代法(最速/牛顿/LM等)求解，Umeyama算法单次则可实现求解运动估计最优解。在时间占用上比使用四元数计算要大，但是比迭代法小很多，但是在精度上提高了很多。列出原文表格：

Umeyama算法运动估计算法介绍

已知点云的匹配关系后，可以根据匹配点的两帧位置的不同，估算出机器人的运动。

假设如果我们已经知道两帧点云图之间的匹配关系，那么求解两帧点云之间的位姿就是构建最小二乘目标函数，求解最优解R与t。这个目标函数实际上就是所有对应点之间的欧式距离的平方和。

其中，$R$是3*3矩阵，$t$是3*1向量。$N_p$表示点数。
R为旋转矩阵，满足单位正交阵的性质，即：
$R^{T}R=R{R}^T=I,det(R)=1$

求解原理：利用SVD先求解旋转矩阵$R$，再求解偏移$t$向量

给定两个点云集合:
$$p^t=\left\{p_1^t,p_2^t,\cdots p_n^t\right\}$$

p s = { p 1 s , p 2 s , ⋯ p n s } p^s=\left\{p_1^s,p_2^s,\cdots p_n^s\right\} ps={p1s​,p2s​,⋯pns​}

求解$R$和$t$，使得目标方程(*)最小。

先求$R$，再求$t$:
$$p^t=R{p}^s-t$$

(1):
$$p_i^t=R{p}_i^s-t$$

令：$c^t,c^s$为两帧点云中心:
$$c^t=\frac{1}{N_p}\sum _{i=1}^{N_p}{p}_i^t$$

$$c^s=\frac{1}{N_p}\sum _{i=1}^{N_p}{p}_i^s$$
则：(2):
$$c^t=R{c}^s-t$$

令：
$$u^t=\left\{p_i^t-c^t\right\}$$

u s = { p i s − c s } u^s=\left\{p_i^s-c^s\right\} us={pis​−cs}

则令（2）减去（1）可得：
$$u^t=R{u}^s$$

即可通过最小化以下目标方程: 得$R$。

其中$R$为旋转阵，满足$R^{T}R=I$，则前两项与$R$无关，问题转化为：

上式利用矩阵迹的性质：
$$tr(x{y}^T)=y^{T}x$$
$$tr(ABCD)=tr(BCDA)=tr(CDAB)=tr(DABC）$$
$$tr(AB)=tr(BA)$$
令：

则问题化为：
$$R^{\ast }=argmaxtr(RH)$$

由定理：
$$tr\left(A{A}^T\right)\ge tr\left(BA{A}^T\right)，B{B}^T=I$$

可知，当$R$阵$RH$使可以化为$A{A}^T$时，$tr(RH)$最大。
将$H$进行SVD分解：
$$H=U\Sigma V^T$$
$\Sigma$为对角阵，由SVD的定义，$\Sigma$的奇异值(对角线元素均非负)，$U$及$V$为正交阵。
令：
$$R^{\ast }H=R^{\ast }U\Sigma V^T=A{A}^T$$

$$R^{\ast }=V{U}^T，A=V{\Sigma }^{\frac{1}{2}}$$
此时，R满足正交阵的性质，但是不能确保$det\left(R\right)=det(V{U}^T)=1$的性质。
所以令：
$$R^{\ast }=V{SU}^T，A=VS{\Sigma }^{\frac{1}{2}}$$
$$S=\{\begin{array}{cc}I& det(V)det(U)=1\\ diag(1,1,-1)& det(V)det(U)=-1\end{array}$$
此时得R，且达到最优值。具体证明过程见

ICP算法流程

不带尺度求解

-------------------------------------------------------------------------------------
Q: 给定两个点云集合:
$$p^t=\left\{p_1^t,p_2^t,\cdots p_n^t\right\}$$ p s = { p 1 s , p 2 s , ⋯ p n s } p^s=\left\{p_1^s,p_2^s,\cdots p_n^s\right\} ps={p1s​,p2s​,⋯pns​}
求解R和t，使得目标方程(*)最小。

S: ================================================

1. 令：$c^t,c^s$为两帧点云中心:
   $$c^t=\frac{1}{N_p}\sum _{i=1}^{N_p}{p}_i^t$$$$c^s=\frac{1}{N_p}\sum _{i=1}^{N_p}{p}_i^s$$
2. 令：
   $$u^t=\left\{p_i^t-u_t\right\}$$ u s = { p i s − u s } u^s=\left\{p_i^s-u_s\right\} us={pis​−us​}
3. 令：
   $$H=\sum _{i=1}^{N_p}{u_i}^s{u_i}^{tT}$$
4. 将H进行SVD分解：
   $$H=U\Sigma V^T$$
   其中，$\Sigma$为对角阵
5. 得：
   $$R^{\ast }=VS{U}^T,\mathbf{t}=c^t-R{c}^s$$
   其中：
   $$S=\{\begin{array}{cc}I& det(V)det(U)=1\\ diag(1,1,-1)& det(V)det(U)=-1\end{array}$$
   ----------------------------------------------------------------------------------------
   若令：
   $$H=\sum _{i=1}^{N_p}{u_i}^t{u_i}^{sT}=U\Sigma V^T$$
   则ICP的解为：
   $$R=US{V}^T,\mathbf{t}=c^t-R{c}^s$$
   ========================================================

两种求解思路

1、ICP求解推导-来自论文《Least-Squares Fitting of Two 3-D Point Sets》

2、UAV轨迹测评使用umeyama (ICP)求解流程：

二者对H矩阵的定义不同
1、ICP
$$H=\sum _{i=1}^{N_p}{u_i}^s{u_i}^{tT}$$$$R^{\ast }=V{U}^T$$
2、Umeyama:
$$H=\sum _{i=1}^{N_p}{u_i}^t{u_i}^{sT}$$$$R^{\ast }=U{V}^T$$
二者计算等价
Umeyama相当于计算：
$$||R{u_i}^s-{u_i}^t|{|}^2={u_i}^{sT}{R}^{T}R{u_i}^s+{u_i}^{tT}{u_i}^t-2{u_i}^{sT}{R}^T{u_i}^t$$$$\sum _{i=1}^{N_p}{u_i}^{sT}{R}^T{u_i}^t=\sum _{i=1}^{N_p}tr\left(R^T{u_i}^t{u_i}^{sT}\right)=tr(R^T\sum _{i=1}^{N_p}{u_i}^t{u_i}^{sT})=tr(R^{T}H)$$$$H=\sum _{i=1}^{N_p}{u_i}^t{u_i}^{sT}=U\Sigma V^T$$$$R^T=VS{U}^T⟹R^{\ast }=US{V}^T$$

---------------------------------------------------------------
简要介绍一下点集到点集ICP配准的算法流程：

1)ICP算法核心是最小化一个目标函数：

就是一对对应点，总共有Np对对应点。这个目标函数实际上就是所有对应点之间的欧式距离的平方和。

2)寻找对应点。可是，我们现在并不知道有哪些对应点。因此，我们在有初值的情况下，假设用初始的旋转平移矩阵对source cloud进行变换，得到的一个变换后的点云。然后将这个变换后的点云与target
cloud进行比较，只要两个点云中存在距离小于一定阈值（ICP中的一个参数），我们就认为这两个点就是对应点。这也是"最邻近点"这个说法的来源。

3)R、T优化。有了对应点之后，我们就可以用对应点对旋转R与平移T进行估计。这里R和T中只有6个自由度，而我们的对应点数量是庞大的（存在多余观测值）。因此，我们可以采用最小二乘等方法求解最优的旋转平移矩阵。一个数值优化问题，这里就不详细讲了。

4)迭代。我们优化得到了一个新的R与T，导致了一些点转换后的位置发生变化，一些最邻近点对也相应的发生了变化。因此，我们又回到了步骤2)中的寻找最邻近点方法。2)3)步骤不停迭代进行，直到满足一些迭代终止条件，如R、T的变化量小于一定值，或者上述目标函数的变化小于一定值，或者邻近点对不再变化等。（ICP算法中的一个参数）

算法大致流程就是上面这样。这里的优化过程是一个贪心的策略。首先固定R跟T利用最邻近算法找到最优的点对，然后固定最优的点对来优化R和T，依次反复迭代进行。这两个步骤都使得目标函数值下降，所以ICP算法总是收敛的，这也就是原文中收敛性的证明过程。这种优化思想与K均值聚类的优化思想非常相似，固定类中心优化每个点的类别，固定每个点的类别优化类中心。

测试代码

1.随机生成三维点，求两帧之间的$(R,t)$

代码文件：ICPtest.cpp
其中加了一些旋转矩阵到欧拉角的转换测试。

#include <iostream>
#include <random>
#include <map>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <pcl/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>

using namespace Eigen;
using namespace std;

map<int, pcl::PointXYZ> mapPoints;
map<int, pcl::PointXYZ> localMapPoints;

//pt = NR*ps + Nt
void ICPRefine(Matrix3d& NR, Vector3d& Nt)
{
  //used to refine vo
  pcl::PointCloud<pcl::PointXYZ>::Ptr ps(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr pt(new pcl::PointCloud<pcl::PointXYZ>());

  map<int, pcl::PointXYZ>::iterator it;
  pcl::PointXYZ psAve, ptAve;
  for (it = localMapPoints.begin(); it != localMapPoints.end(); ++it) {
    if (mapPoints.count(it->first)) {
      pt->push_back(it->second);
      ptAve.x += it->second.x;
      ptAve.y += it->second.y;
      ptAve.z += it->second.z;
      ps->push_back(mapPoints[it->first]);
      psAve.x += mapPoints[it->first].x;
      psAve.y += mapPoints[it->first].y;
      psAve.z += mapPoints[it->first].z;
    }
  }

  int rNum = pt->points.size();

  if (rNum < 10) {
    return;
  }

  psAve.x /= rNum;
  psAve.y /= rNum;
  psAve.z /= rNum;
  ptAve.x /= rNum;
  ptAve.y /= rNum;
  ptAve.z /= rNum;

  Matrix3d W = Matrix3d::Zero();
  for (int i=0; i<rNum; i++) {
    ps->points[i].x -= psAve.x;
    ps->points[i].y -= psAve.y;
    ps->points[i].z -= psAve.z;

    pt->points[i].x -= ptAve.x;
    pt->points[i].y -= ptAve.y;
    pt->points[i].z -= ptAve.z;

    W = W + Vector3d(pt->points[i].x, pt->points[i].y, pt->points[i].z)
          * Vector3d(ps->points[i].x, ps->points[i].y, ps->points[i].z).transpose();
  }

  //cout << "W: " << endl;
  //cout << W << end

  // SVD on W
  JacobiSVD<MatrixXd> svd (W, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();

  if (U.determinant() * V.determinant() < 0)
  {   
      for (int x = 0; x < 3; ++x)
      {
          U(x, 2) *= -1;
      }
  }

  NR = U * (V.transpose());
  Nt = Vector3d(ptAve.x, ptAve.y, ptAve.z)
     - NR * Vector3d(psAve.x, psAve.y, psAve.z);
}

//pw = R^T*pc + t
void pointGenerate(double rx, double ry, double rz, double tx, double ty, double tz)
{
  double srx = sin(rx);
  double crx = cos(rx);
  double sry = sin(ry);
  double cry = cos(ry);
  double srz = sin(rz);
  double crz = cos(rz);

  for (int i = 0; i < 20; i++) {
    static default_random_engine generator(time(NULL));
    static uniform_real_distribution<double> xy_rand(-4, 4.0);
    static uniform_real_distribution<double> z_rand(8., 10.);
    //static normal_distribution<double> n(0.0, 1);
    
    pcl::PointXYZ point;
    point.x = xy_rand(generator);
    point.y = xy_rand(generator);
    point.z = z_rand(generator);
    localMapPoints[i] = point;
 
    double x1 = crz * point.x - srz * point.y;
    double y1 = srz * point.x + crz * point.y;
    double z1 = point.z;

    double x2 = x1;
    double y2 = crx * y1 - srx * z1;
    double z2 = srx * y1 + crx * z1;

    pcl::PointXYZ pointW;
    pointW.x = cry * x2 + sry * z2 + tx;
    pointW.y = y2 + ty;
    pointW.z = -sry * x2 + cry * z2 + tz;
    //pointW.x = cry * x2 + sry * z2 + tx + n(generator);
    //pointW.y = y2 + ty + n(generator);
    //pointW.z = -sry * x2 + cry * z2 + tz + n(generator);
    mapPoints[i] = pointW;
  }
}

int main()
{
  //Set the camera pose (rx,ry,rz,tx,ty,tz)
  double rx=1.0; double ry=2.0; double rz=1.0;
  double tx=-3; double ty=1; double tz=4;

  cout << "truth: " << rx << " " << ry << " " << rz << " " << tx << " " << ty << " " << tz << endl;

  double srx = sin(rx);
  double crx = cos(rx);
  double sry = sin(ry);
  double cry = cos(ry);
  double srz = sin(rz);
  double crz = cos(rz);

  Matrix3d R;
  R << (crz*cry + srz*srx*sry), srz*crx, (-crz*sry + srz*srx*cry),
       (-srz*cry + crz*srx*sry), crz*crx, (srz*sry + crz*srx*cry),
       crx*sry, -srx, crx*cry;

  cout << "truth rotation:" << endl << R << endl;

  //p_w = R^T * p_c + t
  //R << (left, up, forward)
  //     (crz*cry + srz*srx*sry), srz*crx, (-crz*sry + srz*srx*cry);
  //     (-srz*cry + crz*srx*sry), crz*crx, (srz*sry + crz*srx*cry);
  //     crx*sry, -srx, crx*cry;
  pointGenerate(rx, ry, rz, tx, ty, tz);

  //p_c = NR * p_w + Nt
  Matrix3d NR; Vector3d Nt;
  ICPRefine(NR, Nt);

  cout << "rotation estimate:" << endl << NR << endl;

  //p_w = NR^T * p_c - NR^T * Nt
  //R_est = NR; t_est = - NR^T * Nt;
  Vector3d Nr = NR.eulerAngles(2,0,1);

  //Eigen roll pitch yaw in different frame from (left, up, forward), that the reason for negative
  double Nrx, Nry, Nrz, Ntx, Nty, Ntz;
  Nrx = -Nr[1];
  Nry = -Nr[2];
  Nrz = -Nr[0];
  Ntx = -Nt[0];
  Nty = -Nt[1];
  Ntz = -Nt[2];

  srx = sin(Nrx);
  crx = cos(Nrx);
  sry = sin(Nry);
  cry = cos(Nry);
  srz = sin(Nrz);
  crz = cos(Nrz);

  // In ICPRefine Nt trans is in local camera frame
  // But t trans is in world frame
  double x1 = crz * (Ntx) - srz * (Nty);
  double y1 = srz * (Ntx) + crz * (Nty);
  double z1 = Ntz;

  double x2 = x1;
  double y2 = crx * y1 - srx * z1;
  double z2 = srx * y1 + crx * z1;

  Ntx = cry * x2 + sry * z2;
  Nty = y2;
  Ntz = -sry * x2 + cry * z2;

  cout << "pose estimate: " << Nrx << " " << Nry << " " << Nrz << " " << Ntx << " " << Nty << " " << Ntz << endl;
}

编译命令：

g++ ICPtest.cpp `pkg-config eigen3 --libs --cflags`  `pkg-config --cflags pcl_ros` -o test

运行：

./test

运行结果：

2.随机生成3维点，检验求解结果(R阵是否为单位阵)

代码文件：svd_test.cpp

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <random>

using namespace Eigen;
using namespace std;

int main ()
{
  Matrix3d A = Matrix3d::Zero();
  for (int i=0; i < 10; i++) {
    static default_random_engine generator(time(NULL));
    static uniform_real_distribution<double> x_rand(-4, 4.0);
    static uniform_real_distribution<double> y_rand(-2, 2.0);
    static uniform_real_distribution<double> z_rand(8., 10.);
    double x = x_rand(generator);
    double y = y_rand(generator);
    double z = z_rand(generator);
 
    Vector3d p(x,y,z);
    A += p * p.transpose();
  }
  //Matrix3d A;
  //A << 1, 2, 3,
  //     2, 4, 6,
  //     3, 6, 9;

  //static uniform_real_distribution<double> r(-4, 4.0);
  //Matrix3d A;
  //A << r(generator), r(generator), r(generator),
  //     r(generator), r(generator), r(generator),
  //     r(generator), r(generator), r(generator);


  cout << "A: " << endl;
  cout << A << endl;

  JacobiSVD<MatrixXd> svd (A, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();

  cout << "U: " << endl;
  cout << U << endl;
  cout << "V: " << endl;
  cout << V << endl;
  cout << "singularValues:" << endl << svd.singularValues() << endl;

  if ((U * (V.transpose())).determinant() < 0)
  {   
      for (int x = 0; x < 3; ++x)
      {
          U(x, 2) *= -1;
      }
  }
 
  Matrix3d dR;
  dR = U * (V.transpose());
  cout << "dR: " << endl << dR << endl;

  //cout << "dR.transpose()*dR: " << endl << dR.transpose()*dR << endl;

  cout << "****************The End*****************" << endl;
}

编译命令

g++ svd_test.cpp `pkg-config eigen3 --libs --cflags` -o test

运行：

./test

运行结果：

带尺度求解

使用ICP求解带尺度的3D-3D关系：
约束方程变为：（*s）
$$E\left(R,t\right)=\frac{1}{N_p}\sum _i||p_i^t-sR{p}_i^s-t|{|}^2$$
同理去中心可得：
$$u^t=\left\{p_i^t-u_t\right\}$$ u s = { p i s − u s } u^s=\left\{p_i^s-u_s\right\} us={pis​−us​}
有：
$$u^t=R{u}^s$$
即可通过最小化以下目标方程得R。
$$R^{\ast }=argmin\frac{1}{N_p}\sum _i||{u_i}^t-sR{u_i}^s|{|}^2$$$$||{u_i}^t-R{u_i}^s|{|}^2={u_i}^{tT}{u_i}^t+s^2{u_i}^{sT}{R}^{T}R{u_i}^s-2s{u_i}^{tT}R{u_i}^s$$$$={u_i}^{tT}{u_i}^t+s^2{u_i}^{sT}{u_i}^s-2s{u_i}^{tT}R{u_i}^s$$
去掉与s及R无关项后，原目标方程变为：(3)
$$R^{\ast }=argmin(s^2{u_i}^{sT}{u_i}^s-2s{u_i}^{tT}R{u_i}^s)$$
未知项为两项，s为变常量，将上式对s求导，得极值时：（4）
$$s=\frac{{\sum }_{i=1}^{N_p}{u_i}^{tT}R{u_i}^s}{{\sum }_{i=1}^{N_p}{u_i}^{sT}{u_i}^s}=\frac{P}{M}$$
令$P=\frac{1}{N_p}{\sum }_{i=1}^{N_p}{u_i}^{tT}R{u_i}^s$，$M=\frac{1}{N_p}{\sum }_{i=1}^{N_p}{u_i}^{sT}{u_i}^s$
将上式（4）代入化简后的目标方程(3)中，得：
$$R^{\ast }=argmax(\frac{P^2}{M})$$
M正定，回归无尺度求解问题：
$$R^{\ast }=argmax\left(P\right)=argmax\frac{1}{N_p}\sum _{i=1}^{N_p}{u_i}^{tT}R{u_i}^s$$
同理两种求解方式：
1）ICP
令：
$$H=\sum _{i=1}^{N_p}{u_i}^s{u_i}^{tT}=U\Sigma V^T$$
$$R^{\ast }=VS{U}^T$$
$$s=\frac{{\sum }_{i=1}^{N_p}{u_i}^{tT}R{u_i}^s}{{\sum }_{i=1}^{N_p}{u_i}^{sT}{u_i}^s}=\frac{tr(RH)}{M}=\frac{tr(\mathrm{\Sigma S})}{M}$$

2. Umeyama:
   令：
   $$H=\sum _{i=1}^{N_p}{u_i}^t{u_i}^{sT}=U\Sigma V^T$$
   $$R^{\ast }=U{SV}^T$$
   $$s=\frac{{\sum }_{i=1}^{N_p}{u_i}^t{u_i}^{sT}}{{\sum }_{i=1}^{N_p}{u_i}^{sT}{u_i}^s}=\frac{tr(R^{T}H)}{M}=\frac{tr(\mathrm{\Sigma S})}{M}$$

总结，带尺度求解运动(s,R,t)流程

Q: 给定两个点云集合:
$$p^t=\left\{p_1^t,p_2^t,\cdots p_n^t\right\}$$ p s = { p 1 s , p 2 s , ⋯ p n s } p^s=\left\{p_1^s,p_2^s,\cdots p_n^s\right\} ps={p1s​,p2s​,⋯pns​}
求解R和t，使得目标方程(*s)最小。
$$E\left(R,t\right)=\frac{1}{N_p}\sum _i||p_i^t-sR{p}_i^s-t|{|}^2$$
S:

1. 令：$c^t,c^s$为两帧点云中心: $$c^t=\frac{1}{N_p}\sum_{i=1}^{N_p}p_i^t$$$$c^s=\frac{1}{N_p}\sum_{i=1}^{N_p}p_i^s$$ 2. 令： $$u^t=\left\{p_i^t-u_t\right\}$$$$u^s=\left\{p_i^s-u_s\right\}$$ 3. 令： $$H=\sum_{i=1}^{N_p}{{u_i}^s{u_i}^{tT}}$$ 4. 将H进行SVD分解： $$H=U\Sigma V^T$$ 其中，$\Sigma$为对角阵 5. 得： $$R^\ast=VSU^T, \bm{t}=c^t-Rc^s$$ 其中： $$S=\left\{\begin{matrix}I&det(V)det(U)=1\\diag(1,1,-1)&det(V)det(U)=-1\\\end{matrix}\right.$$

若令：
$$H=\sum _{i=1}^{N_p}{u_i}^t{u_i}^{sT}=U\Sigma V^T$$
则ICP的解为：
$$R=US{V}^T$$
$$s=\frac{tr(RH)}{{\sum }_{i=1}^{N_p}{u_i}^{sT}{u_i}^s}=\frac{tr(\mathrm{\Sigma S})}{{\sum }_{i=1}^{N_p}{u_i}^{sT}{u_i}^s}$$
$$\mathbf{t}=c^t-sR{c}^s$$

测试代码

以下代码生成点云和解算使用的Rt不一致，为测试其他完整所用，建议修改一致

#include <iostream>
#include <random>
#include <map>
#include <math.h>
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <pcl/point_cloud.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/pcl_base.h>

using namespace Eigen;
using namespace std;

map<int, pcl::PointXYZ> mapPoints;
map<int, pcl::PointXYZ> localMapPoints;

/****pt = s*R*ps + t
pw->ps  pc->pt -> pc = s*R*pw + t => pw = （1/s)*R^T*pc -(1/s)R^T*t (1)
When generte point:
pw = s_g*R_g^T*pc + t_g (2)

here we want the func of ICPRefine output R_g & t_g
combian (1) & (2) we get:
s_g = 1/s
R_g = R
t_g = -(1/s)R^T*t*****/
void ICPRefine(double &Nrx, double &Nry, double &Nrz,
               double &Ntx, double &Nty, double &Ntz, double &s)
{
  Matrix3d NR; Vector3d Nt; double scale;
  //used to refine vo
  pcl::PointCloud<pcl::PointXYZ>::Ptr ps(new pcl::PointCloud<pcl::PointXYZ>());
  pcl::PointCloud<pcl::PointXYZ>::Ptr pt(new pcl::PointCloud<pcl::PointXYZ>());

  map<int, pcl::PointXYZ>::iterator it;
  pcl::PointXYZ psAve, ptAve;
  for (it = localMapPoints.begin(); it != localMapPoints.end(); ++it) {
    if (mapPoints.count(it->first)) {
      pt->push_back(it->second);
      ptAve.x += it->second.x;
      ptAve.y += it->second.y;
      ptAve.z += it->second.z;
      ps->push_back(mapPoints[it->first]);
      psAve.x += mapPoints[it->first].x;
      psAve.y += mapPoints[it->first].y;
      psAve.z += mapPoints[it->first].z;
    }
  }

  int rNum = pt->points.size();
  int sNum = ps->points.size();

  if (rNum < 10) {
    return;
  }

  psAve.x /= rNum;
  psAve.y /= rNum;
  psAve.z /= rNum;
  ptAve.x /= rNum;
  ptAve.y /= rNum;
  ptAve.z /= rNum;

  Matrix3d W = Matrix3d::Zero();
  double M = 0;
  for (int i=0; i<rNum; i++) {
    ps->points[i].x -= psAve.x;
    ps->points[i].y -= psAve.y;
    ps->points[i].z -= psAve.z;

    pt->points[i].x -= ptAve.x;
    pt->points[i].y -= ptAve.y;
    pt->points[i].z -= ptAve.z;

    W = W + Vector3d(pt->points[i].x, pt->points[i].y, pt->points[i].z)
          * Vector3d(ps->points[i].x, ps->points[i].y, ps->points[i].z).transpose();

    M += Vector3d(ps->points[i].x, ps->points[i].y, ps->points[i].z).transpose()
          * Vector3d(ps->points[i].x, ps->points[i].y, ps->points[i].z);
  }

  //cout << "W: " << endl;
  //cout << W << end

  // SVD on W
  JacobiSVD<MatrixXd> svd (W, ComputeThinU|ComputeThinV);
  Matrix3d U = svd.matrixU();
  Matrix3d V = svd.matrixV();

  if (U.determinant() * V.determinant() < 0)
  {   
      for (int x = 0; x < 3; ++x)
      {
          U(x, 2) *= -1;
      }
  }

  NR = U * (V.transpose());
  s = (NR.transpose() * W).trace() / M;
  Nt = (Vector3d(ptAve.x, ptAve.y, ptAve.z)
     - s * NR * Vector3d(psAve.x, psAve.y, psAve.z))/s;
  s = 1 / s;

  cout << "rotation estimate:" << endl << NR << endl;

  //p_w = NR^T * p_c - NR^T * Nt
  //R_est = NR; t_est = - NR^T * Nt;
  Vector3d Nr = NR.eulerAngles(2,0,1);

  //Eigen roll pitch yaw in different frame from (left, up, forward), that the reason for negative
  Nrx = -Nr[1];
  Nry = -Nr[2];
  Nrz = -Nr[0];
  Ntx = -Nt[0];
  Nty = -Nt[1];
  Ntz = -Nt[2];

  double srx = sin(Nrx);
  double crx = cos(Nrx);
  double sry = sin(Nry);
  double cry = cos(Nry);
  double srz = sin(Nrz);
  double crz = cos(Nrz);

  // In ICPRefine Nt trans is in local camera frame
  // But t trans is in world frame
  double x1 = crz * (Ntx) - srz * (Nty);
  double y1 = srz * (Ntx) + crz * (Nty);
  double z1 = Ntz;

  double x2 = x1;
  double y2 = crx * y1 - srx * z1;
  double z2 = srx * y1 + crx * z1;

  Ntx = cry * x2 + sry * z2;
  Nty = y2;
  Ntz = -sry * x2 + cry * z2;
}

//pw = sR^T*pc + t
void pointGenerate(double rx, double ry, double rz, double tx, double ty, double tz,
                   double scale)
{
  double srx = sin(rx);
  double crx = cos(rx);
  double sry = sin(ry);
  double cry = cos(ry);
  double srz = sin(rz);
  double crz = cos(rz);

  for (int i = 0; i < 20; i++) {
    static default_random_engine generator(time(NULL));
    static uniform_real_distribution<double> xy_rand(-4, 4.0);
    static uniform_real_distribution<double> z_rand(8., 10.);
    static normal_distribution<double> n(0.0, 0.51);

    pcl::PointXYZ point;
    point.x = xy_rand(generator);
    point.y = xy_rand(generator);
    point.z = z_rand(generator);
    localMapPoints[i] = point;
 
    double x1 = crz * point.x - srz * point.y;
    double y1 = srz * point.x + crz * point.y;
    double z1 = point.z;

    double x2 = x1;
    double y2 = crx * y1 - srx * z1;
    double z2 = srx * y1 + crx * z1;

    double x3 = cry * x2 + sry * z2;
    double y3 = y2;
    double z3 = -sry * x2 + cry * z2;

    x3 *= scale; 
    y3 *= scale; 
    z3 *= scale; 

    pcl::PointXYZ pointW;
    //pointW.x = x3 + tx + n(generator);
    //pointW.y = y3 + ty + n(generator);
    //pointW.z = z3 + tz + n(generator);
    pointW.x = x3 + tx;
    pointW.y = y3 + ty;
    pointW.z = z3 + tz;

    mapPoints[i] = pointW;
  }
}


//pw = R^T*pc + t
void pointGenerate(double rx, double ry, double rz, double tx, double ty, double tz)
{
  double srx = sin(rx);
  double crx = cos(rx);
  double sry = sin(ry);
  double cry = cos(ry);
  double srz = sin(rz);
  double crz = cos(rz);

  for (int i = 0; i < 20; i++) {
    static default_random_engine generator(time(NULL));
    static uniform_real_distribution<double> xy_rand(-4, 4.0);
    static uniform_real_distribution<double> z_rand(8., 10.);
    static normal_distribution<double> n(0.0, 0.51);

    pcl::PointXYZ point;
    point.x = xy_rand(generator);
    point.y = xy_rand(generator);
    point.z = z_rand(generator);
    localMapPoints[i] = point;
 
    double x1 = crz * point.x - srz * point.y;
    double y1 = srz * point.x + crz * point.y;
    double z1 = point.z;

    double x2 = x1;
    double y2 = crx * y1 - srx * z1;
    double z2 = srx * y1 + crx * z1;

    pcl::PointXYZ pointW;
    //pointW.x = cry * x2 + sry * z2 + tx + n(generator);
    //pointW.y = y2 + ty + n(generator);
    //pointW.z = -sry * x2 + cry * z2 + tz + n(generator);
    pointW.x = cry * x2 + sry * z2 + tx;
    pointW.y = y2 + ty;
    pointW.z = -sry * x2 + cry * z2 + tz;

    mapPoints[i] = pointW;
  }
}

int main()
{
  //Set the camera pose (rx,ry,rz,tx,ty,tz)
  double rx=1.0; double ry=2.0; double rz=1.0;
  double tx=-3; double ty=1; double tz=4;

  cout << "truth: " << rx << " " << ry << " " << rz << " " << tx << " " << ty << " " << tz << endl;

  double srx = sin(rx);
  double crx = cos(rx);
  double sry = sin(ry);
  double cry = cos(ry);
  double srz = sin(rz);
  double crz = cos(rz);

  Matrix3d R;
  R << (crz*cry + srz*srx*sry), srz*crx, (-crz*sry + srz*srx*cry),
       (-srz*cry + crz*srx*sry), crz*crx, (srz*sry + crz*srx*cry),
       crx*sry, -srx, crx*cry;

  cout << "truth rotation:" << endl << R << endl;

  //p_w = R^T * p_c + t
  //R << (left, up, forward)
  //     (crz*cry + srz*srx*sry), srz*crx, (-crz*sry + srz*srx*cry);
  //     (-srz*cry + crz*srx*sry), crz*crx, (srz*sry + crz*srx*cry);
  //     crx*sry, -srx, crx*cry;
  pointGenerate(rx, ry, rz, tx, ty, tz);

  //p_c = NR * p_w + Nt
  double Nrx, Nry, Nrz, Ntx, Nty, Ntz, scale;
  ICPRefine(Nrx, Nry, Nrz, Ntx, Nty, Ntz, scale);

  cout << "scale: " << scale << endl;
  cout << "pose estimate: " << Nrx << " " << Nry << " " << Nrz << " " << Ntx << " " << Nty << " " << Ntz << endl;

  mapPoints.clear();
  localMapPoints.clear();

  double scale_set = 2;
  cout << "***Test scale evaluation***" << "scale set to: " << scale_set << endl;
  pointGenerate(rx, ry, rz, tx, ty, tz, scale_set);

  //p_c = NR * p_w + Nt
  ICPRefine(Nrx, Nry, Nrz, Ntx, Nty, Ntz, scale);

  cout << "scale: " << scale << endl;
  cout << "pose estimate: " << Nrx << " " << Nry << " " << Nrz << " " << Ntx << " " << Nty << " " << Ntz << endl;

}

//g++ ICPtest.cpp `pkg-config eigen3 --libs --cflags`  `pkg-config --cflags pcl_ros`

结果：

truth: 1 2 1 -3 1 4
truth rotation:
0.419004 0.454649 -0.785958
0.763586 0.291927 0.575947
0.491295 -0.841471 -0.224845
rotation estimate:
0.419004 0.454649 -0.785958
0.763586 0.291927 0.575947
0.491295 -0.841471 -0.224845
scale: 1
pose estimate: 2.14159 -1.14159 -2.14159 -3 1 4
Test scale evaluationscale set to: 2
rotation estimate:
0.419004 0.454649 -0.785958
0.763586 0.291927 0.575947
0.491295 -0.841471 -0.224845
scale: 2
pose estimate: 2.14159 -1.14159 -2.14159 -3 1 4

参考文献

[1] Least Squares Fitting of Two 3-D Point Sets.
[2] https://www.jianshu.com/p/dd4b49650d76
[3] https://blog.csdn.net/qq_27251141/article/details/88735285
[4] https://blog.csdn.net/zhouyelihua/article/details/77807541