引用资料(理论部分其实就是把第一个的不详细和错误的地方说了一下,翻译了一下第二个文献以及不明了的地方说一下O(∩_∩)O哈哈~):
高翔《视觉SLAM十四讲》
K. S. ARUN, T. S. HUANG, AND S. D. BLOSTEIN
Least-Squares Fitting of Two 3-D Point Sets
问题描述
假设存在两个点云集合
{
p
i
}
\{p_i\}
{pi}和
{
p
′
}
\{p'\}
{p′}
求:一个欧氏变换
R
,
t
R,t
R,t使得
∀
i
,
p
i
=
R
p
i
′
+
t
\forall {i,p_i}=Rp'_i+t
∀i,pi=Rpi′+t
求解问题
解:
假设误差项为
e
i
=
p
i
−
(
R
p
i
′
+
t
)
e_i=p_i-(Rp'_i+t)
ei=pi−(Rpi′+t)
那么问题转化为优化问题:
m
i
n
R
,
t
J
=
1
2
∑
i
=
1
n
∥
(
p
i
−
(
R
p
i
′
+
t
)
)
∥
2
\mathop{min}\limits_{R,t}J=\frac{1}{2}\sum_{i=1}^n\|(p_i-(Rp'_i+t_))\|^2
R,tminJ=21i=1∑n∥(pi−(Rpi′+t))∥2
定义质心为:
p
=
1
n
∑
i
=
1
n
(
p
i
)
,
p
′
=
1
n
∑
i
=
1
n
(
p
i
′
)
p=\frac{1}{n}\sum_{i=1}^n(p_i),p'=\frac{1}{n}\sum_{i=1}^n(p'_i)
p=n1i=1∑n(pi),p′=n1i=1∑n(pi′)
那么有:
1
2
∑
i
=
1
n
∥
p
i
−
(
R
p
i
′
+
t
)
∥
=
1
2
∑
i
=
1
n
∥
p
i
−
R
p
i
′
−
t
−
p
+
R
p
′
+
p
−
R
p
′
∥
2
\frac{1}{2}\sum_{i=1}^n\|p_i-(Rp'_i+t)\|=\frac{1}{2}\sum_{i=1}^n\|p_i-Rp'_i-t-p+Rp'+p-Rp'\|^2
21i=1∑n∥pi−(Rpi′+t)∥=21i=1∑n∥pi−Rpi′−t−p+Rp′+p−Rp′∥2
=
1
2
∑
i
=
1
n
∥
(
p
i
−
p
−
R
(
p
i
′
−
p
′
)
)
+
(
p
−
R
p
′
−
t
)
∥
2
=\frac{1}{2}\sum_{i=1}^n\|(p_i-p-R(p'_i-p'))+(p-Rp'-t)\|^2
=21i=1∑n∥(pi−p−R(pi′−p′))+(p−Rp′−t)∥2
=
1
2
∑
i
=
1
n
(
∥
p
i
−
p
−
R
(
p
i
′
−
p
′
)
∥
2
+
∥
p
−
R
p
′
−
t
∥
2
+
2
(
p
i
−
p
−
R
(
p
i
′
−
p
′
)
(
p
−
R
p
′
−
t
)
)
=\frac{1}{2}\sum_{i=1}^n(\|p_i-p-R(p'_i-p')\|^2+\|p-Rp'-t\|^2+2(p_i-p-R(p'_i-p')(p-Rp'-t))
=21i=1∑n(∥pi−p−R(pi′−p′)∥2+∥p−Rp′−t∥2+2(pi−p−R(pi′−p′)(p−Rp′−t))
因为
∑
i
=
1
n
(
p
i
−
p
−
R
(
p
i
′
−
p
′
)
(
p
−
R
p
′
−
t
)
=
0
\sum_{i=1}^n(p_i-p-R(p'_i-p')(p-Rp'-t)=0
i=1∑n(pi−p−R(pi′−p′)(p−Rp′−t)=0
所以问题转化为:
m
i
n
R
,
t
J
=
1
2
∑
i
=
1
n
∥
p
i
−
p
−
R
(
p
i
′
−
p
′
)
∥
2
+
∥
p
−
R
p
′
−
t
∥
2
\mathop{min}\limits_{R,t}J=\frac{1}{2}\sum_{i=1}^n\|p_i-p-R(p'_i-p')\|^2+\|p-Rp'-t\|^2
R,tminJ=21i=1∑n∥pi−p−R(pi′−p′)∥2+∥p−Rp′−t∥2
因为左右两边都大于等于零,而且左边只和
R
R
R相关,可以先求出R在利用R求解第二项
那么按照书里计算过程
- 计算两组质心位置p,p’,然后计算每个点的去质心坐标:
q i = p i − p , q i ′ = p i ′ − p ′ q_i=p_i-p,q'_i=p'_i-p' qi=pi−p,qi′=pi′−p′
2.根据以下优化问题计算旋转矩阵:
R ∗ = a r g m i n R 1 2 ∑ ∥ q i − R q i ′ ∥ 2 R^*=arg \mathop{min}\limits_{R}\frac{1}{2}\sum\|q_i-Rq'_i\|^2 R∗=argRmin21∑∥qi−Rqi′∥2
3.根据2的结果计算t
t ∗ = p − R p ′ t^*=p-Rp' t∗=p−Rp′
展开关于R的误差项有:
1
2
∑
∥
q
i
−
R
q
i
′
∥
2
=
1
2
∑
q
i
T
q
i
+
q
i
′
T
R
T
R
q
i
′
−
2
q
i
T
R
q
i
′
\frac{1}{2}\sum\|q_i-Rq'_i\|^2=\frac{1}{2}\sum {q_i^Tq_i+q_i^{'T}R^TRq'_i-2q_i^TRq'_i}
21∑∥qi−Rqi′∥2=21∑qiTqi+qi′TRTRqi′−2qiTRqi′
因为第一项与R无管,第二项由于
R
T
R
=
I
R^TR=I
RTR=I与R也无关那么问题转化为
∑
i
=
1
n
−
q
i
T
R
q
i
′
=
∑
i
=
1
n
−
t
r
(
R
q
i
′
q
i
T
)
=
−
t
r
(
R
∑
i
=
1
n
q
i
′
q
i
T
)
\sum_{i=1}^n{-q_i^TRq'_i}=\sum_{i=1}^n{-tr(Rq'_iq_i^T)}=-tr(R\sum_{i=1}^nq'_iq_i^T)
i=1∑n−qiTRqi′=i=1∑n−tr(Rqi′qiT)=−tr(Ri=1∑nqi′qiT)
令
H
=
∑
i
=
1
n
q
i
′
q
i
T
H=\sum_{i=1}^nq'_iq_i^T
H=i=1∑nqi′qiT
因为问题是求解
m
i
n
R
.
−
t
r
(
R
H
)
\mathop{min}\limits_{R}{ \mathop{.-tr}(RH)}
Rmin.−tr(RH)
即:
m
a
x
R
.
t
r
(
R
H
)
\mathop{max}\limits_{R}{\mathop{.tr}(RH)}
Rmax.tr(RH)
假设最优解
R
∗
R^*
R∗
那么
t
r
(
R
∗
H
)
≥
t
r
(
R
H
)
=
t
r
(
B
R
∗
H
)
tr(R^*H)\ge tr(RH)=tr(BR^*H)
tr(R∗H)≥tr(RH)=tr(BR∗H)(因为R是正交矩阵)
对H进行SVD分解
H
=
U
Σ
V
T
H=U\Sigma V^T
H=UΣVT
假设
R
∗
=
V
U
T
R^*=VU^T
R∗=VUT
那么
R
∗
H
=
V
U
T
U
Σ
V
T
=
V
Σ
V
T
R^*H=VU^TU\Sigma V^T=V\Sigma V^T
R∗H=VUTUΣVT=VΣVT
令
A
=
V
Σ
1
2
A=V\Sigma^{\frac{1}{2}}
A=VΣ21
因为
t
r
(
R
∗
H
)
=
t
r
(
A
A
T
)
≥
t
r
(
B
A
A
T
)
,
(
B
B
T
=
I
)
tr(R^*H)=tr(AA^T)\ge tr(BAA^T),(BB^T=I)
tr(R∗H)=tr(AAT)≥tr(BAAT),(BBT=I)
所以
R
∗
=
V
U
T
R^*=VU^T
R∗=VUT是
m
a
x
R
.
t
r
(
R
H
)
\mathop{max}\limits_{R}{\mathop{.tr}(RH)}
Rmax.tr(RH)
最优解
现在只要证明
t
r
(
A
A
T
)
≥
t
r
(
B
A
A
T
)
,
(
B
B
T
=
I
)
tr(AA^T)\ge tr(BAA^T),(BB^T=I)
tr(AAT)≥tr(BAAT),(BBT=I)
a
i
a_i
ai是A的第i列,因为
t
r
(
A
B
)
=
t
r
(
B
A
)
tr(AB)=tr(BA)
tr(AB)=tr(BA)那么有
t
r
(
B
A
A
T
)
=
t
r
(
A
T
B
A
)
=
∑
a
i
t
(
B
a
i
)
tr(BAA^T)=tr(A^TBA)=\sum{a_i^t(Ba_i)}
tr(BAAT)=tr(ATBA)=∑ait(Bai)
根据Schwarz不等式
a
i
t
(
B
a
i
)
≤
(
a
i
t
a
i
)
(
a
i
t
B
t
B
a
i
)
=
a
i
t
a
i
a_i^t(Ba_i)\le\sqrt{(a_i^ta_i)(a_i^tB^tBa_i)}=a_i^ta_i
ait(Bai)≤(aitai)(aitBtBai)=aitai
即
t
r
(
B
A
A
T
)
=
t
r
(
A
T
B
A
)
≤
∑
a
i
t
a
i
=
t
r
(
A
A
T
)
tr(BAA^T)=tr(A^TBA)\le \sum{a_i^ta_i}=tr(AA^T)
tr(BAAT)=tr(ATBA)≤∑aitai=tr(AAT)
注意这个计算需要H是满秩,
几个情况需要考虑
1.H是满秩,
{
p
′
}
\{p'\}
{p′}上的点非共平面
2.
{
p
′
}
\{p'\}
{p′}上的点共平面,可以对H求出的解的为0特征值的特征向量计算取反,使得
d
e
t
∣
H
∣
=
1
det|H|=1
det∣H∣=1
3.
{
p
′
}
\{p'\}
{p′}上的点共线,不能用SVD求解
代码
//这个是将点云dstPoint利用RT配到srcPoint上的 srcPoint=dstPoint*R+T
void registrateNPoint(std::vector<cv::Point3d>& srcPoints,std::vector<cv::Point3d>& dstPoints,cv::Mat&R,cv::Mat&T){
if(srcPoints.size()!=dstPoints.size()||srcPoints.size()<3||dstPoints.size()<3)
{
std::cout<<"srcPoints.size():\t"<<srcPoints.size();
std::cout<<"dstPoints.size():\t"<<dstPoints.size();
std::cout<<"registrateNPoint points size donot match!";
}
double srcSumX = 0.0f;
double srcSumY = 0.0f;
double srcSumZ = 0.0f;
double dstSumX = 0.0f;
double dstSumY = 0.0f;
double dstSumZ = 0.0f;
size_t pointsNum=srcPoints.size();
for(size_t i=0;i<pointsNum;i++){
srcSumX+=srcPoints[i].x;
srcSumY+=srcPoints[i].y;
srcSumZ+=srcPoints[i].z;
dstSumX+=dstPoints[i].x;
dstSumY+=dstPoints[i].y;
dstSumZ+=dstPoints[i].z;
}
cv::Point3d srcCentricPt(srcSumX / pointsNum,srcSumY / pointsNum,srcSumZ / pointsNum);
cv::Point3d dstCentricPt(dstSumX / pointsNum,dstSumY / pointsNum,dstSumZ / pointsNum);
cv::Mat srcMat;
srcMat=cv::Mat::zeros(3, pointsNum, CV_64F);
cv::Mat dstMat;
dstMat=cv::Mat::zeros(3, pointsNum, CV_64F);
for (size_t i = 0; i < pointsNum; ++ i)
{
srcMat.at<double>(0,i)=srcPoints[i].x - srcCentricPt.x;
srcMat.at<double>(1,i)=srcPoints[i].y - srcCentricPt.y;
srcMat.at<double>(2,i)=srcPoints[i].z - srcCentricPt.z;
dstMat.at<double>(0,i)=dstPoints[i].x - dstCentricPt.x;
dstMat.at<double>(1,i)=dstPoints[i].y - dstCentricPt.y;
dstMat.at<double>(2,i)=dstPoints[i].z - dstCentricPt.z;
}
cv::Mat matS = srcMat * dstMat.t();
cv::Mat matU, matW, matV;
cv::SVDecomp(matS, matW, matU, matV);
cv::Mat matTemp = matU * matV;
double det = cv::determinant(matTemp);
double datM[] = {1, 0, 0, 0, 1, 0, 0, 0, det};
cv::Mat matM(3, 3, CV_64FC1, datM);
cv::Mat matR = matV.t() * matM * matU.t();
double tx,ty,tz;
tx = dstCentricPt.x- (srcCentricPt.x* matR.at<double>(0,0) + srcCentricPt.y* matR.at<double>(0,1) + srcCentricPt.z* matR.at<double>(0,2));
ty = dstCentricPt.y- (srcCentricPt.x* matR.at<double>(1,0) + srcCentricPt.y* matR.at<double>(1,1) + srcCentricPt.z * matR.at<double>(1,2));
tz = dstCentricPt.z- (srcCentricPt.x* matR.at<double>(2,0) + srcCentricPt.y* matR.at<double>(2,1) + srcCentricPt.z * matR.at<double>(2,2));
double datT[]={tx,ty,tz};
cv::Mat matT(3, 1, CV_64F,datT);
matR.copyTo(R);
matT.copyTo(T);
}