SLAM十四讲—CH3

标签: SLAM十四讲笔记

---

设线性⽅程$Ax=b$，在A 为⽅阵的前提下，请回答以下问题：

熟悉Eigen矩阵运算

1. 在什么条件下， $x$ 有解且唯⼀？

如果A为n维方阵 $$|A|\ne 0或R(A)=R(A,b)=n$$

2. 高斯消元法的原理是什么？

Gauss消去法是一种规则化的加减消元法。它的基本思想是：通过逐次消元计算把需求解的线性方程组转化成上三角形方程组，也就是把线性方程组的系数矩阵转化为上三角矩阵，从而使一般线性方程组的求解转化为等价（同解）的上三角方程组求解。

顺序Gauss消去法列主元Gauss消去法

3. QR 分解的原理是什么？

视频

4.Cholesky分解的原理是什么？

Cholesky 分解是把一个对称正定的矩阵表示成一个下三角矩阵L和其转置的乘积的分解。它要求矩阵的所有特征值必须大于零，故分解的下三角的对角元也是大于零的。Cholesky分解法又称平方根法，是当A为实对称正定矩阵时，LU三角分解法的变形。

5.编程实现A 为100 x 100 随机矩阵时，⽤QR 和Cholesky 分解求x 的程序。你可以参考本次课⽤到的useEigen 例程。

#include "iostream" using namespace std; #include "ctime" #include "Eigen/Core" #include "Eigen/Dense" #include <Eigen/Cholesky> #include <Eigen/QR> int main(int argc,char **argv){ Eigen::Matrix<double,100,100> matrix100_100; matrix100_100 = Eigen::MatrixXd::Random(100,100); Eigen::Matrix<double,100,1> vector100; Eigen::Matrix<double,100,1> x; vector100 = Eigen::MatrixXd::Random(100,1); clock_t time_stt = clock(); x = matrix100_100.colPivHouseholderQr().solve(vector100); cout <<x<<endl; cout <<"time used in Qr decomposition is"<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl; time_stt = clock(); x = matrix100_100.householderQr().solve(vector100); cout <<x<<endl; cout <<"time used in qr decomposition is"<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl; time_stt = clock(); x = matrix100_100.ldlt().solve(vector100); cout <<x<<endl; cout <<"time used in ldlt decomposition is"<<1000*(clock()-time_stt)/(double)CLOCKS_PER_SEC<<"ms"<<endl; }

几何运算练习

四元数->旋转向量->旋转矩阵

#include <iostream> #include <Eigen/Geometry> #include <cmath> using namespace std; int main(int argc,char **argv){ Eigen::Matrix<double,4,1> V_q1,V_q2; V_q1<<0.55 ,0.3,0.2,0.2; V_q2<<-0.1,0.3,-0.7,0.2; double temp; temp = sqrt(pow(V_q1[0],2)+pow(V_q1[1],2)+pow(V_q1[2],2)+pow(V_q1[3],2)); int i; for(i=0;i<=3;i++) { V_q1[i]=V_q1[i]/temp; } temp = sqrt(pow(V_q2[0],2)+pow(V_q2[1],2)+pow(V_q2[2],2)+pow(V_q2[3],2)); for(i=0;i<=3;i++) { V_q2[i] = V_q2[i] / temp; } Eigen::Quaterniond q1(V_q1[0],V_q1[1],V_q1[2],V_q1[3]); cout<<q1.coeffs()<<endl; Eigen::Quaterniond q2(V_q2[0],V_q2[1],V_q2[2],V_q2[3]); cout<<q2.coeffs()<<endl; Eigen::Matrix<double,3,1> p1,t1,t2; t1<<0.7,1.1,0.2; t2<<-0.1,0.4,0.8; p1<<0.5,-0.1,0.2; Eigen::AngleAxisd rotation_vectorT1=Eigen::AngleAxisd(q1); Eigen::Isometry3d T1=Eigen::Isometry3d::Identity(); T1.rotate(rotation_vectorT1); T1.pretranslate(t1); Eigen::AngleAxisd rotation_vectorT2=Eigen::AngleAxisd(q2); Eigen::Isometry3d T2=Eigen::Isometry3d::Identity(); T2.rotate(rotation_vectorT2); T2.pretranslate(t2); Eigen::Vector3d V_rotation; V_rotation =T2*T1.inverse()*p1; cout<<V_rotation.transpose()<<endl; return 0; }

四元数

#include <iostream> #include <Eigen/Geometry> #include <cmath> using namespace std; int main(int argc,char **argv){ Eigen::Matrix<double,4,1> V_q1,V_q2; V_q1<<0.55 ,-0.3,-0.2,-0.2; V_q2<<-0.1,0.3,-0.7,0.2; double temp; temp = sqrt(pow(V_q1[0],2)+pow(V_q1[1],2)+pow(V_q1[2],2)+pow(V_q1[3],2)); int i; for(i=0;i<=3;i++) { V_q1[i]=V_q1[i]/temp; } temp = sqrt(pow(V_q2[0],2)+pow(V_q2[1],2)+pow(V_q2[2],2)+pow(V_q2[3],2)); for(i=0;i<=3;i++) { V_q2[i] = V_q2[i] / temp; } Eigen::Quaterniond q1(V_q1[0],V_q1[1],V_q1[2],V_q1[3]); cout<<q1.coeffs()<<endl; Eigen::Quaterniond q2(V_q2[0],V_q2[1],V_q2[2],V_q2[3]); cout<<q2.coeffs()<<endl; Eigen::Matrix<double,3,1> t1,t2; Eigen::Matrix<double,3,1> p1; t1<<0.7,1.1,0.2; t2<<-0.1,0.4,0.8; Eigen::Vector3d V_rotation; p1<<0.5,-0.1,0.2; V_rotation = q1*(p1-t1); V_rotation = q2*V_rotation+t2; cout<<V_rotation.transpose(); return 0; } #include <iostream> #include <Eigen/Geometry> #include <cmath> using namespace std; using namespace Eigen; int main(int argc,char **argv) { Quaterniond q1(0.55,0.3,0.2,0.2),q2(-0.1,0.3,-0.7,0.2); q1.normalize(); q2.normalize(); Vector3d t1(0.7,1.1,0.2),t2(-0.1,0.4,0.8); Vector3d p1(0.5,-0.1,0.2); Isometry3d T1w(q1),T2w(q2); T1w.pretranslate(t1); T2w.pretranslate(t2); Vector3d p2 = T2w*T1w.inverse()*p1; cout<<endl<<p2.transpose()<<endl; return 0; }

在利用四元数计算，发现

Eigen::Vector3d V_rotation; p1<<0.5,-0.1,0.2; V_rotation = q1*(p1-t1); V_rotation = q2*V_rotation+t2; cout<<V_rotation.transpose();

其中代码为q1*(p1-t1)与q1*p1-t1即先平移再旋转和先旋转后平移结果不一样，并且四元数的逆操作不是乘以逆矩阵而是乘以共轭向量。 矩阵变换

旋转的表达

1.设有旋转矩阵$R$，证明：$R^{T}R=I$ 且$detR=+1^2$。

证明： 在坐标系中$A$中有：${\hat{x}}_A=(1,0,0),$${\hat{y}}_A=(0,1,0),$${\hat{z}}_A=(0,0,1)$。 经过旋转后变成坐标系$B$,用矩阵描述为： $$M_B=R{M}_A$$ 旋转后的X轴在$A$的坐标系中可表示为${\hat{x}}_B=a{\hat{x}}_A+b{\hat{y}}_A+c{\hat{z}}_A$ 也即$${\hat{x}}_B=({\hat{x}}_B{\hat{x}}_A^T,{\hat{x}}_B{\hat{y}}_A^T,{\hat{x}}_B{\hat{z}}_A^T)$$ 同理可得${\hat{y}}_B,{\hat{z}}_B$，则 $$R=\left[\begin{array}{ccc}{\hat{x}}_B{\hat{x}}_A^T& {\hat{y}}_B{\hat{x}}_A^T& {\hat{z}}_B{\hat{x}}_A^T\\ {\hat{x}}_B{\hat{y}}_A^T& {\hat{y}}_B{\hat{y}}_A^T& {\hat{z}}_B{\hat{y}}_A^T\\ {\hat{x}}_B{\hat{z}}_A^T& {\hat{y}}_B{\hat{z}}_A^T& {\hat{z}}_B{\hat{z}}_A^T\end{array}\right]$$ $$R^T=\left[\begin{array}{ccc}{\hat{x}}_B{\hat{x}}_A^T& {\hat{x}}_B{\hat{y}}_A^T& {\hat{x}}_B{\hat{z}}_A^T\\ {\hat{y}}_B{\hat{x}}_A^T& {\hat{y}}_B{\hat{y}}_A^T& {\hat{y}}_B{\hat{z}}_A^T\\ {\hat{z}}_B{\hat{x}}_A^T& {\hat{z}}_B{\hat{y}}_A^T& {\hat{z}}_B{\hat{z}}_A^T\end{array}\right]$$ 将坐标系$B,$逆旋转变成坐标系$A$后，也可得到一个旋转矩阵为$R$的逆矩阵，易知： $$R^{-1}=\left[\begin{array}{ccc}{\hat{x}}_A{\hat{x}}_B^T& {\hat{y}}_A{\hat{x}}_B^T& {\hat{z}}_B{\hat{x}}_B^T\\ {\hat{x}}_A{\hat{y}}_B^T& {\hat{y}}_A{\hat{y}}_B^T& {\hat{z}}_B{\hat{y}}_B^T\\ {\hat{x}}_A{\hat{z}}_B^T& {\hat{y}}_A{\hat{z}}_B^T& {\hat{z}}_B{\hat{z}}_B^T\end{array}\right]$$ 以${\hat{x}}_B{\hat{x}}_A^T$和${\hat{x}}_A{\hat{x}}_B^T$为例： 前者表示在坐标系A下两个向量的点乘，后者表示在坐标系B下两个向量的点乘，两个向量长度相同，在不同坐标系下，点乘结果相同，也即相互的投影长度相等，即${\hat{x}}_B{\hat{x}}_A^T={\hat{x}}_A{\hat{x}}_B^T$，同理可知$R^T$和$R^{-1}$对应位置处的数值相等 $$R^T=R^{-1}$$ 所以旋转矩阵的逆为它的转置矩阵，即$R{R}^T=R{R}^{-1}=E$因此R矩阵为正交矩阵。

2.设有四元数$q$,我们把虚部记为$ϵ$,实部记为$\eta$,那么$q=(ϵ,\eta )$。请说明$ϵ$和$\eta$的维度。

答：四元数$q$,虚部有三个，实部有一个。所以$ϵ$的维度是3，$\eta$的维度1。

3.定义运算${}^{+}$和${}^{⨁}$,为：$$q^{+}=\left[\begin{array}{cc}\eta 1-{ϵ}^{\times }& ϵ\\ {ϵ}^T& \eta \end{array}\right],q^{⨁}=\left[\begin{array}{cc}\eta 1+{ϵ}^{\times }& ϵ\\ -{ϵ}^T& \eta \end{array}\right]\text{\hspace{1em}(1)}$$请证明对任意单位四元数$q_1$,$q_2$,四元数乘法可写成矩阵乘法：$$q_1\cdot q_2=q_1^{+}{q}_2\text{\hspace{1em}(2)}$$或者$$q_1\cdot q_2=q_2^{⨁}{q}_1\text{\hspace{1em}(3)}$$

证明：$$q_1=\left[\begin{array}{c}{ϵ}_1\\ {\eta }_1\end{array}\right],q_2=\left[\begin{array}{c}{ϵ}_2\\ {\eta }_2\end{array}\right]$$ $$q_1{q}_2=\left[\begin{array}{c}{\eta }_1{ϵ}_2+{\eta }_2{ϵ}_1+{ϵ}_1\times {ϵ}_2\\ {\eta }_1{\eta }_2-{ϵ}_1^T{ϵ}_2\end{array}\right]$$ $$q_1^{+}{q}_2=\left[\begin{array}{cc}{\eta }_{1}1+{ϵ}_1^{\times }& {ϵ}_1\\ -{ϵ}_1^T& {\eta }_1\end{array}\right]\left[\begin{array}{c}{ϵ}_2\\ {\eta }_2\end{array}\right]=\left[\begin{array}{c}({\eta }_{1}1+{ϵ}_1^{\times })\ast {ϵ}_2+{ϵ}_1\ast {\eta }_2\\ {\eta }_1\ast {\eta }_2-{ϵ}_1^T\ast {ϵ}_2\end{array}\right]=\left[\begin{array}{c}{\eta }_1\ast {ϵ}_2+{ϵ}_1\ast {\eta }_2+{ϵ}_1\times {ϵ}_2\\ {\eta }_1\ast {\eta }_2-{ϵ}_1^T\ast {ϵ}_2\end{array}\right]=q_1{q}_2$$ $$q_2^{⨁}{q}_1=\left[\begin{array}{cc}{\eta }_{2}1+{ϵ}_2^{\times }& {ϵ}_2\\ -{ϵ}_2^T& {\eta }_2\end{array}\right]\left[\begin{array}{c}{ϵ}_1\\ {\eta }_1\end{array}\right]=\left[\begin{array}{c}({\eta }_{2}1+{ϵ}_2^{\times })\ast {ϵ}_1+{ϵ}_2\ast {\eta }_1\\ {\eta }_1\ast {\eta }_2-{ϵ}_2^T\ast {ϵ}_1\end{array}\right]=\left[\begin{array}{c}{\eta }_1\ast {ϵ}_2+{ϵ}_1\ast {\eta }_2+{ϵ}_1\times {ϵ}_2\\ {\eta }_1\ast {\eta }_2-{ϵ}_1^T\ast {ϵ}_2\end{array}\right]=q_1{q}_2$$

4.罗德里格斯公式证明

证明

5.四元数运算性质的验证

SLAM十四讲第二版P59-60.