视觉SLAM ch3 代码总结（一）

useGeometry

代码分别用几种旋转方式如 angle axis 、matrix 、变换矩阵、四元数旋转一个向量v（1，0，0）

最后得出的结果就是旋转以后的 v丶坐标 。不过变换矩阵的结果加入了平移向量。

在书中不懂的地方自己进行了注释

1.定义

（1）三维空间坐标定义

Vector3d v(1,0,0)

（2）旋转向量定义

AngleAxisd rotation_vector((M_PI / 4,Vector3d(0,0,1))

//沿Z轴旋转45度
//使用AngleAxisd定义，AngleAxisd（旋转角度，旋转轴单位向量Vector3d） 
//AngleAxisd的底层不直接是Matrix，但由于重载了运算符，因此仍可以当做矩阵运算

注意：AngleAxisd不能用cout输出，以该定义为例，要用rotation_vector.matrix()转换成矩阵形式才能输出，但转换后不再是旋转向量形式，而是3x3的旋转矩阵形式。

（3）欧拉角定义

//使用三维纵向量Vector3d定义
Vector3d eulerangles(0,0,0);

（4）欧式变换矩阵定义（4x4矩阵）

Isometry3d T = Isometry3d::Identity();

//使用四元数初始化欧式矩阵
Isometry3d T(q)

注意：欧氏变换矩阵不能直接用cout输出，以该定义为例，要使用T.matrix()转换为矩阵形式输出，转换后仍为4x4的欧氏变换矩阵

（5）四元数定义（使用前要归一化  ）

  归一化方法为：

q.normalize();

//使用Vector4d定义，定义格式为（x,y,z,w)   w是实部 
Quaterniond q(Vector4d(1,2,3,4))

//直接赋值，定义格式为(w,x,y,z)
Quaterniond q((1,2,3,4));

注意：四元数不能直接用cout输出，以该定义为例，输出格式为

cout << q.coeffs() << endl;  
//输出格式为（x,y,z,w)  w是实部 

2. 各种表达形式之间的转换

（1）旋转向量与旋转矩阵的转换

rotation_matrix = rotation_vector.toRotationMatrix();
或者
rotation_matrix = rotation_vector.matrix();    //用matrix转换成矩阵

（2）旋转矩阵与欧拉角的转换

eulerangles = rotation_matrix.eulerAngles(2,1,0);
//参数（2，1，0）表示这是ZYX顺序

（3）欧式变换矩阵按照旋转矩阵和旋转向量 旋转

T.rotate(rotation_matrix);  //按照旋转矩阵进行旋转，
T.rotate(rotation_vector);  //按照旋转向量进行旋转

（4）四元数与旋转矩阵和旋转向量的转换

q = Quaterniond(rotation_matrix);
q = Quaterniond(rotation_vector);

3.坐标变换

Vector3d v(1,0,0); //定义初始坐标v
Vector3d v_rotated； //定义坐标转换后坐标v_rotated

（1）使用旋转矩阵

v_rotated = rotation_matrix * v;

(2)使用旋转向量

v_rotated = rotation_vector * v

( 3)使用变换矩阵

Vector3d v_tranformed = T * v;

//相当于 R * v + t   
//使用变换矩阵得出的坐标中加入了平移向量

(4)使用四元数

v_rotated = q*v; 

//这里的乘法是重载过的
//数学上是 q v q{-1}

参考

第三讲第六节-Eigen几何模块_我只是一只自动小青蛙的博客-CSDN博客