文章目錄
- 前言·與同主題博文的不同之處
- 1.VIO 文獻閱讀
- 2.四元數和李代數更新
-
- 2.1理論依據
- 2.2代碼實作
- 3.其他導數
-
- 3.1第一題
- 3.2第二題
前言·與同主題博文的不同之處
賀一家、高翔、崔華坤老師在深藍學院開設了vSLAM進階課程《從零開始手寫VIO》，課程相當火爆，已經有數篇關于第一章作業的優秀博文。不過小編覺得還可以從兩個方面進一步優化這個主題的博文。首先，第二道程式設計題和第三道公式推導題中涉及到的理論依據和參考資料可以明确給出，給出公式推導每一行對應的理論依據，具體到哪一本書的哪一個公式，這樣對新手更加友好，其次，第二道程式設計題不少博文中直接使用Sophus庫完成旋轉矩陣更新，而Sophus庫是利用Eigen庫中的四元數來求解的，本質上依然是采用的四元數更新，并不是旋轉矩陣更新。
有了解不到位的地方，歡迎大家指出！
1.VIO 文獻閱讀
第一題文獻閱讀參考之前的優秀博文《視覺SLAM進階：從零開始手寫VIO》第一講作業
2.四元數和李代數更新
2.1理論依據
對于旋轉矩陣更新R ← \leftarrow ←Rexp(w ∧ ^{\wedge} ∧)，最重要的是将exp(w ∧ ^{\wedge} ∧)轉換成對應的旋轉矩陣，這裡的理論依據是高翔老師《視覺 SLAM 十四講》P72的公式，
正如書中所說該公式也就是該書P51中所說的羅德裡格斯公式（Rodrigues’s Formula ）：
小編認為，旋轉矩陣更新不能用Sophus庫，檢視《視覺 SLAM 十四講》提供的第三方庫Sophus的源碼，Sophus 是利用的 Eigen 庫中的四元數來完成旋轉更新的，本質上是采用的四元數更新理論，而不是旋轉矩陣更新理論。具體參考Sophus庫中的源碼檔案so3.cpp和so3.h。更多詳細的說明見代碼中的注釋。
2.2代碼實作
#include <iostream>
#include<Eigen/Core>
#include<Eigen/Geometry>
using namespace std;
int main(int argc, char **argv) {
//變量初始化
Eigen::Vector3d w(0.01,0.02,0.03);//小量角速度（旋轉向量）
Eigen::Matrix3d R=Eigen::AngleAxisd(M_PI/4,Eigen::Vector3d(0,0,1)).toRotationMatrix();//初始旋轉矩陣，繞Z軸旋轉45°
Eigen::Quaterniond q(R);//初始四元數
cout<<"初始旋轉矩陣："<<endl;
cout<<R<<endl;
cout<<"初始四元數："<<endl;
cout<<q.coeffs().transpose() <<endl;
//利用Rodrigues's formula完成旋轉矩陣更新
double theta=w.norm();//旋轉向量對應的旋轉角
Eigen::Vector3d n_w=w/theta;//歸一化得到旋轉向量對應的旋轉軸
Eigen::Matrix3d n_w_skew;
n_w_skew<< 0, -n_w(2), n_w(1),
n_w(2), 0, -n_w(0),
-n_w(1), n_w(0), 0;
Eigen::Matrix3d R_w=cos(theta)*Eigen::Matrix3d::Identity()+(1-cos(theta))*n_w*n_w.transpose()+sin(theta)*n_w_skew;//Rodrigues's formula
Eigen::Matrix3d R_update=R*R_w;
cout<<"更新後的旋轉矩陣："<<endl;
cout<<R_update<<endl;
//四元數更新
Eigen::Quaterniond q_w(1,w(0)/2,w(1)/2,w(2)/2);//小量角速度對應的四元數
Eigen::Quaterniond q_update=q*q_w;
q_update=q_update.normalized();//機關四元數才可以表示三維旋轉，是以必須歸一化
cout<<"更新後的四元數："<<endl;
cout<<q_update.coeffs().transpose() <<endl;
//計算兩種方法得到的結果之差
cout<<"兩種方法得到的結果之差："<<endl;
cout<<q_update.toRotationMatrix()-R_update<<endl;
return 0;
}
CMakeLists.txt
cmake_minimum_required(VERSION 2.8)
project(qua_lie_update)
include_directories("/usr/include/eigen3")
add_executable(qua_lie_update main.cpp)
運作結果如下：
初始旋轉矩陣：
0.707107 -0.707107 0
0.707107 0.707107 0
0 0 1
初始四元數：
0 0 0.382683 0.92388
更新後的旋轉矩陣：
0.685368 -0.727891 0.0211022
0.727926 0.685616 0.00738758
-0.0198454 0.0102976 0.99975
更新後的四元數：
0.000792425 0.0111503 0.396472 0.917979
兩種方法得到的結果之差：
2.5963e-06 2.37368e-06 -2.44789e-06
-2.38192e-06 2.53859e-06 -8.98416e-07
2.29623e-06 -1.23557e-06 5.83041e-08
可以看出兩種更新方式差異很小，差異量級約為 10-6。
3.其他導數
先給出公式推導的詳細過程，之後給出每一行對應的理論依據，具體到哪一本書的哪一個公式。
3.1第一題
公式推導：
理論依據，公式的第二行到第五行分别用的如下性質一到性質四：
3.2第二題
公式推導：
下面隻給出3.1中沒有涉及到的理論依據。第三行到第四行用了SO(3)的伴随性質，具體參考《視覺 SLAM 十四講》P84習題5和6。
具體證明過程參考《視覺slam十四講》第4講課後習題_(∠ゝз:)_。
第四行到第五行用了如下性質：
理論依據見《視覺 SLAM 十四講》P75公式（4.29）。