【视觉SLAM十四讲】李群、李代数(第三章实践作业)理论推导+代码实现+过程问题解决
作者:互联网
视觉SLAM十四讲李群、李代数-理论推导+代码实现+过程问题解决
文章目录
2. 群的性质(2分,约1小时)
课上我们讲解了什么是群。请根据群定义,求解以下问题:
群(Group)是一种集合加上一种运算的代数结构。我们把集合记作
A
A
A,运算记作
⋅
\cdot
⋅ ,那么群可以记作
G
=
(
A
,
⋅
)
G = \left( {A, \cdot } \right)
G=(A,⋅)
。群要求这个运算满足以下几个条件:
- 封闭性: ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A . \forall {a_1},{a_2} \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {a_1} \cdot {a_2} \in A. ∀a1,a2∈A,a1⋅a2∈A.
- 结合律: ∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) . \forall {a_1},{a_2},{a_3} \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} ({a_1} \cdot {a_2}) \cdot {a_3} = {a_1} \cdot ({a_2} \cdot {a_3}). ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3).
- 幺元: ∃ a 0 ∈ A , s . t . ∀ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a . \exists {a_0} \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} s.t.{\kern 1pt} {\kern 1pt} \forall a \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {a_0} \cdot a = a \cdot {a_0} = a. ∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a.
- 逆: ∀ a ∈ A , ∃ a − 1 ∈ A , s . t . a ⋅ a − 1 = a 0 . \forall a \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \exists {a^{ - 1}} \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} s.t.{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} a \cdot {a^{ - 1}} = {a_0}. ∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0.
1.{Z,+} 是否为群?若是,验证其满⾜群定义;若不是,说明理由。
答:
{Z,+}是群;
对于{Z,+},设
a
1
,
a
2
,
a
3
∈
Z
{a_1},{a_2},{a_3} \in Z
a1,a2,a3∈Z。
- 封闭性:两个整数相加依然是整数——成立
对于 ∀ a 1 , a 2 , ∈ Z \forall{a_1},{a_2}, \in Z ∀a1,a2,∈Z 有 a 1 + a 2 ∈ Z , {a_1} + {a_2} \in Z, a1+a2∈Z, 因此满足封闭性。 - 结合律:整数的两元加法满足结合律——成立
对于 ∀ a 1 , a 2 , a 3 ∈ Z , ( a 1 + a 2 ) + a 3 = a 1 + ( a 2 + a 3 ) , \forall {a_1},{a_2},{a_3} \in Z,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} ({a_1} + {a_2}) + {a_3} = {a_1} + ({a_2} + {a_3}), ∀a1,a2,a3∈Z,(a1+a2)+a3=a1+(a2+a3), 因此满足结合律 - 幺元:任何整数加上0,等于它本身——成立
对于 ∃ 0 ∈ Z , \exists {\kern 1pt} 0 \in Z, ∃0∈Z, 对于 ∀ a ∈ Z , \forall a \in Z, ∀a∈Z, 有 a + 0 = 0 + a = a , {\kern 1pt} {\kern 1pt} a + 0 = 0 + a = a, a+0=0+a=a,因此满足幺元 - 逆:任何整数加上它的相反数等于幺元0,所以逆是其相反数——成立
对于 ∀ a ∈ Z , \forall a \in Z, ∀a∈Z, ∃ − a ∈ Z , \exists {\kern 1pt} - a \in Z, ∃−a∈Z, 使得 a + ( − a ) = 0 , {\kern 1pt} {\kern 1pt} a + \left( { - a} \right) = 0, a+(−a)=0, 因此满足逆
{Z,+} 满足以上四条性质,因此是群。
2. {N,+}是否为群?若是,验证其满⾜群定义;若不是,说明理由。其中Z 为整数集,N 为自然数集。
答: {N,+} 不是群
1.封闭性:两个自然数相加依然是自然数——成立
2.结合律:两个自然数相加可以互换位置——成立
3.幺元:任何自然数与0相加仍然是自然数本身——成立
4.逆: 自然数都是非负数,不存在逆元素与其相加为幺元——不成立
对于
∀
a
∈
N
,
\forall a \in N,
∀a∈N, 且
a
≠
0
,
−
a
∉
N
{\kern 1pt} {\kern 1pt} a \ne 0, - a \notin N
a=0,−a∈/N不满足逆的性质要求
因此 {N,+}不是群。
3. 验证向量叉乘的李代数性质(2分,约1小时)
(详解版)链接
4. 推导 S E ( 3 ) {\bf{SE}}\left( 3 \right) SE(3) 的指数映射(2分,约1小时)
(详解版)链接
5. 伴随(2分,约1小时)
在
S
O
(
3
)
{\mathbf{SO}}\left( 3 \right)
SO(3) 和
S
E
(
3
)
{\mathbf{SE}}\left( 3 \right)
SE(3) 上,有⼀个东西称为伴随(Adjoint)。下⾯请你证明
S
O
(
3
)
{\mathbf{SO}}\left( 3 \right)
SO(3) 伴随的性质。对于
S
O
(
3
)
{\mathbf{SO}}\left( 3 \right)
SO(3) ,有:
R
exp
(
p
∧
)
R
T
=
exp
(
(
R
p
)
∧
)
.
\operatorname{R} \exp \left( {{p^ \wedge }} \right){{\mathbf{R}}^T} = \exp \left( {{{\left( {{\mathbf{R}}p} \right)}^ \wedge }} \right).
Rexp(p∧)RT=exp((Rp)∧).
此时称
A
d
(
R
)
=
R
Ad\left( R \right)=R
Ad(R)=R 。提⽰:⾸先你需要证明
∀
a
∈
R
3
,
\forall a \in {{\mathbf{R}}^3},
∀a∈R3,
R
a
∧
R
T
=
(
R
a
)
∧
R{a^ \wedge }{R^T}={\left( {Ra} \right)^ \wedge }
Ra∧RT=(Ra)∧ 页⾯https://math.stackexchange.com/questions/2190603/derivation-of-adjoint-for-so3提⽰了⼀种简洁的途径。
对于
S
E
(
3
)
{\mathbf{SE}}\left( 3 \right)
SE(3) ,有:
T
exp
(
ξ
∧
)
T
−
1
=
exp
(
(
A
d
(
T
)
ξ
)
∧
)
T\exp \left( {{\xi ^ \wedge }} \right){T^{ - 1}} = \exp \left( {{{\left( {Ad\left( T \right)\xi } \right)}^ \wedge }} \right)
Texp(ξ∧)T−1=exp((Ad(T)ξ)∧)
其中
A
d
(
T
)
Ad\left( T \right)
Ad(T) 定义为:
这个性质将在后⽂的Pose Graph优化中⽤到。但是
S
E
(
3
)
{\mathbf{SE}}\left( 3 \right)
SE(3) 的证明较为复杂,不作要求。完整的
S
O
(
3
)
{\mathbf{SO}}\left( 3 \right)
SO(3) 和
S
E
(
3
)
{\mathbf{SE}}\left( 3 \right)
SE(3)性质见1和2。
答:证明
我们先来证明
R
a
∧
R
T
=
(
R
a
)
∧
R{a^ \wedge }{R^T}={\left( {Ra} \right)^ \wedge }
Ra∧RT=(Ra)∧
a
∧
v
=
a
×
v
{a^ \wedge }v = a \times v
a∧v=a×v
我们可以通过使等式的RHS作用于任意向量v来证明该等式:
(
R
a
)
∧
v
=
(
R
a
)
×
v
=
(
R
a
)
×
(
R
R
−
1
v
)
(
R
R
−
1
=
I
)
=
R
[
a
×
(
R
−
1
v
)
]
(
d
i
s
t
r
i
b
u
t
i
o
n
l
a
w
)
=
R
a
∧
R
−
1
v
(
a
s
s
o
c
i
a
t
i
v
e
l
a
w
)
\begin{gathered} {\left( {Ra} \right)^ \wedge }v = \left( {Ra} \right) \times v \\ = \left( {Ra} \right) \times (R{R^{ - 1}}v){\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} (R{R^{ - 1}} = I) \\ = R[a \times ({R^{ - 1}}v)]{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} (distribution{\text{ }}law) \\ = R{a^ \wedge }{R^{ - 1}}v{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} (associative{\text{ }}law) \\ \end{gathered}
(Ra)∧v=(Ra)×v=(Ra)×(RR−1v)(RR−1=I)=R[a×(R−1v)](distribution law)=Ra∧R−1v(associative law)
此得到:
(
R
a
)
∧
=
R
a
∧
R
−
1
{\left( {Ra} \right)^ \wedge } = R{a^ \wedge }{R^{ - 1}}
(Ra)∧=Ra∧R−1
而
R
R
R是正交矩阵,因此
(
R
a
)
∧
=
R
a
∧
R
T
{\left( {Ra} \right)^ \wedge } = R{a^ \wedge }{R^T}
(Ra)∧=Ra∧RT
法一:
exp
(
(
R
ρ
)
∧
)
=
∑
n
=
0
∞
1
n
!
(
(
R
ρ
)
∧
)
n
=
∑
n
=
0
∞
1
n
!
(
R
ρ
∧
R
T
)
n
=
∑
n
=
0
∞
1
n
!
(
(
R
ρ
∧
R
T
)
⋅
(
R
ρ
∧
R
T
)
⋅
⋯
⋅
(
R
ρ
∧
R
T
)
)
=
∑
n
=
0
∞
1
n
!
(
R
ρ
∧
R
T
R
ρ
∧
R
T
⋯
R
ρ
∧
R
T
)
=
∑
n
=
0
∞
1
n
!
(
R
ρ
∧
ρ
∧
⋯
ρ
∧
R
T
)
=
R
(
∑
n
=
0
∞
1
n
!
(
ρ
∧
)
n
)
R
T
=
R
exp
(
ρ
∧
)
R
T
\begin{array}{l} \exp \left( {{{\left( {R\rho } \right)}^ \wedge }} \right) = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {{{\left( {R\rho } \right)}^ \wedge }} \right)}^n}} = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {R{\rho ^ \wedge }{R^T}} \right)}^n}} \\ = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}\left( {\left( {R{\rho ^ \wedge }{R^T}} \right) \cdot \left( {R{\rho ^ \wedge }{R^T}} \right) \cdot {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \cdots {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \cdot \left( {R{\rho ^ \wedge }{R^T}} \right)} \right)} \\ = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}\left( {R{\rho ^ \wedge }{R^T}R{\rho ^ \wedge }{R^T} \cdots R{\rho ^ \wedge }{R^T}} \right)} \\ = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}\left( {R{\rho ^ \wedge }{\rho ^ \wedge } \cdots {\rho ^ \wedge }{R^T}} \right)} = R\left( {\sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {{\rho ^ \wedge }} \right)}^n}} } \right){R^T} = {\mathop{\rm R}\nolimits} \exp \left( {{\rho ^ \wedge }} \right){R^T} \end{array}
exp((Rρ)∧)=n=0∑∞n!1((Rρ)∧)n=n=0∑∞n!1(Rρ∧RT)n=n=0∑∞n!1((Rρ∧RT)⋅(Rρ∧RT)⋅⋯⋅(Rρ∧RT))=n=0∑∞n!1(Rρ∧RTRρ∧RT⋯Rρ∧RT)=n=0∑∞n!1(Rρ∧ρ∧⋯ρ∧RT)=R(n=0∑∞n!1(ρ∧)n)RT=Rexp(ρ∧)RT
法二:
令
ρ
=
θ
a
,
a
\rho = \theta a,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} a
ρ=θa,a为单位向量
罗德里格斯公式:
exp
(
ρ
∧
)
=
cos
θ
I
+
(
1
−
cos
θ
)
a
a
T
+
s
i
n
θ
a
∧
\exp \left( {{\rho ^ \wedge }} \right) = \cos \theta I + (1 - \cos \theta )a{a^T} + sin\theta {a^ \wedge }
exp(ρ∧)=cosθI+(1−cosθ)aaT+sinθa∧
R
exp
(
θ
a
∧
)
R
T
=
R
(
cos
θ
I
+
(
1
−
cos
θ
)
a
a
T
+
s
i
n
θ
a
∧
)
R
T
=
cos
θ
I
+
(
1
−
cos
θ
)
R
a
(
R
a
)
T
+
sin
θ
R
a
∧
R
T
=
cos
θ
I
+
(
1
−
cos
θ
)
R
a
(
R
a
)
T
+
sin
θ
(
R
a
)
∧
=
exp
(
θ
(
R
a
)
∧
)
=
exp
(
(
R
ρ
)
∧
)
\begin{array}{c} {\mathop{\rm R}\nolimits} \exp \left( {\theta {a^ \wedge }} \right){R^T} = R(\cos \theta I + (1 - \cos \theta )a{a^T} + sin\theta {a^ \wedge }){R^T}\\ = \cos \theta I + (1 - \cos \theta )Ra{\left( {Ra} \right)^T} + \sin \theta R{a^ \wedge }{R^T}\\ = \cos \theta I + (1 - \cos \theta )Ra{\left( {Ra} \right)^T} + \sin \theta {\left( {Ra} \right)^ \wedge }\\ = \exp \left( {\theta {{\left( {Ra} \right)}^ \wedge }} \right)\\ = \exp \left( {{{\left( {R\rho } \right)}^ \wedge }} \right) \end{array}
Rexp(θa∧)RT=R(cosθI+(1−cosθ)aaT+sinθa∧)RT=cosθI+(1−cosθ)Ra(Ra)T+sinθRa∧RT=cosθI+(1−cosθ)Ra(Ra)T+sinθ(Ra)∧=exp(θ(Ra)∧)=exp((Rρ)∧)
证毕。
在数学中,一个李群
G
G
G的伴随表示(adjoint representation)或伴随作用(adjoint action)是
G
G
G 在它自身的李代数上的自然表示。这个表示是群
G
G
G 在自身上的共轭作用的线性化形式。
6. 轨迹的描绘(2分,约1小时)
我们通常会记录机器⼈的运动轨迹,来观察它的运动是否符合预期。⼤部分数据集都会提供标准轨迹以供参考,如kitti、TUM-RGBD等。这些⽂件会有各⾃的格式,但⾸先你要理解它的内容。记世界坐标系为 W {W} W,机器⼈坐标系为 C {C} C,那么机器⼈的运动可以⽤ T W C {T_{WC}} TWC或 T C W {T_{CW}} TCW来描述。现在,我们希望画出机器⼈在世界当中的运动轨迹,请回答以下问题:
1.事实上,的平移部分即构成了机器⼈的轨迹。它的物理意义是什么?为何画出 T W C {T_{WC}} TWC的平移部分就得到了机器⼈的轨迹?
2.我为你准备了⼀个轨迹⽂件(code/trajectory.txt)。该⽂件的每⼀⾏由若⼲个数据组成,格式为 [ t , t x , t y , t z , q x , q y , q z , q w ] \left[ {t,{t_x},{t_y},{t_z},{q_x},{q_y},{q_z},{q_w}} \right] [t,tx,ty,tz,qx,qy,qz,qw]
其中 t t t为时间, t x , t y , t z {t_x},{t_y},{t_z} tx,ty,tz为的平移部分, q x , q y , q z , q w {q_x},{q_y},{q_z},{q_w} qx,qy,qz,qw是四元数表⽰的 T W C {T_{WC}} TWC的旋转部分, q w {q_w} qw为四元数实部。同时,我为你提供了画图程序draw_trajectory.cpp⽂件。该⽂件提供了画图部分的代码,请你完成数据读取部分的代码,然后书写CMakeLists.txt以让此程序运⾏起来。注意我们需要⽤到Pangolin库来画图,所以你需要事先安装Pangolin(如果你做了第⼀次作业,那么现在已经安装了)。CMakeLists.txt可以参照ORB-SLAM2部分。
答: 把draw_trajectory.cpp程序中增加两个头件#include <unistd.h>和#include <Eigen/Core>。
draw_trajectory.cpp对应代码如下:
#include "sophus/so3.h"
#include "sophus/se3.h"
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <Eigen/Core>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
// path to trajectory file
string trajectory_file = "/home/zhe/1/lianxi/3/plotTrajectory/trajectory.txt";
class SE3d;
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
int main(int argc, char **argv) {
vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;
//文件读取器
ifstream fin(trajectory_file);
if (!fin) {
cerr << "trajectory " << trajectory_file << " not found." << endl;
}
//如果eof()返回0,就没读完
while (!fin.eof()) {
//按照时间 平移 四元素的顺序定义并读取
double time, tx, ty, tz, qx, qy, qz, qw;
fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
Sophus::SE3 p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
poses.push_back(p1);
}
fin.close();
// end your code here
// draw trajectory in pangolin
DrawTrajectory(poses);
return 0;
}
/************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses) {
if (poses.empty()) {
cerr << "Trajectory is empty!" << endl;
return;
}
// create pangolin window and plot the trajectory
pangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
pangolin::OpenGlRenderState s_cam(
pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),
pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0)
);
pangolin::View &d_cam = pangolin::CreateDisplay()
.SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f)
.SetHandler(new pangolin::Handler3D(s_cam));
while (pangolin::ShouldQuit() == false) {
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
d_cam.Activate(s_cam);
glClearColor(1.0f, 1.0f, 1.0f, 1.0f);
glLineWidth(2);
for (size_t i = 0; i < poses.size() - 1; i++) {
glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
glBegin(GL_LINES);
auto p1 = poses[i], p2 = poses[i + 1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000); // sleep 5 ms
}
}
CmakeLists.txt对应代码如下:
cmake_minimum_required(VERSION 2.8)
project(plotTrajectory)
#添加库
#sophus
# 为使用 sophus,需要使用find_package命令找到它并赋给Sophus_INCLUDE_DIRS
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
#Pangolin生成一个libPangolin动态链接库
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")
#编译
add_executable(plotTrajectory draw_trajectory.cpp)
#链接
#target_link_libraries(plotTrajectory Sophus::Sophus)
target_link_libraries(plotTrajectory ${Sophus_LIBRARIES} )
target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES})
运行结果如下所示:
该图中:轨迹首尾颜色不一样,通过观察,发现是着色函数设置的颜色随位置变化.
7*. 轨迹的误差(2分,约1小时)
本题为附加题。除了画出真实轨迹以外,我们经常需要把SLAM估计的轨迹与真实轨迹相⽐较。下⾯说明⽐较的原理,请你完成⽐较部分的代码实现。设真实轨迹(ground-truth)为 T g {T_g} Tg,估计轨迹 T e {T_e} Te。它们都以 T W C {T_WC} TWC的形式存储,格式同上题。现在,你需要计算估计轨迹的误差。我们假设每⼀个 T g {T_g} Tg都与给定的 T e {T_e} Te对应。那么,对于任意第 i i i个位姿,它的误差可定义为:
e i = ∥ l o g ( T g i − 1 T e i ) ∨ ∥ 2 . {e_i} = \parallel log{(T_{gi}^{ - 1}{T_{ei}})^ \vee }{\parallel _{{\kern 1pt} {\kern 1pt} 2}}. ei=∥log(Tgi−1Tei)∨∥2.
即两个位姿之差的李代数⼆范数。于是,可以定义两条轨迹的均⽅根(Root-Mean-Square-Error,RMSE)误差为:
R
M
S
E
(
g
,
e
)
=
1
n
∑
i
=
1
n
e
i
2
.
RMSE\left( {g,e} \right) = \sqrt {\frac{1}{n}\sum\limits_{i = 1}^n {e_i^2} } .
RMSE(g,e)=n1i=1∑nei2
.
我为你准备了code/ground-truth.txt和code/estimate.txt两条轨迹。请你根据上⾯公式,实现RMSE的计算代码,给出最后的RMSE结果。作为验算,参考答案为:2.207。
注:
1.实际当中的轨迹⽐较还要更复杂⼀些。通常ground-truth由其他传感器记录(如vicon),它的采样频率通常⾼于相机的频率,所以在处理之前还需要按照时间戳对齐。另外,由于传感器坐标系不⼀致,还需要计算两个坐标系之间的差异。这件事也可以⽤ICP解得,我们将在后⾯的课程中讲到。
2.你可以⽤上题的画图程序将两条轨迹画在同⼀个图⾥,看看它们相差多少。
安装Sophus及问题解决
git clone https://github.com/strasdat/Sophus.git
cd Sophus
git checkout a621ff
mkdir build
cd build
cmake ..
make
安装过程中若报如下错误
安装sophus存在的问题:
CMakeFiles/Sophus.dir/build.make:65: recipe for target ‘CMakeFiles/Sophus.dir/sophus/so2.cpp.o’ failed
make[2]: *** [CMakeFiles/Sophus.dir/sophus/so2.cpp.o] Error 1
make[2]: Leaving directory ‘/home/drew/svo/workspace/Sophus/build’
CMakeFiles/Makefile2:107: recipe for target ‘CMakeFiles/Sophus.dir/all’ failed
make[1]: *** [CMakeFiles/Sophus.dir/all] Error 2
二、解决办法
1、针对cc1: all warnings being treated as errors
则:在 makefile 中找到 -Werror 将其注释掉或者删除
2、修改Sophus下的so2.cpp文件
将下面这个修改一下:
unit_complex_.real() = 1.;
unit_complex_.imag() = 0.;
修改为:
unit_complex_.real(1.);
unit_complex_.imag(0.);
编译时报:
CMakeFiles/plotTrajectory.dir/build.make:62: recipe for target 'CMakeFiles/plotTrajectory.dir/draw_trajectory.cpp.o' failed
make[2]: *** [CMakeFiles/plotTrajectory.dir/draw_trajectory.cpp.o] Error 1
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/plotTrajectory.dir/all' failed
make[1]: *** [CMakeFiles/plotTrajectory.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2
解决:
1、修改包含的头文件名
#include "sophus/se3.hpp"
改为:
#include "sophus/so3.h"
#include "sophus/se3.h"
2、修改代码
把各个地方的:
Sophus::SO3d
Sophus::SE3d
的“d”都去掉,改为:
Sophus::SO3
Sophus::SE3
3、修改CMakelists.txt
将:
target_link_libraries(useSophus Sophus::Sophus)
改成:
target_link_libraries( useSophus ${Sophus_LIBRARIES} )
**答:**添加的代码主要包括三部分:
- 读取两个文件ground-truth.txt和estimate.txt。
// path to file
//真实轨迹结果
string trajectory_gd = "/home/zhe/1/lianxi/3/wucha/groundtruth.txt";
//估计轨迹结果
string trajectory_est = "/home/zhe/1/lianxi/3/wucha/estimated.txt";
//利用typedef定义一个容器类型,里面装的是位姿(time 平移,四元数【旋转】)
typedef vector<Sophus::SE3,Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType;
TrajectoryType ReadTrajectory(const string &path);
TrajectoryType groundtruth=ReadTrajectory(trajectory_gd);
TrajectoryType estimated=ReadTrajectory(trajectory_est);
TrajectoryType ReadTrajectory(const string &path)
{
ifstream fin(path);//流入
TrajectoryType trajectory;
if(!fin)
{
cerr<<"could not find a trajectory file"<<endl;//cerr
return trajectory;
}
while(!fin.eof())//读文件
{
double time,tx,ty,tz,qx,qy,qz,qw;
fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
Sophus::SE3 p1(Eigen::Quaterniond(qx,qy,qz,qw),Eigen::Vector3d(tx,ty,tz));//得到位姿
trajectory.push_back(p1);
}
return trajectory;
}
2.计算rmse,对于已经得到的两个pose集合,可以通过以下代码计算。
void DrawTrajectory(const TrajectoryType >,const TrajectoryType &esti);
assert(!groundtruth.empty()&&!estimated.empty());
assert(groundtruth.size()==estimated.size());
double rmse=0;
for(std::size_t i=0;i<estimated.size();i++)
{
Sophus::SE3 p1=estimated[i],p2=groundtruth[i];//estimated and groundtruth : vector
double error =(p2.inverse()*p1).log().norm();//模长,见slam十四讲89页 公式4.44
rmse+=error*error;//得到模长的平方
}
rmse=rmse/double(estimated.size());
rmse=sqrt(rmse);
cout<<"绝对误差 rmse= "<< rmse<<endl;
DrawTrajectory(groundtruth,estimated);
return 0;
}
3.画图,修改轨迹绘制代码,函数部分代码如下所示:
while (pangolin::ShouldQuit()==false)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清除屏幕(颜色缓冲、深度缓冲)
d_cam.Activate(s_cam);//激活显示,并设置状态矩阵
glClearColor(1.0f,1.0f,1.0f,1.0f);
glLineWidth(2);//线宽
for(size_t i=0; i< gt.size();i++)
{
glColor3f(0.0f, 0.0f, 1.0f);//blue for 真实轨迹
glBegin(GL_LINES);
auto p1=gt[i],p2=gt[i+1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
for(size_t i=0;i<esti.size();i++)
{
glColor3f(1.0f, 0.0f, 0.0f); // red for 估计轨迹
glBegin(GL_LINES);
auto p1=esti[i],p2=esti[i+1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000);//休眠5ms
最后运行结果如下:
/home/zhe/1anxi/3/wucha/cmake-build-debug/plotError
绝对误差 rmse= 2.20727
Process finished with exit code 0
其中,蓝色轨迹表示真实值,红色轨迹表示估计值
完整代码如下:
1、draw_trajectory_error.cpp对应代码
#include "sophus/so3.h"
#include "sophus/se3.h"
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <Eigen/Core>
#include <Eigen/Geometry>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace Sophus;
using namespace std;
// path to file
//真实轨迹结果
string trajectory_gd = "/home/zhe/1/lianxi/3/wucha/groundtruth.txt";
//估计轨迹结果
string trajectory_est = "/home/zhe/1/lianxi/3/wucha/estimated.txt";
//利用typedef定义一个容器类型,里面装的是位姿(time 平移,四元数【旋转】)
typedef vector<Sophus::SE3,Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType;
void DrawTrajectory(const TrajectoryType >,const TrajectoryType &esti);
TrajectoryType ReadTrajectory(const string &path);
int main(int argc,char **argv)
{
TrajectoryType groundtruth=ReadTrajectory(trajectory_gd);
TrajectoryType estimated=ReadTrajectory(trajectory_est);
//assert宏的原型定义在<assert.h>中,其作用是如果它的条件返回错误,则终止程序执行,原型定义:
//#include <assert.h>
//void assert( int expression );
// assert的作用是现计算表达式 expression ,如果其值为假(即为0)
// 那么它先向stderr打印一条出错信息,然后通过调用 abort 来终止程序运行
assert(!groundtruth.empty()&&!estimated.empty());
assert(groundtruth.size()==estimated.size());
//计算 绝对轨迹误差 rmse
//size_t它是一种“整型”类型,里面保存的是一个整数,就像int, long那样。
// 这种整数用来记录一个大小(size)。size_t的全称应该是size type,就是说“一种用来记录大小的数据类型”
//通常我们用sizeof(XXX)操作,这个操作所得到的结果就是size_t类型。
// 因为size_t类型的数据其实是保存了一个整数,所以它也可以做加减乘除,也可以转化为int并赋值给int类型的变量
//示例代码:
//int i; // 定义一个int类型的变量i
//size_t size = sizeof(i); // 用sizeof操作得到变量i的大小,这是一个size_t类型的值
// // 可以用来对一个size_t类型的变量做初始化
//i = (int)size; // size_t类型的值可以转化为int类型的值
double rmse=0;
for(std::size_t i=0;i<estimated.size();i++)
{
Sophus::SE3 p1=estimated[i],p2=groundtruth[i];//estimated and groundtruth : vector
double error =(p2.inverse()*p1).log().norm();//模长,见slam十四讲89页 公式4.44
rmse+=error*error;//得到模长的平方
}
rmse=rmse/double(estimated.size());
rmse=sqrt(rmse);
cout<<"绝对误差 rmse= "<< rmse<<endl;
DrawTrajectory(groundtruth,estimated);
return 0;
}
TrajectoryType ReadTrajectory(const string &path)
{
ifstream fin(path);//流入
TrajectoryType trajectory;
if(!fin)
{
cerr<<"could not find a trajectory file"<<endl;//cerr
return trajectory;
}
while(!fin.eof())//读文件
{
double time,tx,ty,tz,qx,qy,qz,qw;
fin>>time>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
Sophus::SE3 p1(Eigen::Quaterniond(qx,qy,qz,qw),Eigen::Vector3d(tx,ty,tz));//得到位姿
trajectory.push_back(p1);
}
return trajectory;
}
void DrawTrajectory(const TrajectoryType >,const TrajectoryType &esti)
{
// 创建 pangolin 窗口,画轨迹
//creat and set titles 、 width 、height of windows
pangolin::CreateWindowAndBind(" Trajectory form truth and estimated ", 1024, 768);
//glEnable(GL_DEPTH_TEST)启用了之后,OpenGL在绘制的时候就会检查,当前像素前面是否有别的像素,如果别的像素挡道了它,那它就不会绘制,也就是说,OpenGL就只绘制最前面的一层。
//当我们需要绘制透明图片时,就需要关闭它glDisable(GL_DEPTH_TEST);
//并且打开混合 glEnable(GL_BLEND);
glEnable(GL_DEPTH_TEST);
glEnable(GL_BLEND);
//glBlendFunc(GLenum sfactor,GLenum dfactor);
//源因子和目标因子是可以通过glBlendFunc函数来进行设置的。
// glBlendFunc有两个参数,前者sfactor表示源因子,后者dfactor表示目标因子。
//前者sfactor表示源颜色,后者dfactor表示目标颜色
//GL_SRC_ALPHA:表示使用源颜色的alpha值来作为因子
//GL_ONE_MINUS_SRC_ALPHA:表示用1.0减去源颜色的alpha值来作为因子(1-alpha)
glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);
//OpenGlRenderState 创建一个相机的观察视图,模拟相机
pangolin::OpenGlRenderState s_cam(//定义投影和初始模型视图矩阵
//ProjectionMatrix 前两个参数是相机的宽高,紧接着四个参数是相机的内参,最后两个是最近和最远视距
pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),
//ModelViewLookAt 前三个参数是相机的位置,紧接着三个是相机所看的视点的位置,最后三个参数是一个向量,表示相机的的朝向
pangolin::ModelViewLookAt(0,-0.1,-1.8,0,0,0,0.0,-1.0,0.0)
);
//在窗口创建交互式视图
pangolin::View &d_cam=pangolin::CreateDisplay().SetBounds(0.0,1.0,pangolin::Attach::Pix(1.0),1.0,-1024.0f/768.0f).
//SetBounds() 前四个参数是视图在视窗中的范围(下 上 左 右),建议动手改改参数就明白了
SetHandler(new pangolin::Handler3D(s_cam));
// SetHandle设置相机的视图句柄,需要用它来显示前面设置的 “相机” 所 “拍摄” 的内容
while (pangolin::ShouldQuit()==false)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);//清除屏幕(颜色缓冲、深度缓冲)
d_cam.Activate(s_cam);//激活显示,并设置状态矩阵
glClearColor(1.0f,1.0f,1.0f,1.0f);
glLineWidth(2);//线宽
for(size_t i=0; i< gt.size();i++)
{
glColor3f(0.0f, 0.0f, 1.0f);//blue for 真实轨迹
glBegin(GL_LINES);
auto p1=gt[i],p2=gt[i+1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
for(size_t i=0;i<esti.size();i++)
{
glColor3f(1.0f, 0.0f, 0.0f); // red for 估计轨迹
glBegin(GL_LINES);
auto p1=esti[i],p2=esti[i+1];
glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);
glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);
glEnd();
}
pangolin::FinishFrame();
usleep(5000);//休眠5ms
}
}
2、CMakeLists.txt对应代码
cmake_minimum_required(VERSION 2.8)
project(wucha)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
#set(CMAKE_CXX_STANDARD 11)
#set(CMAKE_BUILD_TYPE "Release")
#set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
#set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)
#添加库
#sophus
# 为使用 sophus,需要使用find_package命令找到它并赋给Sophus_INCLUDE_DIRS
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
#Pangolin生成一个libPangolin动态链接库
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")
#编译
add_executable(plotError draw_trajectory_error.cpp)
#链接
#target_link_libraries(plotError Sophus::Sophus)
target_link_libraries(plotError ${Sophus_LIBRARIES} )
target_link_libraries(plotError ${Pangolin_LIBRARIES})
标签:wedge,right,推导,李群,kern,SLAM,Sophus,1pt,left 来源: https://blog.csdn.net/weixin_38493195/article/details/118053754