【SLAM】视觉SLAM十四讲（二：矩阵运算）

1 熟悉 Eigen 矩阵运算

设线性方程 Ax = b,在 A 为方阵的前提下,请回答以下问题:

1. 在什么条件下,x 有解且唯一?当方程组的系数矩阵的秩与方程组增广矩阵的秩相等且均等于方程组中未知数个数n的时候，方程组有唯一解
2. 高斯消元法的原理是什么?通过用初等行变换将增广矩阵化为行阶梯阵，然后通过回带求解线性方程组的解
3. QR 分解的原理是什么?QR 分解
4. Cholesky 分解的原理是什么?Cholesky 分解
5. 编程实现 A 为 100 × 100 随机矩阵时,用 QR 和 Cholesky 分解求 x 的程序。你可以参考本次课
   用到的 useEigen 例程。

    #include 
    #include 
    using namespace std;
    
    //eigen部分
    #include 
    //稠密矩阵的代数运算
    #include 
    //包含QR分解
    #include
    //包含 Cholesky 分解
    #include 
    /*************************************************************
    *本程序实现 A 为 100 × 100 随机矩阵时,用 QR 和 Cholesky 分解求 x
    *************************************************************/
    
    int main(int argc, char** argv)
    {
        time_t time,start,end;
        //声明一个100*100的随机矩阵m_11
        Eigen::Matrix m_11=Eigen::Matrix::Random(100,100);
        //产生对称矩阵,因为cholesky分解需要对正定对称矩阵进行
        m_11 = m_11.transpose() * m_11;
        //计算行列式
        cout<<"Matrix determination:"< v_11=Eigen::Matrix::Random(100,1);
        //声明未知的x
        Eigen::Matrix x;
        cout<<"A="<

几何运算练习

使用 Eigen/Geometry 计算一个具体的例子。
设有小萝卜一号和小萝卜二号位于世界坐标系中。小萝卜一号的位姿为: q 1 = [0.55, 0.3, 0.2, 0.2], t 1 =
[0.7, 1.1, 0.2] T (q 的第一项为实部)
。这里的 q 和 t 表达的是 Tcw ,也就是世界到相机的变换关系。小萝卜
二号的位姿为 q 2 = [−0.1, 0.3, −0.7, 0.2], t 2 = [−0.1, 0.4, 0.8] T 。现在,小萝卜一号看到某个点在自身的坐
标系下,坐标为 p 1 = [0.5, −0.1, 0.2] T ,求该向量在小萝卜二号坐标系下的坐标。请编程实现此事,并提交
你的程序。
提示:

1. 四元数在使用前需要归一化。
2. 请注意 Eigen 在使用四元数时的虚部和实部顺序。
3. 参考答案为 p 2 = [1.08228, 0.663509, 0.686957] T 。你可以用它验证程序是否正确。

#include 
#include 
#include 
#include 
using namespace std;

int main() {
    //输入小萝卜一号和二号的位姿
    Eigen::Quaterniond q1(0.55,0.3,0.2,0.2);
    q1 = q1.normalized();
    Eigen::Quaterniond q2(-0.1,0.3,-0.7,0.2);
    q2 = q2.normalized();
    Eigen::Matrix transfor1;
    transfor1<<0.7,1.1,0.2;
    Eigen::Matrix transfor2;
    transfor2<<-0.1,0.4,0.8;
    Eigen::Matrix p1;
    p1<<0.5,-0.1,0.2;
   //T1是欧式变换矩阵4*4
    Eigen::Isometry3d T1 = Eigen::Isometry3d::Identity();
    //按照q1进行旋转
    T1.rotate(q1.toRotationMatrix());
    //将平移向量设成小萝卜一号的t1
    T1.pretranslate(transfor1);
    //根据p1在小萝卜一号坐标系中的坐标计算出世界坐标系中的坐标
    p1 = T1.inverse()*p1;

    //T2是小萝卜二号坐标系和世界坐标系的欧式变换矩阵
    Eigen::Isometry3d T2 = Eigen::Isometry3d::Identity();
    T2.rotate(q2.toRotationMatrix());
    T2.pretranslate(transfor2);
    p1 = T2*p1;
    cout<

旋转的表达

1. 设有旋转矩阵 R，证明 RTR = I 且 detR = +1。
   
2. 设有四元数 q，我们把虚部记为 ε，实部记为 η，那么 q = (ε,η)。请说明 ε 和 η 的维度。实部1个，虚部3个，不知道维度啥意思

罗德里格斯公式证明

视觉slam十四讲P69

四元数运算性质的验证

课程中介绍了单位四元数可以表达旋转。其中,在谈论用四元数 q 旋转点 p 时,结果为:p ′ = qpq −1 .
我们说,此时 p ′ 必定为虚四元数(实部为零)。请你验证上述说法。
此外,上式亦可写成矩阵运算:p ′ = Qp。请根据你的推导,给出矩阵 Q。注意此时 p 和 p ′ 都是四元数形式的变量,所以 Q 为 4 × 4 的矩阵。
p ′ = qpq-1 = q + p + q −1 = q + q−1⊕v.
四元数至旋转矩阵的转换方式:
R = q + q−1 ⊕.

点P实部为0