文章目录
前言
系统环境:ubuntu20.04
这几天看了视觉SLAM十四讲第2版的第4讲李群与李代数,简直被吸进去了。感觉还是挺有意思的,这里由于我想总结4.1-4.3部分的东西都是书上的,暂时没有太多新的感悟,因此就不再复述了。主要讲下4.4实践部分和后面的习题部分。
一、4.4实践部分
sophus库直接git clone下来,然后cmake…&make 就好了
1.useSophus项目
直接编译运行项目就好了,不需要像书上88页一样增加什么语句
上图是输出的结果。代码很简单也很好理解。
2.trajectoryError项目
紧接着编译trajectoryError项目就报错了。
主要是报了fatal:error:sophus/se3.hpp没有这个文件或目录。
研究了一下代码,报错的主要是这句代码#include “sophus/se3.hpp”,但在useSophus.cpp中一样有这一句,为什么useSophus项目不报错。再仔细看了两个项目的Cmakelist.txt文件,发现很重要的问题,就是useSophus项目有链接Sophus库,但是trajectoryError项目的Cmakelist.txt文件里没有链接语句。此外还多了一句find_package,但由于trajectoryError项目是useSophus项目的子目录来着,所以估计是共享的,因此在example文件夹里的cmakelist.txt中加上下面这句语句:
target_link_libraries(trajectoryError Sophus::Sophus)
就解决问题了。
在网上搜索相关资料的时候还发现了很多人都在建议make install。想了一下,install是将目前的东西都安装到常用库的位置,因此就可以找到对应的文件,也没有必要特地链接。因此我抱着试一试的态度也试了一下,但是还是报错:
感觉是版本不一致导致的,这里想要unstall恢复原来的还不行,因为没有写好的unstall可以用。
查阅资料后才恢复了原状:如何撤销Install安装的东西
结论:make install不可行,直接在Cmakelist文件中增加链接即可。
改了两个文件的地址后成功运行出结果:RMSE=2.20728
二、第4讲习题
1.验证SO(3)、SE(3)、Sim(3)关于乘法成群
主要是验证满足上面四个条件,下面一一进行验证。
(1)验证SO(3)关于乘法成群
设A,B为两个任意的SO(3),均满足正交且行列式为1的性质。
封闭性:
(
A
B
)
−
1
=
(
B
)
−
1
(
A
)
−
1
=
(
B
)
T
(
A
)
T
=
(
A
B
)
T
(AB)^{-1}=(B)^{-1}(A)^{-1}=(B)^T(A)^T = (AB)^T
(AB)−1=(B)−1(A)−1=(B)T(A)T=(AB)T,因此正交性满足;又两个矩阵相乘后的行列式等于相乘前的行列式直接相乘,即
d
e
t
(
A
B
)
=
d
e
t
(
A
)
d
e
t
(
B
)
=
1
det(AB) = det(A)det(B) = 1
det(AB)=det(A)det(B)=1,所以行列式为1,SO(3)关于乘法的封闭性得证。
结合律:矩阵乘法必定满足结合律。
幺元:3×3的单位矩阵满足这个条件,因此
a
0
=
I
3
×
3
a_0 = I_{3×3}
a0=I3×3,故幺元存在。
逆:SO(3)中的成员均为正交且行列式为1的矩阵,设
B
=
A
−
1
=
A
T
B=A^{-1}=A^T
B=A−1=AT,因此B明显也是行列式为1且正交的矩阵,此时
A
B
=
I
=
a
0
AB=I=a_0
AB=I=a0,故逆得证
(2)验证SE(3)关于乘法成群
封闭性:根据分块矩阵相乘的特性,两个矩阵的相乘结果如下:
因此此时只需要证明上面部分是否符合条件即可。在SO(3)的封闭性中已经证明了
R
1
R
2
R_1R_2
R1R2是一个行列式为1的正交矩阵;又
R
1
t
2
+
t
1
R_1t_2+t_1
R1t2+t1也明显是个三维向量,因此封闭性得证。
结合律:矩阵乘法必定满足结合律。
幺元:4×4的单位矩阵满足这个条件,因此
a
0
=
I
4
×
4
a_0 = I_{4×4}
a0=I4×4
逆:设T为一个任意的SE(3),首先选中最后一行去展开并计算T的行列式,可以得到SE(3)的行列式也为1,不为0,因此必定存在逆,书上已经帮我们求出了它的逆如下:
此时可以较易验证
T
−
1
T^{-1}
T−1满足条件,R部分即
R
T
∈
S
O
(
3
)
R^T∈SO(3)
RT∈SO(3),且t部分即
−
R
T
t
∈
R
3
-R^Tt∈R^3
−RTt∈R3且
T
T
−
1
=
I
=
a
0
TT^{-1}=I=a_0
TT−1=I=a0,故逆得证。
(3)验证Sim(3)关于乘法成群
封闭性:根据分块矩阵相乘的特性,两个矩阵的相乘结果如下:
因此此时只需要证明上面部分是否符合条件即可。在SE(3)的封闭性中已经证明了
R
1
R
2
R_1R_2
R1R2是一个行列式为1的正交矩阵;因此
s
1
s
2
R
1
R
2
s_1s_2R_1R_2
s1s2R1R2明显是满足定义的,又
s
1
R
1
t
2
+
t
1
s_1R_1t_2+t_1
s1R1t2+t1也明显是个三维向量,因此封闭性得证。
结合律:矩阵乘法必定满足结合律。
幺元:4×4的单位矩阵满足这个条件,因此
a
0
=
I
4
×
4
a_0 = I_{4×4}
a0=I4×4
逆:设T为一个任意的Sim(3),首先选中最后一行去展开并计算T的行列式,可以得到Sim(3)的行列式也为s,不为0,因此必定存在逆。我简单算了一下,它的逆如下:
此时可以较易验证
T
−
1
T^{-1}
T−1满足条件,R部分即
1
/
s
R
T
∈
S
O
(
3
)
1/sR^T∈SO(3)
1/sRT∈SO(3),且t部分即
−
1
/
s
R
T
t
∈
R
3
-1/sR^Tt∈R^3
−1/sRTt∈R3且
T
T
−
1
=
I
=
a
0
TT^{-1}=I=a_0
TT−1=I=a0,故逆得证。
综上,SO(3)、SE(3)、Sim(3)关于乘法成群。
参考资料:
①2个矩阵相乘的行列式=它们的行列式相乘
②矩阵分块相乘
2.验证 ( R 3 , R , × ) (R^3,R,×) (R3,R,×)构成李代数
李代数性质如下:
验证过程如下
封闭性:由上图可知两个向量×的结果,必定是一个新的向量,方向与两个向量都垂直,且大小为
∣
X
∣
∣
Y
∣
s
i
n
<
X
,
Y
>
|X||Y|sin<X,Y>
∣X∣∣Y∣sin<X,Y>,因此必定也在
R
3
R^3
R3中。
双线性:将【aX+bY,Z】代入外积公式化简后可得a【X,Z】+b【Y,Z】可知,同理,调换后依然可证,因此其满足双线性
自反性:还是将【X,X】代入外积公式,结果i,j,k对应的系数均为0,故自反性得证。
雅可比等价:验证了一通后发现不对劲,后来才发现上面的截图是第一版,然后公式写错了,第二版纠正过来了。正确的雅可比等价公式如下(标黑处为修改处):
∀X, Y , Z ∈ V, [X, [Y , Z]] + [Z, [X , Y]] + [Y , [Z, X]] = 0
代入外积公式后验证无误,因此雅可比等价得证。
参考资料:向量的内积与外积
3.验证so(3)和se(3)满足李代数要求的性质
SO(3)对应的李代数为定义在
R
3
R^3
R3上的向量,如下
封闭性:设2个李代数为
A
=
[
a
1
,
a
2
,
a
3
]
T
,
B
=
[
b
1
,
b
2
,
b
3
]
T
A=[a_1,a_2,a_3]^T,B=[b_1,b_2,b_3]^T
A=[a1,a2,a3]T,B=[b1,b2,b3]T,验证上面的李括号的封闭性可得[A,B]如下
可以看到明显满足定义,因此封闭性得到证明
双线性:设3个李代数为
X
=
[
x
1
,
x
2
,
x
3
]
T
,
Y
=
[
y
1
,
y
2
,
y
3
]
T
,
Z
=
[
z
1
,
z
2
,
z
3
]
T
X=[x_1,x_2,x_3]^T,Y=[y_1,y_2,y_3]^T,Z=[z_1,z_2,z_3]^T
X=[x1,x2,x3]T,Y=[y1,y2,y3]T,Z=[z1,z2,z3]T,因此
此时可计算
[
X
,
Z
]
、
[
Y
,
Z
]
和
a
[
X
,
Z
]
+
b
[
Y
,
Z
]
如下
[X,Z]、[Y,Z]和a[X,Z]+b[Y,Z]如下
[X,Z]、[Y,Z]和a[X,Z]+b[Y,Z]如下
因此[aX + bY , Z] = a[X, Z] + b[Y , Z]得证,同理也易得[Z, aX + bY ] = a[Z, X] + b[Z, Y ]。
自反性:同上,计算[A,A]结果如下
结果为0,因此自反性得证。
雅可比等价:需证∀X, Y , Z ∈ V, [X, [Y , Z]] + [Z, [X , Y]] + [Y , [Z, X]] = 0
对应代入相关的T、P、Q可以化简得矩阵的每项都为0,因此雅可比等价得证。
4.验证性质(4.20)和(4.21)
将a^和a代入验证即可知成立
综上4.20和4.21的性质得证。
5.证明式子1
故原式得证。
6.证明式子2
SO(3)的伴随性质证明如下:
将中间部分进行泰勒展开,又
R
R
T
=
R
T
R
=
I
RR^T=R^TR=I
RRT=RTR=I,所以
根据定义
R
e
x
p
(
p
ˆ
)
R
T
=
e
x
p
(
R
p
ˆ
R
T
)
Rexp(p\^{})R^T = exp(Rp\^{}R^T)
Rexp(pˆ)RT=exp(RpˆRT)
又由第5题知
R
p
ˆ
R
T
=
(
R
p
)
ˆ
Rp\^{}R^T = (Rp)\^{}
RpˆRT=(Rp)ˆ
所以
R
e
x
p
(
p
ˆ
)
R
T
=
e
x
p
(
R
p
ˆ
R
T
)
=
e
x
p
(
(
R
p
)
ˆ
)
Rexp(p\^{})R^T = exp(Rp\^{}R^T) = exp((Rp)\^{})
Rexp(pˆ)RT=exp(RpˆRT)=exp((Rp)ˆ),故SO(3)的伴随性质得证!
SE(3)不想证了,可以下面的参考资料。
参考资料:SE(3)的伴随性质证明
7.推导SO(3)和SE(3)在右扰动下的导数
关于SO(3)在右扰动下的导数推导如下:
主要利用了v^ p = -p ^v这个公式进行转化,p和v都是三维向量。
关于SE(3)在右扰动下的导数推导如下:
还利用了SO(3)推导过程中的最后一步。
8.cmake的find_package指令如何运作
主要运作是根据库的名字来区分的,一般会进去默认工作模式即Module模式,Module模式也找不到的时候再进入Config模式即高级模式,对库进行搜索,然后再进行配置使用。可选的参数根据两个模式不同也不大一样,而且默认工作模式只会搜索2个地址,但高级模式会搜索很多个地址,尽可能地找到库。
Modelu模式下对应的参数:
find_package(<PackageName> [version] [EXACT] [QUIET] [MODULE]
[REQUIRED] [[COMPONENTS] [components...]]
[OPTIONAL_COMPONENTS components...]
[NO_POLICY_SCOPE])
Config模式下对应的参数:
find_package(<PackageName> [version] [EXACT] [QUIET]
[REQUIRED] [[COMPONENTS] [components...]]
[OPTIONAL_COMPONENTS components...]
[CONFIG|NO_MODULE]
[NO_POLICY_SCOPE]
[NAMES name1 [name2 ...]]
[CONFIGS config1 [config2 ...]]
[HINTS path1 [path2 ... ]]
[PATHS path1 [path2 ... ]]
[PATH_SUFFIXES suffix1 [suffix2 ...]]
[NO_DEFAULT_PATH]
[NO_PACKAGE_ROOT_PATH]
[NO_CMAKE_PATH]
[NO_CMAKE_ENVIRONMENT_PATH]
[NO_SYSTEM_ENVIRONMENT_PATH]
[NO_CMAKE_PACKAGE_REGISTRY]
[NO_CMAKE_BUILDS_PATH] # Deprecated; does nothing.
[NO_CMAKE_SYSTEM_PATH]
[NO_CMAKE_SYSTEM_PACKAGE_REGISTRY]
[CMAKE_FIND_ROOT_PATH_BOTH |
ONLY_CMAKE_FIND_ROOT_PATH |
NO_CMAKE_FIND_ROOT_PATH])
详细解释可以参考我后面的参考资料,然后为了让cmake找到某个库,先决条件我感觉如果安装在了常见的那些include等系统位置,则没有什么问题;否则一般要在Cmakelist.txt文件中添加一些地址提示。但是很神奇的是这次项目中,我没有对Sophus这个库添加任何地址提示,只是使用了find_package,但是还是能找到对应的库并执行相应的操作,感觉是用cmake进行编译的项目可能会有记录?或者是项目所在的上层目录及下层目录还是会进行搜索。欢迎懂的兄弟姐妹来交流下。
参考资料:find_package的相关资料
总结
本文对第4讲的实践部分进行了梳理,同时用一行代码解决了找不到<sophus/se3.hpp>的问题。并对第4讲的习题进行总结。太久没写数学了,这次的博客写了四天左右,太难了。也欢迎大家交流!