1. 四元數的基本運算
- 主要運算
- 四元數乘法
乘法性質
- 滿足結合律
- 不滿足交換律
- 乘積的模等於模的乘積
- 乘積的逆等於各個四元數的逆以相反的順序相乘
- 其他運算
*四元數部分參考:旋轉矩陣、歐拉角、四元數理論及其轉換關係
2. 四元數與旋轉向量
簡單來說,四元數的思想就是把方向餘弦矩陣的三次旋轉表示爲只繞一個旋轉軸旋轉一次完成,因此可以用4個數來表示這個過程,其中包括旋轉軸向量的長度(θ)和旋轉軸單位向量(x,y,z)
2.1. 四元數的表示
⎣⎢⎢⎡q0q1q2q3⎦⎥⎥⎤=⎣⎢⎢⎡cos2θx∗sin2θy∗sin2θz∗sin2θ⎦⎥⎥⎤
3. 四元數對時間的導數
3.1. 關於兩個極限的近似(上面求極限用到的)
4. 旋轉矩陣(李羣SO3)對角速度ω的求導
5. so(3) 導數
5.1. 左乘模型
5.2. 右乘模型
6. 旋轉連乘的雅克比
6.1. 對R2求導
6.2. 對R1求導
6.3. 伴隨性質的證明
6.3.1. 即證明下式
R exp(ϕ ^) RT=exp((Rϕ) ^)
6.3.2. 首先證明
Rp ^RT=(Rp) ^
//或者:(上下是等價的)
R[a]×=[Ra]×R
6.3.2.1. 已知
若
a=b×c
則有
R∈SO3Ra=(Rb)×(Rc)
6.3.2.2. 可證
(Rp) ^=(Rp) ^I=(Rp)×RRTI=(Rp)×(RRTI)=R(p×RTI)=Rp ^RT
6.3.3. 再來證明原式
6.3.3.1. 等式右側
∵Rp ^RT=(Rp) ^∴e(Rp) ^=eRp ^RT≈I+Rp ^RT
6.3.3.2. 等式左側
Re(p ^)RT≈R(I+p ^)RT=RIRT+Rp ^RT=I+Rp ^RT
6.3.3.3. 證畢
7. 不用SE3的討論
8. 作業
8.1. 使用右乘SO3,求dRd(R−1p)
dRd(R−1p)=ϕ→0limdϕd[(Rexp(ϕ ^))−1−R−1p]=ϕ→0limdϕd[exp−1(ϕ ^) R−1p−R−1p]=ϕ→0limdϕd[exp(−ϕ ^) R−1p−R−1p]=ϕ→0limdϕd[(I−ϕ ^) R−1p−R−1p]=ϕ→0limϕ−ϕ ^ R−1p=ϕ→0limϕ(R−1p ^)ϕ=(R−1p) ^
8.2. 使用右乘SO3,求dR2dln(R1R2−1)
dR2dln(R1R2−1)=ϕ→0limdϕdln[R1(R2exp(ϕ ^))−1]−ln(R1R2−1)=ϕ→0limdϕdln[R1exp−1(ϕ ^)R2−1]−ln(R1R2−1)=ϕ→0limdϕdln[R1exp(−ϕ ^)R2−1]−ln(R1R2−1)
再利用伴隨性質
Rexp(ϕ ^)RT=exp((Rϕ) ^)
可得
R2exp(−ϕ ^)R2T=exp((R2∗(−ϕ)) ^)
兩邊同時左乘R2−1
exp(−ϕ ^)R2−1=R2−1exp((R2∗(−ϕ)) ^)
將上式帶入
dR2dln(R1R2−1)=ϕ→0limdϕdln[R1(R2exp(ϕ ^))−1]−ln(R1R2−1)=ϕ→0limdϕdln[R1exp−1(ϕ ^)R2−1]−ln(R1R2−1)=ϕ→0limdϕdln[R1exp(−ϕ ^)R2−1]−ln(R1R2−1)=ϕ→0limdϕdln[R1R2−1exp((R2∗(−ϕ)) ^)]−ln(R1R2−1)=ϕ→0limdϕdln(R1R2−1)+Jr−1R2∗(−ϕ)−ln(R1R2−1)=ϕ→0limdϕdln(R1R2−1)−Jr−1ϕR2−ln(R1R2−1)=−Jr−1(ln(R1R2−1))R2
8.3. CODEing
#include <iostream>
#include <eigen3/Eigen/Core>
#include <Eigen/Geometry>
#include <sophus/so3.hpp>
using namespace Eigen;
using namespace std;
int main()
{
//旋轉向量
Eigen::AngleAxisd angleAxisVec=AngleAxisd(M_PI/2,Vector3d(0,0,1));
//Eigen 旋轉矩陣
Eigen::Matrix3d R_mat=angleAxisVec.toRotationMatrix();
cout<<"Eigen 繞z軸旋轉90度的旋轉矩陣"<<R_mat<<endl;
/**********************************************************
* 當有一個計算出來的w,使用它來對旋轉量進行更新
* 1.使用旋轉矩陣 R=R*exp(w^)
* 2.使用四元數 q=q * q[1,0.5w]
* 結果:這兩種更新方式都是一樣的
**********************************************************/
///1.使用旋轉矩陣更新
Sophus::SO3d R;
cout<<"更新之前的R: \n"<<R.matrix()<<endl;
Vector3d w(0.01,0.02,0.03);
// cout<<"更新量w的反對稱矩陣:\n"<<Sophus::SO3d::hat(w)<<endl;
// Matrix3d hat=R.matrix()*Sophus::SO3d::hat(w); //這個並不是更新之後的R
Sophus::SO3d R_updated_sophus=R*Sophus::SO3d::exp(w);
cout<<"更新之後的R: \n"<<R_updated_sophus.matrix()<<endl;
cout<<"======================================================"<<endl;
///2.使用四元數更新
Quaterniond q(R.matrix());
cout<<"更新之前的q: \n"<<q.coeffs().transpose()<<endl;
cout<<"對應的旋轉矩陣: \n"<<q.toRotationMatrix()<<endl;
//注意:這裏不是用旋轉向量構造q, w是直接構造q的參數
// Quaterniond q_update(Eigen::AngleAxisd(1,Vector3d(0.5*0.01,0.5*0.02,0.5*0.03)));
//注意兩種構造方式
// Quaterniond(Vector4d(x,y,z,w))
// Quaterniond(w,x,y,z,w)
Quaterniond q_update(1,0.5*0.01,0.5*0.02,0.5*0.03); //構造更新量四元數q(w,x,y,z)
// cout<<q_update.coeffs().transpose()<<endl;
Quaterniond q_updated=q*q_update;
cout<<"更新之後的q對應的旋轉矩陣:\n"<<q_updated.toRotationMatrix()<<endl;
return 0;
}
8.3.1. Result