应该是思维导图的形式使用markmap实现,可以使用md文档在VSCode中添加插件预览,预览图如绑定的那个资源的样式。
效果大概是长这个样子
> 但是CSDN不好用,一堆乱码。这里建议从这个链接中复制下载md文件,使用VSCode的markmap插件查看,或者直接把md的内容复制到markmap网站也能查看。
还有这里的文字不会再更新了,直接从上边的github链接找对应的md文件吧。 反正都是开源的,有错误和完善直接git更新,这里的md更新不方便,还有那个啥绑定的资源链接也是不更新了,麻烦还有VIP啥的就上传一下,赖得更新调整,有rsqd的可以下载,不如这个链接、这个链接、这个链接处下载。
markmap:
colorFreezeLevel: 50
initialExpandLevel: 10
maxWidth: 600
VSLAM
传感器输入
相机
相机模型
针孔模型
内参模型
- 相机内参 K K K: ( f x 0 c x 0 f y c y 0 0 1 ) \begin{pmatrix} f_x&0 &c_x \\ 0 &f_y&c_y \\ 0 &0 &1 \end{pmatrix} fx000fy0cxcy1
- 归一化坐标: [ X / Z Y / Z 1 ] T \begin{bmatrix} X/Z &Y/Z &1 \end{bmatrix}^T [X/ZY/Z1]T
- 像素坐标: ( u v 1 ) = 1 Z ( f x 0 c x 0 f y c y 0 0 1 ) ( X Y Z ) = 1 Z K P \begin{pmatrix} u\\ v\\ 1\\ \end{pmatrix}=\frac{1}{Z} \begin{pmatrix} f_x&0 &c_x \\ 0 &f_y&c_y \\ 0 &0 &1 \end{pmatrix}\begin{pmatrix} X\\ Y\\ Z\\ \end{pmatrix}=\frac{1}{Z} KP uv1 =Z1 fx000fy0cxcy1 XYZ =Z1KP
畸变模型
- 径向畸变: x d i s t o r t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) y d i s t o r t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) \begin{matrix} x_{distorted} = x(1+k_1r^2+k_2r^4+k_3r^6)\\ y_{distorted} = y(1+k_1r^2+k_2r^4+k_3r^6) \end{matrix} xdistorted=x(1+k1r2+k2r4+k3r6)ydistorted=y(1+k1r2+k2r4+k3r6)
- 切向畸变: x d i s t o r t e d = x + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y d i s t o r t e d = y + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y \begin{matrix} x_{distorted} = x+2p_1xy+p_2(r^2+2x^2)\\ y_{distorted} = y+p_1(r^2+2y^2)+2p_2xy \end{matrix} xdistorted=x+2p1xy+p2(r2+2x2)ydistorted=y+p1(r2+2y2)+2p2xy
- 畸变后的归一化坐标: x d i s t o r t e d = x ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + 2 p 1 x y + p 2 ( r 2 + 2 x 2 ) y d i s t o r t e d = y ( 1 + k 1 r 2 + k 2 r 4 + k 3 r 6 ) + p 1 ( r 2 + 2 y 2 ) + 2 p 2 x y \begin{matrix} x_{distorted} = x(1+k_1r^2+k_2r^4+k_3r^6)+2p_1xy+p_2(r^2+2x^2)\\ y_{distorted} = y(1+k_1r^2+k_2r^4+k_3r^6)+p_1(r^2+2y^2)+2p_2xy \end{matrix} xdistorted=x(1+k1r2+k2r4+k3r6)+2p1xy+p2(r2+2x2)ydistorted=y(1+k1r2+k2r4+k3r6)+p1(r2+2y2)+2p2xy
- 畸变后的像素坐标: u = f x x d i s t o r t e d + c x v = f y y d i s t o r t e d + c y \begin{matrix} u=f_xx_{distorted}+c_x\\ v=f_yy_{distorted}+c_y \end{matrix} u=fxxdistorted+cxv=fyydistorted+cy
相机类型
单目RGB
双目RGB
双目相机模型
- 相似三角形关系:$\frac{z-f}{z} = \frac{b-u_L+u_R}{b} , 其中 ,其中 ,其中b$是双目相机的基线,两个水平放置的相机光圈在x轴上的距离。
- 深度值 z = f b d z = \frac{fb}{d} z=dfb,视差 d = d e f u L − u R d\overset{def}{=}u_L-u_R d=defuL−uR, b b b是基线
深度图像RGB-D
IMU
IMU参数
积分方法
预积分
中值积分
外参标定
前端里程计 Visual Odometry
图像预处理
特征提取
- 外观相似的特征应该具有相似的描述子
ORB特征 Oriented FAST
- FAST角点 3像素半径灰度值变化明显的点
- 尺度不变性:使用图像金字塔放缩关系处理尺度不变性
- 旋转不变性:通过灰度质心和几何中心确定特征点的方向
- BRIEF描述子
- 使用FAST关键点的方向计算Steer BRIEF获得ORB描述子的旋转不变性
SIFT特征 Scale-Invariant Featrue Transform尺度不变特征变化
- 先进行非极大抑制,再去除低对比度的点,再通过Hessian矩阵去除边缘响应过大的点。
- 利用不同尺寸的图像与高斯差分滤波器卷积
- 在正方形区域内统计梯度幅值的直方图,直方图最大值对应主方向,可以有多个主方向
- 描述子:128维度: 4 ∗ 4 ∗ 8 4*4*8 4∗4∗8
- 旋转、尺度、亮度不变性,对视角、仿射变换、噪声保持稳定性
SURF特征
- 先利用Hessian矩阵确定候选点,然后进行非极大抑制
- 利用原图片与不同尺寸的方框滤波器(box filter)卷积。
- 在圆形区域内计算各个扇形范围内x、y方向的haar小波响应,模最大的扇形方向作为主方向
- 描述子:64维度: 4 ∗ 4 ∗ 4 4*4*4 4∗4∗4
特征匹配
基于描述子
暴力匹配 Brute-Force Matcher
快速近似最近邻匹配 GLANN
基于光流
稀疏光流
Lucas-Kanade 光流
- 灰度不变假设
- 假设图像块中的像素具有相同的运动
- 多层光流,以获得比较好的初始运动估计
稠密光流
计算当前相机位姿
- 2D-2D、2D-3D、3D-3D都有个前提: 已知匹配关系
2D-2D 对极几何
本质矩阵E和基础矩阵F
- 约束关系: x 2 T E x 1 = p 2 T F p 1 x_2^TEx_1 = p_2^TFp_1 x2TEx1=p2TFp1
- 本质矩阵: E = t ∧ R E = t^\wedge R E=t∧R,性质:E的特征值必定为 [ σ , σ , 0 ] T [\sigma,\sigma,0]^T [σ,σ,0]T的样式
- 基础矩阵: F = K − T E K F = K^{-T}EK F=K−TEK
-
x
1
=
K
−
1
p
1
、
x
2
=
K
−
1
p
2
x_1=K^{-1}p_1、x_2=K^{-1}p_2
x1=K−1p1、x2=K−1p2
其中 p 1 、 p 2 p_1、p_2 p1、p2是像素坐标、 x 2 、 x 1 x_2、x_1 x2、x1是两个像素点归一化平面坐标。
求解E的齐次线性方程组
- 八点法, t ∧ R t^\wedge R t∧R共9个参数,线性方程组求解需要8个方程,每对匹配点可以构建一个方程,所有至少需要8对点。
- 可以使用SVD分解的方式求解齐次线程方程组,SVD分解会得到四组解,可以验证深度为正值进行筛选。
- 为了使 E E E满足性质可以对八点法的E进行SVD分解、获取特征值 [ σ 1 , σ 2 , σ 3 ] T [\sigma_1,\sigma_2,\sigma_3]^T [σ1,σ2,σ3]T,其中 σ 1 > = σ 2 > = σ 3 \sigma_1>=\sigma_2>=\sigma_3 σ1>=σ2>=σ3,取 E ∗ = U d i a g ( ( σ 1 + σ 2 ) / 2 , ( σ 1 + σ 2 ) / 2 , 0 ) E^* = Udiag((\sigma_1+\sigma_2)/2,(\sigma_1+\sigma_2)/2,0) E∗=Udiag((σ1+σ2)/2,(σ1+σ2)/2,0)
单应矩阵H
- 用来描述两个平面之间的映射关系。如果匹配点都落入同一个平面上则可以通过单应矩阵求解2D-2D的对极几何。
- 约束关系: p 2 ≃ K ( R − t n T d ) k − 1 p 1 p_2\simeq K(R-\frac{tn^T}{d} )k^{-1}p_1 p2≃K(R−dtnT)k−1p1
- 单应矩阵 H = K ( R − t n T d ) k − 1 = [ h 1 h 2 h 3 h 4 h 5 h 6 h 7 h 8 h 9 ] H = K(R-\frac{tn^T}{d} )k^{-1} = \begin{bmatrix} h_1 &h_2 &h_3 \\ h_4 &h_5 &h_6 \\ h_7 &h_8 &h_9 \end{bmatrix} H=K(R−dtnT)k−1= h1h4h7h2h5h8h3h6h9 ,由于是成比例的关系可以使 h 9 = 1 h_9 = 1 h9=1
- 用来处理纯旋转的特殊情况、或者大多数匹配点共面的情况
求解H的非齐次线性方程组
- H H H共有8个参数,线性方程组求解需要8个方程,每对匹配的像素点提供2个方程,可以通过4对匹配点计算。
- 直接线性变化 DLT DLT Direct Linear Transform求解
- 计算出 H H H后分解可以计算获得 R 、 t R、t R、t
2D-3D PnP Perspective-n-Point详情参考
直接线性变换DLT Direct Linear Transform
- $
\omega\begin{pmatrix}u
\v
\1
\end{pmatrix} = \begin{pmatrix}
t_1 &t_2 &t_3 &t_4 \
t_5 &t_6 &t_7 &t_8 \
t_9 &t_{10}&t_{11} &t_{12}
\end{pmatrix}\begin{pmatrix}x
\y
\z
\1
\end{pmatrix}
$ - 待求 t t t共12个参数,线性方程组求解需要12个方程,每个匹配的2D-3D点提供2个方程,最少6对匹配点
- 没有考虑旋转矩阵 R R R本身的性质
P3P
- 通过相似三角形、余弦定理求解3D点在相机系下的坐标,然后通过ICP求解坐标变化
- 仅仅使用3对点的匹配信息,容易受到噪声影响
EPnP
- 控制点 c c c选择 p = ∑ j 4 c i j ∗ α i j p = \sum_{j}^{4} c_{ij}*\alpha_{ij} p=∑j4cij∗αij
- ω ( u v 1 ) = ( t 1 t 2 t 3 t 4 t 5 t 6 t 7 t 8 t 9 t 10 t 11 t 12 ) ∑ j 4 α i j ( x y z 1 ) \omega\begin{pmatrix}u \\v \\1 \end{pmatrix} = \begin{pmatrix} t_1 &t_2 &t_3 &t_4 \\ t_5 &t_6 &t_7 &t_8 \\ t_9 &t_{10}&t_{11} &t_{12} \end{pmatrix} \sum_{j}^{4} \alpha_{ij} \begin{pmatrix}x \\y \\z \\1 \end{pmatrix} ω uv1 = t1t5t9t2t6t10t3t7t11t4t8t12 ∑j4αij xyz1
- 与P3P类似,使用PCA获取4个控制点(不共面),计算其在相机系下的坐标,然后通过ICP求解坐标变化
BA Bundle Adjustment
- T ∗ = a r g m i n ∑ i n ∥ p i − 1 ω i K T P w i ∥ 2 T^* = argmin \sum_{i}^{n} \left \|p_i-\frac{1}{\omega_i}KTP_{wi} \right \| ^2 T∗=argmin∑in pi−ωi1KTPwi 2
- 重投影误差构建非线性优化问题,误差关于位姿的导数迭代求得所有对应点重投影误差之和最小的位姿估计。
3D-3D 迭代最近点 ICP,Iterative Closest Point
- p i = R p i , + t p_i = Rp_i^,+t pi=Rpi,+t
线性代数求解
- SVD分解法
- 去除质心求解R
- W = ∑ i = 1 n q i q i , T = U Σ V T W= \sum_{i=1}^{n}q_iq_i^{,T} = U\Sigma V^T W=∑i=1nqiqi,T=UΣVT
- R = U V T R = UV^T R=UVT, t = p − R p , t = p-Rp^, t=p−Rp,
非线性优化求解
- 通过李代数扰动模型求解
$\min_{\xi }\frac{1}{2} \sum_{i=1}^{n}\left |p_i- \exp(\xi ^\wedge p_i^,) \right | _2^2
$
直接法
- 不进行特征特提取和匹配,直接通过广度误差计算相机位姿
提取关键帧
后端优化
滑窗优化 Slide Window
局部优化 Loacl Bundle Adjustment
全局优化 Global Bundle Adjustment
位姿图优化
建图
三角测量 三角化
- 用来恢复观测物体or观测点的3D位置信息
- s 2 x 2 = s 1 R x 1 + t s_2x_2=s_1Rx_1+t s2x2=s1Rx1+t
- 通过同时乘 x 2 ∧ x_2^\wedge x2∧可以得到: 0 = s 2 x 2 ∧ x 2 = s 1 x 2 ∧ R x 1 + x 2 ∧ t 0 = s_2x_2^\wedge x_2=s_1x_2^\wedge Rx_1+ x_2^\wedge t 0=s2x2∧x2=s1x2∧Rx1+x2∧t,可以依次计算获得 s 1 s_1 s1和 s 2 s_2 s2。
回环检测
检测
BOW
场景识别
矫正
Sim3矫正,完后posegraph
多视图几何
2D
等距变换
- $X_2 = H_EX = \begin{bmatrix}
R&t\
0^T&1
\end{bmatrix}X_1 $ - 3自由度,旋转R1、平移t2
- 线段长度不变、夹角不变、面积不变
相似变换
- $X_2 = H_SX = \begin{bmatrix}
sR&t\
0^T&1
\end{bmatrix}X_1 $ - 4自由度,尺度s1、旋转R1、平移t2
- 夹角不变,长度比值不变,面积的比值不变
仿射变换
- $X_2 = AX = \begin{bmatrix}
A&t\
0^T&1
\end{bmatrix}X_1 $ - 6自由度,矩阵A是非奇异矩阵4、平移t2
- 夹角不变,长度比值不变,面积的比值不变
- 平行线保持平行,平行线段长度以及面积比值不变
映射变换
- $X_2 = H_pX = \begin{bmatrix}
A&t\
a^T&v
\end{bmatrix}X_1 $ - 8自由度,矩阵A是非奇异矩阵4、平移t2、a矩阵2,v是个比例系数H_p可以同时除以v固定
- 共线不变,交比(比值的比值)不变
-
个人感觉这个玩意的计算和单应矩阵H的形式差不多都是8自由度
3D
- 对应的增加自由度
状态估计
- SLAM求解的是当前 k k k时刻状态 x k x_k xk,可以看做一个状态估计问题。
状态估计问题定义
- 运动方程: x k = f ( x k − 1 , u k ) + w k x_k = f(x_{k-1},u_k)+w_k xk=f(xk−1,uk)+wk, x x x是状态量, w k w_k wk是噪声
- 观测方程: z k , j = h ( y j , x k ) + v k , j z_{k,j}=h(y_j,x_k)+v_{k,j} zk,j=h(yj,xk)+vk,j, z z z是观测量, y 是观测路标 y是观测路标 y是观测路标, v k , j v_{k,j} vk,j是噪声
最大后验估计 Maximum A Posteriori MAP
- 求解 x , y x,y x,y的最优解: x = a r g max x p ( x , y ∣ z , u ) x = arg\max_{x} p(x,y|z,u) x=argmaxxp(x,y∣z,u)
- 通过贝叶斯公式可以表示为 似然 ∗ 先验 似然*先验 似然∗先验的形式: p ( x , y ∣ z , u ) = p ( z , u ∣ x , y ) p ( x , y ) p ( z , u ) ∝ p ( z , u ∣ x , y ) p ( x , y ) p(x,y|z,u) = \frac{p(z,u|x,y)p(x,y)}{p(z,u)}\propto p(z,u|x,y)p(x,y) p(x,y∣z,u)=p(z,u)p(z,u∣x,y)p(x,y)∝p(z,u∣x,y)p(x,y)
- 由于先验 p ( x , y ) p(x,y) p(x,y)无法计算,就只考虑似然 p ( z , u ∣ x , y ) p(z,u|x,y) p(z,u∣x,y),所以MAP问题变成了极大似然估计
极大似然Maximize Likelihood Estimation MLE
- 转换后的: x , y = a r g max x , y p ( z , u ∣ x , y ) = a r g max x , y ∏ k p ( u k ∣ x k − 1 , x k ) ∏ k , j p ( z k , j ∣ x k , y j ) x,y = arg \max_{x,y}p(z,u|x,y) = arg \max_{x,y} \prod_{k}p(u_k|x_k-1,x_k)\prod_{k,j} p(z_{k,j}|x_k,y_j) x,y=argmaxx,yp(z,u∣x,y)=argmaxx,y∏kp(uk∣xk−1,xk)∏k,jp(zk,j∣xk,yj)
- 误差模型: e u , k = x k − f ( x k − 1 , u k ) , e z , k , j = z k , j − h ( x k , y j ) e_{u,k}=x_k-f(x_{k-1},u_k),e_{z,k,j} = z_{k,j} - h(x_k,y_j) eu,k=xk−f(xk−1,uk),ez,k,j=zk,j−h(xk,yj)
- 通过取对数可以转换为: x , y = m i n x , y ∑ k e u , k T R k − 1 e u , k + ∑ k ∑ j e z , k , j T Q k , j − 1 e z , k , j x,y=min_{x,y}\sum_ke^T_{u,k}R_k^{-1}e_{u,k} + \sum_k\sum_je^T_{z,k,j}Q_{k,j}^{-1}e_{z,k,j} x,y=minx,y∑keu,kTRk−1eu,k+∑k∑jez,k,jTQk,j−1ez,k,j,等价为误差模型的最小二乘问题。
优化
常见的优化算法详情参考
- 求解非线性最小二乘问题
min x F ( x ) = 1 2 ∥ f ( x ) ∥ 2 2 \min_{x} F(x) = \frac{1}{2}\left \| f(x)\right \| ^2_2 xminF(x)=21∥f(x)∥22
使用泰勒展开进行近似,求解变化量 Δ x \Delta x Δx
F ( x k + Δ x k ) ≈ F ( x k ) + J T ( x k ) Δ x k + 1 2 Δ x k T H ( x k ) Δ x k F(x_k+\Delta x_k) \approx F(x_k) + J^T(x_k)\Delta x_k + \frac{1}{2}\Delta x_k^TH(x_k)\Delta x_k F(xk+Δxk)≈F(xk)+JT(xk)Δxk+21ΔxkTH(xk)Δxk
最速下降法
- 梯度方向是上升方向,所以沿着负梯度方向 − J -J −J更新 x x x,可以使 F ( x ) F(x) F(x)逐步获得最小值。更新的步长为 − λ -\lambda −λ
- Δ x = − λ J \Delta x=-\lambda J Δx=−λJ
- 是下降算法。容易出现锯齿现象,增加迭代的次数
牛顿法
- 二阶泰勒展开的数值最小,一般是对 Δ x \Delta x Δx求导导数为0的极值点。
- J + H Δ x = 0 J+H\Delta x = 0 J+HΔx=0
- 是下降算法。需要计算H矩阵,计算量大
高斯牛顿法
- 思路使用 f ( x ) f(x) f(x)的雅可比矩阵 J ( x ) J T ( x ) J(x)J^T(x) J(x)JT(x)近似 H H H矩阵。 f ( x + Δ x ) = f ( x ) + J ( x ) T Δ x f(x +\Delta x) = f(x) + J(x)^T \Delta x f(x+Δx)=f(x)+J(x)TΔx。带入计算后使得导数为0可得到:
- J ( x ) J T ( x ) Δ x = − J ( x ) f ( x ) ⇒ H ( x ) Δ x = g ( x ) J(x)J^T(x) \Delta x = - J(x)f(x) \Rightarrow H(x)\Delta x = g(x) J(x)JT(x)Δx=−J(x)f(x)⇒H(x)Δx=g(x)
- 避免了计算 H H H导致的计算量过大的问题,使用 J ( x ) J T ( x ) J(x)J^T(x) J(x)JT(x)近似的 H ( x ) H(x) H(x)半正定肯出现奇异病态;近似的 H ( x ) H(x) H(x)只在 x x x附近效果不错,可能出现求解得到的 Δ x \Delta x Δx的步长过大,局部的近似不准确的问题。
LM优化算法
- 考虑到高斯牛顿法近似不准确,给定一个近似的信赖区间半径 μ \mu μ, ∣ ∣ D Δ x ∣ ∣ 2 ≤ μ ||D \Delta x||^2 \le μ ∣∣DΔx∣∣2≤μ, D D D为系数矩阵。根据 ρ = f ( x + Δ x ) − f ( x ) J T ( x ) Δ x \rho = \frac{f(x + \Delta x)-f(x)}{J^T(x)\Delta x} ρ=JT(x)Δxf(x+Δx)−f(x)指标判断 μ \mu μ的好坏, ρ \rho ρ约接近1近似效果好。 ρ \rho ρ大于一定值可以增加 μ \mu μ半径,反之亦然。
- 通过拉格朗日乘子法将收敛区间融合到目标函数中。求解无约束问题 L ( Δ x , λ ) = 1 2 ∥ f ( x ) + J T Δ x ∥ 2 + λ 2 ( ∥ D Δ x ∥ 2 − μ ) \mathcal{L}(\Delta x , \lambda) = \frac{1}{2}\left \| f(x) +J^T\Delta x \right \|^2 +\frac{\lambda}{2}(\left \| D\Delta x \right \|^2-\mu ) L(Δx,λ)=21 f(x)+JTΔx 2+2λ(∥DΔx∥2−μ),化简得到: ( H ( x ) + λ D T D ) Δ x = g ( x ) (H(x) + \lambda D^TD) \Delta x= g(x) (H(x)+λDTD)Δx=g(x)
- 相比于高斯牛顿法多了 λ D T D \lambda D^TD λDTD,可以将$ D^TD$近似为单位矩阵 I I I进行简化, λ \lambda λ比较小时,近似于高斯牛顿法;。 λ \lambda λ比较大的时候, λ I \lambda I λI占主要地位,近似于最速度下降法。
稀疏性 和 边缘化 Marginalizatio (Schur消元)
稀疏性
- 定义:通常来说求解 H Δ x = g H \Delta x = g HΔx=g时候, H = ∑ J i , j T J i , j H = \sum J_{i,j}TJ_{i,j} H=∑Ji,jTJi,j,如果按照先相机,后观测点的顺序,不同的两个观测点对应位置的矩阵块是为0的,这个就是一般所说H矩阵的稀疏性。
- 利用稀疏性可以加速矩阵计算,这个假设加速计算过程称为边缘化(Marginalization)或者Schur消元。
边缘化 Marginalizatio (Schur消元)
- H Δ x = g H \Delta x = g HΔx=g 可以写成分块的形式- [ B E E T C ] [ Δ x c Δ x p ] = [ v w ] \begin{bmatrix}B&E \\E^T&C\end{bmatrix} \begin{bmatrix}\Delta x_c\\\Delta x_p\end{bmatrix} = \begin{bmatrix}v\\w\end{bmatrix} [BETEC][ΔxcΔxp]=[vw]
- Schur消元:消除右上角的矩阵 E E E,方程变为 [ B − E C − 1 E T 0 E T C ] [ Δ x c Δ x p ] = [ v − E C − 1 w w ] \begin{bmatrix}B - EC^{-1}E^T&0 \\E^T&C\end{bmatrix} \begin{bmatrix}\Delta x_c\\\Delta x_p\end{bmatrix} = \begin{bmatrix}v -EC^{-1}w \\w\end{bmatrix} [B−EC−1ETET0C][ΔxcΔxp]=[v−EC−1ww]
- 求解:1.求解第一行 [ B − E C − 1 E T 0 ] [ Δ x c ] = [ v − E C − 1 w ] \begin{bmatrix}B - EC^{-1}E^T&0 \end{bmatrix} \begin{bmatrix}\Delta x_c\end{bmatrix} = \begin{bmatrix}v -EC^{-1}w \end{bmatrix} [B−EC−1ET0][Δxc]=[v−EC−1w]得到相机状态 Δ x c \Delta x_c Δxc。2.将 Δ x c \Delta x_c Δxc带入,通过第二行求解$\Delta x_p = C^{-1}\left (w -E^T \Delta x_c \right ) $
- 边缘化 Marginalizatio:是因为上说求解过程,可以看做是先固定 Δ x p \Delta x_p Δxp求解 Δ x c \Delta x_c Δxc,从概率上看是求解 P ( x c , x p ) = P ( x c ∣ x p ) P ( x p ) P(x_c,x_p) = P(x_c|x_p)P(x_p) P(xc,xp)=P(xc∣xp)P(xp),就是把 x p x_p xp边缘化。
优化库
g2o
优化器
求解器
顶点 优化变量
边 误差
信息矩阵
鲁棒核函数
ceres
滤波
异常值剔除策略
RANSAC算法
- 基本假设:内点数符合模型,外点不符合。内点数大于外点数目。
- 步骤:1.随机选N个点(至少N个点可以计算出模型参数),计算此次N个点拟合模型。2.计算当前模型的内点数目。3.统计内点数最大的模型参数。4.重复1-3步骤直至模型参数足够好(内点数目足够)或者到大最大迭代次数。
坐标变换与Eigen3实现
欧式变换
旋转
四元数
欧拉角
旋转矩阵
平移
坐标插值
Eigen::Quaterniond q0,q1;
Eigen::Quaterniond deltaq = q0.inverse()*q1;
double t0,t1;
double ratio = (t-t0)/(t1-t0);
Eigen::Quaterniond q = q0.slerp(ratio,deltaq);
李群李代数
-
为什么这么搞用李代数呢?为了求解位姿方便,一般是6自由度位姿,矩阵R是3自由度,但是是3行3列共9个参数,完后自身有一些性质本身还得加上一些约束 R R T = I , d e t ( R ) = 1 RR^T=I,det(R) = 1 RRT=I,det(R)=1。这样子不方便求解,使用李代数位姿6自由度6个参数,没有超参数,也不要加上自身的一些约束。
定义
- 李群:特殊正交群 S O 3 = { R ∈ R 3 × 3 ∣ R R T = I , d e t ( R ) = 1 } SO3 = \left \{R \in \mathbb{R}^{3\times 3}|RR^T = I,det(R) = 1 \right \} SO3={R∈R3×3∣RRT=I,det(R)=1},特殊欧式群 S E 3 = { T = [ R t 0 T 1 ] ∈ R 4 × 4 ∣ R ∈ S O ( 3 ) , T ∈ R 3 } SE3 = \left \{T = \begin{bmatrix} R&t \\ 0^T&1 \end{bmatrix} \in \mathbb{R}^{4\times 4}|R \in SO(3),T \in\mathbb{R}^{3} \right \} SE3={T=[R0Tt1]∈R4×4∣R∈SO(3),T∈R3}
- 李代数:一种指数映射。SO3和SE3使用 exp ( ) \exp() exp()的指数映射展开,并且根据 a ∧ a ∧ a ∧ = − a ∧ a^ \wedge a^ \wedge a^ \wedge = -a^ \wedge a∧a∧a∧=−a∧、 sin ( ) \sin() sin()、 cos ( ) \cos() cos()级数合并化简得到的。
SO3
- $R = \exp (\phi ^ \wedge) = exp (\theta a ^ \wedge) = I + \sin \theta a^ \wedge +(1-\cos \theta)a^ \wedge a^ \wedge $
其中 ϕ \phi ϕ表示旋转的三个维度, θ \theta θ是向量 ϕ \phi ϕ的模, a a a是向量 ϕ \phi ϕ的单位向量。
SE3
-
T
=
e
x
p
(
ξ
∧
)
=
[
R
(
I
+
1
−
cos
θ
θ
a
∧
+
θ
−
sin
θ
θ
a
∧
a
∧
)
ρ
0
T
1
]
T = exp(\xi ^ \wedge) = \begin{bmatrix} R & (I+ \frac{1-\cos\theta}{\theta}a^ \wedge + \frac{\theta-\sin\theta}{\theta}a^ \wedge a^ \wedge)\rho \\ 0^T & 1 \end{bmatrix}
T=exp(ξ∧)=[R0T(I+θ1−cosθa∧+θθ−sinθa∧a∧)ρ1]
其中, ϕ \phi ϕ表示旋转的三个维度, ρ \rho ρ表示平移的三个维度。
求导数
公式
- ∂ exp ( ϕ ∧ ) p ∂ ϕ = lim δ ϕ → 0 exp ( ϕ ∧ + δ ϕ ∧ ) − exp ( ϕ ∧ ) δ ϕ 左雅可比 = lim δ ϕ → 0 exp ( ( J l δ ϕ ) ∧ ) exp ( ϕ ∧ ) − exp ( ϕ ∧ ) δ ϕ 右雅可比 = lim δ ϕ → 0 exp ( ϕ ∧ ) exp ( ( J r δ ϕ ) ∧ ) − exp ( ϕ ∧ ) δ ϕ \begin{matrix} \frac{\partial\exp(\phi^\wedge)p}{\partial\phi} &=\lim_{\delta\phi\to0}\frac{\exp(\phi^\wedge+\delta\phi^\wedge)-\exp(\phi^\wedge)}{\delta\phi}\\ 左雅可比&=\lim_{\delta\phi\to0}\frac{\exp((J_l\delta\phi)^\wedge)\exp(\phi^\wedge)-\exp(\phi^\wedge)}{\delta\phi}\\ 右雅可比&=\lim_{\delta\phi\to0}\frac{\exp(\phi^\wedge)\exp((J_r\delta\phi)^\wedge)-\exp(\phi^\wedge)}{\delta\phi} \end{matrix} ∂ϕ∂exp(ϕ∧)p左雅可比右雅可比=limδϕ→0δϕexp(ϕ∧+δϕ∧)−exp(ϕ∧)=limδϕ→0δϕexp((Jlδϕ)∧)exp(ϕ∧)−exp(ϕ∧)=limδϕ→0δϕexp(ϕ∧)exp((Jrδϕ)∧)−exp(ϕ∧)
BCH近似
- exp ( △ ξ ∧ ) exp ( ξ ∧ ) ≈ exp ( ( J l − 1 △ ξ + ξ ) ∧ ) exp ( ξ ∧ ) exp ( △ ξ ∧ ) ≈ exp ( ( J r − 1 △ ξ + ξ ) ∧ ) \begin{aligned} \exp(\triangle\xi^\wedge)\exp(\xi^\wedge)\approx \exp((J_l^{-1}\triangle\xi + \xi)^\wedge)\\ \exp(\xi^\wedge)\exp(\triangle\xi^\wedge)\approx \exp((J_r^{-1}\triangle\xi + \xi)^\wedge) \end{aligned} exp(△ξ∧)exp(ξ∧)≈exp((Jl−1△ξ+ξ)∧)exp(ξ∧)exp(△ξ∧)≈exp((Jr−1△ξ+ξ)∧)
特点
- 直接对李群李代数求导数的形式会导致需要计算 J l J_l Jl或者 J r J_r Jr计算量较大所以一般使用扰动模型代替直接求导数的形式。
扰动模型求导数
- 左扰动:$\frac{\partial Rp}{\partial\varphi } = \lim_{\varphi \to 0} \frac{\exp(\varphi ^\wedge) \exp(\phi ^\wedge) - \exp(\phi ^\wedge)}{\varphi} $
- 右扰动 ∂ R p ∂ φ = lim φ → 0 exp ( ϕ ∧ ) exp ( φ ∧ ) − exp ( ϕ ∧ ) φ \frac{\partial Rp}{\partial\varphi } = \lim_{\varphi \to 0} \frac{\exp(\phi ^\wedge) \exp(\varphi ^\wedge)- \exp(\phi ^\wedge)}{\varphi} ∂φ∂Rp=limφ→0φexp(ϕ∧)exp(φ∧)−exp(ϕ∧)
- 左扰动右扰动都可以,只是要注意在更新SO3和SE3时,需要保持更新扰动与求导的一致性。
左扰动
- 正常求导数: ∂ T p ∂ δ ξ = [ I − ( R p + t ) ∧ 0 T 0 T ] \frac{\partial Tp}{\partial \delta \xi } = \begin{bmatrix} I& -(Rp+t)^\wedge \\ 0^T&0^T \end{bmatrix} ∂δξ∂Tp=[I0T−(Rp+t)∧0T]
- 对逆求导数: ∂ T − 1 p ∂ δ ξ = [ − R T R T ( p ) ∧ 0 T 0 T ] \frac{\partial T^{-1}p}{\partial \delta \xi } = \begin{bmatrix} -R^T& R^T(p)^\wedge \\ 0^T&0^T \end{bmatrix} ∂δξ∂T−1p=[−RT0TRT(p)∧0T]
- 左扰动求解完变化量 d x = [ d ϕ d ρ ] dx =\begin{bmatrix} d\phi &d\rho \end{bmatrix} dx=[dϕdρ],更新 T = exp ( d ξ ∧ ) ∗ T T =\exp(d\xi ^ \wedge) *T T=exp(dξ∧)∗T
右扰动
- 正常求导数: ∂ T p ∂ δ ξ = [ R − R p ∧ 0 T 0 T ] \frac{\partial Tp}{\partial \delta \xi } = \begin{bmatrix} R& -Rp^\wedge \\ 0^T&0^T \end{bmatrix} ∂δξ∂Tp=[R0T−Rp∧0T]
- 对逆求导数: ∂ T − 1 p ∂ δ ξ = [ − I ( R T p + R T t ) ∧ 0 T 0 T ] \frac{\partial T^{-1}p}{\partial \delta \xi } = \begin{bmatrix} -I& (R^Tp+R^Tt)^\wedge \\ 0^T&0^T \end{bmatrix} ∂δξ∂T−1p=[−I0T(RTp+RTt)∧0T]
- 右扰动求解完变化量 d x = [ d ϕ d ρ ] dx =\begin{bmatrix} d\phi &d\rho \end{bmatrix} dx=[dϕdρ],更新 T = T ∗ exp ( d ξ ∧ ) T =T*\exp(d\xi ^\wedge) T=T∗exp(dξ∧)
与直接求导数的区别
- 相加的微小变化量方式不一致,主要就是减少了计算量。
矩阵
求解齐次线性方程组的解
- 求解 A X = 0 AX = 0 AX=0中的 X X X
对矩阵 A A A做SVD分解,分解后 V V V的最后一列向量即为 A X = 0 AX = 0 AX=0的最小二乘解
求解 A T A A^TA ATA的特征向量和特征值,最小特征值对应的特征向量即为 A X = 0 AX = 0 AX=0的最小二乘解。
求解非齐次线性方程组的解
- 求解 A X = b AX = b AX=b中的 X X X
最小二乘解
- X = ( A T A ) − 1 A T b X = (A^TA)^{-1}A^Tb X=(ATA)−1ATb
矩阵相关
反对称矩阵
- $a^\wedge = A =\begin{bmatrix}
0 &-a_3 &a_2 \
a_3 &0 &-a_1 \
-a_2 &a_1 &0
\end{bmatrix} $
其中 a = [ a 1 , a 2 , a 3 ] T a = [a_1,a_2,a_3]^T a=[a1,a2,a3]T - 如果 a a a是单位向量则有: a ∧ a ∧ a ∧ = − a ∧ a^ \wedge a^ \wedge a^ \wedge = -a^ \wedge a∧a∧a∧=−a∧