视觉SLAM笔记（14） Eigen几何模块
视觉SLAM笔记（14） Eigen几何模块
Eigen几何模块包含了旋转表达方式（四元数、欧拉角和旋转矩阵）及其相互转换关系，并通过可视化程序演示了不同变换形式的异同。该模块还介绍了仿射变换和射影变换的实现方法。此外，提供了使用Pangolin进行三维可视化的小程序，并详细说明了其编译与运行方法。
视觉SLAM笔记（14） Eigen几何模块
- 1. 旋转表达方式
- 2. 旋转矩阵
- 3. 欧拉角
- 3. 仿射和射影变换
- 4. 四元数
- 5. 可视化演示
1. 旋转表达方式
之前我们介绍了各种旋转表达方式。
现在 Eigen 将被用来展示如何将四元数、欧拉角和旋转矩阵相互转换。
还会给出一个可视化程序，帮助理解这几个变换的关系
Eigen 中对各种形式的表达方式总结如下：
2. 旋转矩阵
3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f
Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();
AI助手旋转向量通过AngleAxis表示，在其基础类型并非直接为Matrix的情况下（尽管支持了相应的运算操作），然而，在实际应用中该旋转向量的运算行为等效于矩阵空间中的操作
Eigen::AngleAxisd rotation_vector ( M_PI/4, Eigen::Vector3d ( 0,0,1 ) ); //沿 Z 轴旋转 45 度
cout .precision(3);
cout<<"rotation matrix =\n"<<rotation_vector.matrix() <<endl; //用matrix()转换成矩阵
rotation_matrix = rotation_vector.toRotationMatrix(); // 也可以直接赋值
// rotation matrix =
// 0.707 -0.707 0
// 0.707 0.707 0
// 0 0 1
AI助手也可以直接赋值
rotation_matrix = rotation_vector.toRotationMatrix();
cout<<"rotation matrix =\n"<<rotation_matrix <<endl;
// rotation matrix =
// 0.707 -0.707 0
// 0.707 0.707 0
// 0 0 1
AI助手用 AngleAxis 可以进行坐标变换
Eigen::Vector3d v ( 1,0,0 );
Eigen::Vector3d v_rotated = rotation_vector * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
// (1,0,0) after rotation = 0.707 0.707 0
AI助手或者用旋转矩阵进行坐标变换
v_rotated = rotation_matrix * v;
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
// (1,0,0) after rotation = 0.707 0.707 0
AI助手3. 欧拉角
欧拉角
可以将旋转矩阵直接转换成欧拉角
Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即yaw pitch roll顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;
// yaw pitch roll = 0.785 -0 0
AI助手欧氏变换矩阵使用 Eigen::Isometry
Eigen::Isometry3d T=Eigen::Isometry3d::Identity(); // 虽然称为3d，实质上是4＊4的矩阵
T.rotate ( rotation_vector ); // 按照rotation_vector进行旋转
T.pretranslate ( Eigen::Vector3d ( 1,3,4 ) ); // 把平移向量设成(1,3,4)
cout << "Transform matrix = \n" << T.matrix() <<endl;
// Transform matrix =
// 0.707 -0.707 0 1
// 0.707 0.707 0 3
// 0 0 1 4
// 0 0 0 1
AI助手用变换矩阵进行坐标变换
Eigen::Vector3d v_transformed = T*v; // 相当于R*v+t
cout<<"v tranformed = "<<v_transformed.transpose()<<endl;
// v tranformed = 1.71 3.71 4
AI助手3. 仿射和射影变换
使用 Eigen::Affine3d 和 Eigen::Projective3d 即可
4. 四元数
可以直接把AngleAxis赋值给四元数，反之亦然
Eigen::Quaterniond q = Eigen::Quaterniond ( rotation_vector );
cout<<"quaternion = \n"<<q.coeffs() <<endl; // 请注意coeffs的顺序是(x,y,z,w),w为实部，前三者为虚部
// quaternion =
// 0
// 0
// 0.383
// 0.924
AI助手也可以把旋转矩阵赋给它
q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<<q.coeffs() <<endl;
// quaternion =
// 0
// 0
// 0.383
// 0.924
AI助手使用四元数旋转一个向量，使用重载的乘法即可
v_rotated = q*v; // 注意数学上是qvq^{-1}
cout<<"(1,0,0) after rotation = "<<v_rotated.transpose()<<endl;
// (1,0,0) after rotation = 0.707 0.707 0
AI助手5. 可视化演示
高博士准备了一个小程序visualizeGeometry
这个编译过程较为复杂一点。具体来说，
为了进行编译操作，请先下载并解压Pangolin.tar.gz文件。
该压缩包的具体位置为VSLAM_note/3rdparty/Pangolin.tar.gz。
在进行编译操作之前，请仔细查阅相关资料以解决可能出现的问题：运行Pangoline时提示错误
完成上述所有设置后，请尝试重新运行编译程序。
$ mkdir build
$ cd build
$ cmake ..
$ make
$ sudo make install
AI助手整个过程顺利的情况下，请执行以下操作：编译位于VSLAM_note/014/visualizeGeometry/的执行文件夹
$ mkdir build
$ cd build
$ cmake ..
$ make
AI助手开启小程序
$ ./visualizeGeometry
AI助手它通过可视化展示不同表达方法间的差异；试着用鼠标操作一下并观察数据的变化情况
在该应用程序中，将一个三维色块放置于坐标原点。通过鼠标操作，在该系统中可以在坐标空间内自由移动和调整观察角度，并即时观察到物体的姿态变化情况。
该文呈现了变换矩阵 R 和 t 以及欧拉角和四元数三种姿态的表现形式，并建议读者可以通过实验来体验这些量的变化规律。
然而根据经验观察发现，在实际应用中除了欧拉角以外其他几种量并没有明显的直观意义。
值得注意的是，在实际应用中, 至少需要定义两个不同的参考框架: 世界参考框架与相机参考框架. 在此框架下, 考虑一个点P, 其在世界参考框架下的位置表示为P_w, 在相机参考框架下的位置表示为P_c, 则:
在本节所述的具体场景下，在世界坐标系与相机坐标系之间建立的变换关系具体表现为矩阵形式中的变量 Tcw。
一般情况下，在计算机视觉中，Tcw 和 Twc 都被用于描述相机的位姿信息；值得注意的是，它们仅相差一个逆操作。
在实践中，Tcw 的应用更为广泛；相比之下, Twc 的实现更加直接。
在数学表达中，在两式中的pc被设定为零向量，则等价于将该值设定为相机坐标系的原点位置。由此可见，在这种情况下，pw就直接表示了相机在世界坐标系中的位置。
观察到该算法的输出结果即为相机位置。
由此可见，在该方法中可以直接推导出相机位置的坐标值。
基于上述分析，在可视化界面中我们采用了该算法而非其镜像版本。
参考：
相关推荐：
计算机视觉中的 SLAM 技术学习笔记（十三） 坐标系转换
计算机视觉中的 SLAM 技术学习笔记（十二） 四元数表示
计算机视觉中的 SLAM 技术学习笔记（十一） 欧拉角介绍
计算机视觉中的 SLAM 技术学习笔记（十） 旋转向量与旋转矩阵
计算机视觉中的 SLAM 技术学习笔记（九） Eigen 应用
谢谢！