個人博客：http://www.chenjianqu.com/
原文鏈接：http://www.chenjianqu.com/show-83.html
在三維空間中剛體運動 的基礎上，假設機器人某時刻的位姿爲T (也就是世界座標系到機器人座標系的變換矩陣爲T)，它觀察到了世界座標爲p的點，產生了觀測數據z，則由座標變換關係，得z=Tp+w，其中w爲隨機噪聲。由於噪聲的存在，z不能精確滿足z=Tp。由此我們想尋找一個最優的T，使得誤差最小。理想觀測和實際數據的差爲：e=z-Tp。假設有N個路標和觀測點，則有：
求解此問題，需要計算目標函數J關於變換矩陣T的導數。舉着個例子是想說，我們經常會構建與位姿有關的函數，然後討論該函數關於位姿的導數，以調整當前的估計值。
導數的定義如下：
而三維旋轉矩陣構成特殊正交羣SO(3)，三維變換矩陣構成特殊歐式羣SE(3)：
SO(3), SE(3) 上並沒有良好定義的加法，它們只是羣。如果我們把 T 當成一個普通矩陣來處理優化，那就必須對它加以約束。而從李代數角度來說，由於李代數由向量組成，具有良好的加法運算。因此用李代數對位姿進行求導會很方便，這就是爲什麼需要用到李羣李代數的原因之一。
概念
反對稱矩陣
滿足AT=-A的矩陣稱爲反對稱矩陣。性質：
(1).反對稱矩陣的主對角線上的元素全爲零，而位於主對角線兩側對稱的元反號。若A爲反對稱矩陣，則A'，λA均爲反對稱矩陣；
(2)若A,B均爲反對稱矩陣，則A±B也爲反對稱矩陣；
(3)設A爲反對稱矩陣，B爲對稱矩陣，則AB-BA爲對稱矩陣；
(4)奇數階反對稱矩陣的行列式必爲0。
(5)反對稱矩陣的特徵值是0或純虛數，並且對應於純虛數的特徵向量的實部和虛部形成的實向量等長且互相正交。
反對稱矩陣與向量的轉換
^表示一個向量對應一個反對稱矩陣，ˇ表示反對稱矩陣對應一個向量：
羣
羣(Group)是一種集合加上一種運算的代數結構，要求這個運算滿足“鳳姐咬你”，如下：
因此旋轉矩陣與矩陣乘法，變換矩陣與矩陣乘法都構成羣。矩陣中常見的羣有：
• 一般線性羣 GL(n) 指 n × n 的可逆矩陣，它們對矩陣乘法成羣。
• 特殊正交羣 SO(n) 也就是所謂的旋轉矩陣羣，其中 SO(2) 和 SO(3) 最爲常見。
• 特殊歐氏羣 SE(n) 也就是前面提到的 n 維歐氏變換，如 SE(2) 和 SE(3)。
李羣
李羣是指具有連續（光滑）性質的羣。SO(n) 和 SE(n) 在實數空間上是連續的，我們能夠直觀地想象一個剛體能夠連續地在空間中運動，所以它們都是李羣。
李代數
每個李羣都有與之對應的李代數。李代數描述了李羣的局部性質，準確地說，是單位元附近的正切空間。李代數由一個集合V，一個數域F和一個二元運算[, ]組成(其中二元運算被稱爲李括號)。如果它們滿足以下幾條性質，則稱(V, F, [, ]) 爲一個李代數，記作g：
比如，三維向量上定義的叉積x是一種李括號，g=(R^3,R,x)構成李代數。
李代數so(3)
SO(3) 對應的李代數是定義在R^3上的向量，記作 ϕ。下式的ϕ是李代數裏的向量，ϕ該向量對應的反對稱矩陣：
兩個向量Ф1,Ф2對應的李括號爲：
由於向量 ϕ 與反對稱矩陣是一一對應的，在不引起歧義的情況下，就說 so(3) 的元素是三維向量或者三維反對稱矩陣。
李代數so(3)裏面是由三維向量組成的集合，每個向量對應到一個反對稱矩陣，可用於表達旋轉矩陣的導數。
李代數se(3)
每個se(3)的元素是一個六維向量，前三維爲平移，後三維爲旋轉，實質上是so(3)中的元素。這裏的^被拓展了含義，其中下面的四維矩陣不具有反對稱：
李代數se(3)的括號爲：
李羣和李代數的關係
對於任意旋轉矩陣R，有R*RT=I。設R隨着時間變化，得R(t)*R(t)T=I。兩邊對時間求導，得
因此Ŕ(t)*R(t)T是反對稱矩陣，可以找到一個三維向量Ф(t)與之對應，即Ŕ(t)*R(t)T=Ф(t)^。等式兩邊右乘R(t)，得：
即旋轉矩陣的求導，只需左乘Ф^(t)即可。設t0=0，R(0)=I，把R(t)在t=0處一階泰勒展開：
可以看到，Ф(t)反映了R的導數性質，故稱Ф(t)在SO(3) 原點附近的正切空間 (Tangent Space) 上。在t0附近時，設Ф保持常數Ф(t0)=Ф0，則有Ŕ(t)=Ф(t0)^R(t)=Ф0^R(t)。解該微分方程，且R(0)=I，得：
旋轉矩陣 R 與一個反對稱矩陣Ф0^(t)通過指數關係發生了聯繫。給定某時刻的R，我們就能求得一個Ф，它描述了R在局部的導數關係，ϕ正是對應到 SO(3)上的李代數so(3)。給定Ф計算exp(Ф^)和給定R計算Ф，是李羣與李代數之間的指數/對數映射。
so(3)映射到SO(3)
由上面的推導可知，so(3)可以通過指數映射到SO(3)。第一種方法是將矩陣的指數映射可以寫成一個泰勒展開：
但是這個定義難以計算。
另一種方法：ϕ 是三維向量，我們可以定義它的模長和它的方向，分別記作 θ 和 a，於是有 ϕ = θa。這裏 a 是一個長度爲 1 的方向向量，即 |a| = 1。對於a^，可推導得以下兩條性質：
根據這兩個公式，可以將泰勒展開變形爲：
最後得到的公式其實就是用於旋轉向量轉換到旋轉矩陣的羅德里格斯公式：
這表明，so(3)實際上就是由旋轉向量組成的空間，而指數映射就是羅德里格斯公式。指數映射是滿射，即每個SO(3)的元素可能對應多個so(3)中的元素。映射的種類：
SO(3)映射到so(3)
可以通過對數映射將SO(3)映射到so(3)。對數映射的泰勒展開如下：
利用跡的性質求更快。羅德里格斯公式：
取兩邊的跡：
最後得：
對於轉軸n，由於旋轉軸上的向量在旋轉後不發生改變，即Rn=n。因此，轉軸 n 是矩陣 R 特徵值 1 對應的特徵向量。求解此方程，再歸一化，就得到了旋轉軸。
se(3)映射到SE(3)
se(3)的元素：
se(3)上的指數映射：
其中J的推導過程：令Ф=θɑ,其中ɑ是單位向量，則：
最後得：
SE(3)映射到so(3)
同樣使用對數映射，se(6)中的旋轉向量使用R計算，跟so(3)方法一樣；平移向量t=Jρ。
李羣和李代數的關係
李代數求導
BCH公式及其意義
SO(3)的乘法並不對應se(3)的加法，而是由Baker-Campbell-Hausdorff(BCH)公式給出：ln( exp(A) exp(B) ) = A + B + [A,B]/2 + [A,[A,B]]/12 - [B,[A,B]]/12 + ...，公式中的[]爲李括號。BCH 公式告訴我們，當處理兩個矩陣指數之積時，它們會產生一些由李括號組成的餘項。
考慮SO(3)上的李代數ln(exp(Ф1ˆ)*exp(Ф2ˆ))ˇ，當Ф1和Ф2爲小量時，可以忽略上式中小量二次以上的餘項，近似表達爲：
以第一個近似爲例，該式告訴我們，當對一個旋轉矩陣 R2（李代數爲 ϕ2）左乘一個微小旋轉矩陣 R1（李代數爲 ϕ1）時，可以近似地看作，在原有的李代數 ϕ2 上加上了一項Jl(ϕ2)-1*ϕ1。同理，第二個近似描述了R1右乘一個微小位移的情況。其中Jl是左乘近似雅可比：
Jr是右乘近似雅可比：Jr(ϕ)=Jl(-ϕ)。
總結一下BCH近似的意義，假定對某個旋轉 R，對應的李代數爲 ϕ。有一個微小旋轉，記作ΔR，對應的李代數爲Δϕ。那麼旋轉R左乘微小旋轉ΔR，在李羣上得到的結果爲ΔR，在李代數上，根據BCH近似，爲 Jl-1( ϕ ) Δϕ+ϕ。合併起來，得：
反之，在李代數上進行加法，ϕ+Δϕ，在李羣上近似爲帶左右雅可比的乘法：
同樣的，對於SE(3)，也有類似的BCH近似：
求導思路
前面說過，SO(3), SE(3) 上並沒有良好定義的加法，它們只是羣。如果我們把 T 當成一個普通矩陣來處理優化，那就必須對它加以約束。而從李代數角度來說，由於李代數由向量組成，具有良好的加法運算。因此，使用李代數解決求導問題的思路分爲兩種：
1. 用李代數表示姿態，然後根據李代數加法來對李代數求導。
2. 對李羣左乘或右乘微小擾動，然後對該擾動求導，稱爲左擾動和右擾動模型。
so(3)李代數求導
假設我們對一個空間點 p 進行了旋轉，得到了 Rp。現在，要計算旋轉之後點的座標相對於旋轉的導數，推導得：
so(3)擾動求導
另一種求導方式是對 R 進行一次擾動 ∆R，看結果相對於擾動的變化率。這個擾動可以乘在左邊也可以乘在右邊，最後結果會有一點兒微小的差異，我們以左擾動爲例。設左擾動 ∆R 對應的李代數爲 φ。然後，對 φ 求導，即：
se(3)擾動求導
假設某空間點 p經過一次變換 T（對應李代數爲 ξ），得到 Tp。給 T 左乘一個擾動 ∆T = exp (δξ^)，設擾動項的李代數爲δξ = [δρ, δϕ]T，則：
把最後的結果定義成一個算符 ⊙，它把一個齊次座標的空間點變換成一個 4 × 6 的矩陣。其中矩陣求導的順序：
Sophus代碼
基於模板的 Sophus 庫和 Eigen 一樣，是僅含頭文件而沒有源文件的。
CMakeLists.txt
cmake_minimum_required(VERSION 2.6)
project(sophustest)
set( CMAKE_CXX_FLAGS "-std=c++11" )
include_directories("/home/chenjianqu/software/eigen-3.3.5")
find_package( Sophus REQUIRED )
include_directories( ${Sophus_INCLUDE_DIRS} )
add_executable(sophustest main.cpp)
install(TARGETS sophustest RUNTIME DESTINATION bin)
main.cpp
#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/se3.hpp>
using namespace std;
using namespace Eigen;
using namespace Sophus;
int main(int argc, char **argv) {
// 沿Z軸轉90度的旋轉矩陣
Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();
// 或者四元數
Quaterniond q(R);
SO3d SO3_R(R); // Sophus::SO3d可以直接從旋轉矩陣構造
SO3d SO3_q(q); // 也可以通過四元數構造,二者是等價的
cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;
cout << "SO(3) from quaternion:\n" << SO3_q.matrix() << endl;
// 使用對數映射獲得它的李代數
Vector3d so3 = SO3_R.log();
cout << "so3 = " << so3<< endl;
// hat 爲向量到反對稱矩陣
Matrix3d so3_hat=SO3d::hat(so3);
cout << "so3 hat=\n" << so3_hat<< endl;
//vee爲反對稱到向量
Vector3d v=SO3d::vee(so3_hat);
cout << "so3 hat vee= " << v<< endl;
// 增量擾動模型的更新
Vector3d delta_so3(1e-4, 0, 0); //假設更新量爲這麼多
SO3d SO3_updated = SO3d::exp(delta_so3) * SO3_R;
cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;
cout << "*******************************" << endl;
// 對SE(3)操作大同小異
Vector3d t(1, 0, 0); // 沿X軸平移1
SE3d SE3_Rt(R, t); // 從R,t構造SE(3)
SE3d SE3_qt(q, t); // 從q,t構造SE(3)
cout << "SE3 from R,t= \n" << SE3_Rt.matrix() << endl;
cout << "SE3 from q,t= \n" << SE3_qt.matrix() << endl;
// 李代數se(3) 是一個六維向量，方便起見先typedef一下
typedef Eigen::Matrix<double, 6, 1> Vector6d;
Vector6d se3 = SE3_Rt.log();
cout << "se3 = " << se3.transpose() << endl;
// 觀察輸出，會發現在Sophus中，se(3)的平移在前，旋轉在後.
// 同樣的，有hat和vee兩個算符
cout << "se3 hat = \n" << SE3d::hat(se3) << endl;
cout << "se3 hat vee = " << SE3d::vee(SE3d::hat(se3)).transpose() << endl;
// 最後，演示一下更新
Vector6d update_se3; //更新量
update_se3.setZero();
update_se3(0, 0) = 1e-4d;
SE3d SE3_updated = SE3d::exp(update_se3) * SE3_Rt;
cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;
return 0;
}