利用SVD的方法求解ICP(详细推导)

引用资料(理论部分其实就是把第一个的不详细和错误的地方说了一下,翻译了一下第二个文献以及不明了的地方说一下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=1n(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=1n(pi),p=n1i=1n(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=1npi(Rpi+t)=21i=1npiRpitp+Rp+pRp2
= 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=1n(pipR(pip))+(pRpt)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=1n(pipR(pip)2+pRpt2+2(pipR(pip)(pRpt))

因为
∑ 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=1n(pipR(pip)(pRpt)=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=1npipR(pip)2+pRpt2
因为左右两边都大于等于零,而且左边只和 R R R相关,可以先求出R在利用R求解第二项

那么按照书里计算过程

  1. 计算两组质心位置p,p’,然后计算每个点的去质心坐标:
    q i = p i − p , q i ′ = p i ′ − p ′ q_i=p_i-p,q'_i=p'_i-p' qi=pip,qi=pip
    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=argRmin21qiRqi2
    3.根据2的结果计算t
    t ∗ = p − R p ′ t^*=p-Rp' t=pRp

展开关于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} 21qiRqi2=21qiTqi+qiTRTRqi2qiTRqi
因为第一项与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=1nqiTRqi=i=1ntr(RqiqiT)=tr(Ri=1nqiqiT)

H = ∑ i = 1 n q i ′ q i T H=\sum_{i=1}^nq'_iq_i^T H=i=1nqiqiT
因为问题是求解
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(RH)tr(RH)=tr(BRH)(因为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 RH=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(RH)=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 detH=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);
}
  • 19
    点赞
  • 112
    收藏
    觉得还不错? 一键收藏
  • 15
    评论
评论 15
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值