视觉SLAM十四讲笔记-第三讲刚体运动

最新推荐文章于 2023-07-05 18:54:11 发布

独自悠哉独自在

最新推荐文章于 2023-07-05 18:54:11 发布

阅读量1.7k

点赞数 2

分类专栏：视觉SLAM 文章标签：经验分享

本文链接：https://blog.csdn.net/m0_55941084/article/details/122548342

版权

视觉SLAM 专栏收录该内容

4 篇文章 1 订阅

订阅专栏

4.1 旋转向量（Rotation Vector，又称角轴/轴角（Angle Axis）

4.2 欧拉角（Euler Angles）——SLAM领域不用这种表达方式

5.四元数

5.1 三位情况下，四元数可作为复数的扩充：

5.2 单位四元数可表达旋转

5.3 四元数的变换运算

5.4 四元数的旋转前后表示（纯虚四元数）

1.两条基本公式：运动方程和观测方程

2.点与坐标系：

一般的比较熟悉，需要了解的是外积：熟悉一下上三角符号

3. 旋转矩阵

3.1 两个条件：

是一个正交矩阵；
行列式为1.

4.旋转向量和欧拉角

4.1 旋转向量（Rotation Vector，又称角轴/轴角（Angle Axis）

方向为旋转轴，长度为转过的角度；
仅有三个自由度，而R旋转矩阵只有九个自由度；
无约束，更直观；
需要注意的是，这里介绍的四个量是同一个东西的不同表达方式，它们之间可以互相转换

4.2 欧拉角（Euler Angles）——SLAM领域不用这种表达方式

4.2.1 将旋转分解成三个方向上的转动

例如，按Z-Y-X顺序旋转：

绕物体的Z轴旋转，得到偏航角yaw；
绕旋转后的Y轴旋转，得到俯仰角pitch；
绕旋转之后的X轴旋转，得到滚转角roll.
需要注意的是，绕的轴是动轴还是定轴;
顺序可以不同，且机器人学中的欧拉角与航天领域中的欧拉角不太相同。

4.2.2 特点：

非常直观；

4.2.3 由于万向锁存在，不可避免的存在奇异性，故欧拉角不适合SLAM使用。

5.四元数

5.1 三位情况下，四元数可作为复数的扩充：

一个实部和三个虚部

5.2 单位四元数可表达旋转

乘以 i 表示旋转 90°，乘以 -i 表示旋转 -90°，与复数神似；
自己和自己运算像复数，自己和别人的运算像叉乘.

5.3 四元数的变换运算

5.4 四元数的旋转前后表示（纯虚四元数）

5.5 四元数的优势

紧凑、无奇异性（对比欧拉角、旋转向量）;
自由度小（对比旋转矩阵）.

6. 实践环节——Eigen库

Eigen通过typedef提供了很多内置类型，底层仍是Eigen::Matrix;

Eigen库被分为一个核心模块和几个别的模块，每个模块有一个对应的头文件，使用该模块需要包含对应头文件。还提供了 Dense 和 Eigen 头文件方便同时使用多个模块。

6.1 如何定义矩阵：

1.声明一个2*3的 float 类型矩阵

Eigen::Matrix<float,2,3> matrix_23;

2.Vector3d实质上是Eigen:::Matrix<double,3,1>，即三维列向量，注意这里是double 类型

Eigen::Vector3d v_3d;

3.Matrix3d实质上是Eigen:::Matrix<double,3,3>，可以看出Matrix直接定义的都是方阵类型

Eigen::Matrix3d matrix_33 = Eigen::Matrix3d::Zero();//初始化为零

4.如果不确定矩阵大小，可以用动态矩阵C

Eigen::Matrix<double,Eigen::Dynamic,Eigen::Dynamic> matrix_dynamic; //动态矩阵

也有更简单的写法

Eigen::MatrixXd matrix_dynamic;                         //动态矩阵

5.数据存储：Matrix创建的矩阵默认是按列存储，Eigen在处理按列存储的矩阵时会更加高效。如果想修改可以在创建矩阵的时候加入参数，如:

Matrix<int,3, 4, ColMajor> Acolmajor;
Matrix<int,3, 4, RowMajor> Arowmajor;

6.2 如何操作矩阵：

1.初始化（矩阵赋值）

类型一:

matrix_23 << 1,2,3,4,5,6;

类型二:（第五讲和稠密建图用的也是这种）

Isometry3d T = Isometry3d::Identity();   //用Isometry3d定义的是4*4矩阵，不要被3d迷惑
T.rotate(rotation_vector);             //按输入的rotatin_vector作为旋转矩阵部分，即R
T.pretranslate(Vector3d(1,3,4));     //按Vector作为平移向量部分，即t

类型三：（接二）

Isometry3d T = Isometry3d::Identity(); 
//定义一个四元数变量q，注意顺序是（x，y，z，w），w为实部         
Quanernioud q = Quanterniond(rotatin_vector);  
T.rotate( q );               //按输入的四元数 q 变换作为旋转矩阵部分
T.pretranslate(Vector3d(1,3,4));                
q = Quanterniond(rotation_matrix);  //同理，也可以按输入旋转矩阵变换成四元数 q

2.输出

cout << matrix_23 << endl;

3.访问第 i 行第 j 列的矩阵元素

for (int i = 0 ; i < 2 ; i++){
    for (int j = 0 ; j < 3 ; j++)
        cout << matrix_23(i,j) <<endl;
    cout << endl;
}

4.矩阵相乘(注意不要混淆数据类型，float和double)

假如这里的matrix_23是float类型，则需要强制转换成double类型才成功

Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * vd_3d;

5.随机数矩阵

Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * vd_3d;

Eigen::Matrix<double,2,1> result = matrix_23.cast<double>() * vd_3d;

6.其它运算

cout << matrix_33.transpose() << endl;      //转置
cout << matrix_33.sum() << endl;            //求各元素之和
cout << matrix_33.trace() << endl;          //求迹，主对角线元素之和
cout << 10 * matrix_33 << endl;             //数乘
cout << matrix_33.inverse() << endl;        //求逆
cout << matrix_33.determinant() << endl;    //求行列式
cout << matrix_33.conjugate() << endl;      //求共轭矩阵
cout << matrix_33.adjoint() << endl;        //求共轭转置

短短几行一下子就好了，比起线性代数那会手算好多了。

-------------------------------------------------------------------------------------

若文章有误请私聊指教、更改。

致谢：

《视觉SLAM十四讲》——高翔