Simon's《SLAM 14讲》笔记
本人小白一枚,因选了搭建3D虚拟环境的课题需要学习SLAM相关知识,被迫在一个星期之内学完了《SLAM 14讲》,从此感觉到了SLAM的有趣和神奇。为了时常能温习书中所涉及的原理与模型,也为了能帮助广大刚入这个坑的小白们,便想写这篇书中每章内容的摘要与一些知识的理解。
2. 第二讲:初识SLAM
- 视觉SLAM框架
- Linux下程序的编译与运行
2.1. SLAM框架
- 视觉里程计(Visual Odometry,VO):前端,估算相邻图像间相机的运动。
- 非线性优化(Optimization):后端,优化得到的相机位姿。
- 回环检测(Loop Closing):判断相机是否到过先前位置,传给后端以修正误差。
- 建图(Mapping):根据估计轨迹建立所需的地图。
2.2. Linux下程序的编译
3. 第三讲:三维空间刚体运动
- 刚体在三维空间里的旋转
- 刚体在三维空间里的旋转加平移
本章代码使用了Eigen须在 “.cpp” 文件开头加上:
#include <Eigen/Core>
#include <Eigen/Dense>
#include <Eigen/Geometry>
3.1. 刚体在三维空间里的旋转
Eigen中矩阵的基本操作:
Eigen::Matrix<type, row, colon> //typedef: Matrix3d, Vector3d...
Eigen::MatrixXd //unkown size matrix
matrix(i, j) //the value of i th row and j th colon
//the operations
matrix.transpose(); matrix.trace(); matrix.sum(); matrix.inverse(); matrix.determinant();
//find the proper values and vectors
Eigen::SelfAdjointEigenSolver<Eigen::Matrix3d> solver(matrix.transport()*matrix);
solver.eigenvalues();
solver.eigenvectors();
//solve Matrix_NN * x = v_Nd
x = matrix_NN.colpivHouseholderQr().solve(v_Nd);
基本数学知识:
[
x
y
z
]
T
=
p
,
p
Λ
=
[
0
−
z
y
z
0
−
x
−
y
x
0
]
[x\ \ y\ \ z]^T=\textbf{p},\ \ \textbf{p}^\Lambda=\begin{bmatrix}0&-z&y\\z&0&-x\\-y&x&0\end{bmatrix}
[x y z]T=p, pΛ=⎣⎡0z−y−z0xy−x0⎦⎤
3.1.1. 旋转矩阵 R \textbf{R} R
设刚体上某点 x = [ x 1 , x 2 , x 3 ] T \textbf{x}=[x_1, x_2, x_3]^T x=[x1,x2,x3]T,在经过旋转之后变成 x ′ = [ x 1 ′ , x 2 ′ , x 3 ′ ] T \textbf{x}'=[x_1', x_2', x_3']^T x′=[x1′,x2′,x3′]T,我们可以用旋转矩阵 R \textbf{R} R 来表示该旋转,则有 x ′ = R . x \textbf{x}'=\textbf{R}.\textbf{x} x′=R.x。
旋转矩阵 R \textbf{R} R 是一个正交矩阵(Matrice Orthogonale),即有性质 R T R = I \textbf{R}^T\textbf{R}=\textbf{I} RTR=I.
3.1.2. 旋转向量 θ n \theta\textbf{n} θn
旋转向量 θ n \theta\textbf{n} θn意为刚体绕 n \textbf{n} n 旋转 θ \theta θ 。根据 Rodrigues’s Formula , 旋转矩阵与旋转向量的转化关系如下:
R = c o s ( θ ) I + ( 1 − c o s ( θ ) n n T + s i n ( θ ) n Λ ) \textbf{R}=cos(\theta)\textbf{I}+(1-cos(\theta)\textbf{n}\textbf{n}^T+sin(\theta)\textbf{n}^\Lambda) R=cos(θ)I+(1−cos(θ)nnT+sin(θ)nΛ)
{ t r ( R ) = 1 + 2 c o s ( θ ) Rn = n \left\{\begin{matrix} tr(\textbf{R})=1+2cos(\theta)\\ \textbf{R}\textbf{n}=\textbf{n} \end{matrix}\right. {tr(R)=1+2cos(θ)Rn=n
Eigen::AngleAxisd rotation_vector(theta, Eigen::Vector3d(x, y, z)); //rotation vector
rotation_matrix = rotation_vector.toRotationMatrix(); //transform rotation vector to rotation matrix
x_rotated = rotation_vector*x; //calculate rotated vector x'
x_rotated = rotation_matrix*x; //calculate rotated vector x'
3.1.3. 四元数(Quaternion)
以下与书中相同,设四元数第一项为实数项。设在三维空间某一点 p = [ 0 , x , y , z ] \textbf{p}=[0, x, y, z] p=[0,x,y,z] ,旋转向量 θ n \theta\textbf{n} θn 可记为 q = [ c o s ( θ ) , n s i n ( θ ) ] \textbf{q}=[cos(\theta), \textbf{n}sin(\theta)] q=[cos(θ),nsin(θ)]。
则旋转后的向量 p ′ = qp q − 1 \textbf{p}'=\textbf{q}\textbf{p}\textbf{q}^{-1} p′=qpq−1。
四元数与旋转矩阵的转换关系如下(并不唯一):
设 q = q 0 + q 1 i + q 2 j + q 3 k \textbf{q}=q_0+q_1\textbf{i}+q_2\textbf{j}+q_3\textbf{k} q=q0+q1i+q2j+q3k
则有 R = [ 1 − 2 q 2 2 − q 3 2 2 q 1 q 2 + 2 q 0 q 3 2 q 1 q 3 − 2 q 0 q 2 2 q 1 q 2 − 2 q 0 q 3 1 − 2 q 1 2 − 2 q 3 2 2 q 2 q 3 + 2 q 0 q 1 2 q 1 q 3 + 2 q 0 q 2 2 q 2 q 3 − 2 q 0 q 1 1 − 2 q 1 2 − 2 q 2 2 ] \textbf{R}=\begin{bmatrix} 1-2q_2^2-q_3^2 & 2q_1q_2+2q_0q_3 & 2q_1q_3-2q_0q_2 \\ 2q_1q_2-2q_0q_3 & 1-2q_1^2-2q_3^2 & 2q_2q_3+2q_0q_1 \\2q_1q_3+2q_0q_2 & 2q_2q_3-2q_0q_1 & 1-2q_1^2-2q_2^2\end{bmatrix} R=⎣⎡1−2q22−q322q1q2−2q0q32q1q3+2q0q22q1q2+2q0q31−2q12−2q322q2q3−2q0q12q1q3−2q0q22q2q3+2q0q11−2q12−2q22⎦⎤
或者 q 0 = t r ( R ) + 1 2 , q 1 = m 23 − m 32 4 q 0 , q 2 = m 31 − m 13 4 q 0 , q 3 = m 12 − m 21 4 q 0 q_0=\frac{\sqrt{tr(R)+1}}{2},q_1=\frac{m_{23}-m_{32}}{4q_0},q_2=\frac{m_{31}-m_{13}}{4q_0},q_3=\frac{m_{12}-m_{21}}{4q_0} q0=2tr(R)+1,q1=4q0m23−m32,q2=4q0m31−m13,q3=4q0m12−m21
q = Eigen::Quaterniond(w, x, y, z) //create a quaternion from four values
q = Eigen::Quaterniond(rotation_vector) //create a quaternion from rotaion vector
q = Eigen::Quaterniond(rotation_matrix) //create a quaternion from rotaion matrix
x_rotated = q * x //calculate rotated vector x'
3.2. 刚体在三维空间里的旋转加平移
3.2.1. 变换矩阵 T \textbf{T} T
设刚体上某点 x = [ x 1 , x 2 , x 3 ] T \textbf{x}=[x_1, x_2, x_3]^T x=[x1,x2,x3]T,在经过旋转平移之后变成 x ′ = [ x 1 ′ , x 2 ′ , x 3 ′ ] T \textbf{x}'=[x_1', x_2', x_3']^T x′=[x1′,x2′,x3′]T,则有 x ′ = Rx + t \textbf{x}'=\textbf{R}\textbf{x}+\textbf{t} x′=Rx+t,这里的 t \textbf{t} t 为平移向量。
我们记 T = [ R t 0 T 1 ] \textbf{T}=\begin{bmatrix} \textbf{R} & \textbf{t} \\ \textbf{0}^T & 1\end{bmatrix} T=[R0Tt1],然后把 x ′ \textbf{x}' x′ 与 x \textbf{x} x 写成齐次坐标的形式, 即 x = [ x 1 , x 2 , x 3 , 1 ] T , x ′ = [ x 1 ′ , x 2 ′ , x 3 ′ , 1 ] T \textbf{x}=[x_1, x_2,x_3,1]^T,\textbf{x}'=[x_1', x_2', x_3',1]^T x=[x1,x2,x3,1]T,x′=[x1′,x2′,x3′,1]T,则有 x ′ = Tx \textbf{x}'=\textbf{T}\textbf{x} x′=Tx。
//create a transform matrix
Eigen::Isometry3d T = Eigen::Isometry3d::Identity();
T.rotate(rotation_vector);
/*
T.rotate(rotation_matrix);
T.rotate(quaternion);
*/
T.pretranslate(Eigen::Vector3d(x, y, z));
4. 第四讲:李群与李代数
- 李群与李代数
- BCH公式与近似形式
- 扰动模型(左扰动)
本章代码使用了Eigen须在 “.cpp” 文件开头加上:
#include "sophus/so3.h"
#include "sophus/se3.h"
4.1. 李群与李代数
三维旋转矩阵构成了特殊正交群:
S
O
(
3
)
=
{
R
∈
R
3
×
3
∣
R
T
R
=
I
,
d
e
t
(
R
)
=
1
}
SO(3)=\{\textbf{R}\in\mathbb{R}^{3\times3} | \textbf{R}^T\textbf{R}=\textbf{I},det(\textbf{R})=1\}
SO(3)={R∈R3×3∣RTR=I,det(R)=1},
变换矩阵构成了特殊欧式群:
S
E
(
3
)
=
{
T
=
[
R
t
0
T
1
]
∈
R
4
×
4
∣
R
∈
S
O
(
3
)
,
t
∈
R
3
}
SE(3)=\{\textbf{T}=\begin{bmatrix} \textbf{R} & \textbf{t} \\ \textbf{0}^T & 1\end{bmatrix}\in\mathbb{R}^{4\times4}|\textbf{R}\in SO(3), \textbf{t} \in \mathbb{R}^3\}
SE(3)={T=[R0Tt1]∈R4×4∣R∈SO(3),t∈R3},
一个小公式: a Λ = A = [ 0 − a 3 a 2 a 3 0 − a 1 − a 2 a 1 0 ] , A V = a \textbf{a}^\Lambda=\textbf{A}=\begin{bmatrix}0 & -a_3 & a_2\\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0\end{bmatrix}, \textbf{A}^\textbf{V} =\textbf{a} aΛ=A=⎣⎡0a3−a2−a30a1a2−a10⎦⎤,AV=a
李代数:
s
o
(
3
)
=
{
ϕ
∈
R
3
,
Φ
=
ϕ
Λ
∈
R
3
×
3
}
so(3)=\{\phi\in\mathbb{R}^3,\Phi=\phi^\Lambda\in\mathbb{R}^{3\times3}\}
so(3)={ϕ∈R3,Φ=ϕΛ∈R3×3},
s
e
(
3
)
=
{
ξ
=
[
ρ
ϕ
]
∈
R
6
,
ρ
∈
R
3
,
ϕ
∈
s
o
(
3
)
,
ξ
Λ
=
[
ϕ
Λ
ρ
0
T
0
]
∈
R
4
×
4
}
se(3)=\{\xi=\begin{bmatrix} \rho \\ \phi\end{bmatrix}\in\mathbb{R}^6, \rho\in \mathbb{R}^3,\phi\in so(3), \xi^\Lambda=\begin{bmatrix}\phi^\Lambda & \rho \\ \textbf{0}^T & 0\end{bmatrix}\in\mathbb{R}^{4\times4}\}
se(3)={ξ=[ρϕ]∈R6,ρ∈R3,ϕ∈so(3),ξΛ=[ϕΛ0Tρ0]∈R4×4}
指数和对数映射:
S
O
(
3
)
SO(3)
SO(3) 与
s
o
(
3
)
so(3)
so(3):
- e x p ( θ a Λ ) = c o s ( θ ) I + ( 1 − c o s ( θ ) a a T + s i n ( θ ) a Λ ) exp(\theta\textbf{a}^\Lambda)=cos(\theta)\textbf{I}+(1-cos(\theta)\textbf{a}\textbf{a}^T+sin(\theta)\textbf{a}^\Lambda) exp(θaΛ)=cos(θ)I+(1−cos(θ)aaT+sin(θ)aΛ)
- θ = a r c c o s t r ( R ) − 1 2 , Ra = a \theta=arccos\frac{tr(\textbf{R})-1}{2}, \textbf{R}\textbf{a}=\textbf{a} θ=arccos2tr(R)−1,Ra=a
S E ( 3 ) SE(3) SE(3) 与 s e ( 3 ) se(3) se(3):
- e x p ( ξ Λ ) = [ e x p ( ϕ Λ ) J ρ 0 T 1 ] , J = s i n ( θ ) θ I + ( 1 − s i n ( θ ) θ ) a a T + 1 − c o s ( θ ) θ a Λ exp(\xi^\Lambda)=\begin{bmatrix}exp(\phi^\Lambda) & J\rho \\ \textbf{0}^T & 1\end{bmatrix},J=\frac{sin(\theta)}{\theta}\textbf{I}+(1-\frac{sin(\theta)}{\theta})\textbf{a}\textbf{a}^T+\frac{1-cos(\theta)}{\theta}\textbf{a}^\Lambda exp(ξΛ)=[exp(ϕΛ)0TJρ1],J=θsin(θ)I+(1−θsin(θ))aaT+θ1−cos(θ)aΛ
- θ = a r c c o s t r ( R ) − 1 2 , Ra = a , t = J ρ \theta=arccos\frac{tr(\textbf{R})-1}{2}, \textbf{R}\textbf{a}=\textbf{a},\textbf{t}=J\rho θ=arccos2tr(R)−1,Ra=a,t=Jρ
Sophus::SO3 SO3_R(R); //construct SO_R by a rotation matrix
Sophus::SO3 SO3_v(0, 0, M_PI/2); //construct SO_R by a rotation vector, using <cmath> std::M_PI/2
Sophus::SO3 SO3_q(q); //construct SO_R by a quaternion
Eigen::Vector3d so3 = SO3_R.log(); //logarithm map
Sophus::SO3::hat(so3); //so3^
Sophus::SO3::vee(SO3); //SO3v
Sophus::SO3::exp(so3); //exp(so3^)
Sophus::SE3 SE3_Rt(R, t); //construct SE_Rt by a rotation matrix and a translation vector(Eigen::Vector3d)
Sophus::SE3 SE3_qt(q, t); //construct SE_Rt by a quaternion and a translation vector
//how to update
Eigen::Matrix<double, 6, 1> update_se3;
update_se3.setZero();
update_se3(0, 0) = 1e-4d;
Sophus::SE3 SE3_updated = Sophus::SE3::exp(update_se3)*SE3_Rt;
4.2. BCH公式与近似模型
BCH近似公式:
l
n
(
e
x
p
(
ϕ
1
Λ
)
e
x
p
(
ϕ
2
Λ
)
)
V
≈
{
J
l
(
ϕ
2
)
−
1
ϕ
1
+
ϕ
2
)
i
f
ϕ
1
i
s
s
m
a
l
l
J
r
(
ϕ
1
)
−
1
ϕ
2
+
ϕ
1
)
i
f
ϕ
2
i
s
s
m
a
l
l
ln(exp(\phi_1^\Lambda)exp(\phi_2^\Lambda))^{\textbf{V}}\approx \left\{\begin{matrix} J_l(\phi_2)^{-1}\phi_1+\phi2) \ if \ \phi_1\ is\ small\ \\ J_r(\phi_1)^{-1}\phi_2+\phi1) \ if \ \phi_2\ is\ small\ \end{matrix}\right.
ln(exp(ϕ1Λ)exp(ϕ2Λ))V≈{Jl(ϕ2)−1ϕ1+ϕ2) if ϕ1 is small Jr(ϕ1)−1ϕ2+ϕ1) if ϕ2 is small
J l = J = s i n ( θ ) θ I + ( 1 − s i n ( θ ) θ ) a a T + 1 − c o s ( θ ) θ a Λ J_l=J=\frac{sin(\theta)}{\theta}\textbf{I}+(1-\frac{sin(\theta)}{\theta})\textbf{a}\textbf{a}^T+\frac{1-cos(\theta)}{\theta}\textbf{a}^\Lambda Jl=J=θsin(θ)I+(1−θsin(θ))aaT+θ1−cos(θ)aΛ,
J l − 1 = θ 2 c o t ( θ 2 ) I + ( 1 − θ 2 c o t ( θ 2 ) ) a a T − θ 2 a Λ J_l^{-1}=\frac{\theta}{2}cot(\frac{\theta}{2})\textbf{I}+(1-\frac{\theta}{2}cot(\frac{\theta}{2}))\textbf{a}\textbf{a}^T-\frac{\theta}{2}\textbf{a}^\Lambda Jl−1=2θcot(2θ)I+(1−2θcot(2θ))aaT−2θaΛ,
J r ( ϕ ) = J l ( − ϕ ) J_r(\phi)=J_l(-\phi) Jr(ϕ)=Jl(−ϕ)
微小扰动公式:
e x p ( Δ ϕ Λ ) e x p ( ϕ Λ ) = e x p ( ( ϕ + J l − 1 ( ϕ ) Δ ϕ ) Λ ) exp(\Delta\phi^\Lambda)exp(\phi^\Lambda)=exp((\phi+J_l^{-1}(\phi)\Delta\phi)^\Lambda) exp(ΔϕΛ)exp(ϕΛ)=exp((ϕ+Jl−1(ϕ)Δϕ)Λ)
e x p ( ( ϕ + Δ ϕ ) Λ ) = e x p ( ( J l Δ ϕ ) Λ ) e x p ( ϕ Λ ) = e x p ( ϕ Λ ) e x p ( ( J r Δ ϕ ) Λ ) exp((\phi+\Delta\phi)^\Lambda)=exp((J_l\Delta\phi)^\Lambda)exp(\phi^\Lambda)=exp(\phi^\Lambda)exp((J_r\Delta\phi)^\Lambda) exp((ϕ+Δϕ)Λ)=exp((JlΔϕ)Λ)exp(ϕΛ)=exp(ϕΛ)exp((JrΔϕ)Λ)
e x p ( Δ ξ Λ ) e x p ( ξ Λ ) = e x p ( ( ξ + J l − 1 Δ ξ ) Λ ) exp(\Delta\xi^\Lambda)exp(\xi^\Lambda)=exp((\xi+\mathcal{J}_l^{-1}\Delta\xi)^\Lambda) exp(ΔξΛ)exp(ξΛ)=exp((ξ+Jl−1Δξ)Λ)
e x p ( ξ Λ ) e x p ( Δ ξ Λ ) = e x p ( ( ξ + J r − 1 Δ ξ ) Λ ) exp(\xi^\Lambda)exp(\Delta\xi^\Lambda)=exp((\xi+\mathcal{J}_r^{-1}\Delta\xi)^\Lambda) exp(ξΛ)exp(ΔξΛ)=exp((ξ+Jr−1Δξ)Λ)
4.3. 扰动模型(左扰动)
S O ( 3 ) SO(3) SO(3) 上的李代数求导:
∂ Rp ∂ φ = lim φ → 0 e x p ( φ Λ ) e x p ( ϕ Λ ) p − e x p ( ϕ Λ ) p φ = lim φ → 0 ( 1 + φ Λ ) e x p ( ϕ Λ ) p − e x p ( ϕ Λ ) p φ = lim φ → 0 φ Λ Rp φ = − ( Rp ) Λ \frac{\partial \textbf{R}\textbf{p}}{\partial \varphi}=\lim_{\varphi\rightarrow 0}\frac{exp(\varphi^\Lambda)exp(\phi^\Lambda)\textbf{p}-exp(\phi^\Lambda)\textbf{p}}{\varphi}=\lim_{\varphi\rightarrow 0}\frac{(1+\varphi^\Lambda)exp(\phi^\Lambda)\textbf{p}-exp(\phi^\Lambda)\textbf{p}}{\varphi}=\lim_{\varphi\rightarrow 0}\frac{\varphi^\Lambda\textbf{R}\textbf{p}}{\varphi}=-(\textbf{R}\textbf{p})^\Lambda ∂φ∂Rp=φ→0limφexp(φΛ)exp(ϕΛ)p−exp(ϕΛ)p=φ→0limφ(1+φΛ)exp(ϕΛ)p−exp(ϕΛ)p=φ→0limφφΛRp=−(Rp)Λ
S E ( 3 ) SE(3) SE(3) 上的李代数求导:
∂ Tp ∂ δ ξ = [ I − ( Rp + t ) Λ 0 T 0 T ] \frac{\partial \textbf{T}\textbf{p}}{\partial \delta\xi}=\begin{bmatrix}\textbf{I} & -(\textbf{R}\textbf{p}+\textbf{t})^\Lambda\\ \textbf{0}^T & \textbf{0}^T\end{bmatrix} ∂δξ∂Tp=[I0T−(Rp+t)Λ0T]
5. 第五讲:相机与图像
- 相机模型
- 相机的标定
本章要用到OpenCV,PCL与boost,PCL用于拼接云点图,OpenCV的具体用法请参照OpenCV官网。
使用boost快速读取图片:
#include <boost/format.hpp>
boost::format fmt(./%s/%d.%s);
cv::imread((fmt%"director_name"%(number)%"file_type").str(), -1);
5.1. 相机模型
5.1.1. 针孔相机模型
设像素平面上某点 P u v = [ u , v ] T \textbf{P}_{uv}=[u,v]^T Puv=[u,v]T,其对应的在世界坐标系下的某点 P w = [ X , Y , Z ] T \textbf{P}_w=[X,Y,Z]^T Pw=[X,Y,Z]T,在相机归一化平面上对应的点为 P c = [ x , y , 1 ] T \textbf{P}_c=[x,y,1]^T Pc=[x,y,1]T。
我们有:
{
u
=
f
x
X
Z
+
c
x
v
=
f
y
Y
Z
+
c
y
\left\{\begin{matrix} u=f_x \frac{X}{Z}+c_x\\ \\ v=f_y\frac{Y}{Z}+c_y \end{matrix}\right.
⎩⎨⎧u=fxZX+cxv=fyZY+cy
即使 P u v = [ u , v ] T \textbf{P}_{uv}=[u,v]^T Puv=[u,v]T 化为齐次形式 P u v = [ u , v , 1 ] T \textbf{P}_{uv}=[u,v,1]^T Puv=[u,v,1]T,
Z
P
u
v
=
KT
P
w
Z\textbf{P}_{uv}=\textbf{K}\textbf{T}\textbf{P}_w
ZPuv=KTPw
或者:
P
u
v
=
K
P
c
\textbf{P}_{uv}=\textbf{K}\textbf{P}_c
Puv=KPc
在这两个式子中
K
\textbf{K}
K 为相机的内参矩阵(Intrinsic) 即为
K
=
[
f
x
0
c
x
0
f
y
c
y
0
0
1
]
\textbf{K}=\begin{bmatrix}f_x & 0 & c_x\\0 & f_y & c_y\\0 & 0 & 1\end{bmatrix}
K=⎣⎡fx000fy0cxcy1⎦⎤。
在G2O中内参 f x f_x fx 与 f y f_y fy 均为 f f f。
5.1.2. 畸变
径向畸变:
{
x
c
o
r
r
e
c
t
e
d
=
x
(
1
+
k
1
r
2
+
k
2
r
2
+
k
3
r
6
)
y
c
o
r
r
e
c
t
e
d
=
y
(
1
+
k
1
r
2
+
k
2
r
2
+
k
3
r
6
)
\left\{\begin{matrix} x_{corrected}=x(1+k_1r^2+k_2r^2+k_3r^6)\\ \\ y_{corrected}=y(1+k_1r^2+k_2r^2+k_3r^6) \end{matrix}\right.
⎩⎨⎧xcorrected=x(1+k1r2+k2r2+k3r6)ycorrected=y(1+k1r2+k2r2+k3r6)
则有:
{
u
=
f
x
x
c
o
r
r
e
c
t
e
d
+
c
x
v
=
f
y
y
c
o
r
r
e
c
t
e
d
+
c
y
\left\{\begin{matrix} u=f_x x_{corrected}+c_x\\ \\ v=f_yy_{corrected}+c_y \end{matrix}\right.
⎩⎨⎧u=fxxcorrected+cxv=fyycorrected+cy
这里
x
x
x,
y
y
y,
x
c
o
r
r
e
c
t
e
d
x_{corrected}
xcorrected,
y
c
o
r
r
e
c
t
e
d
y_{corrected}
ycorrected 均为归一平面上的点。
切向畸变:
{
x
c
o
r
r
e
c
t
e
d
=
x
+
2
p
1
x
y
+
p
2
(
r
2
+
2
x
2
)
y
c
o
r
r
e
c
t
e
d
=
y
+
p
1
(
r
2
+
2
y
2
)
+
2
p
2
x
y
\left\{\begin{matrix} x_{corrected}=x+2p_1xy+p_2(r^2+2x^2)\\ \\ y_{corrected}=y+p_1(r^2+2y^2)+2p_2xy \end{matrix}\right.
⎩⎨⎧xcorrected=x+2p1xy+p2(r2+2x2)ycorrected=y+p1(r2+2y2)+2p2xy
5.2. 相机的标定
具体原理参见:
摄像机内参标定《A Flexible New Technique for Camera Calibration》;
摄像机-激光雷达静态外参联合标定《Extrinsic calibration of a camera and laser range finder (improves camera calibration)》;
注意结合运动信息,物体的运动与激光雷达的旋转扫描同时发生;
运动补偿激光雷达与相机之间的标定;
因为某些原因,我现在只做了手机相机的标定,在这里就不详述了,准备令写一篇文章记录一下相机标定的过程。用Matlab或者OpenCV都能简单的实现单目相机的标定。
6. 第六讲:非线性优化
- 状态估计问题
- 最小二乘问题
- 用Ceres与G2O求解最小二乘问题
本章为《SLAM 14讲》中最后的与数学理论相关的章节,也是之后用的最多的一部分。
6.1. 状态估计问题
对于经典SLAM模型,它由一个状态方程和运动方程构成:
{
x
k
=
f
(
x
k
−
1
,
u
k
)
+
w
k
z
k
,
j
=
h
(
y
j
,
x
k
)
+
v
k
,
j
\left\{\begin{matrix} \textbf{x}_{k}=f(\textbf{x}_{k-1}, \textbf{u}_k)+\textbf{w}_k\\ \\ \textbf{z}_{k,j}=h(\textbf{y}_{j}, \textbf{x}_k)+\textbf{v}_{k,j} \end{matrix}\right.
⎩⎨⎧xk=f(xk−1,uk)+wkzk,j=h(yj,xk)+vk,j
相机位姿为
x
k
x_k
xk,可以用
T
k
\textbf{T}_k
Tk 或是
e
x
p
(
ξ
k
Λ
)
exp(\xi_k^\Lambda)
exp(ξkΛ) 表示。路标位置为
y
j
y_j
yj 。
w
k
{w}_k
wk 和
v
k
,
j
{v}_{k,j}
vk,j 为噪声项,且满足高斯分布:
w
k
∼
N
(
0
,
R
k
)
,
v
k
∼
N
(
0
,
Q
k
,
j
)
w_k\sim N(0,R_k),\ \ v_k\sim N(0,Q_{k,j})
wk∼N(0,Rk), vk∼N(0,Qk,j)
把所有待估计的变量放到一个“状态变量”中:
x
=
{
x
1
,
.
.
.
,
x
N
,
y
1
,
.
.
.
,
y
M
}
x=\{x_1,...,x_N,y_1,...,y_M\}
x={x1,...,xN,y1,...,yM}
则即是估计 P ( x ∣ z , u ) P(x|z, u) P(x∣z,u) 的概率分布,或是估计 P ( x ∣ z ) P(x|z) P(x∣z) 的概率分布,我们认为大多数情况下我们没有测量运动的传感器 ,只考虑观测方程带来的数据。
根据叶贝斯法则,我们有:
P
(
x
∣
z
)
=
P
(
z
∣
x
)
P
(
x
)
P
(
z
)
∝
P
(
z
∣
x
)
P
(
x
)
P(x|z)=\frac{P(z|x)P(x)}{P(z)} \propto P(z|x)P(x)
P(x∣z)=P(z)P(z∣x)P(x)∝P(z∣x)P(x)
后验概率最大化(Maximize a Posterior):
x
M
A
P
∗
=
a
r
g
m
a
x
P
(
x
∣
z
)
=
a
r
g
m
a
x
P
(
z
∣
x
)
P
(
x
)
x^*_{MAP}=arg\ max\ P(x|z) =arg\ max\ P(z|x)P(x)
xMAP∗=arg max P(x∣z)=arg max P(z∣x)P(x)
最大似然估计(Maximize Likelyhood Estimation):
x
M
L
E
∗
=
a
r
g
m
a
x
P
(
z
∣
x
)
x^*_{MLE}=arg\ max\ P(z|x)
xMLE∗=arg max P(z∣x)
6.2. 最小二乘问题
6.2.1. 引出
高维高斯分布: P ( x ) = 1 ( 2 π ) N d e t ( Σ ) e x p ( − 1 2 ( x − μ ) T Σ − 1 ( x − μ ) ) P(\textbf{x})=\frac{1}{\sqrt{(2\pi)^Ndet(\Sigma)}}exp(-\frac{1}{2}(\textbf{x}-\mu)^T\Sigma^{-1}(\textbf{x}-\mu)) P(x)=(2π)Ndet(Σ)1exp(−21(x−μ)TΣ−1(x−μ))
对于之前的求最大似然估计的问题即为:
x
∗
=
a
r
g
m
i
n
(
(
z
k
,
j
−
h
(
x
k
,
y
j
)
)
T
Q
k
,
j
−
1
(
z
z
,
j
−
h
(
x
k
,
y
j
)
)
)
x^*=arg\ min\ ((z_{k,j}-h(x_k,y_j))^TQ_{k,j}^{-1}(z_{z,j}-h(x_k,y_j)))
x∗=arg min ((zk,j−h(xk,yj))TQk,j−1(zz,j−h(xk,yj)))
因此对于所有的运动和任意的观测,我们定义数据与估计值之间的误差:
e
v
,
k
=
x
k
−
f
(
x
k
−
1
,
u
k
)
e
y
,
j
,
k
=
z
k
,
j
−
h
(
x
k
,
y
j
)
e_{v,k}=x_k-f(x_{k-1},u_k)\\ e_{y,j,k}=z_{k,j}-h(x_k,y_j)
ev,k=xk−f(xk−1,uk)ey,j,k=zk,j−h(xk,yj)
并求该误差的平方和:
J
(
x
)
=
∑
k
e
v
,
k
T
R
k
−
1
e
v
,
k
+
∑
k
∑
j
e
y
,
k
,
j
T
Q
k
,
j
−
1
e
y
,
k
,
j
J(x)=\sum_{k}e_{v,k}^TR_k^{-1}e_{v,k}+\sum_{k}\sum_{j}e_{y,k,j}^TQ_{k,j}^{-1}e_{y,k,j}
J(x)=k∑ev,kTRk−1ev,k+k∑j∑ey,k,jTQk,j−1ey,k,j
于是我们得到了一个总体意义下的最小二乘问题!
6.2.2. 非线性最小二乘问题的一般解法 - 迭代法
下面以最简单的最小二乘问题为例: min x 1 2 ∣ ∣ f ( x ) ∣ ∣ 2 2 \min_{x}\frac{1}{2}||f(x)||_2^2 minx21∣∣f(x)∣∣22
- 给定某个初值 x 0 x_0 x0 。
- 对于第 k k k 次迭代,寻找增量 Δ x k \Delta x_k Δxk ,使得 ∣ ∣ f ( x k + Δ x k ) ∣ ∣ 2 2 ||f(x_k +\Delta x_k)||_2^2 ∣∣f(xk+Δxk)∣∣22 最小。
- 若 Δ x k \Delta x_k Δxk 足够小,则停止。
- 否则,令 x k + 1 = x k + Δ x k x_{k+1}=x_k+\Delta x_k xk+1=xk+Δxk ,返回到 2 。
6.2.3. 一阶和二阶梯度法
将目标函数在
x
x
x 附近展开:
∣
∣
f
(
x
+
Δ
x
)
∣
∣
2
2
≈
∣
∣
f
(
x
)
∣
∣
2
2
+
J
(
x
)
Δ
x
+
1
2
Δ
x
T
H
Δ
x
||f(x+\Delta x)||^2_2\approx ||f(x)||_2^2+J(x)\Delta x+\frac{1}{2}\Delta x^TH\Delta x
∣∣f(x+Δx)∣∣22≈∣∣f(x)∣∣22+J(x)Δx+21ΔxTHΔx
保留一阶梯度: Δ x ∗ = − J T ( x ) \Delta x^*=-J^T(x) Δx∗=−JT(x)
保留二阶梯度: H Δ x = − J T H\Delta x=-J^T HΔx=−JT
6.2.4. Gauss-Newton
将
f
(
x
)
f(x)
f(x)进行一阶泰勒展开:
f
(
x
+
Δ
x
)
≈
f
(
x
)
+
J
(
x
)
Δ
x
f(x+\Delta x)\approx f(x)+J(x)\Delta x
f(x+Δx)≈f(x)+J(x)Δx
代入最小二乘问题的式子便得到了:
J
(
x
)
T
J
(
x
)
Δ
x
=
−
J
(
x
)
T
f
(
x
)
J(x)^TJ(x)\Delta x=-J(x)^Tf(x)
J(x)TJ(x)Δx=−J(x)Tf(x)
或把左右两边系数分别定义为
H
H
H 和
g
g
g,则有:
H
Δ
x
=
g
H\Delta x=g
HΔx=g
Gauss-Newton 的算法步骤如下:
- 给定某个初值 x 0 x_0 x0 。
- 对于第 k k k 次迭代,求出雅可比矩阵 J ( x k ) J(x_k) J(xk) 和误差 f ( x k ) f(x_k) f(xk) 。
- 求解增量方程: H Δ x = g H\Delta x=g HΔx=g 。
- 若 Δ x k \Delta x_k Δxk 足够小,则停止,否则,令 x k + 1 = x k + Δ x k x_{k+1}=x_k+\Delta x_k xk+1=xk+Δxk ,返回到 2 。
6.2.5. Levenberg-Marquadt
使用 ρ = f ( x + Δ x ) − f ( x ) J ( x ) Δ x \rho=\frac{f(x+\Delta x)-f(x)}{J(x)\Delta x} ρ=J(x)Δxf(x+Δx)−f(x) 来判断泰勒近似是否够好。 ρ \rho ρ 接近于 1 ,则近似很好。如果 ρ \rho ρ 太小, 则实际减小的值远小于近似减小的值,则需要缩小近似范围。反之,则说明实际下降的比预计的更大,我们要放大近似的范围。
Levenberg-Marquadt 的算法步骤如下:
- 给定某个初值 x 0 x_0 x0 。
- 对于第
k
k
k 次迭代,求解:
min Δ x k 1 2 ∣ ∣ f ( x k ) + J ( x k ) Δ x k ∣ ∣ 2 , s . t . ∣ ∣ D Δ x k ∣ ∣ 2 ⩽ μ \min_{\Delta x_k} \frac{1}{2}||f(x_k)+J(x_k)\Delta x_k||^2, s.t.||D\Delta x_k||^2 \leqslant \mu Δxkmin21∣∣f(xk)+J(xk)Δxk∣∣2,s.t.∣∣DΔxk∣∣2⩽μ这里 μ \mu μ 是信赖区间半径, D D D 为对角矩阵(Diagonal)把增量限制在椭球中。 - 计算 ρ \rho ρ 。
- 若 ρ > 3 4 \rho>\frac{3}{4} ρ>43 ,则 μ = 2 μ \mu=2\mu μ=2μ 。
- 若 ρ < 1 4 \rho<\frac{1}{4} ρ<41 ,则 μ = 1 2 μ \mu=\frac{1}{2}\mu μ=21μ 。
- 如果 ρ \rho ρ 大于某阈值,则认为近似可行。令 x k + 1 = x k + Δ x k x_{k+1}=x_k+\Delta x_k xk+1=xk+Δxk 。
- 判断算法是否收敛。如不收敛则返回到 2 。
对于 2 ,我们也可以求解:
min
Δ
x
k
1
2
∣
∣
f
(
x
k
)
+
J
(
x
k
)
Δ
x
k
∣
∣
2
+
λ
2
∣
∣
D
Δ
x
k
∣
∣
2
\min_{\Delta x_k} \frac{1}{2}||f(x_k)+J(x_k)\Delta x_k||^2+\frac{\lambda}{2}||D\Delta x_k||^2
Δxkmin21∣∣f(xk)+J(xk)Δxk∣∣2+2λ∣∣DΔxk∣∣2也相当于是计算增量的线性方程:
(
H
+
λ
D
T
D
)
Δ
x
=
g
(H+\lambda D^TD)\Delta x=g
(H+λDTD)Δx=g
6.3. 用Ceres与G2O求解最小二乘问题
6.3.1. Ceres
详见Ceres官网。
栗子: y = e x p ( a x 2 + b x + c ) y=exp(ax^2+bx+c) y=exp(ax2+bx+c) 函数的拟合
#include <iostram>
#include <opencv2/core/.hpp>
#include <ceres/ceres.h>
// Cost Function Model
struct CURVE_FITTING_COST
{
CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {}
//calculate residual
template <typename T>
bool operator() (
const T* const abc, // Model parameter, 3 dimensions
T* residual ) const // residual
{
residual[0] = T ( _y ) - ceres::exp ( ab[0]*T ( _x ) *T ( _x ) + ab[1]*T ( _x ) + c[0] ); // y-exp(ax^2+bx+c)
return true;
}
const double _x, _y; // x,y datas
};
...
// construct least square problem
ceres::Problem problem;
for ( int i=0; i<N; i++ )
{
ceres::CostFunction *costfunction = new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 2, 1> (new CURVE_FITTING_COST (x_data[i], y_data[i]));
problem.AddResidualBlock ( costfunction,
new ceres::CauchyLoss(0.5), // core function
abc // estimate parameters
);
}
// set solver
ceres::Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; // use QR to solve update function
options.minimizer_progress_to_stdout = true; // output to cout
ceres::Solver::Summary summary; // optimization information
ceres::Solve ( options, &problem, &summary ); // begin to optimize
// output the result
cout<<summary.BriefReport() <<endl;
cout<<"estimated a,b,c = ";
for ( auto a:abc ) cout<<a<<" ";
6.3.2. G2O
同样的栗子:
#include <iostream>
#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/optimization_algorithm_dogleg.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <Eigen/Core>
#include <opencv2/core/core.hpp>
#include <cmath>
using namespace std;
//set vertex, template: dimension, type of optimized parameter
class CurveFittingVertex: public g2o::BaseVertex<3, Eigen::Vector3d>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
virtual void setToOriginImpl()
{
_estimate << 0,0,0;
}
virtual void oplusImpl( const double* update ) // update
{
_estimate += Eigen::Vector3d(update);
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
};
//error Model template parameter:observed datas' dimension,type,type of linked vertex
class CurveFittingEdge: public g2o::BaseUnaryEdge<1,double,CurveFittingVertex>
{
public:
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
CurveFittingEdge( double x ): BaseUnaryEdge(), _x(x) {}
// compute error
void computeError()
{
const CurveFittingVertex* v = static_cast<const CurveFittingVertex*> (_vertices[0]);
const Eigen::Vector3d abc = v->estimate();
_error(0,0) = _measurement - std::exp( abc(0,0)*_x*_x + abc(1,0)*_x + abc(2,0) ) ;
}
virtual bool read( istream& in ) {}
virtual bool write( ostream& out ) const {}
public:
double _x; // x value, y is _measurement
};
...
// construct optimization graph
typedef g2o::BlockSolver< g2o::BlockSolverTraits<3,1> > Block; // dimension of optimizing parameter is 3, error is 1
Block::LinearSolverType* linearSolver = new g2o::LinearSolverDense<Block::PoseMatrixType>(); // linear equation solver
Block* solver_ptr = new Block( linearSolver ); // Matrix block solver
// Method: Gauss-Newton, LB, Dogleg
g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg( solver_ptr );
// g2o::OptimizationAlgorithmGaussNewton* solver = new g2o::OptimizationAlgorithmGaussNewton( solver_ptr );
// g2o::OptimizationAlgorithmDogleg* solver = new g2o::OptimizationAlgorithmDogleg( solver_ptr );
g2o::SparseOptimizer optimizer; // graph model
optimizer.setAlgorithm( solver ); // set solver
optimizer.setVerbose( true ); // set output verbose
// add vertex to the graph
CurveFittingVertex* v = new CurveFittingVertex();
v->setEstimate( Eigen::Vector3d(0,0,0) );
v->setId(0);
optimizer.addVertex( v );
// add eges to the graph
for ( int i=0; i<N; i++ )
{
CurveFittingEdge* edge = new CurveFittingEdge( x_data[i] );
edge->setId(i);
edge->setVertex( 0, v ); // set linked vertex
edge->setMeasurement( y_data[i] ); // set mesurement
edge->setInformation( Eigen::Matrix<double,1,1>::Identity()*1/(w_sigma*w_sigma) ); // Information matrix
optimizer.addEdge( edge );
}
// optimizing
cout<<"start optimization"<<endl;
optimizer.initializeOptimization();
optimizer.optimize(100);
// output the result
Eigen::Vector3d abc_estimate = v->estimate();
cout<<"estimated model: "<<abc_estimate.transpose()<<endl;
7. 第七 - 八 - 九讲:视觉里程计
- 特征点法
- 2D-2D:对极几何
- 3D-2D:PnP
- 3D-3D:ICP
- 直接法
- 设计前端
这部分主要介绍了视觉里程计中的一些模型,即如何从一堆照片中求解相机的位姿,从而求出相机大概的运动轨迹,以求得空间点的大致坐标。
7.1. 特征点法
特征点由关键点和描述子两部分组成。常用的ORB特征由FAST角点和BRIEF描述子组成。特征点的原理在此就不复述了,用OpenCV就可以简单的匹配到两张图片之间的特征点。
具体代码见 "slambook/cha7/feature_extraction.cpp"
7.2. 2D-2D:对极几何
对极约束:
x
2
T
t
Λ
R
x
1
=
0
\textbf{x}^T_2\textbf{t}^\Lambda \textbf{R}\textbf{x}_1=0
x2TtΛRx1=0
这里式子中的
x
i
x_i
xi 均是归一化平面上的坐标,重新代入
p
1
,
p
2
p_1,p_2
p1,p2 有:
p
2
T
K
−
T
t
Λ
R
K
−
1
p
1
=
0
\textbf{p}_2^T\textbf{K}^{-T}\textbf{t}^\Lambda \textbf{R}\textbf{K}^{-1}\textbf{p}_1=0
p2TK−TtΛRK−1p1=0
我们把中间部分记作两个矩阵:基础矩阵
F
\textbf{F}
F 和本质矩阵
E
\textbf{E}
E,进一步简化对极约束:
E
=
t
Λ
R
,
F
=
K
−
T
E
K
−
1
,
x
2
T
E
x
1
=
p
2
T
F
p
1
=
0
\textbf{E}=\textbf{t}^\Lambda \textbf{R},\ \textbf{F}=\textbf{K}^{-T}\textbf{E}\textbf{K}^{-1},\ \textbf{x}_2^T\textbf{E}\textbf{x}_1=\textbf{p}_2^T\textbf{F}\textbf{p}_1=0
E=tΛR, F=K−TEK−1, x2TEx1=p2TFp1=0
我们可以通过八点法求得 E \textbf{E} E ,再通过奇异值(SVD)分解即可求得 t \textbf{t} t 与 R \textbf{R} R 。
单应矩阵 H \textbf{H} H 描述了平面到平面之间的映射关系,在单目相机的标定中有用到: p 2 = H p 1 \textbf{p}_2=\textbf{H}\textbf{p}_1 p2=Hp1 ,也可以用类似八点法的方式来确定。
三角测量: 对于两个归一化平面上的点
x
1
\textbf{x}_1
x1 和
x
2
\textbf{x}_2
x2 ,那么它们满足:
s
1
x
1
=
s
2
R
x
2
+
t
s_1\textbf{x}_1=s_2\textbf{R}\textbf{x}_2+\textbf{t}
s1x1=s2Rx2+t稍微转化则有:
s
1
x
1
Λ
x
1
=
0
=
s
2
x
1
Λ
R
x
2
+
x
1
Λ
t
s_1\textbf{x}_1^\Lambda\textbf{x}_1=0=s_2\textbf{x}_1^\Lambda\textbf{R}\textbf{x}_2+\textbf{x}_1^\Lambda\textbf{t}
s1x1Λx1=0=s2x1ΛRx2+x1Λt
于是便可以求出深度 s 2 s_2 s2 ,用同样的方法也可以求出 s 1 s_1 s1 。另外,三角测量是由平移得到的,纯旋转无法使用三角测量。
7.3. 3D-2D:PnP
7.3.1. DLT:直接线性变换
直接从某个空间点 P = ( X , Y , Z , 1 ) T \textbf{P}=(X,Y,Z,1)^T P=(X,Y,Z,1)T ,和与之对应的特征点(归一化平面) x 1 = ( u 1 , v 1 , 1 ) T \textbf{x}_1=(u_1,v_1,1)^T x1=(u1,v1,1)T ,利用 x = RP + t \textbf{x}=\textbf{R}\textbf{P}+\textbf{t} x=RP+t来求解相机的位姿。
7.3.2. P3P
根据相似三角形,我们很容易可以求出:
(
1
−
u
)
y
2
−
u
x
2
−
c
o
s
⟨
b
,
c
⟩
y
+
2
u
x
y
c
o
s
⟨
a
,
b
⟩
+
1
=
0
(
1
−
w
)
x
2
−
w
y
2
−
c
o
s
⟨
a
,
c
⟩
x
+
2
w
x
y
c
o
s
⟨
a
,
b
⟩
+
1
=
0
(1-u)y^2-ux^2-cos\left \langle b,c \right \rangle y+2uxy \ cos\left \langle a,b\right \rangle+1=0\\ (1-w)x^2-wy^2-cos\left \langle a,c \right \rangle x+2wxy \ cos\left \langle a,b\right \rangle+1=0
(1−u)y2−ux2−cos⟨b,c⟩y+2uxy cos⟨a,b⟩+1=0(1−w)x2−wy2−cos⟨a,c⟩x+2wxy cos⟨a,b⟩+1=0
7.3.3. Bundle Adjustment
构建最小二乘问题,使重投影误差最小:
ξ
∗
=
a
r
g
min
ξ
1
2
∑
i
=
1
n
∣
∣
u
i
−
1
s
i
K
e
x
p
(
ξ
Λ
)
P
i
∣
∣
2
2
\xi^*=arg\ \min_{\xi}\ \frac{1}{2}\sum_{i=1}^{n}||\textbf{u}_i-\frac{1}{s_i}\textbf{K}exp(\xi^\Lambda)\textbf{P}_i||_2^2
ξ∗=arg ξmin 21i=1∑n∣∣ui−si1Kexp(ξΛ)Pi∣∣22
由
e
(
x
+
Δ
x
)
≈
e
(
x
)
+
J
Δ
x
e(x+\Delta x) \approx e(x) +J\Delta x
e(x+Δx)≈e(x)+JΔx 可知,我们需要求
J
J
J , 在这里
J
J
J 应该是一个
2
×
6
2\times6
2×6 的矩阵。令
P
′
=
(
e
x
p
(
ξ
Λ
)
P
)
1
:
3
=
[
X
′
,
Y
′
,
Z
′
]
T
\textbf{P}'=(exp(\xi^\Lambda)\textbf{P})_{1:3}=[X',Y',Z']^T
P′=(exp(ξΛ)P)1:3=[X′,Y′,Z′]T 。
∂
e
∂
δ
ξ
=
−
[
f
x
Z
′
0
−
f
x
X
′
Z
′
2
−
f
x
X
′
Y
′
Z
′
2
f
x
+
f
x
X
2
Z
′
2
−
f
x
Y
Z
′
0
f
y
Z
′
−
f
y
Y
′
Z
′
2
−
f
y
−
f
y
Y
′
2
Z
′
2
f
y
X
′
Y
′
Z
′
2
f
y
X
′
Z
′
]
\frac{\partial\textbf{e}}{\partial\delta\xi}=-\begin{bmatrix}\frac{f_x}{Z'} & 0 & -\frac{f_xX'}{Z'^2} & -\frac{f_xX'Y'}{Z'^2} & f_x+\frac{f_xX^2}{Z'^2} & -\frac{f_xY}{Z'}\\0 & \frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}&-f_y-\frac{f_yY'^2}{Z'^2}&\frac{f_yX'Y'}{Z'^2}&\frac{f_yX'}{Z'}\end{bmatrix}
∂δξ∂e=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′−Z′2fxX′Y′−fy−Z′2fyY′2fx+Z′2fxX2Z′2fyX′Y′−Z′fxYZ′fyX′]
∂
e
∂
P
=
−
[
f
x
Z
′
0
−
f
x
X
′
Z
′
2
0
f
y
Z
′
−
f
y
Y
′
Z
′
2
]
R
\frac{\partial\textbf{e}}{\partial\textbf{P}}=-\begin{bmatrix}\frac{f_x}{Z'}&0&-\frac{f_xX'}{Z'^2}\\0&\frac{f_y}{Z'}&-\frac{f_yY'}{Z'^2}\end{bmatrix}\textbf{R}
∂P∂e=−[Z′fx00Z′fy−Z′2fxX′−Z′2fyY′]R
之后便可以用G2O求解这个最小二乘问题。
7.4. 3D-3D:ICP
构建的最小二乘问题为 min R , t J = 1 2 ∑ i = 1 n ∣ ∣ ( p i − ( R p i ′ + t ) ) ∣ ∣ 2 2 \min_{\textbf{R},\textbf{t}} J=\frac{1}{2}\sum_{i=1}^{n}||(\textbf{p}_i-(\textbf{R}\textbf{p}'_i+\textbf{t}))||_2^2 minR,tJ=21∑i=1n∣∣(pi−(Rpi′+t))∣∣22 。
7.4.1. SVD方法
- 计算两组点的质心位置
p
,
p
′
\textbf{p},\textbf{p}'
p,p′ ,然后计算每个点的去质心坐标:
q i = p i − p , q i ′ = p i ′ − p ′ \textbf{q}_i=\textbf{p}_i-\textbf{p},\ \ \textbf{q}_i'=\textbf{p}'_i-\textbf{p}' qi=pi−p, qi′=pi′−p′ - 根据 R ∗ = a r g min R 1 2 ∑ i = 1 n ∣ ∣ q i − R q i ′ ∣ ∣ 2 = a r g min R 1 2 ∑ i = 1 n q i T q i + q i ′ T q i ′ − 2 q i T R q i ′ = a r g min R − t r ( R ∑ i = 1 n q i ′ q i T ) \textbf{R}^*=arg\ \min_{\textbf{R}}\frac{1}{2}\sum_{i=1}^{n}||\textbf{q}_i-\textbf{R}\textbf{q}'_i||^2=arg\ \min_{\textbf{R}}\frac{1}{2}\sum_{i=1}^n\textbf{q}^T_i\textbf{q}_i+\textbf{q}_i'^T\textbf{q}_i'-2\textbf{q}_i^T\textbf{R}\textbf{q}'_i=arg\ \min_{\textbf{R}}-tr(\textbf{R}\sum_{i=1}^n\textbf{q}'_i\textbf{q}_i^T) R∗=arg Rmin21i=1∑n∣∣qi−Rqi′∣∣2=arg Rmin21i=1∑nqiTqi+qi′Tqi′−2qiTRqi′=arg Rmin−tr(Ri=1∑nqi′qiT) 计算旋转矩阵。
- 根据旋转矩阵计算 t \textbf{t} t 。
7.4.2. BA方法
求导式子为:
∂
e
∂
δ
ξ
=
−
[
I
−
(
e
x
p
(
ξ
Λ
)
p
i
′
)
Λ
0
0
]
\frac{\partial\textbf{e}}{\partial\delta\xi}=-\begin{bmatrix}\textbf{I} & -(exp(\xi^\Lambda)\textbf{p}'_i)^\Lambda\\\textbf{0}&\textbf{0}\end{bmatrix}
∂δξ∂e=−[I0−(exp(ξΛ)pi′)Λ0]
7.5. 直接法
7.5.1. 引出
为了克服特征点法的一些缺点,比如:耗时,丢弃了大部分可能有用的图像信息,相机有时会运动到特征缺失的地方,我们有以下几种思路:
- 只计算关键点,不计算描述子。使用 光流法(Optical Flow) 来跟踪特征点的运动。
- 只计算关键点,不计算描述子。使用 直接法(Direct Method) 来计算特征点在下一时刻图像的位置。
- 根据像素灰度的差异,直接计算相机的运动。
在直接法(后两种)中我们通过最小化 光度误差(Photometric error) 来优化相机的运动。
7.5.2. Lucas-Kanade 光流
灰度不变假设: 同一个空间点的像素灰度值,在各个图像中是固定不变的。
设
I
x
=
∂
I
∂
x
,
I
y
=
∂
I
∂
y
,
u
=
d
x
d
t
,
v
=
d
y
d
t
,
I
t
=
∂
I
∂
t
\textbf{I}_x=\frac{\partial \textbf{I}}{\partial x},\ \ \ \textbf{I}_y=\frac{\partial \textbf{I}}{\partial y},\ \ \ u=\frac{dx}{dt},\ \ \ v=\frac{dy}{dt},\ \ \ \textbf{I}_t=\frac{\partial \textbf{I}}{\partial t}
Ix=∂x∂I, Iy=∂y∂I, u=dtdx, v=dtdy, It=∂t∂I
根据灰度不变假设,我们有:
I
x
u
+
I
y
v
=
−
I
t
o
r
[
I
x
I
y
]
[
u
v
]
=
−
I
t
\textbf{I}_xu+\textbf{I}_yv=-\textbf{I}_t\ \ \ or \ \ \ \begin{bmatrix}\textbf{I}_x&\textbf{I}_y\end{bmatrix}\begin{bmatrix}u\\v\end{bmatrix}=-\textbf{I}_t
Ixu+Iyv=−It or [IxIy][uv]=−It
令:
A
=
[
[
I
x
I
y
]
1
.
.
.
[
I
x
I
y
]
k
]
,
b
=
[
I
t
1
.
.
.
I
t
2
]
\textbf{A}=\begin{bmatrix}\begin{bmatrix}\textbf{I}_x&\textbf{I}_y\end{bmatrix}_1\\...\\\begin{bmatrix}\textbf{I}_x&\textbf{I}_y\end{bmatrix}_k\end{bmatrix}, \textbf{b}=\begin{bmatrix}\textbf{I}_{t1}\\...\\\textbf{I}_{t2}\end{bmatrix}
A=⎣⎡[IxIy]1...[IxIy]k⎦⎤,b=⎣⎡It1...It2⎦⎤
则有:
A
[
u
v
]
=
−
b
\textbf{A} \begin{bmatrix}u\\v\end{bmatrix}=-\textbf{b}
A[uv]=−b求其最小二乘解即可。
7.5.3. 直接法
光度误差 记为:
e
=
I
1
(
p
1
)
−
I
2
(
p
2
)
e=\textbf{I}_1(\textbf{p}_1)-\textbf{I}_2(\textbf{p}_2)
e=I1(p1)−I2(p2)
对于多个空间点
P
i
P_i
Pi ,相机位姿估计问题变为:
min
ξ
J
(
ξ
)
=
∑
i
=
1
N
e
i
T
e
i
,
e
i
=
I
1
(
p
1
,
i
)
−
I
2
(
p
2
,
i
)
\min_{\xi}J(\xi)=\sum_{i=1}^Ne_i^Te_i,\ \ \ \ \ e_i=\textbf{I}_1(\textbf{p}_{1,i})-\textbf{I}_2(\textbf{p}_{2,i})
ξminJ(ξ)=i=1∑NeiTei, ei=I1(p1,i)−I2(p2,i)
误差相对于李代数的雅可比矩阵为:
J
=
−
∂
I
2
∂
u
∂
u
∂
δ
ξ
J=-\frac{\partial\textbf{I}_2}{\partial \textbf{u}}\frac{\partial \textbf{u}}{\partial\delta\xi}
J=−∂u∂I2∂δξ∂u
7.6. 设计前端
详见《SLAM 14讲》:第九讲 - 视觉里程计完整实现过程 1.0。
8. 第十 - 十一讲:后端
- KF滤波和EKF滤波
- BA 与图优化
- 位姿图的优化
- 因子图优化
8.2. BA与图优化
详见这篇文章:SLAM后端:优化空间点、相机位姿与参数 - 《SLAM 14讲》第十讲
9. 第十二讲:回环检测
- 基本概念
- 词袋模型与字典
- 相似度计算
9.1. 基本概念
回环检测结果分类:
算法 \ 事实 | 是回环 | 不是回环 |
---|---|---|
是回环 | 真阳性(True Positive) | 假阳性(False Postive) |
不是回环 | 假阴性(False Negative) | 真阴性(True Negative) |
准确率: P r e c i s i o n = T P T P + F P Precision=\frac{TP}{TP+FP} Precision=TP+FPTP
召回率: R e c a l l = T P T P + F N Recall=\frac{TP}{TP+FN} Recall=TP+FNTP
为了评价算法的好坏,我测试它在各种配置下的 P P P 值和 R R R 值,然后做出一条 Precision-Recall 曲线。
9.2. 词袋模型和字典
K-means 的步骤:
- 随机取 k k k 个中心点: c 1 , . . c k c_1,..c_k c1,..ck ;
- 对每一个样本,计算与每个中心点之间的距离,取最小的作为它的归类;
- 重新计算每个类的中心点。
- 如果每个中心点都变化很小,则算法收敛,退出;否则返回 1。
用 k k k 叉树来表达字典的步骤:
- 在根节点,用 K-means 把所有样本聚成 k k k 类(实际中为保证聚类均匀性会使用k-means++)。这样得到了第一层。
- 对第一层的每个节点,把属于该节点的样本再聚成 k k k 类,得到下一层。
- 依此类推,最后得到叶子层。叶子层即为所谓的 Words。
用 BoW 创建字典:
#include "DBoW3/DBoW3.h"
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/features2d/features2d.hpp>
#include <iostream>
#include <vector>
#include <string>
using namespace cv;
using namespace std;
int main( int argc, char** argv )
{
// read the image
cout<<"reading images... "<<endl;
vector<Mat> images;
for ( int i=0; i<10; i++ )
{
string path = "./data/"+to_string(i+1)+".png";
images.push_back( imread(path) );
}
// detect ORB features
cout<<"detecting ORB features ... "<<endl;
Ptr< Feature2D > detector = ORB::create();
vector<Mat> descriptors;
for ( Mat& image:images )
{
vector<KeyPoint> keypoints;
Mat descriptor;
detector->detectAndCompute( image, Mat(), keypoints, descriptor );
descriptors.push_back( descriptor );
}
// create vocabulary
cout<<"creating vocabulary ... "<<endl;
DBoW3::Vocabulary vocab;
vocab.create( descriptors );
cout<<"vocabulary info: "<<vocab<<endl;
vocab.save( "vocabulary.yml.gz" );
cout<<"done"<<endl;
return 0;
}
9.3. 相似度计算
TF-IDF(Term Frequency-Inverse Document Frequency):
我们统计某个叶子节点
w
i
w_i
wi 中的特征数量相对于所有特征数量的比例,作为 IDF 部分。假设所有特征数量为
n
n
n,
w
i
w_i
wi 数量为
n
i
n_i
ni ,那么该单词的 IDF 为:
I
D
F
i
=
log
(
n
n
i
)
IDF_i=\log(\frac{n}{n_i})
IDFi=log(nin)
TF 部分则是指某个特征在单个图像中出现的频率。假设图像 A 中,单词
w
i
w_i
wi 出现了
n
i
n_i
ni 次,而一共出现的单词次数为
n
n
n,那么 TF 为:
T
F
i
=
n
i
n
TF_i=\frac{n_i}{n}
TFi=nni
于是
w
i
w_i
wi 的权重等于 TF 乘 IDF 之积:
η
i
=
T
F
i
×
I
D
F
i
\eta_i=TF_i\times IDF_i
ηi=TFi×IDFi
于是对于图像 A 我们便可以这样描述:
A
=
{
(
w
1
,
η
1
)
,
.
.
.
,
(
w
N
,
η
N
)
}
=
v
A
A=\{(w_1,\eta_1),...,(w_N,\eta_N)\}=\textbf{v}_A
A={(w1,η1),...,(wN,ηN)}=vA
则对于给定的
v
A
\textbf{v}_A
vA 与
v
B
\textbf{v}_B
vB 我们可以这样计算他们的差异:
s
(
v
A
−
v
B
)
=
2
∑
i
=
1
N
∣
v
A
i
∣
+
∣
v
B
i
∣
−
∣
v
A
i
−
v
B
i
∣
s(\textbf{v}_A-\textbf{v}_B)=2\sum_{i=1}^N|\textbf{v}_{Ai}|+|\textbf{v}_{Bi}|-|\textbf{v}_{Ai}-\textbf{v}_{Bi}|
s(vA−vB)=2i=1∑N∣vAi∣+∣vBi∣−∣vAi−vBi∣
*这部分代码就不详述了
关键帧的处理:
这部分还没有细究 ,曾经看到过一篇论文,不知道有没有用:《一种基于时空切片的SLAM关键帧提取方法》
10. 第十三讲:建图
终于写到了最后的最后,SLAM 的最终目的。《SLAM 十四讲》中这章占比很小,也只是介绍了几种滤波器,然后给了几个栗子。因为项目用的是RGB-D相机,所以对于建图部分,我略去了单目相机的部分。
理由同 9 ,这部分还未开始实践,暂且留空。