视觉SLAM笔记（14） Eigen几何模块

---

1. 旋转表达方式

之前讲到的各种旋转表达方式
现在将在 Eigen 中使用四元数、欧拉角和旋转矩阵，演示它们之间的变换方式

还会给出一个可视化程序，帮助理解这几个变换的关系

Eigen 中对各种形式的表达方式总结如下：

---

2. 旋转矩阵

3D 旋转矩阵直接使用 Matrix3d 或 Matrix3f

Eigen::Matrix3d rotation_matrix = Eigen::Matrix3d::Identity();

旋转向量使用 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

也可以直接赋值

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

用 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

或者用旋转矩阵进行坐标变换

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

---

3. 欧拉角

欧拉角
可以将旋转矩阵直接转换成欧拉角

Eigen::Vector3d euler_angles = rotation_matrix.eulerAngles ( 2,1,0 ); // ZYX顺序，即roll pitch yaw顺序
cout<<"yaw pitch roll = "<<euler_angles.transpose()<<endl;

// yaw pitch roll = 0.785    -0     0

欧氏变换矩阵使用 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

用变换矩阵进行坐标变换

Eigen::Vector3d v_transformed = T*v;                              // 相当于R*v+t
cout<<"v tranformed = "<<v_transformed.transpose()<<endl;

// v tranformed = 1.71 3.71    4

---

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

也可以把旋转矩阵赋给它

q = Eigen::Quaterniond ( rotation_matrix );
cout<<"quaternion = \n"<<q.coeffs() <<endl;

// quaternion = 
//     0
//     0
// 0.383
// 0.924

使用四元数旋转一个向量，使用重载的乘法即可

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

---

5. 可视化演示

高博士准备了一个小程序visualizeGeometry

这个编译比较复杂一点
首先需要先下载解压Pangolin.tar.gz文件
压缩文件位于VSLAM_note/3rdparty/Pangolin.tar.gz
在编译前还需要做一些修改，参考 运行Pangolin时提示错误
修改好之后进行编译

$ mkdir build
$ cd build
$ cmake ..
$ make 
$ sudo make install 

一切顺利就进行小程序执行文件夹VSLAM_note/014/visualizeGeometry/的编译

$ mkdir build
$ cd build
$ cmake ..
$ make 

开启小程序

$ ./visualizeGeometry

它以可视化的形式演示了各种表达方式的异同
可以用鼠标操作一下，看看数据是如何变化的

这个小程序中，在坐标原点放置一个彩色立方体
用鼠标可以平移/旋转相机，可以实时地看到相机姿态的变化

显示了变换矩阵 R，t、欧拉角和四元数的三种姿态，可以实验体验一下这几个量是如何变化的
然而根据经验，除了欧拉角之外，应该看不出它们直观的含义

值得一提的是，实际当中，至少定义两个坐标系： 世界坐标系和相机坐标系
在该定义下，设某个点在世界坐标系中坐标为 $p$_w，在相机坐标系下为 $p$_c，那么：

这里 $T$_cw 表示世界坐标系到相机坐标系间的变换
或者可以用反过来的 $T$_wc：

原则上， $T$_cw 和 $T$_wc 都可以用来表示相机的位姿，事实上它们也只差一个逆而已
实践当中使用 $T$_cw 更加常见，而 $T$_wc 更为直观

如果把上面两式的 $p$_c 取成零向量，也就是相机坐标系中的原点
那么，此时的 $p$_w 就是相机原点在世界坐标系下的坐标：

发现这正是 $T$_wc 的平移部分
因此，可以从 $T$_wc 中直接看到相机在何处，这也是 $T$_wc 更为直观的原因
因此，在可视化程序里，显示了 $T$_wc 而不是 $T$_cw

---

参考：

《视觉SLAM十四讲》

---

相关推荐：

视觉SLAM笔记（13） 空间变换
视觉SLAM笔记（12） 四元数
视觉SLAM笔记（11） 欧拉角
视觉SLAM笔记（10） 旋转向量
视觉SLAM笔记（9） Eigen

---

谢谢！

转载：https://blog.csdn.net/qq_32618327/article/details/101599067