slam十四讲第3章作业
2.1
{ Z , + } \{\mathbb Z,+\} {Z,+}是群
封闭性: ∀ Z 1 , Z 2 ∈ Z , 满足 Z 1 + Z 2 ∈ Z 结合性: ∀ Z 1 , Z 2 , Z 3 ∈ Z , 满足 ( Z 1 + Z 2 ) + Z 3 = Z 1 + ( Z 2 + Z 3 ) 幺元: ∃ Z 0 = 0 , ∀ Z 1 ∈ Z , 满足 Z 1 + 0 = 0 + Z 1 = Z 1 逆: ∀ Z 1 ∈ Z , ∃ ( − Z 1 ) ∈ Z , 满足 Z 1 + ( − Z 1 ) = 0 \begin{aligned} &\text { 封闭性: } \forall Z_{1}, Z_{2} \in \mathbb Z, \text { 满足 } Z_{1}+Z_{2} \in \mathbb Z\\ &\text { 结合性: } \forall Z_{1}, Z_{2}, Z_{3} \in \mathbb Z, \text { 满足 }\left(Z_{1}+Z_{2}\right)+Z_{3}=Z_{1}+\left(Z_{2}+Z_{3}\right)\\ &\text { 幺元: } \exists Z_{0}=0, \forall Z_{1} \in \mathbb Z, \text { 满足 } Z_{1}+0=0+Z_{1}=Z_{1}\\ &\text { 逆: } \forall Z_{1} \in \mathbb Z, \exists(-Z_{1} )\in \mathbb Z, \text { 满足 } Z_{1}+\left(-Z_{1}\right)=0 \end{aligned} 封闭性: ∀Z1,Z2∈Z, 满足 Z1+Z2∈Z 结合性: ∀Z1,Z2,Z3∈Z, 满足 (Z1+Z2)+Z3=Z1+(Z2+Z3) 幺元: ∃Z0=0,∀Z1∈Z, 满足 Z1+0=0+Z1=Z1 逆: ∀Z1∈Z,∃(−Z1)∈Z, 满足 Z1+(−Z1)=0
2.2
{ N , + } \{\mathbb N,+\} {N,+}不是群，自然数集里面没有负数，不满足逆条件
除 0 以 外 ， ∀ N 1 ∈ N , 都 不 存 在 ( − N 1 ) ∈ N , 满足 N 1 + ( − N 1 ) = 0 除0以外，\forall N_{1} \in \mathbb N, 都不存在(-N_{1} )\in \mathbb N, \text { 满足 } N_{1}+\left(-N_{1}\right)=0 除0以外，∀N1∈N,都不存在(−N1)∈N, 满足 N1+(−N1)=0
3.
- 封闭性 ∀ X , Y ∈ R 3 , X × Y ∈ R 3 \quad \forall X, Y \in \mathbb{R}^3,X\times Y \in \mathbb{R}^3 ∀X,Y∈R3,X×Y∈R3
- 双线性 ∀ X , Y , Z ∈ R 3 , a , b ∈ R , \quad \forall X, Y, Z \in \mathbb{R}^3, a, b \in \mathbb{R}, ∀X,Y,Z∈R3,a,b∈R, 有:
( a X + b Y ) × Z = a ( X × Z ) + b ( Y × Z ) , Z × ( a X + b Y ) = a ( Z × X ) + b ( Z × Y ) (a X+b Y)\times Z=a(X\times Z)+b(Y\times Z), \quad Z\times (a X+b Y)=a(Z\times X)+b(Z\times Y) (aX+bY)×Z=a(X×Z)+b(Y×Z),Z×(aX+bY)=a(Z×X)+b(Z×Y) - 自反性 ∀ X ∈ R 3 , X × X = 0 \quad \forall X \in \mathbb{R}^3,X\times X=0 ∀X∈R3,X×X=0
- 雅可比等价
由向量叉乘的拉格朗日公式： ( a × b ) × c = b ( a ⋅ c ) − a ( b ⋅ c ) a × ( b × c ) = b ( a ⋅ c ) − c ( a ⋅ b ) (a\times b)\times c=b(a\cdot c)-a(b\cdot c) \quad a\times (b\times c)=b(a\cdot c)-c(a\cdot b) (a×b)×c=b(a⋅c)−a(b⋅c)a×(b×c)=b(a⋅c)−c(a⋅b) （定理证明如下，就是展开计算）
∀ X , Y , Z ∈ R 3 , X × ( Y × Z ) + Y × ( Z × X ) + Z × ( X × Y ) = Y ( X ⋅ Z ) − Z ( X ⋅ Y ) + Z ( Y ⋅ X ) − X ( Y ⋅ Z ) + X ( Z ⋅ Y ) − Y ( Z ⋅ X ) = 0 \quad \forall X, Y, Z \in \mathbb{R}^3,X\times(Y\times Z)+Y\times(Z\times X)+Z\times(X\times Y)\\=Y(X\cdot Z)-Z(X\cdot Y) + Z(Y\cdot X)-X(Y\cdot Z)+ X(Z\cdot Y)-Y(Z\cdot X)=0 ∀X,Y,Z∈R3,X×(Y×Z)+Y×(Z×X)+Z×(X×Y)=Y(X⋅Z)−Z(X⋅Y)+Z(Y⋅X)−X(Y⋅Z)+X(Z⋅Y)−Y(Z⋅X)=0
4.
题中小错误，应该为令 ϕ = θ a \phi=\theta \bm a ϕ=θa
有 a ∧ a ∧ = a a T − I , a ∧ a ∧ a ∧ = − a ∧ \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}=\boldsymbol{a}\boldsymbol{a}^{T}-\boldsymbol{I}, \quad \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}=-\boldsymbol{a}^{\wedge} a∧a∧=aaT−I,a∧a∧a∧=−a∧
∑ n = 0 ∞ 1 n ! ( θ a ∧ ) n = I + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ + 1 3 ! θ 3 a ∧ a ∧ a ∧ + 1 4 ! θ 4 ( a ∧ ) 4 + … = a a T − a ∧ a ∧ + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ − 1 3 ! θ 3 a ∧ − 1 4 ! θ 4 ( a ∧ ) 2 + … = a a T + ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 − … ) a ∧ − ( 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − … ) a ∧ a ∧ = a ∧ a ∧ + I + sin θ a ∧ − cos θ a ∧ a ∧ = ( 1 − cos θ ) a ∧ a ∧ + I + sin θ a ∧ = cos θ I + ( 1 − cos θ ) a a T + sin θ a ∧ = d e f R \begin{aligned} \sum_{n=0} ^{\infty}\frac{1}{n !}\left(\theta \boldsymbol{a}^{\wedge}\right)^{n} &=\boldsymbol{I}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}-\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}-\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge}-\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}+\left(\theta-\frac{1}{3 !} \theta^{3}+\frac{1}{5 !} \theta^{5}-\ldots\right) \boldsymbol{a}^{\wedge}-\left(1-\frac{1}{2 !} \theta^{2}+\frac{1}{4 !} \theta^{4}-\ldots\right) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge}-\cos \theta \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=(1-\cos \theta) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge} \\ &=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge}\stackrel{\mathrm{def}}{=} \boldsymbol{R} \end{aligned} n=0∑∞n!1(θa∧)n=I+θa∧+2!1θ2a∧a∧+3!1θ3a∧a∧a∧+4!1θ4(a∧)4+…=aaT−a∧a∧+θa∧+2!1θ2a∧a∧−3!1θ3a∧−4!1θ4(a∧)2+…=aaT+(θ−3!1θ3+5!1θ5−…)a∧−(1−2!1θ2+4!1θ4−…)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧=defR
∑ n = 0 ∞ 1 ( n + 1 ) ! ( θ a ∧ ) n = I + 1 2 ! θ a ∧ + 1 3 ! θ 2 ( a ∧ ) 2 + 1 4 ! θ 3 ( a ∧ ) 3 + 1 5 ! θ 4 ( a ∧ ) 4 ⋯ = 1 θ ( 1 2 ! θ 2 − 1 4 ! θ 4 + ⋯ ) ( a ∧ ) + 1 θ ( 1 3 ! θ 3 − 1 5 θ 5 + ⋯ ) ( a ∧ ) 2 + I = 1 θ ( 1 − cos θ ) ( a ∧ ) + θ − sin θ θ ( a a T − I ) + I = sin θ θ I + ( 1 − sin θ θ ) a a T + 1 − cos θ θ a ∧ = d e f J \begin{aligned} \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\theta \boldsymbol{a}^{\wedge}\right)^{n} &=\boldsymbol{I}+\frac{1}{2 !} \theta \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{2}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\frac{1}{4 !} \theta^{3}\left(\boldsymbol{a}^{\wedge}\right)^{3}+\frac{1}{5 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4} \cdots \\ &=\frac{1}{\theta}\left(\frac{1}{2 !} \theta^{2}-\frac{1}{4 !} \theta^{4}+\cdots\right)\left(\boldsymbol{a}^{\wedge}\right)+\frac{1}{\theta}\left(\frac{1}{3 !} \theta^{3}-\frac{1}{5} \theta^{5}+\cdots\right)\left(\boldsymbol{a}^{\wedge}\right)^{2}+\boldsymbol{I} \\ &=\frac{1}{\theta}(1-\cos \theta)\left(\boldsymbol{a}^{\wedge}\right)+\frac{\theta-\sin \theta}{\theta}\left(\boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}\right)+\boldsymbol{I} \\ &=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \stackrel{\mathrm{def}}{=} \boldsymbol{J} \end{aligned} n=0∑∞(n+1)!1(θa∧)n=I+2!1θa∧+3!1θ2(a∧)2+4!1θ3(a∧)3+5!1θ4(a∧)4⋯=θ1(2!1θ2−4!1θ4+⋯)(a∧)+θ1(3!1θ3−51θ5+⋯)(a∧)2+I=θ1(1−cosθ)(a∧)+θθ−sinθ(aaT−I)+I=θsinθI+(1−θsinθ)aaT+θ1−cosθa∧=defJ
故最终有：
exp ( ξ ∧ ) = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] ≜ [ R J ρ 0 T 1 ] = T \begin{aligned} \exp \left(\boldsymbol{\xi}^{\wedge}\right) &= \left[ \begin{array}{cc} \sum\limits_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} & \sum\limits_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} \boldsymbol{\rho} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array} \right] \\ & \triangleq\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{J} \rho \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right]=\boldsymbol{T} \end{aligned} exp(ξ∧)=⎣⎡n=0∑∞n!1(ϕ∧)n0Tn=0∑∞(n+1)!1(ϕ∧)nρ1⎦⎤≜[R0TJρ1]=T
5.
参考链接 https://math.stackexchange.com/questions/2190603/derivation-of-adjoint-for-so3
先证： R a ∧ R T = ( R a ) ∧ \bm R\bm a^{\wedge} \bm R^T = (\bm R \bm a)^{\wedge} Ra∧RT=(Ra)∧
∀ v ∈ R 3 , ( R a ) ∧ v = ( R a ) × v = ( R a ) × ( R R − 1 v ) = R [ a × ( R − 1 v ) ] = R a ∧ R − 1 v \forall \bm{v} \in \mathbb{R}^3,(\bm{R} \bm a)^{\wedge}\bm{v}=(\bm{R} \bm a) \times \bm{v}=(\bm{R} \bm a) \times\left(\bm{R} \bm{R}^{-1} \bm{v}\right)=\bm{R}\left[\boldsymbol{\bm a} \times\left(\bm{R}^{-1} \mathbf{v}\right)\right]=\bm{R} \bm a^{\wedge} \bm{R}^{-1} \bm{v} ∀v∈R3,(Ra)∧v=(Ra)×v=(Ra)×(RR−1v)=R[a×(R−1v)]=Ra∧R−1v
对exp进行泰勒展开再合并
exp ( ( R p ) ∧ ) = ∑ n = 0 ∞ 1 n ! ( ( R p ) ∧ ) n = ∑ n = 0 ∞ 1 n ! ( R p ∧ R T ) n = ∑ n = 0 ∞ 1 n ! ( ( R p ∧ R T ) ⋅ ( R p ∧ R T ) ⋅ ⋯ ⋅ ( R p ∧ R T ) ) = ∑ n = 0 ∞ 1 n ! ( R p ∧ R T R p ∧ R T ⋯ R p ∧ R T ) = ∑ n = 0 ∞ 1 n ! ( R p ∧ p ∧ ⋯ p ∧ R T ) = R ( ∑ n = 0 ∞ 1 n ! ( p ∧ ) n ) R T = R exp ( p ∧ ) R T \exp \left((\boldsymbol{R} \boldsymbol{p})^{\wedge}\right)=\sum_{n=0} ^{\infty}\frac{1}{n !}\left((\boldsymbol{R} \boldsymbol{p})^{\wedge}\right)^{n} =\sum_{n=0} ^{\infty}\frac{1}{n !}\left(\bm R\bm p^{\wedge} \bm R^T \right)^{n} \\ =\sum_{n=0} ^{\infty}\frac{1}{n !} \left( \left(\bm R\bm p^{\wedge} \bm R^T \right) \cdot \left(\bm R\bm p^{\wedge} \bm R^T \right)\cdot \quad \cdots \quad \cdot \left(\bm R\bm p^{\wedge} \bm R^T \right) \right) \\ =\sum_{n=0} ^{\infty}\frac{1}{n !} \left(\bm R\bm p^{\wedge} \bm R^T\bm R\bm p^{\wedge} \bm R^T \cdots \bm R\bm p^{\wedge} \bm R^T \right) \\ =\sum_{n=0} ^{\infty}\frac{1}{n !} \left(\bm R\bm p^{\wedge} \bm p^{\wedge} \cdots\bm p^{\wedge} \bm R^T \right) =\bm R \left(\sum_{n=0} ^{\infty}\frac{1}{n !}\left(\bm p^{\wedge} \right)^{n} \right) \bm R^T =\boldsymbol{R} \exp \left(\boldsymbol{p}^{\wedge}\right) \boldsymbol{R}^{\mathrm{T}} exp((Rp)∧)=n=0∑∞n!1((Rp)∧)n=n=0∑∞n!1(Rp∧RT)n=n=0∑∞n!1((Rp∧RT)⋅(Rp∧RT)⋅⋯⋅(Rp∧RT))=n=0∑∞n!1(Rp∧RTRp∧RT⋯Rp∧RT)=n=0∑∞n!1(Rp∧p∧⋯p∧RT)=R(n=0∑∞n!1(p∧)n)RT=Rexp(p∧)RT
6.1
T W C \bm T_{WC} TWC的物理意义：某点从相机坐标系到世界坐标系的变换矩阵，当该点为相机坐标系原点时该矩阵表示了相机在世界坐标系中的位姿，其平移部分即为相机在世界坐标系中的坐标。画出 T W C \bm T_{WC} TWC的平移部分（相机在世界坐标系下坐标）便画出了机器人的轨迹。
6.2
cmake_minimum_required(VERSION 3.10)
project(plotTrajectory)
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)
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
include_directories("/usr/include/eigen3" ${Sophus_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})
add_executable(plotTrajectory draw_trajectory.cpp)
target_link_libraries(plotTrajectory Sophus::Sophus ${Pangolin_LIBRARIES})
#include 运行结果：
7.
cmake_minimum_required(VERSION 3.10)
project(TrajectoryError)
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)
find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
include_directories("/usr/include/eigen3" ${Pangolin_INCLUDE_DIRS}) #${Sophus_INCLUDE_DIRS}
add_executable(TrajectoryError TrajectoryError.cpp)
target_link_libraries(TrajectoryError Sophus::Sophus ${Pangolin_LIBRARIES}) # ${Sophus_LIBRARIES} Sophus::Sophus
#include 运行结果：
