VSLAM学习(一) 三维运动、相机模型、SLAM模型
目录
VSLAM学习(一) 三维运动、相机模型、SLAM模型
VSLAM学习(二) 非线性优化
VSLAM学习(三) 单目相机位姿估计
VSLAM学习(四) Bundle Adjustment
一、三维空间刚体运动
1.1 旋转矩阵
同一向量在不同坐标系下的坐标不同
假如有两组坐标系，他们的基分别为(e1,e2,e3)(\boldsymbol e_1,\boldsymbol e_2,\boldsymbol e_3)(e1,e2,e3)和(e1′,e2′,e3′)(\boldsymbol e_1',\boldsymbol e_2',\boldsymbol e_3')(e1′,e2′,e3′)，向量a\boldsymbol aa在这两组坐标系下的坐标分别为(a1,a2,a3)T(a_1,a_2,a_3)^{\mathrm T}(a1,a2,a3)T和(a1′,a2′,a3′)T(a_1',a_2',a_3')^{\mathrm T}(a1′,a2′,a3′)T，那么他们满足
(e1,e2,e3)(a1a2a3)=(e1′,e2′,e3′)(a1′a2′a3′)(\boldsymbol e_1,\boldsymbol e_2,\boldsymbol e_3) \begin{pmatrix} a_1 \\ a_2 \\ a_3 \end{pmatrix}= (\boldsymbol e_1',\boldsymbol e_2',\boldsymbol e_3') \begin{pmatrix} a_1' \\ a_2' \\ a_3' \end{pmatrix} (e1,e2,e3)⎝⎛a1a2a3⎠⎞=(e1′,e2′,e3′)⎝⎛a1′a2′a3′⎠⎞
即
a=(a1a2a3)=(e1Te2Te3T)(e1′,e2′,e3′)(a1′a2′a3′)=(e1Te1′e1Te2′e1Te3′e2Te1′e2Te2′e2Te3′e3Te1′e3Te2′e3Te3′)a′=defRa′\begin{aligned} \boldsymbol a= \begin{pmatrix} a_1 \\ a_2 \\ a_3 \end{pmatrix}&= \begin{pmatrix} \boldsymbol e_1^{\mathrm T} \\ \boldsymbol e_2^{\mathrm T} \\ \boldsymbol e_3^{\mathrm T} \end{pmatrix} (\boldsymbol e_1',\boldsymbol e_2',\boldsymbol e_3') \begin{pmatrix} a_1' \\ a_2' \\ a_3' \end{pmatrix} \\ &=\begin{pmatrix} \boldsymbol e_1^{\mathrm T}\boldsymbol e_1' & \boldsymbol e_1^{\mathrm T}\boldsymbol e_2' & \boldsymbol e_1^{\mathrm T}\boldsymbol e_3' \\ \boldsymbol e_2^{\mathrm T}\boldsymbol e_1' & \boldsymbol e_2^{\mathrm T}\boldsymbol e_2' & \boldsymbol e_2^{\mathrm T}\boldsymbol e_3' \\ \boldsymbol e_3^{\mathrm T}\boldsymbol e_1' & \boldsymbol e_3^{\mathrm T}\boldsymbol e_2' & \boldsymbol e_3^{\mathrm T}\boldsymbol e_3' \end{pmatrix} \boldsymbol a'\overset{\mathrm{def}}{=}\boldsymbol{Ra}' \end{aligned} a=⎝⎛a1a2a3⎠⎞=⎝⎛e1Te2Te3T⎠⎞(e1′,e2′,e3′)⎝⎛a1′a2′a3′⎠⎞=⎝⎛e1Te1′e2Te1′e3Te1′e1Te2′e2Te2′e3Te2′e1Te3′e2Te3′e3Te3′⎠⎞a′=defRa′
此处的R\boldsymbol RR即为a′\boldsymbol a'a′坐标系到a\boldsymbol aa坐标系的旋转矩阵
可以看出RRT=I\boldsymbol R\boldsymbol R^{\mathrm T}=\boldsymbol IRRT=I且det(R)=1\mathrm{det}(\boldsymbol R)=1det(R)=1
1.2 空间坐标变换
三维空间的刚体运动是由旋转与平移组成的，可以表示为
b=R1a+t1\boldsymbol{b}=\boldsymbol R_1\boldsymbol a+\boldsymbol t_1 b=R1a+t1
其中R1∈R3×3\boldsymbol R_1\in \mathbb{R}^{3\times3}R1∈R3×3是旋转矩阵，t1∈R3\boldsymbol t_1\in \mathbb{R}^{3}t1∈R3为平移量
但是这种表示方法在多坐标系转换时会很复杂：
b=R1a+t1,c=R2b+t2⇒c=R2(R1a+t1)+t2\boldsymbol{b}=\boldsymbol R_1\boldsymbol a+\boldsymbol t_1,\quad\quad \boldsymbol{c}=\boldsymbol R_2\boldsymbol b+\boldsymbol t_2 \\ \Rightarrow \boldsymbol{c}=\boldsymbol R_2(\boldsymbol R_1\boldsymbol a+\boldsymbol t_1)+\boldsymbol t_2 b=R1a+t1,c=R2b+t2⇒c=R2(R1a+t1)+t2
因此为了避免麻烦，可引入齐次坐标和变换矩阵
b=R1a+t1⇒(b1)=(R1t10T1)(a1)=defT1(a1)⇒b~=T1a~\boldsymbol{b}=\boldsymbol R_1\boldsymbol a+\boldsymbol t_1 \\ \Rightarrow \begin{pmatrix} \boldsymbol{b} \\ 1 \end{pmatrix}= \begin{pmatrix} \boldsymbol{R_1} & \boldsymbol{t_1} \\ \boldsymbol{0}^{\mathrm T} & 1 \end{pmatrix} \begin{pmatrix} \boldsymbol{a} \\ 1 \end{pmatrix} \overset{\mathrm{def}}{=} \boldsymbol{T}_1\begin{pmatrix} \boldsymbol{a} \\ 1 \end{pmatrix} \\ \Rightarrow \tilde{\boldsymbol b}=\boldsymbol{T}_1\tilde{\boldsymbol a} b=R1a+t1⇒(b1)=(R10Tt11)(a1)=defT1(a1)⇒b~=T1a~
那么连续转换就可以表示成
c~=T2b~=T2T1a~\tilde{\boldsymbol c}=\boldsymbol{T}_2\tilde{\boldsymbol b} =\boldsymbol{T}_2\boldsymbol{T}_1\tilde{\boldsymbol a} c~=T2b~=T2T1a~
1.3 旋转向量 与 罗德里格斯公式
除了可以用旋转矩阵R\boldsymbol RR来表示一个旋转以外
还可以用一个旋转轴和一个旋转角来表示一个旋转
旋转轴、角与旋转向量的关系可以由罗德里格斯公式(Rodrigues’s Formula)来表达：
R=cosθI+(1−cosθ)nnT+sinθn∧orR=I+(1−cosθ)(n∧)2+sinθn∧\boldsymbol R=\cos\theta\boldsymbol I+(1-\cos\theta)\boldsymbol n\boldsymbol n^{\mathrm T}+\sin\theta\boldsymbol n^{\land} \\ or \\ \boldsymbol R=\boldsymbol I+(1-\cos\theta)(\boldsymbol n^{\land})^2+\sin\theta\boldsymbol n^{\land} R=cosθI+(1−cosθ)nnT+sinθn∧orR=I+(1−cosθ)(n∧)2+sinθn∧
其中θ\thetaθ表示旋转角，n\boldsymbol nn表示旋转轴单位向量，n∧\boldsymbol n^{\land}n∧表示向量n\boldsymbol nn的反对称矩阵(skew-symmetric matrix)或叉积矩阵
也有很多其他地方将反对称矩阵记为n×\boldsymbol n_\timesn×
- 补充知识点：叉积矩阵
a×b=(ijka1a2a3b1b2b3)=(a2b3−a3b2a3b1−a1b3a1b2−a2b1)=(0−a3a2a30−a1−a2a10)b=defa∧⋅b\begin{aligned} \boldsymbol a\times\boldsymbol b&= \begin{pmatrix} \boldsymbol{i} & \boldsymbol{j} & \boldsymbol{k} \\ a_1 & a_2 & a_3 \\ b_1 & b_2 & b_3 \end{pmatrix}= \begin{pmatrix} a_2b_3-a_3b_2 \\ a_3b_1-a_1b_3 \\ a_1b_2-a_2b_1 \end{pmatrix} \\ &= \begin{pmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{pmatrix}\boldsymbol b \\ &\overset{\mathrm{def}}{=} \boldsymbol a^{\wedge}\cdot\boldsymbol b \end{aligned} a×b=⎝⎛ia1b1ja2b2ka3b3⎠⎞=⎝⎛a2b3−a3b2a3b1−a1b3a1b2−a2b1⎠⎞=⎝⎛0a3−a2−a30a1a2−a10⎠⎞b=defa∧⋅b
这里(0−a3a2a30−a1−a2a10)\displaystyle\begin{pmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{pmatrix}⎝⎛0a3−a2−a30a1a2−a10⎠⎞称为向量 a\boldsymbol aa 的叉积矩阵，或者反对称矩阵，记为 a∧\boldsymbol a^{\wedge}a∧ 或者 a×\boldsymbol a_\timesa×
同时我们可以看出叉积矩阵的性质：(a∧)2=aaT−I,(a∧)3=−a∧(\boldsymbol a^{\land})^2=\boldsymbol a\boldsymbol a^T-\boldsymbol I,\quad(\boldsymbol a^{\land})^3=-\boldsymbol a^{\land}(a∧)2=aaT−I,(a∧)3=−a∧
由此可见，使用向量ϕ=θn\boldsymbol \phi=\theta\boldsymbol nϕ=θn同样可以描述一个三维旋转R\boldsymbol RR
ϕ\boldsymbol \phiϕ 与 R\boldsymbol RR 的具体关系可以学习李代数与李群
特殊地，
当 θ\thetaθ 很小的时候，sinθ∼θ,1−cosθ∼o(θ)\sin\theta\sim\theta,~~1-\cos\theta\sim o(\theta)sinθ∼θ, 1−cosθ∼o(θ)
那么罗德里格斯公式可以表示为：
R≈I+θn∧=(1−ϕ3ϕ2ϕ31−ϕ1−ϕ2ϕ11)\boldsymbol R\approx\boldsymbol I+\theta\boldsymbol n^{\land}= \begin{pmatrix} 1 & -\phi_3 & \phi_2 \\ \phi_3 & 1 & -\phi_1 \\ -\phi_2 & \phi_1 & 1 \end{pmatrix} R≈I+θn∧=⎝⎛1ϕ3−ϕ2−ϕ31ϕ1ϕ2−ϕ11⎠⎞
二、相机模型
2.1 针孔相机模型
针孔相机的成像原理就是小孔成像
(图片制作中…)
为了计算方便，一般把成像平面翻转到物体同一侧
Zf=XX′=YY′\frac{Z}{f}=\frac{X}{X'}=\frac{Y}{Y'} fZ=X′X=Y′Y
我们将成像的像素坐标记为(x,y)T(x,y)^T(x,y)T
图片中心的像素坐标记为(cx,cy)T(c_x,c_y)^T(cx,cy)T
成像图片的像素与实际世界长度的比例记为横轴比例：α\alphaα，纵轴比例：β\betaβ
那么有
{x=αX′+cxy=βY′+cy\left\{ \begin{array}{l} x=\alpha X'+c_x \\ y=\beta Y'+c_y \end{array} \right. {x=αX′+cxy=βY′+cy
整合之前关系式，记fx=αf,fy=βff_x=\alpha f,\quad f_y=\beta ffx=αf,fy=βf
{x=fxXZ+cxy=fyYZ+cy\left\{ \begin{array}{l} x=f_x\frac{X}{Z}+c_x \\ y=f_y\frac{Y}{Z}+c_y \end{array} \right. {x=fxZX+cxy=fyZY+cy
整理为齐次坐标
(xy1)=1Z(fx0cx0fycy001)(XYZ)\begin{pmatrix} x \\ y \\ 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} ⎝⎛xy1⎠⎞=Z1⎝⎛fx000fy0cxcy1⎠⎞⎝⎛XYZ⎠⎞
矩阵(fx0cx0fycy001)\begin{pmatrix} f_x & 0 & c_x \\ 0 & f_y & c_y \\ 0 & 0 & 1 \end{pmatrix}⎝⎛fx000fy0cxcy1⎠⎞叫做相机内参矩阵，暂记为K\boldsymbol KK，即
Zppixel=Kpcamera=K(Rpworld+t)=KTcwpworldZ\boldsymbol p_{pixel}=\boldsymbol K\boldsymbol p_{camera}=\boldsymbol K(\boldsymbol R\boldsymbol p_{world}+\boldsymbol t)=\boldsymbol K\boldsymbol T_{cw}\boldsymbol p_{world} Zppixel=Kpcamera=K(Rpworld+t)=KTcwpworld
(第一步和最后一步都隐含了齐次转换，等号并不严谨，理解意思即可)
2.2 图像畸变
图像畸变一般分为径向畸变和切向畸变
2.2.1 径向畸变
我们使用p′=(x′,y′)T\boldsymbol p'=(x',y')^{\mathrm T}p′=(x′,y′)T表示发生畸变以后图片上的坐标
用p=(x,y)T\boldsymbol p=(x,y)^{\mathrm T}p=(x,y)T表示真实应该在的图片上的位置坐标
则
{x=x′(1+k1r2+k2r4+k3r6+…)y=y′(1+k1r2+k2r4+k3r6+…)\left\{ \begin{aligned} x&=x'(1+k_1r^2+k_2r^4+k_3r^6+\dots) \\ y&=y'(1+k_1r^2+k_2r^4+k_3r^6+\dots) \end{aligned} \right. {xy=x′(1+k1r2+k2r4+k3r6+…)=y′(1+k1r2+k2r4+k3r6+…)
其中r=∥p′∥2=x′2+y′2r=\|\boldsymbol p'\|_2=\sqrt{x'^2+y'^2}r=∥p′∥2=x′2+y′2
kkk为参数，畸变越大的相机需要的参数越多。
2.2.2 切向畸变
沿用上面的符号
{x=x′+2p1x′y′+p2(r2+2x′2)y=y′+2p2x′y′+p1(r2+2y′2)\left\{ \begin{aligned} x&=x'+2p_1x'y'+p_2(r^2+2x'^2) \\ y&=y'+2p_2x'y'+p_1(r^2+2y'^2) \end{aligned} \right. {xy=x′+2p1x′y′+p2(r2+2x′2)=y′+2p2x′y′+p1(r2+2y′2)
p1,p2p_1,p_2p1,p2为参数
2.2.3 混合畸变
其实就是把两种畸变加起来即可
{x=x′(1+k1r2+k2r4+k3r6+…)+2p1x′y′+p2(r2+2x′2)y=y′(1+k1r2+k2r4+k3r6+…)+2p2x′y′+p1(r2+2y′2)\left\{ \begin{aligned} x&=x'(1+k_1r^2+k_2r^4+k_3r^6+\dots)+2p_1x'y'+p_2(r^2+2x'^2) \\ y&=y'(1+k_1r^2+k_2r^4+k_3r^6+\dots)+2p_2x'y'+p_1(r^2+2y'^2) \end{aligned} \right. {xy=x′(1+k1r2+k2r4+k3r6+…)+2p1x′y′+p2(r2+2x′2)=y′(1+k1r2+k2r4+k3r6+…)+2p2x′y′+p1(r2+2y′2)
三、SLAM模型
3.1 运动观测方程
xk\boldsymbol x_kxk是指kkk时刻的机器人位姿（VSLAM中一般是相机的位姿）
zk,j\boldsymbol z_{k,j}zk,j是指kkk时刻对第jjj个路标点的观测值（VSLAM中一般是特征点在照片上的像素点坐标）
{xk=f(xk−1,uk)+wk⋅⋅⋅运动方程zk,j=h(yj,xk)+vk,j⋅⋅⋅观测方程\left \{ \begin{aligned} \boldsymbol x_k&=f(\boldsymbol x_{k-1},\boldsymbol u_k)+\boldsymbol w_k ~~~~~~~~~&···运动方程\\ \boldsymbol z_{k,j}&=h(\boldsymbol y_j,\boldsymbol x_k)+\boldsymbol v_{k,j} &···观测方程 \end{aligned} \right. {xkzk,j=f(xk−1,uk)+wk =h(yj,xk)+vk,j⋅⋅⋅运动方程⋅⋅⋅观测方程
其中，uk\boldsymbol u_kuk是指kkk时刻的运动传感器的读数或者输入
wk\boldsymbol w_kwk是指kkk时刻的运动噪声
yj\boldsymbol y_jyj表示第jjj个路标点
vk,j\boldsymbol v_{k,j}vk,j表示kkk时刻第jjj个路标点的观测噪声
f(⋅)f(·)f(⋅)是运动函数
h(⋅)h(·)h(⋅)是观测函数
3.2 投影模型
观测函数h(⋅)h(·)h(⋅)是一个物体从世界到相机图片的投影过程，该过程分为以下几步：
我们先记某一物体在世界坐标系下的坐标为Pw\boldsymbol P_wPw，现在我们要将其投影到相机图片中的像素位置。
再记该物体在相机坐标系下的坐标和归一化坐标分别为Pc′\boldsymbol P_c'Pc′和Pc\boldsymbol P_cPc
根据三维空间旋转，我们知道
Pc′=RwcPw+t=(XcYcZc)\boldsymbol P_c'=\boldsymbol R_{wc}\boldsymbol P_w+\boldsymbol t= \begin{pmatrix} X_c \\ Y_c \\ Z_c \end{pmatrix} Pc′=RwcPw+t=⎝⎛XcYcZc⎠⎞
对相机坐标归一化
Pc=(ucvc1)=(Xc/ZcYc/Zc1)\boldsymbol P_c= \begin{pmatrix} u_c \\ v_c \\ 1 \end{pmatrix}= \begin{pmatrix} X_c/Z_c \\ Y_c/Z_c \\ 1 \end{pmatrix} Pc=⎝⎛ucvc1⎠⎞=⎝⎛Xc/ZcYc/Zc1⎠⎞
在对相机的畸变进行处理（这里以镜像畸变为例）
{uc′=uc(1+k1rc2+k2rc4)vc′=vc(1+k1rc2+k2rc4)\left\{ \begin{aligned} u_c'=u_c(1+k_1r_c^2+k_2r_c^4) \\ v_c'=v_c(1+k_1r_c^2+k_2r_c^4) \end{aligned} \right. {uc′=uc(1+k1rc2+k2rc4)vc′=vc(1+k1rc2+k2rc4)
最后在根据相机内参算出函数h(⋅)h(·)h(⋅)最终结果（齐次变换最后一行的1我省去了）：
(uv)=K(uc′vc′)=(fxuc′+cxfyvc′+cy)\begin{pmatrix} u \\ v \end{pmatrix}= \boldsymbol K \begin{pmatrix} u_c' \\ v_c' \end{pmatrix}= \begin{pmatrix} f_xu_c'+c_x \\ f_yv_c'+c_y \end{pmatrix} (uv)=K(uc′vc′)=(fxuc′+cxfyvc′+cy)
3.3 优化
我们所使用的设备和所在的环境都是存在噪声的，所以为导致仪器的测量数据不准确，需要使用算法进行优化。
一般的优化方法有两种：
- 滤波器
- 非线性优化
若wk\boldsymbol w_kwk与vk,j\boldsymbol v_{k,j}vk,j都是高斯白噪声，且观测方程h(⋅)\boldsymbol h(·)h(⋅)是线性的，那么可以直接使用滤波器来进行优化。
现在平面SLAM很多使用的还是滤波器的原理，例如Gmapping
而三维的视觉SLAM，一般则需要使用非线性优化。
VSLAM学习(一) 三维运动、相机模型、SLAM模型相关推荐
- 视觉SLAM十四讲学习笔记-第五讲-相机模型
专栏系列文章如下: 视觉SLAM十四讲学习笔记-第一讲_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习笔记-第二讲-初识SLAM_goldqiu的博客-CSDN博客 视觉SLAM十四讲学习 ...
- AI学习笔记（六）三维计算机视觉与点云模型
AI学习笔记之三维计算机视觉与点云模型 立体视觉 立体视觉的概念 立体视觉的原理 单目系统 双目系统和视差 对极几何约束 SIFT sift特征的特点 sift算法总体介绍 sift特征提取和匹配具体 ...
- 视觉SLAM十四讲学习笔记——第五讲相机与图像
文章目录 前言 一.相机模型 针孔相机模型 畸变模型 双目相机模型 RGB-D相机模型 二.图像 三.代码复现 总结 前言 研0学生,放假宅家,本科期间对其感兴趣,故开始学习. 一个人学有点无聊,可以 ...
- SLAM学习之路---双目相机照片生成点云（附C++代码）
一.概述 本文记录如何根据双目相机得到的两帧图片数据来计算出三维点云.这里主要涉及到双目视觉的成像原理,在<视觉SLAM十四讲>第2版的第五讲中有详细介绍.本文的代码根据书上给出的代码进行 ...
- 【三维深度学习】基于片元的渐进式三维点云上采样模型
点云上采样对于从稀疏三维数据重建稠密三维点云十分有效.但面对非规则.无需.稀疏.噪声和不完整的点云结构,图像领域的超分辨.补全.稀疏加密等方法无法直接用于点云上采样中.PointNet系列方法基于全连 ...
- 深度学习在三维环境重建中的应用
点击上方"小白学视觉",选择加"星标"或"置顶" 重磅干货,第一时间送达 本文转自:计算机视觉联盟 最经典的计算机视觉问题是3-D重建.基本 ...
- 基于鱼眼相机的SLAM方法介绍
本文是一篇关于鱼眼相机的SLAM的介绍以及开源demo体验的介绍,希望有兴趣的小伙伴能够自行体验,并积极分享相关内容.欢迎交流和讨论,联系邮箱:dianyunpcl@163.com {MultiCol ...
- 基于深度学习的三维语义理解（分割）综述列表
基于深度学习的三维语义理解(分割)综述列表 文章目录 基于深度学习的三维语义理解(分割)综述列表 前言 基于深度学习的三维语义理解(分割)综述列表 一. 从单一三维模型中进行深度学习 1.1基于点云的 ...
- 运动规划和SLAM什么关系？
原文链接:运动规划和SLAM什么关系? 前言 SLAM知识星球里经常有小伙伴问我,学习了SLAM开源代码后,如何进行导航.星球里我是这样回复的: 这里解释下运动规划和SLAM什么关系? 其实在企业里, ...
最新文章
- sql 多行转成一行
- App.config的典型应用
- pythongps聚类_python实现K聚类算法
- Unity--------------------万向锁的概念
- 推荐：腾讯开源的词向量精简版本下载
- 如何通过 C# 比较两幅图片的相似度？
- python input 文件路径_python – 将目录路径作为用户输入的正确方法是什么？
- 通过管理门户部署云服务与VIP交换
- 更新管理器_Excel2016数据透视表06：名称管理器实现透视表的动态更新
- 情人节程序员用HTML网页表白【时光不老,我们不散love520】 HTML5七夕情人节表白网页源码 HTML+CSS+JavaScript
- Windows C++界面库
- 安卓游戏广告加速插件_游戏加速器（强烈推荐！）[PC+安卓]
- matlab拟合例子,MATLAB数据拟合例子
- 现在国内程序员的工资是什么行情？
- idea 替换 choose initializr service URL.
- Memcached完整教程
- Editor编程 GUILayout为什么控件一多一复杂性能就差的不行？
- 读王蒙的《庄子的快活》
- WQ7033开发指南（基础篇）之1.2 烧录固件详解
- 百度云主机BCC挂载云盘CDS