视觉SLAM十四讲笔记-- 第三讲

目录

第三讲课后题

第三讲课后题

1、验证旋转矩阵是正交矩阵。

（1）正交矩阵
定义：设A为n阶方阵，如果 $A^TA=I$ 或者 $AA^T=I$ ，就称A为正交矩阵
性质：

正交矩阵的每一个列向量都是单位向量，且向量之间两两正交。
正交矩阵的行列式为1 或者 -1
$A^{-1}=A^T$ (充要条件)

（2）旋转矩阵
旋转矩阵描述了坐标系之间的旋转变换，由3 × 3 的矩阵描述。

验证思路是利用正交矩阵的充要条件（ $A^{-1}=A^T$ ）来验证。

根据书中的例子设单位正交基（ $e_1, e_2,e_3$ ）经过一次旋转变成了（ $e^{'}_1, e^{'}_2,e^{'}_3$ ），因为同一向量 a 在两个坐标系下分别为（ $a_1, a_2,a_3$ ）,（ $a^{'}_1, a^{'}_2,a^{'}_3$ ），没有发生变化，根据坐标定义，因此有：
$\begin{bmatrix} e_1&e_2&e_3\end{bmatrix} \begin{bmatrix} a_1\\\\a_2\\\\a_3\end{bmatrix} =\begin{bmatrix} e^{'}_1& e^{'}_2&e^{'}_3\end{bmatrix} \begin{bmatrix} a^{'}_1\\\\a^{'}_2\\\\a^{'}_3\end{bmatrix} \color{red}（式子1.1）$

两边同时左乘 $\begin{bmatrix} e^T_1\\\\e^T_2\\\\e^T_3\end{bmatrix}$ ，就变成了如下：
$\begin{bmatrix} e^T_1\\\\e^T_2\\\\e^T_3\end{bmatrix} \begin{bmatrix} e_1&e_2&e_3\end{bmatrix} \begin{bmatrix} a_1\\\\a_2\\\\a_3\end{bmatrix} = \begin{bmatrix} e^T_1\\\\e^T_2\\\\e^T_3\end{bmatrix} \begin{bmatrix} e^{'}_1& e^{'}_2&e^{'}_3\end{bmatrix} \begin{bmatrix} a^{'}_1\\\\a^{'}_2\\\\a^{'}_3\end{bmatrix}$
左式由于基向量正交所以得到单位阵，所以式子化简如下：
$a=\begin{bmatrix} a_1\\\\a_2\\\\a_3\end{bmatrix} =\begin{bmatrix} e^T_1e^{'}_1 & e^T_1e^{'}_2 & e^T_1e^{'}_3 \\\\ e^T_2e^{'}_1 & e^T_2e^{'}_2 & e^T_2e^{'}_3 \\\\ e^T_3e^{'}_1 & e^T_3e^{'}_2 & e^T_3e^{'}_3 \end{bmatrix} \begin{bmatrix} a^{'}_1\\\\a^{'}_2\\\\a^{'}_3\end{bmatrix} = Ra^{'} \color{red}（式子1.2）$
同理，式子1.1 两边同时左乘 $\begin{bmatrix} e^{'T}_1\\\\e^{'T}_2\\\\e^{'T}_3\end{bmatrix}$ ，就变成了如下：
$\begin{bmatrix} e^{'T}_1\\\\e^{'T}_2\\\\e^{'T}_3\end{bmatrix} \begin{bmatrix} e_1&e_2&e_3\end{bmatrix} \begin{bmatrix} a_1\\\\a_2\\\\a_3\end{bmatrix} = \begin{bmatrix} e^{'T}_1\\\\e^{'T}_2\\\\e^{'T}_3\end{bmatrix} \begin{bmatrix} e^{'}_1& e^{'}_2&e^{'}_3\end{bmatrix} \begin{bmatrix} a^{'}_1\\\\a^{'}_2\\\\a^{'}_3\end{bmatrix}$
右式由于基向量正交所以得到单位阵，所以式子化简如下：
$R^T= \begin{bmatrix} e^{'T}_1e_1& e^{'T}_1e_2&e^{'T}_1e_3\\\\ e^{'T}_2e_1&e^{'T}_2e_2&e^{'T}_2e_3\\\\ e^{'T}_3e_1&e^{'T}_3e_2&e^{'T}_3e_3\end{bmatrix} \begin{bmatrix} a_1\\\\a_2\\\\a_3\end{bmatrix} = \begin{bmatrix} a^{'}_1\\\\a^{'}_2\\\\a^{'}_3\end{bmatrix} = a^{'}$
所以由式子1.2 $R^T =R^{-1}$
由正交矩阵充要条件 $A^{-1}=A^T$ 可得，旋转矩阵就是正交矩阵。

2.*寻找罗德里格斯公式的推导过程并加以理解。

答：个人理解，就是旋转向量到旋转矩阵的转换公式。
可以参考这个博客：https://blog.csdn.net/q583956932/article/details/78933245

3.验证四元数旋转某个点后，结果是一个虚四元数(实部为零)，所以仍然对应到一个三维空间点，见式(3.33)。

答：
设空间中某个虚四元数 $p=\begin{bmatrix} 0, v\\ \end{bmatrix}$ ,旋转 $q=\begin{bmatrix} s_a, v_a\\ \end{bmatrix}$ ，由书中的各种公式：
（1）旋转公式： $p^{'}=qpq^{-1}$
（2）求逆公式： $q^{-1}=q^*/||q||^2$
（3）共轭公式： $q*=[s, -v]^T$
（4）乘法法则： $q_aq_b=[s_as_b-v^T_av_b, s_av_b + s_bv_a + v_a × v_b ]^T$
第一步计算： $qp=[-v^T_av,s_av_b + v_a × v ]^T$
第二步计算： $qpq^{-1}=qpq^*/||q||^2$ ，完全计算比较麻烦，现在就可以转换成计算 $qpq^*$ 的实部
$qpq^*=[-v^T_av,s_av_b + v_a × v ]^T [s_a, -v_a]^T$
实部为： $-v^T_avs_a-[s_av_b + v_a × v ]^T(-v_a)=-v^T_avs_a+v^T_avs_a+(v_a × v )^Tv_a=(v_a × v)^Tv_a$
因为 $v_a × v$ 与 $v_a$ 垂直，因此 $qpq^{-1}$ 的实部为0,为纯虚四元数。

4．画表总结旋转矩阵、轴角、欧拉角、四元数的转换关系。

答：如下表格：

	旋转矩阵	轴角	四元数
旋转矩阵		$\theta=\arccos\bigg(\tfrac{tr(R)-1}{2}\bigg)$ 转轴n是矩阵R特征值1对应的特征向量	$q_0=\tfrac{\sqrt{\smash[b]{tr(R)+1}}}{2}$ $q_1=\tfrac{m_{23} - m{32}}{4q_0}$ $q_2=\tfrac{m_{31} - m{13}}{4q_0}$ $q_3=\tfrac{m_{12} - m{21}}{4q_0}$
轴角	$R=\cos\theta\boldsymbol I+(1-\cos\theta)nn^T+\sin\theta\boldsymbol n^{\land}$	–	逆变换即可
四元数	$R=vv^T+s^2I+2sv^{\land}+(v^{\land})^2$	$\theta=2\arccos{q_0}$ $[n_x, n_y, n_z]^T=[q_1, q_2, q_3]^T/\sin{ \theta \over 2 }$	–

5．假设有一个大的Eigen矩阵，想把它的左上角3×3的块取出来,然后赋值为 $\boldsymbol I_{3 × 3}$ ，请编程实现。

答：代码如下

// Eigen 几何模块
#include <Eigen/Dense>
#include <iostream>

#define Matrix_size 50

using namespace std;
using namespace Eigen;

int main ( int argc, char** argv )
{
        MatrixXd matrix_x;

        matrix_x=MatrixXd::Random(Matrix_size,Matrix_size);

        cout<<"matrix_x=\n"<<matrix_x<<endl;
        
        //方法1：直接取左上角的矩阵
        Matrix3d m;
        m=matrix_x.topLeftCorner(3,3);
        cout<<"方法1：\n"<<m<<endl;
        m=Matrix3d::Identity(3,3);
        cout<<"Matrix3d::Identity(3,3)=\n"<<m<<endl;
        //方法2：待定
        matrix_x.block<3,3>(0,0);
        cout<<"matrix_x.block=\n"<<matrix_x.block<3,3>(0,0).Identity()<<endl;
        return 0;
}

6.*一般线性方程 An =b有哪几种做法?你能在 Eigen中实现吗?

答：代码如下

#include <iostream>
#include <Eigen/Core>
#include <Eigen/Dense>

using namespace Eigen;
using namespace std;

#define Matrix_size 10


int main ( int argc, char** argv )
{
        // 动态维度的矩阵
        MatrixXd A=MatrixXd::Random(Matrix_size,Matrix_size);
        MatrixXd b=MatrixXd::Random(Matrix_size,1);
//         cout<<"A=\n"<<A<<endl;
//         cout<<"b=\n"<<b<<endl;
        
        //1:直接求逆
        Matrix<double,Matrix_size,1> x=A.inverse()*b;
        cout<<"直接求逆=\n"<<x.transpose()<<endl<<endl;

        //2:QR分解
        x=A.colPivHouseholderQr().solve(b);
        cout<<"QR分解=\n"<<x.transpose()<<endl<<endl;

        //3:LLT分解
//        x=A.llt().solve(b);//要求矩阵正定
//        cout<<x.transpose()<<endl<<endl;

        //4: LDLT分解法
//        x=A.ldlt().solve(b);//要求矩阵正或负半定
//        cout<<x.transpose()<<endl<<endl;

        //5:
        x=A.fullPivHouseholderQr().solve(b);
        cout<<"fullPivHouseholderQr=\n"<<x.transpose()<<endl<<endl;

        //6:LU分解法
        x=A.householderQr().solve(b);
        cout<<"householderQr=\n"<<x.transpose()<<endl<<endl;

        //7:partialPivLu
        x=A.partialPivLu().solve(b);
        cout<<"partialPivLu=\n"<<x.transpose()<<endl<<endl;


        return 0;
}

参考文献：
[1]. https://www.cnblogs.com/cc111/p/9240545.html
[2]. https://katex.org/docs/supported.html
[3]. https://www.cnblogs.com/cc111/p/9354924.html
[4]. https://zhuanlan.zhihu.com/p/338322663