个人博客：http://www.chenjianqu.com/
原文链接：http://www.chenjianqu.com/show-82.html
本文是总结<视觉SLAM14讲>所做的笔记。在前面的博文ROS-坐标转换(TF)介绍了ROS中的坐标变换通信的机制，但是没有提到坐标变换本身，因此这篇博文补充这些基本概念。
两个坐标系之间的运动由一个旋转加上一个平移组成，若同一个向量在各个坐标系下的长度和夹角都不会发生变化，则这种运动称为刚体运动。刚体运动前后的两个坐标系相差一个欧式变换。除了欧式变换之外，还有其它的变换，汇总如下：
欧式变换：保持向量的长度和夹角，包括旋转和平移。
相似变换：比欧式变换多了一个缩放因子s，可以在旋转之后进行缩放。
仿射变换：仿射变换要求A是是一个可逆矩阵，而不一定是正交矩阵。经过放射变换后，立方体可能会变成斜的，但是各个面仍然是平行四边形。
射影变换:射影变换是最一般的变换，左上角为可逆矩阵A，右上角为平移t，左下角为缩放a。
各个变换对比：
一般说的坐标变换就是欧式变换。
旋转矩阵和变换矩阵
旋转矩阵
对于坐标变换，向量本身没有发生变换，只是坐标系变了，根据坐标的定义有：
对上式左右两边同时左乘[e1,e2,e3]T，得：
得到旋转矩阵R， 该矩阵各分量是两个坐标系基的内积，由于基向量的长度为 1，所以实际上是各基向量的夹角之余弦，所以也叫方向余弦矩阵(Direction Cosine matrix)。旋转矩阵是行列式为1的正交矩阵，反之行列式为1的正交矩阵也是旋转矩阵。旋转矩阵集合：
SO(n)称为特殊正交群(special Orthogonal Group)。旋转矩阵的逆描述了一个相反的旋转，且R-1=RT。
平移向量
欧式变换由旋转变换和平移变换组成，旋转变换由旋转矩阵R描述，平移变换由平移向量t描述，即坐标变换A’=RA+t。
变换矩阵
使用旋转矩阵和平移向量描述坐标系变换，多次变换后公式过于复杂，因此引入齐次坐标和变换矩阵。齐次坐标：在三维向量末尾添加1,变成四维向量。变换矩阵T；将旋转和平移写在一个矩阵里。坐标变换可表示如下：
变换矩阵集合：
SE(3)的意思是特殊欧式群(Special Euclidean Group)。变换矩阵的逆也表示一个反向的变换：
旋转向量和欧拉角
旋转向量
用旋转矩阵描述旋转存在以下缺点：
1.SO(3)的旋转矩阵有9个量，但是一次旋转只有三个自由度，因此这种方式存在冗余。变换矩阵同理。
2.旋转矩阵自身带有约束：必须是正交矩阵，而且行列式为1。当想要估计或优化一个旋转矩阵/变换矩阵时，这些约束会使得求解变得更加困难。
因此需要一种更加紧凑方式的描述旋转和平移。任意旋转都可以用一个旋转轴和一个旋转角来刻画，使用一个向量，其方向和旋转轴一致，长度等于旋转角，这种向量称为旋转向量（或轴角，Axis-Angle）。这种表示法仅需要一个三维向量即可描述旋转。使用旋转向量加平移向量，只有六维，即可描述一次变换。
旋转向量和旋转矩阵之间的转换
假设有一个旋转轴为n，角度为θ的旋转，则其对应的旋转向量为θ*n。从旋转向量到旋转矩阵的转换使用罗德里格斯公式：
符号^表示向量到反对称的转换符，如下：
反之从旋转矩阵转换到旋转向量，对于转角θ，有：
其中tr(R)表示矩阵R的迹。对于转轴n，由于旋转轴上的向量在旋转后不发生改变，即Rn=n，因此转轴n是旋转矩阵R特征值1对应的特征向量。求解此方程，再归一化，就得到了旋转轴。
欧拉角
旋转矩阵和旋转向量虽然能描述旋转，但是非常不直观。欧拉角提供了非常直观的方式描述旋转，将一个旋转分解为3次绕不同轴的旋转。比如先绕X轴，再绕Y轴，最后绕Z轴就得到XYZ轴的旋转。同理得ZYX,ZXY等方式的旋转。这里使用ZYX分解，假设一个刚体的前方(朝向我们的方向)为X轴，右侧为Y轴，上方为Z轴。
1.绕物体的Z轴旋转，得到偏航角yaw；
2.绕旋转之后的Y轴旋转，得到俯仰角pitch；
3.绕旋转之后的X轴旋转，得到滚转角roll。
欧拉角和旋转向量的一个重大缺点是万向锁问题(Gimbal Lock)：在俯仰角为+-90度时，第一次旋转和第三次旋转将使用同一个轴，这使得系统丢失了一个自由度，这被称为奇异性问题。三个实数表示旋转一定会碰到奇异性问题，因为三维旋转是一个三维流形，想要无奇异的表达它，三个量是不够的。因此欧拉角往往只用于人际交互。
四元数
概念
旋转矩阵具有冗余性，而欧拉角和旋转向量虽然紧凑，但是具有奇异性。四元数(Quaternion)是一种扩展的复数，用它来表示旋转既紧凑，又没有奇异性。 一个四元数q有一个实部和三个虚部，如q = q0 + q1*i + q2*j + q3*k ，其中i,j,k为四元数的三个虚部。三个虚部满足如下关系式：
也可以用一个标量和一个向量表示四元数：q=[s,v]，s=q0，v=[q1,q2,q3]，s称为实部，v称为虚部。若虚部为0，则该四元数称为实四元数，反之称之为虚四元数。
四元数的运算
1.加减法
2.乘法
或
3.模长
两个四元数乘积的模即为模的乘积
4.共轭
四元数共轭与其本身相乘，得到一个实四元数，其实部为模长的平方：
5.逆
一个四元数的逆为：
且：
6.数乘
用四元数表示旋转
可以用一个单位四元数q表示旋转。假设空间点p为[x,y,z]，旋转后的点为p’。用一个虚四元数描述p点p=[0,x,y,z]，则p’=q*p*q-1，结果p’的虚部就是旋转后的坐标。
将四元数乘法写成矩阵形式
q=[s,v]，定义如下符号：
则可以将四元数的乘法写成矩阵形式：
则用四元数矩阵乘法表示旋转：
四元数和旋转矩阵的转换
四元数转换为旋转矩阵：
设四元数q=q0+q1*i+q2*j+q3*k，对应的旋转矩阵R为：
旋转矩阵到四元数的转换：已知旋转矩阵为R={m_ij}，i,j属于[1,2,3]，其对应的四元数为：
四元数和旋转向量的转换
Eigen实现
Eigen是一个开源的C++线性代数库。坐标变换的Eigen数据类型：
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(eigengeometrytest)
include_directories("/usr/include/eigen3")
add_executable(eigengeometrytest main.cpp)
install(TARGETS eigengeometrytest RUNTIME DESTINATION bin)
main.cpp
#include <iostream>
#include<ctime>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include<Eigen/Dense>
using namespace std;
using namespace Eigen;
int main(int argc, char **argv) {
std::cout << "Hello, world!" << std::endl;
//旋转矩阵使用Matrix3d
Eigen::Matrix3d rotation_m=Eigen::Matrix3d::Identity();
//旋转向量 转角,转轴
Eigen::AngleAxisd rotation_v(M_PI/4,Eigen::Vector3d(0,0,1));
cout.precision(3);
//将旋转向量转换为旋转矩阵
cout<<"rotation_v.matrix:\n"<<rotation_v.matrix()<<endl<<endl;
rotation_m=rotation_v.toRotationMatrix();
cout<<"rotation_m:\n"<<rotation_m<<endl<<endl;
//使用旋转向量进行旋转变换
Eigen::Vector3d v(1,0,0);
Eigen::Vector3d v_r=rotation_v*v;
cout<<v<<endl<<"after v rotated:"<<v_r<<endl<<endl;
//使用旋转矩阵进行旋转变换
v_r=rotation_m*v;
cout<<v<<endl<<"after m rotated:"<<v_r<<endl<<endl;
//将旋转矩阵转换为欧拉角ZYX
Eigen::Vector3d euler_a=rotation_m.eulerAngles(2,1,0);
cout<<"yaw pitch roll="<<euler_a.transpose()<<endl<<endl;
//欧式变换矩阵
Eigen::Isometry3d T=Eigen::Isometry3d::Identity();//实质是4x4矩阵
//设置旋转
T.rotate(rotation_v);
//设置平移向量
T.pretranslate(Eigen::Vector3d(1,3,4));
cout<<"Transform m:\n"<<T.matrix()<<endl<<endl;
//旋转向量转换为四元数
Eigen::Quaterniond q=Eigen::Quaterniond(rotation_v);
//输出为q1 q2 q3 q0，最后一个是实部
cout<<"q:\n"<<q.coeffs()<<endl<<endl;
//旋转矩阵转换为旋转向量
q=Eigen::Quaterniond(rotation_m);
cout<<"q\n"<<q.coeffs()<<endl<<endl;
//使用四元数旋转
v_r=q*v; //乘法是重载的，数学上是qvq^-1
cout<<"v_q_r:"<<v_r<<endl;
return 0;
}
