視覺SLAM十四講實踐之Sophus下的位姿表示和更新優化

旋轉矩陣是三維空間剛體運動的描述方式之一

李群SO(3)旋轉矩陣
李代so(3) 三維向量實際上是旋轉向量組成的空間

李群SE(3)變換矩陣
李代se(3) 六維向量 平移在前 旋轉在後

旋轉矩陣的導數由旋轉向量指定，指導著如何在旋轉矩陣中進行微積分運算。

每個李群都有與之對應的李代數，李代數描述了李群的局部性質。

Sophus(李群、李代數庫)
Eigen庫是一個開源的C++線性代數庫，它提供了快速的有關矩陣的線性代數運算，還包括解方程等功能。Eigen庫提供了集合模塊，但沒有提供李代數的支持。

Sophus庫是基於Eigen基礎上開發的，繼承了Eigen庫中的定義的各個類。因此在使用Eigen庫中的類時，既可以使用Eigen命名空間，也可以使用Sophus命名空間，Sophus庫很好的支持了李群SO3 李代so3 李群SE3 李代se3。

高博視覺SLAM十四講的這張圖總結的很好

用沿Z軸轉90度的Eigen旋轉向量構造Eigen的旋轉矩陣

/*******************************************     原型            李代數* SO3 (3*3)=="R"          --> so3 (3*3)* SE3 (4*4)=="T"          --> se3 (6*1)********************************************/// 旋轉向量: 第一個參數為旋轉角度，第二個參數哪個為1就繞哪軸旋轉Eigen::Matrix3d rotation_matrix = Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d(0, 0, 1)).toRotationMatrix();cout << "用沿Z軸轉90度的Eigen旋轉向量構造Eigen的旋轉矩陣:\n" << rotation_matrix << endl;/* * 用沿Z軸轉90度的Eigen旋轉向量構造Eigen的旋轉矩陣: * 0.707107 -0.707107         0 * 0.707107  0.707107         0 * 0         0         1 */
用Eigen的旋轉矩陣構造Sophus李群旋轉矩陣SO3
Sophus::SO3d SO3_rotation_matrix(rotation_matrix);cout << "用Eigen的旋轉矩陣構造Sophus李群旋轉矩陣SO3:\n" << SO3_rotation_matrix.matrix() << endl;
對數映射求李群SO3的李代數so3
Eigen::Vector3d so3 = SO3_rotation_matrix.log();cout << "對數映射求李群SO3的李代數so3 = " << so3.transpose() << endl;
指數映射求李代數so3的李群SO3
Sophus::SO3d SO3_rotation_matrix1 = Sophus::SO3d::exp(so3);cout << "指數映射求李代數so3的李群SO3 = \n" << SO3_rotation_matrix1.matrix() << endl;
so3 hat 求向量到反對稱矩陣
cout << "hat 求向量到反對稱矩陣 =\n" << Sophus::SO3d::hat(so3) << endl;
so3 hat vee 求反對稱矩陣到向量
cout << "vee 求反對稱矩陣到向量 = " << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;
更新李群的旋轉矩陣SO3
Eigen::Vector3d update_so3(1e-4, 0, 0);Sophus::SO3d SO3_updated = Sophus::SO3d::exp(update_so3) * SO3_rotation_matrix;cout << "更新李群SO3的旋轉矩陣 = \n" << SO3_updated.matrix() << endl;
用旋轉矩陣R,平移向量t構造Sophus表示的李群變換矩陣SE3
Eigen::Vector3d t(1, 0, 0);Sophus::SE3d SE3_rotation_translation(rotation_matrix, t);cout << "用旋轉矩陣R,平移向量t構造Sophus表示的李群變換矩陣SE3= \n" << SE3_rotation_translation.matrix() << endl;
李代數se3 是一個六維向量 李群SE3的對數映射求李代數se3
typedef Eigen::Matrix<double, 6, 1> Vector6d;Vector6d se3 = SE3_rotation_translation.log();cout << "李代數se3 是一個六維向量 李群SE3的對數映射求李代數se3 \n" << se3.transpose() << endl;
指數映射求李代數se3的李群SE3
Sophus::SE3d SO3_rotation_matrix2 = Sophus::SE3d::exp(se3);cout << "指數映射求李代數se3的李群SE3 = \n" << SO3_rotation_matrix2.matrix() << endl;
se3 hat 求向量到反對稱矩陣
cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
se3 hat 求向量到反對稱矩陣
cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
更新李群的變換矩陣SE3
Vector6d update_se3;update_se3.setZero();update_se3(0, 0) = 1e-4;Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_rotation_translation;cout << "SE3 updated = " << endl << SE3_updated.matrix() << endl;
交流答疑微信群
視覺SLAM十四講實踐筆記
視覺SLAM十四講實踐筆記（一）
視覺SLAM十四講實踐筆記之Eigen下的四種位姿表示
視覺SLAM十四講實踐之坐標系之間的歐式變換
視覺SLAM十四講實踐之軌跡可視化