深蓝学院视觉slam十四讲第2章作业

最新推荐文章于 2024-06-26 21:25:55 发布

Jevin-L

最新推荐文章于 2024-06-26 21:25:55 发布

阅读量1.7k

点赞数 5

分类专栏：深蓝学院视觉slam十四讲作业文章标签： slam 矩阵线性代数自动驾驶 c++

本文链接：https://blog.csdn.net/hitljy/article/details/106445558

版权

深蓝学院视觉slam十四讲作业专栏收录该内容

7 篇文章 47 订阅

订阅专栏

首先给出有关四元数讲解一份比较好的pdf

在这里插入图片描述
2.1 设线性⽅程 $\bm Ax = b$ ，则有解充要条件为：系数矩阵 $\bm A_{n \times n}$ 与其增广矩阵 $\bm B_{n \times (n+1)}$ 的秩相，即 $R(\bm A)=R(\bm B)$ ；且解唯一的条件为 $R(\bm A)=R(\bm B)=n$ 。

2.2 高斯消元法的原理是通过初等行变换把 $A$ 变换为上三角矩阵，然后从最下面一行（仅有一个未知量）开始求解，逐行回代依次解除每个未知量

2.3 实数矩阵 $\bm A$ 的QR分解是把 $\bm A$ 分解为 $\bm A = \bm Q \bm R$ ，这里的 $\bm Q$ 是正交矩阵，而 $\bm R$ 是上三角矩阵，如果 $\bm A$ 是非奇异的，且限定 $\bm R$ 的对角线元素为正，则这个分解是唯一的。计算有很多方法，例如Givens旋转、Householder变换，以及Gram-Schmidt正交化等等。
用QR分解建立了计算矩阵特征值的QR方法

2.4 摘自wiki百科：Cholesky分解
Alt
2.5

#include <iostream>
#include <ctime>
// Eigen 核心部分
#include <Eigen/Core>
// 稠密矩阵的代数运算（逆，特征值等）
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

#define MATRIX_SIZE 100

int main(int argc, char **argv) {

    MatrixXd matrix_NN = MatrixXd::Random(MATRIX_SIZE, MATRIX_SIZE);
    matrix_NN = matrix_NN * matrix_NN.transpose();  // 对称阵，保证半正定
//    cout << "matrix_NN:\n" << matrix_NN << endl;

    // 实对称矩阵可以正交相似对角化，保证对角化成功
    SelfAdjointEigenSolver<MatrixXd> eigen_solver(matrix_NN);
    cout << "Eigen values = \n" << eigen_solver.eigenvalues() << endl;
//    The eigenvalues are sorted in increasing order. 所有特征值均大于0，保证正定
    cout << "Eigen values min = \n" << eigen_solver.eigenvalues()(0) << endl;

    clock_t time_stt = clock(); // 计时
    Matrix<double, MATRIX_SIZE, 1> v_Nd = MatrixXd::Random(MATRIX_SIZE, 1);
    Matrix<double, MATRIX_SIZE, 1> x;

    // 通常用矩阵分解来求，例如QR分解，速度会快很多
    time_stt = clock();
    x = matrix_NN.colPivHouseholderQr().solve(v_Nd);
    cout << "time of Qr decomposition is "
         << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    // 对于正定矩阵，还可以用cholesky分解来解方程
    time_stt = clock();
    x = matrix_NN.ldlt().solve(v_Nd);
    cout << "time of ldlt decomposition is "
         << 1000 * (clock() - time_stt) / (double) CLOCKS_PER_SEC << "ms" << endl;
    cout << "x = " << x.transpose() << endl;

    return 0;
}

运行结果：
Alt

在这里插入图片描述 3.

#include<iostream>
#include<cmath>
using namespace std;
#include<Eigen/Core>
#include<Eigen/Dense>
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);
    Vector3d t1(0.7,1.1,0.2),t2(-0.1,0.4,0.8);
    q1.normalize();
    q2.normalize();
    Vector3d p1(0.5,-0.1,0.2);

    //记得初始化为单位阵
    Isometry3d T1 = Isometry3d::Identity();
    Isometry3d T2 = Isometry3d::Identity();
    T1.rotate(q1);
    T2.rotate(q2);
    T1.pretranslate(t1);
    T2.pretranslate(t2);

    Vector3d p2=T2*T1.inverse()*p1;
    cout<<endl<<p2.transpose()<<endl;
    return 0;
}

运行结果：
在这里插入图片描述

在这里插入图片描述 4.1
坐标系 $\left[\bm e_{1}, \bm e_{2}, \bm e_{3}\right]$ 发生了旋转，变成 $\left[\bm e_{1}^{\prime}, \bm e_{2}^{\prime}, \bm e_{3}^{\prime}\right]$ 对应坐标变换如下：
$\left[\bm e_{1}, \bm e_{2}, \bm e_{3}\right] \left[ \begin{array}{c}a_{1} \\ a_{2} \\ a_{3}\end{array}\right]= \left[\bm e_{1}^{\prime}, \bm e_{2}^{\prime}, \bm e_{3}^{\prime}\right] \left[\begin{array}{c}a_{1}^{\prime} \\ a_{2}^{\prime} \\ a_{3}^{\prime}\end{array}\right]$
上式同时左乘 $\left[\begin{array}{c}\bm e_{1}^{T} \\ \bm e_{2}^{T} \\ \bm e_{3}^{T}\end{array}\right]$ 得：
$\bm a \triangleq \left[\begin{array}{l} a_{1} \\a_{2} \\a_{3} \end{array}\right]= \left[\begin{array}{lll} \bm e_{1}^{T} \bm{e}_{1}^{\prime} & \bm e_{1}^{T} \bm{e}_{2}^{\prime} & \bm{e}_{1}^{T} \bm{e}_{3}^{\prime} \\ \bm e_{2}^{T} \bm{e}_{1}^{\prime} & \bm e_{2}^{T} \bm{e}_{2}^{\prime} & \bm{e}_{2}^{T} \bm{e}_{3}^{\prime} \\ \bm e_{3}^{T} \bm{e}_{1}^{\prime} & \bm e_{3}^{T} \bm{e}_{2}^{\prime} & \bm{e}_{3}^{T} \bm{e}_{3}^{\prime} \end{array}\right] \left[\begin{array}{l} a_{1}^{\prime} \\a_{2}^{\prime} \\a_{3}^{\prime} \end{array}\right] \triangleq \bm R \bm a^{\prime}$

同理若上式同时左乘 $\left[\begin{array}{c}\bm e_{1}^{\prime T} \\ \bm e_{2}^{\prime T} \\ \bm e_{3}^{\prime T}\end{array}\right]$ 得：
$\bm R^{-1} \bm a \triangleq \left[\begin{array}{lll} \bm e_{1}^{\prime T} \bm{e}_{1} & \bm e_{1}^{\prime T} \bm{e}_{2} & \bm{e}_{1}^{\prime T} \bm{e}_{3} \\ \bm e_{2}^{\prime T} \bm{e}_{1} & \bm e_{2}^{\prime T} \bm{e}_{2} & \bm{e}_{2}^{\prime T} \bm{e}_{3} \\ \bm e_{3}^{\prime T} \bm{e}_{1}^{\prime} & \bm e_{3}^{\prime T} \bm{e}_{2} & \bm{e}_{3}^{\prime T} \bm{e}_{3} \end{array}\right] \left[\begin{array}{l} a_{1} \\a_{2} \\a_{3} \end{array}\right]= \left[\begin{array}{l} a_{1}^{\prime} \\a_{2}^{\prime} \\a_{3}^{\prime} \end{array}\right] \triangleq \bm a^{\prime}$
即
$\bm R^{-1} = \left[\begin{array}{lll} \bm e_{1}^{\prime T} \bm{e}_{1} & \bm e_{1}^{\prime T} \bm{e}_{2} & \bm{e}_{1}^{\prime T} \bm{e}_{3} \\ \bm e_{2}^{\prime T} \bm{e}_{1} & \bm e_{2}^{\prime T} \bm{e}_{2} & \bm{e}_{2}^{\prime T} \bm{e}_{3} \\ \bm e_{3}^{\prime T} \bm{e}_{1}^{\prime} & \bm e_{3}^{\prime T} \bm{e}_{2} & \bm{e}_{3}^{\prime T} \bm{e}_{3} \end{array}\right]$
而
$\bm R^{T} =\left[\begin{array}{lll} \bm e_{1}^{T} \bm{e}_{1}^{\prime} & \bm e_{2}^{T} \bm{e}_{1}^{\prime} & \bm e_{3}^{T} \bm{e}_{1}^{\prime} \\ \bm e_{1}^{T} \bm{e}_{2}^{\prime} & \bm e_{2}^{T} \bm{e}_{2}^{\prime} & \bm e_{3}^{T} \bm{e}_{2}^{\prime}\\ \bm{e}_{1}^{T} \bm{e}_{3}^{\prime}& \bm{e}_{2}^{T} \bm{e}_{3}^{\prime} & \bm{e}_{3}^{T} \bm{e}_{3}^{\prime} \end{array}\right]$
同时
$\left[ e_{i1}, e_{i2}, e_{i3}\right] \left[ \begin{array}{c}e_{j1}^{\prime} \\e_{j2}^{\prime} \\e_{j3}^{\prime}\end{array}\right]= \bm e_{i}^{T} \bm{e}_{j}^{\prime} = \bm e_{j}^{\prime T} \bm{e}_{i}= \left[ e_{j1}^{\prime }, e_{j2}^{\prime }, e_{j3}^{\prime } \right] \left[ \begin{array}{c}e_{i1}\\ e_{i2}\\ e_{i3}\end{array}\right]$
故
$\bm R^{-1}=\bm R^{T}$

$|\bm R| =\left|\begin{array}{c}\bm e_{1}^{T} \\ \bm e_{2}^{T} \\ \bm e_{3}^{T}\end{array}\right| \left|\bm e_{1}^{\prime}, \bm e_{2}^{\prime}, \bm e_{3}^{\prime}\right| =1$

4.2 四元数的实部 $\eta$ 是一维，虚部 $\bm \varepsilon$ 是三维

4.3
记
$\bm {q}_{1}=[\bm \varepsilon_1,\eta_1]^T, \quad \bm {q}_{2}=[\bm \varepsilon_2,\eta_2]^T$

或原始四元数表示法：
$\bm {q}_{1}=\eta_1+x_1 \bm i + y_1 \bm j + z_1\bm k, \quad \bm {q}_{2}=\eta_2+x_2 \bm i + y_2 \bm j + z_2\bm k$
有
$\begin{aligned} \bm {q}_{1}\bm {q}_{2}= &\eta_{1} \eta_{2}-x_{1} x_{2}-y_{1} y_{2}-z_{1} z_{2} \\ &+ \left(\eta_{1} x_{2}+x_{1} \eta_{2}+y_{1} z_{2}-z_{1} y_{2}\right)\bm i \\ &+\left(\eta_{1} y_{2}-x_{1} z_{2}+y_{1} \eta_{2}+z_{1} x_{2}\right)\bm j \\ &+\left(\eta_{1} z_{2}+x_{1} y_{2}-y_{1} x_{2}+z_{1} \eta_{2}\right)\bm k \end{aligned}$
$\begin{aligned} \bm {q}_{1}\bm {q}_{2}= &\eta_1 \eta_2-x_1 x_2-y_1 y_2-z_1 z_2\\ &+(x_1 \eta_2+\eta_1 x_2-z_1 y_2+y_1 z_2)\bm i \\ &+(y_1 \eta_2+z_1 x_2+\eta_1 y_2-x_1 z_2) \bm j \\ &+(z_1 \eta_2-y_1 x_2+x_1 y_2+\eta_1 z_2)\bm k \end{aligned}$

写成向量形式并利用内外积运算表示:
$\bm {q}_{1}\bm {q}_{2}=[\eta_1 \bm \varepsilon_2 + \eta_2 \bm \varepsilon_1 + \bm \varepsilon_1 \times \bm \varepsilon_2, \eta_1 \eta_2 - \bm \varepsilon_1^T \bm \varepsilon_2]^T$
记
$\bm {q}^{+}_1=\left[\begin{array}{cc} \eta_1 \bm {1}+ \bm {\varepsilon}^{\times}_1 & \bm {\varepsilon}_1 \\ -\bm \varepsilon^{T}_1 & \eta_1 \end{array}\right] , \quad \bm {q}^{\oplus}_2=\left[\begin{array}{cc} \eta_2 \bm 1-\bm \varepsilon^{\times}_2 & \bm \varepsilon_2 \\ -\bm \varepsilon^{T}_2 & \eta_2 \end{array}\right]$
则
$\bm {q}^{+}_1\bm {q}_{2}=\left[\begin{array}{cc} \eta_1 \bm {1}+ \bm {\varepsilon}^{\times}_1 & \bm {\varepsilon}_1 \\ -\bm \varepsilon^{T}_1 & \eta_1 \end{array}\right] \left[\begin{array}{c} \bm \varepsilon_2 \\ \eta_2 \end{array}\right]= \left[\begin{array}{c} \eta_1 \bm \varepsilon_2 + \bm \varepsilon_1 \times \bm \varepsilon_2 +\eta_2 \bm \varepsilon_1 \\ -\bm \varepsilon_1^T \bm \varepsilon_2 +\eta_1 \eta_2 \end{array}\right] = \bm {q}_{1}\bm {q}_{2}$
$\bm {q}^{\oplus}_2\bm {q}_{1}= \left[\begin{array}{cc} \eta_2 \bm 1-\bm \varepsilon^{\times}_2 & \bm \varepsilon_2 \\ -\bm \varepsilon^{T}_2 & \eta_2 \end{array}\right] \left[\begin{array}{c} \bm \varepsilon_1 \\ \eta_1 \end{array}\right]= \left[\begin{array}{c} \eta_2 \bm \varepsilon_1 - \bm \varepsilon_2 \times \bm \varepsilon_1 +\eta_1 \bm \varepsilon_2 \\ -\bm \varepsilon_2^T \bm \varepsilon_1 +\eta_1 \eta_2 \end{array}\right] = \bm {q}_{1}\bm {q}_{2}$

在这里插入图片描述 5. 摘自维基百科 Rodrigues’ rotation formula

基本原理是将向量 $\bm v$ 绕转轴 $\bm k$ 的旋转分解为平行于转轴的 $\bm v_{\parallel }$ 和垂直于转轴的 $\bm v _{\perp }$ 的旋转，并用 $\bm v$ 和 $\bm k$ 表示出来； $\bm v_{\parallel }$ 旋转后的 $\bm{v}_{\parallel\mathrm{rot}}$ 保持不变， $\bm v_{\perp }$ 旋转后的 $\bm{v}_{\perp \mathrm{rot}}$ 在由 $\bm v_{\perp }$ 和 $\bm w$ （与 $\bm v_{\perp }$ 垂直）张成的二维平面内表示出来；最终获得 $\bm{v}_{\mathrm{rot}}$

在这里插入图片描述

在这里插入图片描述
记
$\bm {q}=[s,\bm v]^{\mathrm{T}} , \bm {p}=[0,\bm u]^{\mathrm{T}}$
有
$\bm q^{+}\left(\bm q^{-1}\right)^{\oplus}=\left[\begin{array}{cc} s & -\bm v^{\mathrm{T}} \\ \bm v & s \bm I+\bm v^{\wedge} \end{array}\right]\left[\begin{array}{cc} s & \bm v^{\mathrm{T}} \\ -\bm v & s\bm I+\bm v^{\wedge} \end{array}\right]= \left[\begin{array}{cc} 1 & \bm 0 \\ \bm 0^{\mathrm{T}} & \bm v\bm v^{\mathrm{T}}+s^{2} \bm I+2 s \bm v^{\wedge}+\left(\bm v^{\wedge}\right)^{2} \end{array}\right]$
$\bm p^{\prime} =\bm q \bm p \bm q^{-1}=\bm q^{+}\left(\bm q^{-1}\right)^{\oplus}\bm p= \left[\begin{array}{cc} 1 & \bm 0^{\mathrm{T}} \\ \bm 0 & \bm v\bm v^{\mathrm{T}}+s^{2} \bm I+2 s \bm v^{\wedge}+\left(\bm v^{\wedge}\right)^{2} \end{array}\right] \left[\begin{array}{c} 0 \\ \bm u \end{array}\right]= \left[\begin{array}{c} 0 \\ \left((\bm v\bm v^{\mathrm{T}}+s^{2} \bm I+2 s \bm v^{\wedge}+\left(\bm v^{\wedge}\right)^{2}\right) \bm u \end{array}\right]$
由上式结果看 $\bm p^{\prime}$ 实部为 $0$ 即 $\bm p^{\prime}$ 为虚四元数
若记 $\bm q =s+x\bm i+y\bm j+z\bm k$ 则旋转矩阵 $\bm Q$ 可表示为：
$\bm Q =\bm q^{+}\left(\bm q^{-1}\right)^{\oplus}= \left[\begin{array}{cc} 1 & \bm 0 \\ \bm 0^{\mathrm{T}} & \bm v\bm v^{\mathrm{T}}+s^{2} \bm I+2 s \bm v^{\wedge}+\left(\bm v^{\wedge}\right)^{2} \end{array}\right]= \left[\begin{array}{cccc} 1 & 0 & 0 & 0 \\ 0 & 1-2 y^{2}-2 z^{2} & 2 x y-2 s z & 2 s y+2 x z \\ 0 & 2 x y+2 s z & 1-2 x^{2}-2 z^{2} & 2 y z-2 s x \\ 0 & 2 x z-2 s y & 2 s x+2 y z & 1-2 x^{2}-2 y^{2} \end{array}\right]$

方法二：摘自 A brief introduction to the quaternions and its applications in 3D geometry
在这里插入图片描述

在这里插入图片描述 7.
所用的新特性已写在注释中

#include <iostream>
#include <vector>
#include <algorithm>

using namespace std;

class A {
public:
    A(const int &i) : index(i) {}

    int index = 0;
};

int main() {
    A a1(3), a2(5), a3(9);
    vector<A> avec{a1, a2, a3}; // 初始化列表

    // 运用lambda表达式 []
    // 提供了一个类似匿名函数的特性，而匿名函数则是在需要一个函数，但是又不想费力去命名一个函数的情况下去使用
    std::sort(avec.begin(), avec.end(), [](const A&a1, const A&a2) {return a1.index<a2.index;});

    // 范围for循环：用a遍历容器avec中的每个量  auto自动类型推导：根据a获得的值，自动推断出a的类型
    for ( auto& a: avec ) cout<<a.index<<" ";
    cout<<endl;
    return 0;
}