ICP(Iterative Closest Point)算法和Umeyama算法
文章目录
-
- ICP算法介绍
- Umeyama算法运动估计算法介绍
- ICP算法流程
-
- 不带尺度求解
-
- 两种求解思路
- 测试代码
-
- 1.随机生成三维点，求两帧之间的 ( R , t ) (R,t) (R,t)
- 2.随机生成3维点，检验求解结果(R阵是否为单位阵)
- 带尺度求解
-
- 测试代码
- 参考文献
ICP算法介绍
ICP算法是基于EM(Expectation-maximization algorithm)思想的方法，采用交替迭代法优化得到最优值。即ICP分为两步迭代优化，优化点云匹配及优化运动估计。
点云匹配是将两帧点云数据在同一个坐标系下，一帧数据中的点找到另一帧数据最近的点，就作为一对匹配点。
运动估计就是根据得到得两帧点云得匹配情况，构建最小二乘方程，求解。假设已知两帧点云图之间的匹配关系，那么求解两帧点云之间的位姿就是求最小二乘方程的解，即以下方程的解 R R R与 t t t。
其中， R R R是3*3矩阵， t t t是3*1向量。 N p N_p Np表示点数。
求解得到运动估计，ICP采用Umeyama算法通过SVD分解求的运动估计得最优值。
除了使用逐步迭代法(最速/牛顿/LM等)求解，Umeyama算法单次则可实现求解运动估计最优解。在时间占用上比使用四元数计算要大，但是比迭代法小很多，但是在精度上提高了很多。列出原文表格：
Umeyama算法运动估计算法介绍
已知点云的匹配关系后，可以根据匹配点的两帧位置的不同，估算出机器人的运动。
假设如果我们已经知道两帧点云图之间的匹配关系，那么求解两帧点云之间的位姿就是构建最小二乘目标函数，求解最优解R与t。这个目标函数实际上就是所有对应点之间的欧式距离的平方和。
其中， R R R是3*3矩阵， t t t是3*1向量。 N p N_p Np表示点数。
R为旋转矩阵，满足单位正交阵的性质，即：
R T R = R R T = I , d e t ( R ) = 1 R^TR = RR^T =I, det(R)=1 RTR=RRT=I,det(R)=1
求解原理：利用SVD先求解旋转矩阵 R R R，再求解偏移 t t t向量
给定两个点云集合:
p t = { p 1 t , p 2 t , ⋯ p n t } p^t=\left\{p_1^t,p_2^t,\cdots p_n^t\right\} pt={p1t,p2t,⋯pnt}
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 R R和 t t t，使得目标方程(*)最小。
先求 R R R，再求 t t t:
p t = R p s − t p^t=Rp^s-t pt=Rps−t
(1):
p i t = R p i s − t p_i^t=Rp_i^s-t pit=Rpis−t
令： c t , c s c^t, c^s ct,cs为两帧点云中心:
c t = 1 N p ∑ i = 1 N p p i t c^t=\frac{1}{N_p}\sum_{i=1}^{N_p}p_i^t ct=Np1i=1∑Nppit
c s = 1 N p ∑ i = 1 N p p i s c^s=\frac{1}{N_p}\sum_{i=1}^{N_p}p_i^s cs=Np1i=1∑Nppis
则：(2):
c t = R c s − t c^t=Rc^s-t ct=Rcs−t
令：
u t = { p i t − c t } u^t=\left\{p_i^t-c^t\right\} ut={pit−ct}
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 u^t=Ru^s ut=Rus
即可通过最小化以下目标方程: 得 R R R。
其中 R R R为旋转阵，满足 R T R = I R^TR=I RTR=I，则前两项与 R R R无关，问题转化为：
上式利用矩阵迹的性质：
t r ( x y T ) = y T x tr(xy^T)=y^Tx tr(xyT)=yTx
t r ( A B C D ) = t r ( B C D A ) = t r ( C D A B ) = t r ( D A B C ） tr(ABCD)=tr(BCDA)=tr(CDAB)=tr(DABC） tr(ABCD)=tr(BCDA)=tr(CDAB)=tr(DABC）
t r ( A B ) = t r ( B A ) tr(AB)=tr(BA) tr(AB)=tr(BA)
令：
则问题化为：
R ∗ = a r g m a x t r ( R H ) R^\ast=argmax\ tr(RH) R∗=argmax tr(RH)
由定理：
t r ( A A T ) ≥ t r ( B A A T ) ， B B T = I tr\left(AA^T\right)\geq\ tr\left(BAA^T\right)，BB^T=I tr(AAT)≥ tr(BAAT)，BBT=I
可知，当 R R R阵 R H RH RH使可以化为 A A T AA^T AAT时， t r ( R H ) tr(RH) tr(RH)最大。
将 H H H进行SVD分解：
H = U Σ V T H=U\Sigma V^T H=UΣVT
Σ \Sigma Σ为对角阵，由SVD的定义， Σ \Sigma Σ的奇异值(对角线元素均非负)， U U U及 V V V为正交阵。
令：
R ∗ H = R ∗ U Σ V T = A A T R^\ast H=R^\ast U\Sigma V^T=AA^T R∗H=R∗UΣVT=AAT
R ∗ = V U T ， A = V Σ 1 2 R^\ast=VU^T，A=VΣ^\frac12 R∗=VUT，A=VΣ21
此时，R满足正交阵的性质，但是不能确保 d e t ( R ) = d e t ( V U T ) = 1 det{\left(R\right)}=det(VU^T)=1 det(R)=det(VUT)=1的性质。
所以令：
R ∗ = V S U T ， A = V S Σ 1 2 R^\ast=V{SU}^T， A=VSΣ^\frac12 R∗=VSUT，A=VSΣ21
S = { I d e t ( V ) d e t ( U ) = 1 d i a g ( 1 , 1 , − 1 ) d e t ( V ) d e t ( U ) = − 1 S=\left\{\begin{matrix}I&det(V)det(U)=1\\diag(1,1,-1)&det(V)det(U)=-1\\\end{matrix}\right. S={Idiag(1,1,−1)det(V)det(U)=1det(V)det(U)=−1
此时得R，且达到最优值。具体证明过程见
ICP算法流程
不带尺度求解
-------------------------------------------------------------------------------------
Q: 给定两个点云集合:
p t = { p 1 t , p 2 t , ⋯ p n t } p^t=\left\{p_1^t,p_2^t,\cdots p_n^t\right\} pt={p1t,p2t,⋯pnt} 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: ================================================
- 令： c t , c s c^t,c^s ct,cs为两帧点云中心:
c t = 1 N p ∑ i = 1 N p p i t c^t=\frac{1}{N_p}\sum_{i=1}^{N_p}p_i^t ct=Np1i=1∑Nppit c s = 1 N p ∑ i = 1 N p p i s c^s=\frac{1}{N_p}\sum_{i=1}^{N_p}p_i^s cs=Np1i=1∑Nppis - 令：
u t = { p i t − u t } u^t=\left\{p_i^t-u_t\right\} ut={pit−ut} u s = { p i s − u s } u^s=\left\{p_i^s-u_s\right\} us={pis−us} - 令：
H = ∑ i = 1 N p u i s u i t T H=\sum_{i=1}^{N_p}{{u_i}^s{u_i}^{tT}} H=i=1∑NpuisuitT - 将H进行SVD分解：
H = U Σ V T H=U\Sigma V^T H=UΣVT
其中， Σ \Sigma Σ为对角阵 - 得：
R ∗ = V S U T , t = c t − R c s R^\ast=VSU^T, \bm{t}=c^t-Rc^s R∗=VSUT,t=ct−Rcs
其中：
S = { I d e t ( V ) d e t ( U ) = 1 d i a g ( 1 , 1 , − 1 ) d e t ( V ) d e t ( U ) = − 1 S=\left\{\begin{matrix}I&det(V)det(U)=1\\diag(1,1,-1)&det(V)det(U)=-1\\\end{matrix}\right. S={Idiag(1,1,−1)det(V)det(U)=1det(V)det(U)=−1
----------------------------------------------------------------------------------------
若令：
H = ∑ i = 1 N p u i t u i s T = U Σ V T H=\sum_{i=1}^{N_p}{{u_i}^t{u_i}^{sT}}=U\Sigma V^T H=i=1∑NpuituisT=UΣVT
则ICP的解为：
R = U S V T , t = c t − R c s R=USV^T, \bm{t}=c^t-Rc^s R=USVT,t=ct−Rcs
========================================================
两种求解思路
1、ICP求解推导-来自论文《Least-Squares Fitting of Two 3-D Point Sets》
2、UAV轨迹测评使用umeyama (ICP)求解流程：
二者对H矩阵的定义不同
1、ICP
H = ∑ i = 1 N p u i s u i t T H=\sum_{i=1}^{N_p}{{u_i}^s{u_i}^{tT}} H=i=1∑NpuisuitT R ∗ = V U T R^\ast=VU^T R∗=VUT
2、Umeyama:
H = ∑ i = 1 N p u i t u i s T H=\sum_{i=1}^{N_p}{{u_i}^t{u_i}^{sT}} H=i=1∑NpuituisT R ∗ = U V T R^\ast=UV^T R∗=UVT
二者计算等价
Umeyama相当于计算：
∣ ∣ R u i s − u i t ∣ ∣ 2 = u i s T R T R u i s + u i t T u i t − 2 u i s T R T u i t ||R{u_i}^s-{u_i}^t||^2={u_i}^{sT}R^TR{u_i}^s+{u_i}^{tT}{u_i}^t-2{u_i}^{sT}R^T{u_i}^t ∣∣Ruis−uit∣∣2=uisTRTRuis+uitTuit−2uisTRTuit ∑ i = 1 N p u i s T R T u i t = ∑ i = 1 N p t r ( R T u i t u i s T ) = t r ( R T ∑ i = 1 N p u i t u i s T ) = t r ( R T H ) \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^TH) i=1∑NpuisTRTuit=i=1∑Nptr(RTuituisT)=tr(RTi=1∑NpuituisT)=tr(RTH) H = ∑ i = 1 N p u i t u i s T = U Σ V T H=\sum_{i=1}^{N_p}{{u_i}^t{u_i}^{sT}}=U\Sigma V^T H=i=1∑NpuituisT=UΣVT R T = V S U T ⟹ R ∗ = U S V T R^T=\ VSU^T\Longrightarrow R^\ast=USV^T RT= VSUT⟹R∗=USVT
---------------------------------------------------------------
简要介绍一下点集到点集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 ) (R,t) (R,t)
代码文件：ICPtest.cpp
其中加了一些旋转矩阵到欧拉角的转换测试。
#include 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 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 ( R , t ) = 1 N p ∑ i ∣ ∣ p i t − s R p i s − t ∣ ∣ 2 E\left(R,t\right)=\frac{1}{N_p}\sum_i{||p_i^t-sRp_i^s-t||^2} E(R,t)=Np1i∑∣∣pit−sRpis−t∣∣2
同理去中心可得：
u t = { p i t − u t } u^t=\left\{p_i^t-u_t\right\} ut={pit−ut} u s = { p i s − u s } u^s=\left\{p_i^s-u_s\right\} us={pis−us}
有：
u t = R u s u^t=Ru^s ut=Rus
即可通过最小化以下目标方程得R。
R ∗ = a r g m i n 1 N p ∑ i ∣ ∣ u i t − s R u i s ∣ ∣ 2 R^\ast=argmin\frac{1}{N_p}\sum_i{||{u_i}^t-sR{u_i}^s||^2} R∗=argminNp1i∑∣∣uit−sRuis∣∣2 ∣ ∣ u i t − R u i s ∣ ∣ 2 = u i t T u i t + s 2 u i s T R T R u i s − 2 s u i t T R u i s ||{u_i}^t-R{u_i}^s||^2={u_i}^{tT}{u_i}^t+s^2{u_i}^{sT}R^TR{u_i}^s-2s{u_i}^{tT}R{u_i}^s ∣∣uit−Ruis∣∣2=uitTuit+s2uisTRTRuis−2suitTRuis = u i t T u i t + s 2 u i s T u i s − 2 s u i t T 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 =uitTuit+s2uisTuis−2suitTRuis
去掉与s及R无关项后，原目标方程变为：(3)
R ∗ = a r g m i n ( s 2 u i s T u i s − 2 s u i t T R u i s ) R^\ast=argmin(s^2{u_i}^{sT}{u_i}^s-2s{u_i}^{tT}R{u_i}^s) R∗=argmin(s2uisTuis−2suitTRuis)
未知项为两项，s为变常量，将上式对s求导，得极值时：（4）
s = ∑ i = 1 N p u i t T R u i s ∑ i = 1 N p u i s T u i s = P M 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} s=∑i=1NpuisTuis∑i=1NpuitTRuis=MP
令 P = 1 N p ∑ i = 1 N p u i t T R u i s P=\frac{1}{N_p}\sum_{i=1}^{N_p}{{u_i}^{tT}R{u_i}^s} P=Np1∑i=1NpuitTRuis， M = 1 N p ∑ i = 1 N p u i s T u i s M=\frac{1}{N_p}\sum_{i=1}^{N_p}{{u_i}^{sT}{u_i}^s} M=Np1∑i=1NpuisTuis
将上式（4）代入化简后的目标方程(3)中，得：
R ∗ = a r g m a x ( P 2 M ) R^\ast=argmax(\frac{P^2}{M}) R∗=argmax(MP2)
M正定，回归无尺度求解问题：
R ∗ = a r g m a x ( P ) = a r g m a x 1 N p ∑ i = 1 N p u i t T R u i s R^\ast=argmax\left(P\right)=argmax\frac{1}{N_p}\sum_{i=1}^{N_p}{{u_i}^{tT}R{u_i}^s} R∗=argmax(P)=argmaxNp1i=1∑NpuitTRuis
同理两种求解方式：
1）ICP
令：
H = ∑ i = 1 N p u i s u i t T = U Σ V T H=\sum_{i=1}^{N_p}{{u_i}^s{u_i}^{tT}}=U\Sigma V^T H=i=1∑NpuisuitT=UΣVT
R ∗ = V S U T R^\ast=VSU^T R∗=VSUT
s = ∑ i = 1 N p u i t T R u i s ∑ i = 1 N p u i s T u i s = t r ( R H ) M = t r ( Σ S ) M 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} s=∑i=1NpuisTuis∑i=1NpuitTRuis=Mtr(RH)=Mtr(ΣS)
- Umeyama:
令：
H = ∑ i = 1 N p u i t u i s T = U Σ V T H=\sum_{i=1}^{N_p}{{u_i}^t{u_i}^{sT}}=U\Sigma V^T H=i=1∑NpuituisT=UΣVT
R ∗ = U S V T R^\ast=U{SV}^T R∗=USVT
s = ∑ i = 1 N p u i t u i s T ∑ i = 1 N p u i s T u i s = t r ( R T H ) M = t r ( Σ S ) M 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^TH)}{M}=\frac{tr(\mathrm{\Sigma S})}{M} s=∑i=1NpuisTuis∑i=1NpuituisT=Mtr(RTH)=Mtr(ΣS)
总结，带尺度求解运动(s,R,t)流程
Q: 给定两个点云集合:
p t = { p 1 t , p 2 t , ⋯ p n t } p^t=\left\{p_1^t,p_2^t,\cdots p_n^t\right\} pt={p1t,p2t,⋯pnt} 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 ( R , t ) = 1 N p ∑ i ∣ ∣ p i t − s R p i s − t ∣ ∣ 2 E\left(R,t\right)=\frac{1}{N_p}\sum_i{||p_i^t-sRp_i^s-t||^2} E(R,t)=Np1i∑∣∣pit−sRpis−t∣∣2
S:
若令：
H = ∑ i = 1 N p u i t u i s T = U Σ V T H=\sum_{i=1}^{N_p}{{u_i}^t{u_i}^{sT}}=U\Sigma V^T H=i=1∑NpuituisT=UΣVT
则ICP的解为：
R = U S V T R=USV^T R=USVT
s = t r ( R H ) ∑ i = 1 N p u i s T u i s = t r ( Σ S ) ∑ i = 1 N p u i s T u i s 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}} s=∑i=1NpuisTuistr(RH)=∑i=1NpuisTuistr(ΣS)
t = c t − s R c s \bm{t}=c^t-sRc^s t=ct−sRcs
测试代码
以下代码生成点云和解算使用的Rt不一致，为测试其他完整所用，建议修改一致
#include 结果：
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