视觉SLAM前端—

当我们知道空间点在两个坐标系下的位置时，可以通过ICP来求解其相对位姿。ICP最初常用在激光SLAM中，因为激光可以获得3D位置，而不像视觉还需要经过相机的投影变换，但是从激光雷达获得的三维点云中，我们很难对两帧点云进行匹配，通常是寻找距离最近的点作为匹配点，所以称为迭代最近法（ICP）。当我们已知相机的位姿后，可以把相机坐标系下的特征点 P i c P_i^c Pic转换到世界坐标系下，这与世界坐标系下对应的特征点 P i w P_i^w Piw存在误差，即：
e i = P i w − ( R w c P i c + t w c ) e_i=P_i^w-(R_{wc}P_i^c+t_{wc}) ei=Piw−(RwcPic+twc)
考虑所有特征点的误差，我们定义总误差为：
J ( R , t ) = 1 2 ∑ i = 1 n ∥ e i ∥ 2 2 J(R,t) =\frac 1 2\sum_{i=1}^n\|e_i\|_2^2 J(R,t)=21i=1∑n∥ei∥22
我们的目标是寻找一个最优的 R R R、 t t t，使得总误差 J J J最小，即：
T ∗ = arg min ⁡ R . t J ( R , t ) T^*=\argmin_{R.t}J(R,t) T∗=R.targminJ(R,t)
这是一个非线性最小二乘问题，通常可以用非线性优化的方法进行求解，但对于ICP来说，我们还可以使用SVD分解的方式求出解析解。

SVD方法

首先我们先定义相机坐标系下和世界坐标系下所有3D特征点的质心分别为（注意质心是没有下标的）：
P c = 1 n ∑ i = 1 n P i c P^c=\frac 1 n \sum_{i=1}^nP_i^c Pc=n1i=1∑nPic P w = 1 n ∑ i = 1 n P i w P^w=\frac 1 n \sum_{i=1}^nP_i^w Pw=n1i=1∑nPiw
考虑第 i i i个误差项：
e i = P i w − ( R w c P i c + t w c ) = P i w − P w + P w − R w c ( P i c − P c + P c ) − t w c = P i w − P w − R w c ( P i c − P c ) + P w − ( R w c P c + t w c ) = P ′ i w − R w c P ′ i c ⏟ e i a + P w − ( R w c P c + t w c ) ⏟ e i b \begin{aligned}e_i &=P_i^w-(R_{wc}P_i^c+t_{wc}) \\ &=P_i^w-P^w+P^w-R_{wc}(P_i^c-P^c+P^c)-t_{wc} \\ &=P_i^w-P^w-R_{wc}(P_i^c-P^c)+P^w-(R_{wc}P^c+t_{wc}) \\&=\underbrace{{P'}_i^w-R_{wc}{P'}_i^c}_{e_{ia}}+\underbrace{P^w-(R_{wc}P^c+t_{wc})}_{e_{ib}} \end{aligned} ei=Piw−(RwcPic+twc)=Piw−Pw+Pw−Rwc(Pic−Pc+Pc)−twc=Piw−Pw−Rwc(Pic−Pc)+Pw−(RwcPc+twc)=eia P′iw−RwcP′ic+eib Pw−(RwcPc+twc)
其中 P ′ i w = P i w − P w {P'}_i^w=P_i^w-P^w P′iw=Piw−Pw， P ′ i c = P i c − P c {P'}_i^c=P_i^c-P^c P′ic=Pic−Pc，分别表示特征点在世界和相机坐标系下的去质心坐标。由于：
∑ i = 1 n e i a = ∑ i = 1 n ( P ′ i w − R w c P ′ i c ) = 0 \sum_{i=1}^ne_{ia} =\sum_{i=1}^n({{P'}_i^w-R_{wc}{P'}_i^c})=0 i=1∑neia=i=1∑n(P′iw−RwcP′ic)=0
那么总误差可以化简为：
J ( R , t ) = 1 2 ∑ i = 1 n ∥ e i ∥ 2 2 = 1 2 ∑ i = 1 n ∥ e i a ∥ 2 2 + 1 2 ∑ i = 1 n ∥ e i b ∥ 2 2 = J a ( R ) + J b ( R , t ) J(R,t) =\frac 1 2\sum_{i=1}^n\|e_i\|_2^2=\frac 1 2\sum_{i=1}^n\|e_{ia}\|_2^2+\frac 1 2\sum_{i=1}^n\|e_{ib}\|_2^2=J_a(R)+J_b(R,t) J(R,t)=21i=1∑n∥ei∥22=21i=1∑n∥eia∥22+21i=1∑n∥eib∥22=Ja(R)+Jb(R,t)
由于 J a J_a Ja只与 R R R有关，与 t t t无关，我们可以先通过优化 J a J_a Ja求得 R R R，再带入 J b ( R , t ) = 0 J_b(R,t)=0 Jb(R,t)=0，求得 t t t。 J a J_a Ja可进一步展开为：
J a ( R ) = 1 2 ∑ i = 1 n ∥ P ′ i w − R w c P ′ i c ∥ 2 2 = 1 2 ∑ i = 1 n ( P ′ i w T P ′ i w + P ′ i c T R w c T R w c P ′ i c − 2 P ′ i w T R w c P ′ i c ) J_a(R)=\frac 1 2\sum_{i=1}^n\|{P'}_i^w-R_{wc}{P'}_i^c\|_2^2=\frac 1 2\sum_{i=1}^n({{P'}_i^w}^T{P'}_i^w+{{P'}_i^c}^TR_{wc}^TR_{wc}{P'}_i^c-2{{P'}_i^w}^TR_{wc}{P'}_i^c) Ja(R)=21i=1∑n∥P′iw−RwcP′ic∥22=21i=1∑n(P′iwTP′iw+P′icTRwcTRwcP′ic−2P′iwTRwcP′ic)
由于前两项与 R R R无关，所以优化目标可以简化为：
J ′ a ( R ) = − ∑ i = 1 n P ′ i w T R w c P ′ i c = − ∑ i = 1 n t r ( R w c P ′ i c P ′ i w T ) = − t r ( R w c ∑ i = 1 n P ′ i c P ′ i w T ) {J'}_a(R) =-\sum_{i=1}^n {{P'}_i^w}^TR_{wc}{P'}_i^c=-\sum_{i=1}^n tr(R_{wc}{P'}_i^c{{P'}_i^w}^T)=-tr( R_{wc} \sum_{i=1}^n{P'}_i^c{{P'}_i^w}^T) J′a(R)=−i=1∑nP′iwTRwcP′ic=−i=1∑ntr(RwcP′icP′iwT)=−tr(Rwci=1∑nP′icP′iwT)
令 W = ∑ i = 1 n P ′ i c P ′ i w T W=\sum_{i=1}^n{P'}_i^c{{P'}_i^w}^T W=∑i=1nP′icP′iwT，对 W W W进行SVD分解，即 W = U Σ V T W=U\Sigma V^T W=UΣVT， Σ \Sigma Σ对应的奇异值从大到小排列。根据最优性证明，当 W W W满秩时，最优的 R = U V T R=UV^T R=UVT，如果 R R R行列式小于0，取负作为最优值。

非线性优化方法

由于ICP存在无穷多解或者唯一解，如果我们使用迭代的方法获取局部最优解，即为全局最优解，因此对于ICP问题，我们可以任意选定初始值并进行迭代。误差的一阶导数即雅克比矩阵为：
∂ e i ∂ R = − ( R w c P i c ) ∧ = ( − P i w ) ∧ ， ∂ e i ∂ t = I \frac {\partial e_i} {\partial R}=-(R_{wc}P_i^c)^{\land}=(-P_i^w)^{\land}，\frac {\partial e_i} {\partial t}=I ∂R∂ei=−(RwcPic)∧=(−Piw)∧，∂t∂ei=I

视觉SLAM前端——ICP相关推荐

视觉SLAM前端特征检测与跟踪的思考
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达就目前视觉SLAM的引用来区分,分为基于特征法的和直接法的视觉SL ...
视觉SLAM前端——LK光流法
目录: LK光流介绍单层LK光流多层LK光流 LK光流 LK光流是一种描述图像运动的方法,利用LK光流可以实现对图像的追踪,从而求解图像运动的位姿.其基本思想如下: img1,img2分别 ...
视觉SLAM前端——PnP
如果我们已知多对匹配好的2D/3D特征点,我们可以用PnP方法去求得相机的运动.对于单目相机来说,我们可以通过利用对极约束或者单应性求得相机的运动,然后三角化获得3D点:对于双目可以通过视差恢复深度信 ...
视觉SLAM十四讲学习笔记-第七讲-视觉里程计-特征点法和特征提取和匹配实践
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客视觉SLAM十四讲学习 ...
八种常用激光雷达和视觉SLAM算法的评估与比较
文章:Evaluation and comparison of eight popular Lidar and Visual SLAM algorithms 作者:Bharath Garigipati ...
视觉SLAM综述（入门篇）
最近几天刚刚接触到视觉SLAM这个研究方向,发现这个方向的知识点挺多挺杂的,于是就自己看文献,刷博客,大致对视觉SLAM有了整体的了解. 这篇博客也算是一个简单的小综述,具体的知识点正在学习之 ...
SLAM | 视觉SLAM中的后端：后端优化算法与建图模板
点击上方"AI算法修炼营",选择加星标或"置顶" 标题以下,全是干货前面的话前面系列一中我们介绍了,VSLAM 是利用多视图几何理论,根据相机拍摄的图像信息 ...
SLAM前端中的视觉里程计和回环检测
1. 通常的惯例是把 VSLAM 分为前端和后端.前端为视觉里程计和回环检测,相当于是对图像数据进行关联:后端是对前端输出的结果进行优化,利用滤波或非线性优化理论,得到最优的位姿估计和全局一致性地图. ...
视觉SLAM笔记（39）求解 ICP
视觉SLAM笔记(39) 求解 ICP 1. SVD 方法 2. 非线性优化方法 1. SVD 方法使用 SVD 以及非线性优化来求解 ICP 使用两个 RGB-D 图像,通过特征匹配获取两组 3D ...

视觉SLAM前端——ICP

SVD方法

非线性优化方法

视觉SLAM前端——ICP相关推荐

最新文章

热门文章