個人博客：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;
}