SLAM十四讲第二次作业-深蓝学院
- r(A)=n:矩阵A的秩等于未知数的个数。
- ⾼斯消元法：通过用初等行变换将增广矩阵化为行阶梯阵，然后通过回代求解线性方程组的解。原理是将方程组中每个方程含有的未知数的个数降到最低，并且最下面的方程含有的未知数的个数最少。
- QR分解：把矩阵分解成一个列向量正交矩阵与一个上三角矩阵的积。原理是将矩阵每个列作为一个基本单元，将其化为正交的基向量与在这个基向量上的投影长度的积。
- Cholesky 分解：将一个对称正定矩阵分解成一个下三角矩阵与其共轭转置之乘积。
- 代码见：eigenMatrix.cpp与CMakeLists.txt
eigenMatrix.cpp文件
/*求解 A * x = B 这个方程*/
#include CMaleLists.txt文件
cmake_minimum_required( VERSION 2.8 )
project( useEigen )
set( CMAKE_BUILD_TYPE "Release" )
set( CMAKE_CXX_FLAGS "-O3" )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable( eigen eigenMatrix.cpp )
代码见：useGeometry.cpp与CMakeLists.txt
useGeometry.cpp文件
#include CMakeLists.txt文件
cmake_minimum_required( VERSION 2.8 )
project( geometry )
# 添加Eigen头文件
include_directories( "/usr/include/eigen3" )
add_executable( eigenGeometry eigenGeometry.cpp )
- 证明：
设某个单位正交基 ( e 1 , e 2 , e 3 ) (e_1,e_2,e_3) (e1,e2,e3)经过一次旋转变成了 ( f 1 , f 2 , f 3 ) (f_1,f_2,f_3) (f1,f2,f3)，那么旋转矩阵
R = [ e 1 T e 2 T e 3 T ] ⋅ [ f 1 f 2 f 3 ] ∵ [ e 1 T e 2 T e 3 T ] ⋅ [ e 1 e 2 e 3 ] = I ∴ R T ⋅ R = [ f 1 T f 2 T f 3 T ] ⋅ [ e 1 e 2 e 3 ] ⋅ [ e 1 T e 2 T e 3 T ] ⋅ [ f 1 f 2 f 3 ] = [ f 1 T f 2 T f 3 T ] ⋅ I ⋅ [ f 1 f 2 f 3 ] = I ∴ R T ⋅ R = I R = \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \cdot \begin{bmatrix} f_1 & f_2 &f_3 \end{bmatrix} \\ \because \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \cdot \begin{bmatrix} e_1& e_2 &e_3 \end{bmatrix} = I \\ \begin{aligned}\therefore R^T \cdot R &= \begin{bmatrix} f_1^T \\ f_2^T \\ f_3^T \end{bmatrix} \cdot \begin{bmatrix} e_1 & e_2 & e_3 \end{bmatrix} \cdot \begin{bmatrix} e_1^T \\ e_2^T \\ e_3^T \end{bmatrix} \cdot \begin{bmatrix} f_1 & f_2 &f_3 \end{bmatrix} \\ &= \begin{bmatrix} f_1^T \\ f_2^T \\ f_3^T \end{bmatrix} \cdot I \cdot \begin{bmatrix} f_1 & f_2 &f_3 \end{bmatrix} \\ &= I \end{aligned} \\ \therefore R^T \cdot R=I R=⎣⎡e1Te2Te3T⎦⎤⋅[f1f2f3]∵⎣⎡e1Te2Te3T⎦⎤⋅[e1e2e3]=I∴RT⋅R=⎣⎡f1Tf2Tf3T⎦⎤⋅[e1e2e3]⋅⎣⎡e1Te2Te3T⎦⎤⋅[f1f2f3]=⎣⎡f1Tf2Tf3T⎦⎤⋅I⋅[f1f2f3]=I∴RT⋅R=I
∣ R ∣ = ∣ e 1 T e 2 T e 3 T ∣ ⋅ ∣ f 1 f 2 f 3 ∣ = 1 ∴ d e t R = ± 1 \begin{aligned} |R| &= \begin{vmatrix} e_1^T \\ e_2^T \\ e_3^T \end{vmatrix} \cdot \begin{vmatrix} f_1 & f_2 &f_3 \end{vmatrix} \\ &=1 \\ \therefore detR = \pm 1 \end{aligned} ∣R∣∴detR=±1=∣∣∣∣∣∣e1Te2Te3T∣∣∣∣∣∣⋅∣∣f1f2f3∣∣=1
又因为单位正交基的模为+1，
∴ d e t R = + 1 \therefore detR = +1 ∴detR=+1
所以 R T R = I R^TR=I RTR=I，且 d e t R = + 1 detR=+1 detR=+1 - 证明：
四元数由四个数组成，有一个实部和三个虚部，形如： q = q 0 + q 1 i + q 2 j + q 3 k q=q_0+q_1i+q_2j+q_3k q=q0+q1i+q2j+q3k。那么上述的 ε = [ q 1 , q 2 , q 3 ] T ∈ R 3 \varepsilon = [ q_1,q_2,q_3]^T \in \mathbb{R}^3 ε=[q1,q2,q3]T∈R3， η = q 0 ∈ R \eta = q_0 \in \mathbb{R} η=q0∈R。
所以 ε \varepsilon ε和 η \eta η的维度分别为 3和1。 - 证明： （一定要注意四元数实部与虚部的顺序,这里实部在最后）
令 q 1 = [ x 1 , y 1 , z 1 , w 1 ] , q 2 = [ x 2 , y 2 , z 2 , w 2 ] q_1=[x_1,y_1,z_1,w_1],q_2=[x_2,y_2,z_2,w_2] q1=[x1,y1,z1,w1],q2=[x2,y2,z2,w2]，其中 w 1 , w 2 w_1,w_2 w1,w2为实部
q 1 ⋅ q 2 = ( w 1 x 2 + x 1 w 2 + y 1 z 2 − z 1 y 2 ) i + ( w 1 y 2 − x 1 z 2 + y 1 w 2 + z 1 x 2 ) j + ( w 1 z 2 + x 1 y 2 − y 1 x 2 + z 1 w 2 ) k + w 1 w 2 − x 1 x 2 − y 1 y 2 − z 1 z 2 \begin{aligned} q_1\cdot q_2 &=(w_1x_2+x_1w_2+y_1z_2-z_1y_2)i \\&+(w_1y_2-x_1z_2+y_1w_2+z_1x_2) j \\&+(w_1z_2+x_1y_2-y_1x_2+z_1w_2) k \\&+w_1w_2-x_1x_2-y_1y_2-z_1z_2 \end{aligned} q1⋅q2=(w1x2+x1w2+y1z2−z1y2)i+(w1y2−x1z2+y1w2+z1x2)j+(w1z2+x1y2−y1x2+z1w2)k+w1w2−x1x2−y1y2−z1z2
q 1 + q 2 = [ w 1 − z 1 y 1 x 1 z 1 w 1 − x 1 y 1 − y 1 x 1 w 1 z 1 − x 1 − y 1 − z 1 w 1 ] [ x 2 y 2 z 2 w 2 ] = [ w 1 x 2 + x 1 w 2 + y 1 z 2 − z 1 y 2 w 1 y 2 − x 1 z 2 + y 1 w 2 + z 1 x 2 w 1 z 2 + x 1 y 2 − y 1 x 2 + z 1 w 2 w 1 w 2 − x 1 x 2 − y 1 y 2 − z 1 z 2 ] = ( w 1 x 2 + x 1 w 2 + y 1 z 2 − z 1 y 2 ) i + ( w 1 y 2 − x 1 z 2 + y 1 w 2 + z 1 x 2 ) j + ( w 1 z 2 + x 1 y 2 − y 1 x 2 + z 1 w 2 ) k + w 1 w 2 − x 1 x 2 − y 1 y 2 − z 1 z 2 = q 1 ⋅ q 2 \begin{aligned} q_1^+q_2 &= \begin{bmatrix} w_1 & -z_1 & y_1 & x_1 \\z_1 & w_1 & -x_1 & y_1\\-y_1 & x_1 & w_1 & z_1 \\ -x_1 & -y_1 & -z_1 & w_1 \end{bmatrix} \begin{bmatrix} x_2 \\ y_2 \\z_2 \\w_2 \end{bmatrix} \\ &= \begin{bmatrix} w_1x_2+x_1w_2+y_1z_2-z_1y_2 \\ w_1y_2-x_1z_2+y_1w_2+z_1x_2 \\ w_1z_2+x_1y_2-y_1x_2+z_1w_2 \\ w_1w_2-x_1x_2-y_1y_2-z_1z_2 \end{bmatrix} \\&=(w_1x_2+x_1w_2+y_1z_2-z_1y_2)i \\&+(w_1y_2-x_1z_2+y_1w_2+z_1x_2) j \\&+(w_1z_2+x_1y_2-y_1x_2+z_1w_2) k \\&+w_1w_2-x_1x_2-y_1y_2-z_1z_2 \\ &= q_1\cdot q_2 \end{aligned} q1+q2=⎣⎢⎢⎡w1z1−y1−x1−z1w1x1−y1y1−x1w1−z1x1y1z1w1⎦⎥⎥⎤⎣⎢⎢⎡x2y2z2w2⎦⎥⎥⎤=⎣⎢⎢⎡w1x2+x1w2+y1z2−z1y2w1y2−x1z2+y1w2+z1x2w1z2+x1y2−y1x2+z1w2w1w2−x1x2−y1y2−z1z2⎦⎥⎥⎤=(w1x2+x1w2+y1z2−z1y2)i+(w1y2−x1z2+y1w2+z1x2)j+(w1z2+x1y2−y1x2+z1w2)k+w1w2−x1x2−y1y2−z1z2=q1⋅q2
所以： q 1 ⋅ q 2 = q 1 + q 2 q_1 \cdot q_2 = q_1^+q_2 q1⋅q2=q1+q2
同理可证： q 1 ⋅ q 2 = q 2 ⊕ q 1 q_1 \cdot q_2 = q_2^ \oplus q_1 q1⋅q2=q2⊕q1
证明方法二：
证明：
如上图， V r o t V_{rot} Vrot是由 V V V通过旋转得到，旋转轴是K，旋转的角度为 θ \theta θ，用旋转矩阵R表示该旋转，即为： V r o t = R ⋅ V V_{rot} = R \cdot V Vrot=R⋅V, 且 K K K为单位向量， ∣ ∣ K ⃗ ∣ ∣ = 1 ||\vec{K} || =1 ∣∣K∣∣=1,
V = V ⊥ + V ∥ V=V_\perp + V_\parallel V=V⊥+V∥,
V ⊥ = V − V ∥ = − K × ( K × V ) V_\perp = V-V_\parallel = -K \times (K \times V) V⊥=V−V∥=−K×(K×V)，
V ∥ = ( K ⋅ V ) ⋅ K = K K T V V_\parallel = (K \cdot V) \cdot K = K K^T V V∥=(K⋅V)⋅K=KKTV,
则垂直分量和平行分量各自的旋转分量为：
V ∥ r o t = V ∥ V_{\parallel rot} = V_\parallel V∥rot=V∥
V ⊥ r o t = c o s θ V ⊥ + s i n θ K × V V_{\perp rot} = cos\theta V_\perp+sin\theta K \times V V⊥rot=cosθV⊥+sinθK×V
∴ V r o t = V ∥ r o t + V ⊥ r o t = V ∥ + c o s θ V ⊥ + s i n θ K × V = V ∥ + c o s θ ( V − V ∥ ) + s i n θ K × V = c o s θ V + ( 1 − c o s θ ) V ∥ + s i n θ K × V = c o s θ V + ( 1 − c o s θ ) K K T V + s i n θ K × V = R ⋅ V \begin{aligned}\therefore V_{rot} &= V_{\parallel rot} + V_{\perp rot} \\&= V_\parallel + cos\theta V_\perp +sin\theta K\times V \\&=V_\parallel + cos\theta(V-V_\parallel) + sin\theta K \times V \\&= cos\theta V + (1-cos\theta)V_\parallel +sin\theta K \times V \\&= cos\theta V+ (1-cos\theta) K K^T V + sin\theta K^\times V \\&=R \cdot V \end{aligned} ∴Vrot=V∥rot+V⊥rot=V∥+cosθV⊥+sinθK×V=V∥+cosθ(V−V∥)+sinθK×V=cosθV+(1−cosθ)V∥+sinθK×V=cosθV+(1−cosθ)KKTV+sinθK×V=R⋅V
∴ R = c o s θ I + ( 1 − c o s θ ) K K T + s i n θ K × \therefore R = cos\theta I + (1-cos\theta) K K^T + sin\theta K^\times ∴R=cosθI+(1−cosθ)KKT+sinθK×
∴ R = c o s θ I + ( 1 − c o s θ ) n n T + s i n θ n × \therefore R = cos\theta I + (1-cos\theta)nn^T +sin\theta n^\times ∴R=cosθI+(1−cosθ)nnT+sinθn×
证明：
（1） p ′ p^{'} p′的实部为0
假定点p的坐标为 p ( x , y , z ) p(x,y,z) p(x,y,z)，则用四元数表示为： p = [ 0 , x , y , z ] = x i + y j + z k p=[0,x,y,z]=xi+yj+zk p=[0,x,y,z]=xi+yj+zk,
令 q = [ c o s θ 2 , n s i n θ 2 ] = c o s θ 2 + n x s i n θ 2 i + n y s i n θ 2 j + n z s i n θ 2 k q=[cos\frac{\theta}{2} ,nsin\frac{\theta}{2}]=cos\frac{\theta}{2}+n_x sin\frac{\theta}{2}i+n_y sin\frac{\theta}{2}j+n_z sin\frac{\theta}{2}k q=[cos2θ,nsin2θ]=cos2θ+nxsin2θi+nysin2θj+nzsin2θk
∵ q ∗ = [ c o s θ 2 , − n s i n θ 2 ] , q − 1 = q ∗ / ∣ ∣ q ∣ ∣ 2 \because q^\ast=[cos\frac{\theta}{2} ,-\mathbf{n}sin\frac{\theta}{2}],q^{-1}=q^\ast/||q||^2 ∵q∗=[cos2θ,−nsin2θ],q−1=q∗/∣∣q∣∣2
则 q − 1 = [ c o s θ 2 , − n s i n θ 2 ] / ∣ ∣ q ∣ ∣ 2 = ( c o s θ 2 − n x s i n θ 2 i − n y s i n θ 2 j − n z s i n θ 2 k ) / ∣ ∣ q ∣ ∣ 2 q^{-1} = [cos\frac{\theta}{2} ,-nsin\frac{\theta}{2}]/||q||^2=(cos\frac{\theta}{2} - n_x sin\frac{\theta}{2}i - n_y sin\frac{\theta}{2}j - n_z sin\frac{\theta}{2}k)/||q||^2 q−1=[cos2θ,−nsin2θ]/∣∣q∣∣2=(cos2θ−nxsin2θi−nysin2θj−nzsin2θk)/∣∣q∣∣2
∴ p ′ = q p q − 1 = [ c o s θ 2 , n s i n θ 2 ] [ 0 , x , y , z ] [ c o s θ 2 , − n s i n θ 2 ] / ∣ ∣ q ∣ ∣ 2 = ( c o s θ 2 + n x s i n θ 2 i + n y s i n θ 2 j + n z s i n θ 2 k ) ( x i + y j + z k ) ( ( c o s θ 2 − n x s i n θ 2 i − n y s i n θ 2 j − n z s i n θ 2 k ) / ∣ ∣ q ∣ ∣ 2 ) \begin{aligned} \therefore p^{'} &= qpq^{-1} \\&=[cos\frac{\theta}{2} ,nsin\frac{\theta}{2}] [0,x,y,z] [cos\frac{\theta}{2} ,-nsin\frac{\theta}{2}]/||q||^2\\&=(cos\frac{\theta}{2}+n_x sin\frac{\theta}{2}i+n_y sin\frac{\theta}{2}j+n_z sin\frac{\theta}{2}k)(xi+yj+zk)((cos\frac{\theta}{2} - n_x sin\frac{\theta}{2}i - n_y sin\frac{\theta}{2}j - n_z sin\frac{\theta}{2}k)/||q||^2) \end{aligned} ∴p′=qpq−1=[cos2θ,nsin2θ][0,x,y,z][cos2θ,−nsin2θ]/∣∣q∣∣2=(cos2θ+nxsin2θi+nysin2θj+nzsin2θk)(xi+yj+zk)((cos2θ−nxsin2θi−nysin2θj−nzsin2θk)/∣∣q∣∣2)
将所有的实数取出,且忽略 ∣ ∣ q ∣ ∣ 2 ||q||^2 ∣∣q∣∣2，则：
x n x c o s θ 2 s i n θ 2 + y n y c o s θ 2 s i n θ 2 + z n z c o s θ 2 s i n θ 2 + ( − 1 ) x n x s i n θ 2 x c o s θ 2 + y n x n z s i n θ 2 s i n θ 2 + ( − 1 ) z n x n y s i n θ 2 s i n θ 2 + ( − 1 ) y n y s i n θ 2 c o s θ 2 + ( − 1 ) x n y n z s i n θ 2 s i n θ 2 + z n x n y s i n θ 2 s i n θ 2 + x n y n z s i n θ 2 s i n θ 2 + ( − 1 ) y n x n z s i n θ 2 s i n θ 2 + ( − 1 ) z n z s i n θ 2 c o s θ 2 = 0 x n_x cos\frac{\theta}{2} sin\frac{\theta}{2} + y n_y cos\frac{\theta}{2} sin\frac{\theta}{2} +z n_z cos\frac{\theta}{2} sin\frac{\theta}{2} \\ + (-1) x n_x sin\frac{\theta}{2} x cos\frac{\theta}{2} + y n_x n_z sin\frac{\theta}{2} sin\frac{\theta}{2} + (-1)z n_x n_ysin\frac{\theta}{2} sin\frac{\theta}{2} \\+(-1)y n_y sin\frac{\theta}{2} cos\frac{\theta}{2} + (-1) x n_y n_z sin\frac{\theta}{2} sin\frac{\theta}{2} + z n_x n_y sin\frac{\theta}{2} sin\frac{\theta}{2} \\+ x n_y n_z sin\frac{\theta}{2} sin\frac{\theta}{2} +(-1) y n_x n_z sin\frac{\theta}{2}sin\frac{\theta}{2} + (-1) z n_z sin\frac{\theta}{2}cos\frac{\theta}{2} \\ \\=0 xnxcos2θsin2θ+ynycos2θsin2θ+znzcos2θsin2θ+(−1)xnxsin2θxcos2θ+ynxnzsin2θsin2θ+(−1)znxnysin2θsin2θ+(−1)ynysin2θcos2θ+(−1)xnynzsin2θsin2θ+znxnysin2θsin2θ+xnynzsin2θsin2θ+(−1)ynxnzsin2θsin2θ+(−1)znzsin2θcos2θ=0
所以实部为0，即 p ′ p^{'} p′的实部为0。
（2） p ′ = Q p p^{'} = Qp p′=Qp，写出矩阵 Q Q Q
p ′ = q p q − 1 = q + p + q − 1 = q + q − 1 ⊕ p \begin{aligned} p^{'} &= qpq^{-1} \\ &= q^+ p^+q^{-1} \\&= q^+ q^{-1^\oplus}p \end{aligned} p′=qpq−1=q+p+q−1=q+q−1⊕p
∴ Q = q + q − 1 ⊕ \therefore Q= q^+ q^{-1^\oplus} ∴Q=q+q−1⊕
假定四元数 q = [ x , y , z , w ] q=[x,y,z,w] q=[x,y,z,w]，其中 w w w是实部，那么 q − 1 = [ − x , − y , − z , w ] / ∣ ∣ q ∣ ∣ 2 q^{-1}=[-x,-y,-z,w]/||q||^2 q−1=[−x,−y,−z,w]/∣∣q∣∣2
q + = [ η 1 + ε × ε − ε T η ] = [ w − z y x z w − x y − y x w z − x − y − z w ] q^+ = \begin{bmatrix} \eta1 + \varepsilon ^\times & \varepsilon \\ -\varepsilon^T & \eta \end{bmatrix}=\begin{bmatrix} w & -z & y & x\\z & w & -x & y\\-y & x & w & z \\ -x & -y & -z & w\end{bmatrix} q+=[η1+ε×−εTεη]=⎣⎢⎢⎡wz−y−x−zwx−yy−xw−zxyzw⎦⎥⎥⎤,
q − 1 ⊕ = [ η 1 − ( − ε ) × − ε − ( − ε ) T η ] / ∣ ∣ q ∣ ∣ 2 = 1 ∣ ∣ q ∣ ∣ 2 ⋅ [ η 1 + ε × − ε ε T η ] = 1 ∣ ∣ q ∣ ∣ 2 ⋅ [ w − z y − x z w − x − y − y x w − z x y z w ] q^{-1^\oplus}= \begin{bmatrix} \eta1 - (-\varepsilon) ^\times & -\varepsilon \\ -(-\varepsilon)^T & \eta \end{bmatrix}/||q||^2 = \frac{1}{||q||^2 }\cdot \begin{bmatrix} \eta1 + \varepsilon ^\times & -\varepsilon \\ \varepsilon^T & \eta \end{bmatrix}=\frac{1}{||q||^2 }\cdot \begin{bmatrix} w & -z & y & -x\\z & w & -x & -y\\-y & x & w & -z \\ x & y & z & w\end{bmatrix} q−1⊕=[η1−(−ε)×−(−ε)T−εη]/∣∣q∣∣2=∣∣q∣∣21⋅[η1+ε×εT−εη]=∣∣q∣∣21⋅⎣⎢⎢⎡wz−yx−zwxyy−xwz−x−y−zw⎦⎥⎥⎤
∴ Q = q + q − 1 ⊕ = 1 ∣ ∣ q ∣ ∣ 2 ⋅ [ w − z y x z w − x y − y x w z − x − y − z w ] [ w − z y − x z w − x − y − y x w − z x y z w ] = 1 ∣ ∣ q ∣ ∣ 2 ⋅ [ w 2 + x 2 − y 2 + z 2 2 x y − 2 z w 2 w y + 2 x z 0 2 x y + 2 z w w 2 − x 2 + y 2 − z 2 2 y z − 2 x w 0 2 x z − 2 y w 2 y z + 2 x w w 2 − x 2 − y 2 + z 2 0 0 0 0 w 2 + x 2 + y 2 + z 2 ] \begin{aligned} \therefore Q &= q^+ q^{-1^\oplus} \\&= \frac{1}{||q||^2 }\cdot\begin{bmatrix} w & -z & y & x\\z & w & -x & y\\-y & x & w & z \\ -x & -y & -z & w\end{bmatrix}\begin{bmatrix} w & -z & y & -x\\z & w & -x & -y\\-y & x & w & -z \\ x & y & z & w\end{bmatrix} \\&= \frac{1}{||q||^2 }\cdot \begin{bmatrix}w^2+x^2-y^2+z^2& 2xy-2zw & 2wy+2xz & 0\\ 2xy+2zw& w^2-x^2+y^2-z^2 & 2yz-2xw & 0\\2xz-2yw & 2yz+2xw & w^2-x^2-y^2+z^2 & 0 \\ 0 & 0 & 0 & w^2+x^2+y^2+z^2\end{bmatrix} \end{aligned} ∴Q=q+q−1⊕=∣∣q∣∣21⋅⎣⎢⎢⎡wz−y−x−zwx−yy−xw−zxyzw⎦⎥⎥⎤⎣⎢⎢⎡wz−yx−zwxyy−xwz−x−y−zw⎦⎥⎥⎤=∣∣q∣∣21⋅⎣⎢⎢⎡w2+x2−y2+z22xy+2zw2xz−2yw02xy−2zww2−x2+y2−z22yz+2xw02wy+2xz2yz−2xww2−x2−y2+z20000w2+x2+y2+z2⎦⎥⎥⎤
证明方法二：
- for(atuo& a: avec)
范围for循环，用a遍历avec中的每个量； - for(atuo& a: avec)
自动类型推导，根据a获得的值，用auto自动推断出a的类型； - [](const A&a1,const A&a2){return a1.index
- begin()