【SLAM十四讲】CH3

SLAM十四讲—CH3

标签: SLAM十四讲笔记

---

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

熟悉Eigen矩阵运算

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

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

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

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

1. 顺序Gauss消去法
2. 列主元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.