eigen 列向量转矩阵_视觉SLAM十四讲实践笔记之Eigen下的四种位姿表示方法

姿态的四种表达方式

1. 旋转矩阵也叫方向余弦旋转矩阵是一个 3 x3 阶的矩阵，矩阵的列表示载体坐标系中的单位矢量在参考坐标系中的投影。

2. 旋转向量也叫轴角任意旋转都可以用一个旋转轴和一个旋转角刻画。旋转向量可以由罗德里格斯公式转换成旋转矩阵

3. 欧拉角从一个坐标系到另一个坐标系的变换可以通过依次绕不同坐标轴的3次连续转动来定义，从物理角度看欧拉角表示法可能是最简单的方法之一。

4. 四元数四元数姿态表示法，通过绕参考坐标系中两个矢量的单次转动来实现一个坐标系到另一个坐标系的转换。四元数是一个具有四个元素的矢量表达式，各个元素为矢量方向和转动大小的函数。

旋转矩阵(方向余弦)

旋转矩阵的推倒过程

Eigen中的旋转矩阵

//  旋转矩阵
Eigen::Matrix3d rotation_matrix;
// 绕 Z 轴旋转 45 度的旋转矩阵 sin45 = cos45 = 0.707107
rotation_matrix << 0.707107, -0.707107, 0,
                   0.707107,  0.707107, 0,
                   0,         0,        1;
std::cout << "rotation_matrix =n" << rotation_matrix << std::endl;
/*
 * rotation_matrix =
 * 0.707107   -0.707107   0
 * 0.707107    0.707107    0
 * 0           0         1
 */

Eigen中的旋转向量(轴角)

// 旋转矩阵转换为旋转向量
Eigen::AngleAxisd rotation_vector;
rotation_vector.fromRotationMatrix(rotation_matrix);
std::cout << "rotation_vector " << "angle is: " << rotation_vector.angle() << " axis is: "
          << rotation_vector.axis().transpose() << std::endl;
/*
 * rotation_vector angle is: 0.785398 axis is: 0 0 1
 */
// 弧度转角
cout << 0.785398 * 180 / M_PI << endl; // 45

欧拉角

欧拉角是描述刚体姿态的三个角，欧拉角有静态和动态两种，静态的是绕静止的惯性坐标系三个轴进行旋转，而动态的在旋转过程中旋转坐标轴会发生变化，除了第一次旋转是绕惯性系的坐标轴进行之外，后续两次旋转都是动态的，并且前面旋转的角度对后面的旋转轴是有影响的，按照不同的轴顺序进行旋转得到的欧拉角也是不同的，旋转变换可以归结为若干个沿着坐标轴旋转的组合，组合个数不超过三个并且两个相邻的旋转必须沿着不同坐标轴，总共有12种旋转方式，分别是XYZ、XZY、XYX、XZX、YXZ、YZX、YXY、YZY、ZXY、ZYX、ZXZ、ZYZ。ROS里面URDF采用的是XYZ(rpy)，规定右手坐标系中，物体旋转的正方向是右手螺旋方向。
在右手直角坐标系中，从原点看，沿每一根轴的顺时针方向定义为这根轴的正向转动，负向转动正好相反，即为逆时针方向。

必须记住的是，当绕不同的轴系作一系列转动时，载体姿态的变化不仅是绕每根轴转动角度的函数，而且还是转动顺序的函数。

左半图转动的顺序:先绕俯仰轴 y 转动 90 再绕偏航轴 z 转动 90 最后绕俯仰轴 y 转动 90

依次完成转动后，可以看到绕横滚轴 x 发生了 90 的净转动。

右半图中转动的顺序正好相反虽然转动结束时，载体的横滚轴仍对准在原来的方向，但横滚轴 x 发生了 -90 的净转动。

各个轴的转动顺序是不可交换的。很明显，如不考虑轴系的转动顺序，在计算姿态时将会引起很大的误差。

Eigen中的欧拉角

// 旋转矩阵转换为欧拉角
// "2" represents the z axis and "0" the x axis "1" the y axis
// 动轴: ZYX顺序,即先绕x轴roll旋转,再绕y轴pitch旋转,最后绕z轴yaw旋转,
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles(2, 1, 0);
// 默认向量都是列向量，所以为了书写和显示方便，转置成行向量
std::cout << "yaw(z) pitch(y) roll(x) = " << euler_angles.transpose() << std::endl;
/*
 *  yaw(z) pitch(y) roll(x) = 0.785398  -0  0
 */
cout << 0.785398 * 180 / M_PI << endl; // 45

Eigen中的四元数

// 找不到不带奇异性的三维向量描述方式，四元数是紧凑的，没有奇异性
// 旋转矩阵转换为四元数
Eigen::Quaterniond quaterniond = Eigen::Quaterniond(rotation_matrix);
std::cout << "quaternion from rotation matrix = n" << quaterniond.coeffs() << std::endl;
/*
 * quaternion from rotation matrix =
 * 0
 * 0
 * 0.382684
 * 0.92388
 */
std::cout << "quaterniond x: " << quaterniond.x() << std::endl;
std::cout << "quaterniond y: " << quaterniond.y() << std::endl;
std::cout << "quaterniond z: " << quaterniond.z() << std::endl;
std::cout << "quaterniond w: " << quaterniond.w() << std::endl;
/*
 * quaternion2 x: 0
 * quaternion2 y: 0
 * quaternion2 z: 0.382684
 * quaternion2 w: 0.92388
 */

四种位姿可视化

视觉SLAM十四讲实践笔记之四种位姿可视化

变换矩阵

// 变换矩阵
Eigen::Isometry3d transform_matrix = Eigen::Isometry3d::Identity();
transform_matrix.rotate(rotation_matrix);
transform_matrix.pretranslate(Eigen::Vector3d(1, 3, 4));
std::cout << "Transform matrix = n" << transform_matrix.matrix() << std::endl;
/*
 * Transform matrix =
 * 0.707107 -0.707107         0         1
 * 0.707107  0.707107         0         3
 *        0         0         1         4
 *        0         0         0         1
 */

用变换矩阵进行坐标变换

视觉SLAM十四讲实践之坐标系之间的欧式变换

// 坐标变换
Eigen::Vector3d v_src(1, 0, 0);
Eigen::Vector3d v_tar;
v_tar = transform_matrix * v_src;
std::cout << "(1,0,0) after rotation = " << v_tar.transpose() << std::endl;
// (1,0,0) after rotation = 1.70711 3.70711  4

位姿的估计和优化

视觉SLAM十四讲实践之Sophus下的位姿表示和更新优化

点击下方阅读原文可以获得更好阅读体验，并且会对原文进行迭代更新；欢迎和我一起交流学习；所有内容来自网络如有侵权请联系删除；仅代表个人观点，不喜勿喷！

阅读原文