P71

什么樣的相機(jī)位姿 最符合 當(dāng)前觀測(cè)數(shù)據(jù)。

求解最優(yōu)的 $\mathbf{R},\mathbf{t}$， 使得誤差最小化。

4.1 李群與李代數(shù)基礎(chǔ)

群： 只有一個(gè)(良好的)運(yùn)算的集合。



封結(jié)幺逆 、 豐儉由你

李群： 具有連續(xù)(光滑)性質(zhì)的群。

在 t = 0 附近，旋轉(zhuǎn)矩陣可以由$exp({?}_0\stackrel{?}{}t)$計(jì)算得到

4.1.3 李代數(shù)的定義

李代數(shù) 描述了李群的局部性質(zhì)

• 單位元 附近的正切空間。

$\mathfrak{g}=({\mathbb{R}}^3,\mathbb{R},\times )$構(gòu)成了一個(gè)李代數(shù)

4.1.4 李代數(shù) so(3)

$\mathfrak{so}(3)$: 一個(gè)由三維向量組成的集合，每個(gè)向量對(duì)應(yīng)一個(gè)反對(duì)稱矩陣，可以用于表達(dá)旋轉(zhuǎn)矩陣的導(dǎo)數(shù)。
s o ( 3 ) = { ? ∈ R 3 , Φ = ? ? ∈ R 3 × 3 } \mathfrak{so}(3)=\{\phi\in\mathbb{R}^3,\bm{\Phi}=\phi\^{}\in\mathbb{R}^{3\times3}\} so(3)={?∈R3,Φ=??∈R3×3}

4.1.5 李代數(shù) se(3)

李群 $SE(3)$ 對(duì)應(yīng)的李代數(shù) $\mathfrak{se}(3)$

4.2 指數(shù)與對(duì)數(shù)映射

4.2.1 SO(3)上的指數(shù)映射

$\mathfrak{so}(3)$： 旋轉(zhuǎn)向量 組成的空間
指數(shù)映射： 羅德里格斯公式

$\mathbf{R}=exp({?}^{\wedge })=exp(\theta {\mathbf{a}}^{\wedge })=cos\theta \mathbf{I}+(1?cos\theta )\mathbf{a}{\mathbf{a}}^T+sin\theta {\mathbf{a}}^{\wedge }$

對(duì)數(shù)映射： 李群$SO(3)$ 中的元素 ——> 李代數(shù)$\mathfrak{so}(3)$

把旋轉(zhuǎn)角固定在 $\pm \pi$ 之間，則李群和李代數(shù) 元素 一一對(duì)應(yīng)。

————————————————

羅德里格斯公式推導(dǎo)

$$\begin{array}{rl}exp({?}^{\wedge })& =exp(\theta {\mathbf{a}}^{\wedge })=\sum _{n=0}^{\infty }\frac{1}{n!}(\theta {\mathbf{a}}^{\wedge }{)}^n\\ & n=0,1,2,3,...\\ 原式& =\mathbf{I}+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\frac{1}{4!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^4+???\\ & 將{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\mathbf{a}{\mathbf{a}}^T?\mathbf{I};{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=({\mathbf{a}}^{\wedge }{)}^3=?{\mathbf{a}}^{\wedge }代入\\ 原式& =\mathbf{a}{\mathbf{a}}^T?{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\theta {\mathbf{a}}^{\wedge }+\frac{1}{2!}{\theta }^2{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }?\frac{1}{3!}{\theta }^3{\mathbf{a}}^{\wedge }?\frac{1}{4!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^2+???\\ & =\mathbf{a}{\mathbf{a}}^T+(\theta ?\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5????){\mathbf{a}}^{\wedge }?(1?\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4????){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & ={\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I}+sin\theta {\mathbf{a}}^{\wedge }?cos\theta {\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }\\ & =(1?cos\theta ){\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }+\mathbf{I}+sin\theta {\mathbf{a}}^{\wedge }\\ & =(1?cos\theta )(\mathbf{a}{\mathbf{a}}^T?\mathbf{I})+\mathbf{I}+sin\theta {\mathbf{a}}^{\wedge }\\ & =cos\theta \mathbf{I}+(1?cos\theta )\mathbf{a}{\mathbf{a}}^T+sin\theta {\mathbf{a}}^{\wedge }\end{array}$$

$exp(\theta {\mathbf{a}}^{\wedge })=cos\theta \mathbf{I}+(1?cos\theta )\mathbf{a}{\mathbf{a}}^T+sin\theta {\mathbf{a}}^{\wedge }$

—————

$\bm{a}^{\land}$

${\mathbf{a}}^{\wedge }$
————————————————

4.2.2 SE(3) 上的指數(shù)映射

——————
推導(dǎo)：
$$\begin{array}{rl}\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({?}^{\wedge }{)}^n& =\sum _{n=0}^{\infty }\frac{1}{(n+1)!}(\theta {\mathbf{a}}^{\wedge }{)}^n\\ & =\mathbf{I}+\frac{1}{2!}\theta {\mathbf{a}}^{\wedge }+\frac{1}{3!}{\theta }^2({\mathbf{a}}^{\wedge }{)}^2+\frac{1}{4!}{\theta }^3({\mathbf{a}}^{\wedge }{)}^3+\frac{1}{5!}{\theta }^4({\mathbf{a}}^{\wedge }{)}^4+???\\ & =\frac{1}{\theta }(\frac{1}{2!}{\theta }^2?\frac{1}{4!}{\theta }^4+???){\mathbf{a}}^{\wedge }+\frac{1}{\theta }(\frac{1}{3!}{\theta }^3?\frac{1}{5!}{\theta }^5+???)({\mathbf{a}}^{\wedge }{)}^2+\mathbf{I}\\ & =\frac{1}{\theta }(1?cos\theta ){\mathbf{a}}^{\wedge }+\frac{1}{\theta }(\theta ?sin\theta )(\mathbf{a}{\mathbf{a}}^T?\mathbf{I})+\mathbf{I}\\ & =\frac{sin\theta }{\theta }\mathbf{I}+(1?\frac{sin\theta }{\theta })\mathbf{a}{\mathbf{a}}^T+\frac{1?cos\theta }{\theta }{\mathbf{a}}^{\wedge }\\ & \stackrel{\mathrm{def}}{=}\mathbf{J}\end{array}$$

$\mathbf{J}=\frac{sin\theta }{\theta }\mathbf{I}+(1?\frac{sin\theta }{\theta })\mathbf{a}{\mathbf{a}}^T+\frac{1?cos\theta }{\theta }{\mathbf{a}}^{\wedge }$

$\mathbf{R}=cos\theta \mathbf{I}+(1?cos\theta )\mathbf{a}{\mathbf{a}}^T+sin\theta {\mathbf{a}}^{\wedge }$

————

$\mathbf{t}=\mathbf{J\rho }$
平移部分發(fā)生了一次以 $\mathbf{J}$ 為系數(shù)矩陣的 線性變換。

SO(3),SE(3),so(3),se(3)的對(duì)應(yīng)關(guān)系

4.3 李代數(shù)求導(dǎo)與擾動(dòng)模型

Baker-Campbell-Hausdorff公式(BCH公式)

4.3.2 SO(3)上的李代數(shù)求導(dǎo)

位姿由SO(3)上的旋轉(zhuǎn)矩陣 或 SE(3)上的變換矩陣 描述

設(shè)某時(shí)刻機(jī)器人的位姿為 $\mathbf{T}$， 觀察到了一個(gè)世界坐標(biāo)位于 $\mathbf{p}$ 的點(diǎn)，產(chǎn)生了一個(gè)觀測(cè)數(shù)據(jù) $\mathbf{z}$
計(jì)算理想的觀測(cè)與實(shí)際數(shù)據(jù)之間的誤差： $\mathbf{e}=\mathbf{z}?\mathbf{Tp}$
假設(shè)一共有 $N$ 個(gè)這樣的路標(biāo)點(diǎn)和觀測(cè)，對(duì)機(jī)器人進(jìn)行位姿估計(jì)，相當(dāng)于尋找一個(gè)最優(yōu)的 $\mathbf{T}$ ,使得整體誤差最小化：
$$\underset{\mathbf{T}}{\min }J(\mathbf{T})=\sum _{i=1}^N||{\mathbf{z}}_{\mathbf{i}}?\mathbf{T}{\mathbf{p}}_{\mathbf{i}}|{|}_2^2$$

求解上述問(wèn)題，需要計(jì)算目標(biāo)函數(shù) $J$ 關(guān)于變換矩陣 $\mathbf{T}$ 的導(dǎo)數(shù)。

使用 李代數(shù) 解決 求導(dǎo)問(wèn)題 的2種思路：
1、用李代數(shù)表示姿態(tài)，然后根據(jù)李代數(shù)加法對(duì)李代數(shù)求導(dǎo)。
2、對(duì)李群左乘或右乘微小擾動(dòng)，然后對(duì)該擾動(dòng)求導(dǎo)，稱為左擾動(dòng)和右擾動(dòng)模型。

4.3.3 李代數(shù)求導(dǎo)

——————

推導(dǎo)：
$$\begin{array}{rl}\frac{?(\mathbf{Rp})}{?\mathbf{R}}& \stackrel{R對(duì)應(yīng)的李代數(shù)為?}{=}\frac{?(\exp ({?}^{\wedge })\mathbf{p})}{??}\\ & =\underset{\Delta ?\to 0}{\lim }\frac{\exp ((?+\Delta ?{)}^{\wedge })\mathbf{p}?\exp ({?}^{\wedge })\mathbf{p}}{\Delta ?}\\ & 由式(4.35)\\ & =\underset{\Delta ?\to 0}{\lim }\frac{\exp (({\mathbf{J}}_l\Delta ?{)}^{\wedge })\exp ({?}^{\wedge })\mathbf{p}?\exp ({?}^{\wedge })\mathbf{p}}{\Delta ?}\\ & =\underset{\Delta ?\to 0}{\lim }\frac{(\mathbf{I}+({\mathbf{J}}_l\Delta ?{)}^{\wedge })\exp ({?}^{\wedge })\mathbf{p}?\exp ({?}^{\wedge })\mathbf{p}}{\Delta ?}\\ & =\underset{\Delta ?\to 0}{\lim }\frac{({\mathbf{J}}_l\Delta ?{)}^{\wedge }\exp ({?}^{\wedge })\mathbf{p}}{\Delta ?}\\ & a^{\wedge }等效于a\times ,因此根據(jù)叉乘的性質(zhì)\\ & =\underset{\Delta ?\to 0}{\lim }\frac{?(\exp ({?}^{\wedge })\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\Delta ?}{\Delta ?}\\ & =?(\mathbf{Rp}{)}^{\wedge }{\mathbf{J}}_l\end{array}$$

$$\frac{?(\exp ({?}^{\wedge })\mathbf{p})}{??}=?(\mathbf{Rp}{)}^{\wedge }{\mathbf{J}}_l$$
——————

4.3.4 擾動(dòng)模型(左乘)【更簡(jiǎn)單 的導(dǎo)數(shù)計(jì)算模型】

對(duì) $\mathbf{R}$ 進(jìn)行一次擾動(dòng) $\Delta \mathbf{R}$ ，看結(jié)果對(duì)于 擾動(dòng)的變化率。

設(shè)左擾動(dòng) $\Delta \mathbf{R}$ 對(duì)應(yīng)的李代數(shù) 為 $\phi$

$$\begin{array}{rl}\frac{?(\mathbf{Rp})}{?\phi }& =\underset{\phi \to 0}{\lim }\frac{\exp ({\phi }^{\wedge })\exp ({?}^{\wedge })\mathbf{p}?\exp ({?}^{\wedge })\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{(\mathbf{I}+{\phi }^{\wedge })\exp ({?}^{\wedge })\mathbf{p}?\exp ({?}^{\wedge })\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\exp ({?}^{\wedge })\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\mathbf{Rp}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{?(\mathbf{Rp}{)}^{\wedge }\phi }{\phi }\\ & =?(\mathbf{Rp}{)}^{\wedge }\end{array}$$

4.3.5 SE(3)上的李代數(shù)求導(dǎo)

假設(shè)某空間點(diǎn) $\mathbf{p}$ 經(jīng)過(guò)一次變換 $\mathbf{T}$ (對(duì)應(yīng)李代數(shù)為 $\mathbf{\xi }$)， 得到 $\mathbf{Tp}$
現(xiàn)在給 $\mathbf{T}$ 左乘一個(gè)擾動(dòng) $\Delta \mathbf{T}=\exp (\Delta {\mathbf{\xi }}^{\wedge })$
設(shè)擾動(dòng)項(xiàng)的李代數(shù)為 $\Delta \mathbf{\xi }=[\Delta \mathbf{\rho },\Delta ?{]}^T$，則

$$\begin{array}{rl}\frac{?(\mathbf{Tp})}{?\Delta \mathbf{\xi }}& =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\exp (\Delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}?\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{(\mathbf{I}+\Delta {\mathbf{\xi }}^{\wedge })\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}?\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\Delta {\mathbf{\xi }}^{\wedge }\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\Delta {?}^{\wedge }& \Delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{c}\mathbf{Rp}+\mathbf{t}\\ 1\end{array}\right]}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\Delta {?}^{\wedge }(\mathbf{Rp}+\mathbf{t})+\Delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\Delta \mathbf{\rho },\Delta ?{]}^T}\\ & =\left[\begin{array}{cc}\mathbf{I}& ?(\mathbf{Rp}+\mathbf{t}{)}^{\wedge }\\ 0^T& 0^T\end{array}\right]\\ & \stackrel{\mathrm{def}}{=}(\mathbf{Tp}{)}^{?}\end{array}$$

$\overset{\mathrm{def}}{=}(\bm{Tp})^{\bigodot}$

4.4 Sophus應(yīng)用 【Code】

SO(3)、SE(3)
二維運(yùn)動(dòng)SO(2)和SE(2)
相似變換 Sim(3)

mkdir build && cd build
cmake ..
make 
./useSophus

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)project(useSophus)find_package(Sophus REQUIRED)
include_directories( ${Sophus_INCLUDE_DIRS})add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus ${Sophus_LIBRARIES})

#include<iostream>
#include<cmath>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include "sophus/se3.h"using namespace std;
using namespace Eigen;/* sophus 的基本用法 */
int main(int argc, char **argv){// 沿 Z軸  轉(zhuǎn)90° 的旋轉(zhuǎn)矩陣Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0, 0, 1)).toRotationMatrix();/* 四元數(shù) */Quaterniond q(R);Sophus::SO3 SO3_R(R);Sophus::SO3 SO3_q(q);cout << "SO(3) from matrix:\n" << SO3_R.matrix() << endl;cout << "SO(3) from quatenion:\n" << SO3_q.matrix() << endl;cout << "they are equal" << endl;return 0;
}

#include<iostream>
#include<cmath>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include "sophus/se3.h"using namespace std;
using namespace Eigen;/* sophus 的基本用法 */
int main(int argc, char **argv){/* 使用 對(duì)數(shù)映射 獲得  李代數(shù)*/Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0, 0, 1)).toRotationMatrix();Sophus::SO3 SO3_R(R);Vector3d so3 = SO3_R.log();cout << "so3 = " << so3.transpose() << endl;// hat  向量 ——> 反對(duì)稱矩陣cout << "so3 hat = \n" << Sophus::SO3::hat(so3)<< endl;// vee  反對(duì)稱 ——> 向量cout << "so3 hat vee = " << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl;Vector3d update_so3(1e-4, 0, 0);// 假設(shè)更新量為這么多Sophus::SO3 SO3_updated =Sophus::SO3::exp(update_so3) * SO3_R;cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;return 0;
}

#include<iostream>
#include<cmath>
#include<Eigen/Core>
#include<Eigen/Geometry>
#include "sophus/se3.h"using namespace std;
using namespace Eigen;/* sophus   SE(3) 的基本用法 */
int main(int argc, char **argv){Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0, 0, 1)).toRotationMatrix();  // 沿 Z軸 旋轉(zhuǎn) 90° 的旋轉(zhuǎn)矩陣Vector3d t(1, 0, 0);  // 沿 X 軸平移1Sophus::SE3 SE3_Rt(R, t);  // 從R,t 構(gòu)造 SE(3)Quaterniond q(R);Sophus::SE3 SE3_qt(q, t); // 從q, t 構(gòu)造 SE(3)cout << "SE3 from R, t = \n" << SE3_Rt.matrix() << endl;cout << "SE3 from q, t = \n" << SE3_qt.matrix() << endl; /* 李代數(shù)se(3) 是一個(gè) 6 維 向量*/typedef Eigen::Matrix<double, 6, 1> Vector6d;Vector6d se3 = SE3_Rt.log();cout << "se3 = " << se3.transpose() << endl;// hat  向量 ——> 反對(duì)稱矩陣cout << "se3 hat = \n" << Sophus::SE3::hat(se3)<< endl;// vee  反對(duì)稱 ——> 向量cout << "se3 hat vee = " << Sophus::SE3::vee(Sophus::SE3::hat(se3)).transpose() << endl;// 更新Vector6d update_se3;// 更新量update_se3.setZero();update_se3(0, 0) = 1e-4;Sophus::SE3 SE3_updated =Sophus::SE3::exp(update_se3) * SE3_Rt;cout << "SE3 updated = \n" << SE3_updated.matrix() << endl;return 0;
}

4.4.2 評(píng)估軌跡的誤差 【Code】

————————
考慮一條估計(jì)軌跡 ${\mathbf{T}}_{esti,i}$ 和真實(shí)軌跡 ${\mathbf{T}}_{gt,i}$ ，其中 $i=1，???，N$

1、絕對(duì)誤差軌跡(Absolute Trajectory Error, ATE) 旋轉(zhuǎn)和平移誤差
$$AT{E}_{all}=\sqrt{\frac{1}{N}\sum _{i=1}^N||log({\mathbf{T}}_{gt,i}^{?1}{\mathbf{T}}_{esti,i}{)}^{\vee }|{|}_2^2}$$

• 每個(gè)位姿 李代數(shù) 的均方根誤差 (Root-Mean-Squared Error，RMSE)

2、絕對(duì)平移誤差(Average Translational Error)

$$AT{E}_{trans}=\sqrt{\frac{1}{N}\sum _{i=1}^N||trans({\mathbf{T}}_{gt,i}^{?1}{\mathbf{T}}_{esti,i})|{|}_2^2}$$

其中 trans 表示 取括號(hào)內(nèi)部變量的平移部分。

• 從整條軌跡上看，旋轉(zhuǎn)出現(xiàn)偏差后，隨后的軌跡在平移上也會(huì)出現(xiàn)誤差。

3、相對(duì)誤差
考慮 $i$ 時(shí)刻到 $i+\Delta t$ 的運(yùn)動(dòng)，相對(duì)位姿誤差(Relative Pose Error, RPE)

$$RP{E}_{all}=\sqrt{\frac{1}{N?\Delta t}\sum _{i=1}^{N?\Delta t}||log(({\mathbf{T}}_{gt,i}^{?1}{\mathbf{T}}_{gt,i+\Delta t}{)}^{?1}({\mathbf{T}}_{esti,i}^{?1}{\mathbf{T}}_{esti,i+\Delta t}){)}^{\vee }|{|}_2^2}$$

$$RP{E}_{trans}=\sqrt{\frac{1}{N?\Delta t}\sum _{i=1}^{N?\Delta t}||trans(({\mathbf{T}}_{gt,i}^{?1}{\mathbf{T}}_{gt,i+\Delta t}{)}^{?1}({\mathbf{T}}_{esti,i}^{?1}{\mathbf{T}}_{esti,i+\Delta t}))|{|}_2^2}$$

————————

mkdir build && cd build
cmake ..
make 
./trajectoryError

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)project(trajectoryError)find_package(Sophus REQUIRED)
include_directories( ${Sophus_INCLUDE_DIRS})option(USE_UBUNTU_20 "Set to ON if you are using Ubuntu 20.04" OFF)
find_package(Pangolin REQUIRED)
if(USE_UBUNTU_20)message("You are using Ubuntu 20.04, fmt::fmt will be linked")find_package(fmt REQUIRED)set(FMT_LIBRARIES fmt::fmt)
endif()
include_directories(${Pangolin_INCLUDE_DIRS})add_executable(trajectoryError trajectoryError.cpp)
target_link_libraries(trajectoryError ${Sophus_LIBRARIES})
target_link_libraries(trajectoryError ${Pangolin_LIBRARIES} ${FMT_LIBRARIES})

trajectoryError.cpp

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.h>using namespace Sophus;
using namespace std;string groundtruth_file = "../groundtruth.txt";
string estimated_file = "../estimated.txt";typedef vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType;void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti);TrajectoryType ReadTrajectory(const string &path);int main(int argc, char **argv) {TrajectoryType groundtruth = ReadTrajectory(groundtruth_file);TrajectoryType estimated = ReadTrajectory(estimated_file);assert(!groundtruth.empty() && !estimated.empty());assert(groundtruth.size() == estimated.size());// compute rmsedouble rmse = 0;for (size_t i = 0; i < estimated.size(); i++) {Sophus::SE3 p1 = estimated[i], p2 = groundtruth[i];double error = (p2.inverse() * p1).log().norm();rmse += error * error;}rmse = rmse / double(estimated.size());rmse = sqrt(rmse);cout << "RMSE = " << rmse << endl;DrawTrajectory(groundtruth, estimated);return 0;
}TrajectoryType ReadTrajectory(const string &path) {ifstream fin(path);TrajectoryType trajectory;if (!fin) {cerr << "trajectory " << path << " not found." << endl;return trajectory;}while (!fin.eof()) {double time, tx, ty, tz, qx, qy, qz, qw;fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;Sophus::SE3 p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));trajectory.push_back(p1);}return trajectory;
}void DrawTrajectory(const TrajectoryType &gt, const TrajectoryType &esti) {// create pangolin window and plot the trajectorypangolin::CreateWindowAndBind("Trajectory Viewer", 1024, 768);glEnable(GL_DEPTH_TEST);glEnable(GL_BLEND);glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);pangolin::OpenGlRenderState s_cam(pangolin::ProjectionMatrix(1024, 768, 500, 500, 512, 389, 0.1, 1000),pangolin::ModelViewLookAt(0, -0.1, -1.8, 0, 0, 0, 0.0, -1.0, 0.0));pangolin::View &d_cam = pangolin::CreateDisplay().SetBounds(0.0, 1.0, pangolin::Attach::Pix(175), 1.0, -1024.0f / 768.0f).SetHandler(new pangolin::Handler3D(s_cam));while (pangolin::ShouldQuit() == false) {glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);d_cam.Activate(s_cam);glClearColor(1.0f, 1.0f, 1.0f, 1.0f);glLineWidth(2);for (size_t i = 0; i < gt.size() - 1; i++) {glColor3f(0.0f, 0.0f, 1.0f);  // blue for ground truthglBegin(GL_LINES);auto p1 = gt[i], p2 = gt[i + 1];glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);glEnd();}for (size_t i = 0; i < esti.size() - 1; i++) {glColor3f(1.0f, 0.0f, 0.0f);  // red for estimatedglBegin(GL_LINES);auto p1 = esti[i], p2 = esti[i + 1];glVertex3d(p1.translation()[0], p1.translation()[1], p1.translation()[2]);glVertex3d(p2.translation()[0], p2.translation()[1], p2.translation()[2]);glEnd();}pangolin::FinishFrame();usleep(5000);   // sleep 5 ms}}

4.5 相似變換群 與 李代數(shù)

單目視覺(jué) 相似變換群Sim(3)
尺度不確定性 與 尺度漂移
對(duì)位于空間的點(diǎn) $\mathbf{p}$ ，在相機(jī)坐標(biāo)系下要經(jīng)過(guò)一個(gè)相似變換。
$${\mathbf{p}}^{\prime }=\left[\begin{array}{cc}s\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\mathbf{p}=s\mathbf{Rp}+\mathbf{t}$$

對(duì)于尺度因子，李群中的 $s$ 即為李代數(shù)中 $\sigma$ 的指數(shù)函數(shù)

4.6 小結(jié)
李群 SO(3) 和 SE(3) 以及對(duì)應(yīng)的李代數(shù) $\mathfrak{so}(3)$ 和 $\mathfrak{se}(3)$

BCH 線性近似， 對(duì)位姿進(jìn)行擾動(dòng)并求導(dǎo)

習(xí)題

題1

驗(yàn)證SO(3)、SE(3)、Sim(3)關(guān)于乘法成群

特殊正交群SO(n) 旋轉(zhuǎn)矩陣群
特殊歐式群SE(n) n維歐式變換

題2

題4

√ 題5

證明：

√ 題6

$$\begin{array}{rl}根據(jù)題5的結(jié)論& ：{(\mathbf{Rp})}^{\wedge }=\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}\\ \exp ((\mathbf{Rp}{)}^{\wedge })& =\exp (\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}})\\ 級(jí)數(shù)展開& \\ & =\sum _{n=0}^{\infty }\frac{(\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}{)}^n}{N!}\\ & =\sum _{n=0}^{\infty }\frac{\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}?\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}?，???，?\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}?\mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}}{N!}\\ 其中{\mathbf{R}}^{\mathbf{T}}\mathbf{R}=\mathbf{I}& \\ & =\sum _{n=0}^{\infty }\frac{\mathbf{R}({\mathbf{p}}^{\wedge }{)}^{\mathbf{n}}{\mathbf{R}}^{\mathbf{T}}}{N!}\\ & =\mathbf{R}\sum _{n=0}^{\infty }\frac{({\mathbf{p}}^{\wedge }{)}^{\mathbf{n}}}{N!}{\mathbf{R}}^T\\ & =\mathbf{R}\exp ({\mathbf{p}}^{\wedge }){\mathbf{R}}^T\end{array}$$

證畢。
——————

6.2 SE(3)伴隨性質(zhì)

$$\begin{array}{rl}\mathbf{T}\exp ({\mathbf{\xi }}^{\wedge }){\mathbf{T}}^{?1}& =\mathbf{T}\sum _{n=0}^{\infty }\frac{({\mathbf{\xi }}^{\wedge }{)}^n}{n!}{\mathbf{T}}^{?1}\\ 由于{\mathbf{T}}^{?1}\mathbf{T}=\mathbf{I}& \\ & =\sum _{n=0}^{\infty }\frac{\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{?1}?\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{?1}?\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{?1}?\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{?1}?,???,\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{?1}?\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{?1}}{n!}\\ & =\sum _{n=0}^{\infty }\frac{(\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{?1}{)}^n}{n!}\\ & =\exp (\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{?1})\end{array}$$

$$\mathbf{\xi }=\left[\begin{array}{c}\mathbf{\rho }\\ ?\end{array}\right]，{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{?}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]，\mathbf{T}=\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]，{\mathbf{T}}^{?1}=\left[\begin{array}{cc}{\mathbf{R}}^T& ?{\mathbf{R}}^T\mathbf{t}\\ 0^T& 1\end{array}\right]$$

則：
$$\begin{array}{rl}\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{?1}& =\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0^T& 1\end{array}\right]\left[\begin{array}{cc}{?}^{\wedge }& \mathbf{\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\mathbf{R}}^T& ?{\mathbf{R}}^T\mathbf{t}\\ 0^T& 1\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{R}{?}^{\wedge }& \mathbf{R\rho }\\ 0^T& 0\end{array}\right]\left[\begin{array}{cc}{\mathbf{R}}^T& ?{\mathbf{R}}^T\mathbf{t}\\ 0^T& 1\end{array}\right]\\ & =\left[\begin{array}{cc}\mathbf{R}{?}^{\wedge }{\mathbf{R}}^T& ?\mathbf{R}{?}^{\wedge }{\mathbf{R}}^T\mathbf{t}+\mathbf{R\rho }\\ 0^T& 0\end{array}\right]\\ 由題5的結(jié)論& \mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}={(\mathbf{Rp})}^{\wedge }\\ & =\left[\begin{array}{cc}(\mathbf{R}?{)}^{\wedge }& ?(\mathbf{R}?{)}^{\wedge }\mathbf{t}+\mathbf{R\rho }\\ 0^T& 0\end{array}\right]\\ & 對(duì)比\mathbf{\xi },{\mathbf{\xi }}^{\wedge }進(jìn)行轉(zhuǎn)換\\ & ={\left[\begin{array}{c}?(\mathbf{R}?{)}^{\wedge }\mathbf{t}+\mathbf{R\rho }\\ \mathbf{R}?\end{array}\right]}^{\wedge }\\ 由叉乘性質(zhì)，& \\ & ={\left[\begin{array}{c}{\mathbf{t}}^{\wedge }\mathbf{R}?+\mathbf{R\rho }\\ \mathbf{R}?\end{array}\right]}^{\wedge }\\ & =\left[\begin{array}{cc}\mathbf{R}& {\mathbf{t}}^{\wedge }\mathbf{R}\\ 0& \mathbf{R}\end{array}\right]\left[\begin{array}{c}\mathbf{\rho }\\ ?\end{array}\right]{)}^{\wedge }\\ & =(Ad(\mathbf{T})\mathbf{\xi }{)}^{\wedge }\end{array}$$

綜上：
$$\mathbf{T}\exp ({\mathbf{\xi }}^{\wedge }){\mathbf{T}}^{?1}=\exp ((Ad(\mathbf{T})\mathbf{\xi }{)}^{\wedge })$$
其中
$$Ad(\mathbf{T})=\left[\begin{array}{cc}\mathbf{R}& {\mathbf{t}}^{\wedge }\mathbf{R}\\ 0& \mathbf{R}\end{array}\right]$$

證畢
————————————————

√ 題7

SO(3):
設(shè)右擾動(dòng) $\Delta \mathbf{R}$ 對(duì)應(yīng)的李代數(shù) 為 $\phi$

$$\begin{array}{rl}\frac{?(\mathbf{Rp})}{?\phi }& =\underset{\phi \to 0}{\lim }\frac{\exp ({?}^{\wedge })\exp ({\phi }^{\wedge })\mathbf{p}?\exp ({?}^{\wedge })\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\exp ({?}^{\wedge })(\mathbf{I}+{\phi }^{\wedge })\mathbf{p}?\exp ({?}^{\wedge })\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\exp ({?}^{\wedge }){\phi }^{\wedge }\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}{\phi }^{\wedge }\mathbf{p}}{\phi }\\ 由題5:\mathbf{R}{\mathbf{p}}^{\wedge }& ={(\mathbf{Rp})}^{\wedge }\mathbf{R}\\ & =\underset{\phi \to 0}{\lim }\frac{(\mathbf{R}\phi {)}^{\wedge }\mathbf{Rp}}{\phi }\\ 由叉乘性質(zhì)& \\ & =\underset{\phi \to 0}{\lim }\frac{?(\mathbf{Rp}{)}^{\wedge }\mathbf{R}\phi }{\phi }\\ & =?(\mathbf{Rp}{)}^{\wedge }\mathbf{R}\end{array}$$

SE(3):

假設(shè)某空間點(diǎn) $\mathbf{p}$ 經(jīng)過(guò)一次變換 $\mathbf{T}$ (對(duì)應(yīng)李代數(shù)為 $\mathbf{\xi }$)， 得到 $\mathbf{Tp}$
現(xiàn)在給 $\mathbf{T}$ 右乘一個(gè)擾動(dòng) $\Delta \mathbf{T}=\exp (\Delta {\mathbf{\xi }}^{\wedge })$
設(shè)擾動(dòng)項(xiàng)的李代數(shù)為 $\Delta \mathbf{\xi }=[\Delta \mathbf{\rho },\Delta ?{]}^T$，則

$$\begin{array}{rl}\frac{?(\mathbf{Tp})}{?\Delta \mathbf{\xi }}& =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\exp ({\mathbf{\xi }}^{\wedge })\exp (\Delta {\mathbf{\xi }}^{\wedge })\mathbf{p}?\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\exp ({\mathbf{\xi }}^{\wedge })(\mathbf{I}+\Delta {\mathbf{\xi }}^{\wedge })\mathbf{p}?\exp ({\mathbf{\xi }}^{\wedge })\mathbf{p}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\exp ({\mathbf{\xi }}^{\wedge })\Delta {\mathbf{\xi }}^{\wedge }\mathbf{p}}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right]\left[\begin{array}{cc}\Delta {?}^{\wedge }& \Delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\mathbf{p}}{\Delta \mathbf{\xi }}\\ 把\mathbf{p}前的& 矩陣當(dāng)做旋轉(zhuǎn)矩陣，結(jié)合向量旋轉(zhuǎn)的性質(zhì)，\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{cc}\mathbf{R}& \mathbf{t}\\ 0& 1\end{array}\right]\left[\begin{array}{c}\Delta {?}^{\wedge }\mathbf{p}+\Delta \mathbf{\rho }\\ 0\end{array}\right]}{\Delta \mathbf{\xi }}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}\mathbf{R}\Delta {?}^{\wedge }\mathbf{p}+\mathbf{R}\Delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\Delta \mathbf{\rho },\Delta ?{]}^T}\\ 將& \Delta \mathbf{\rho },\Delta ?提出來(lái)，方便求導(dǎo)\\ & 根據(jù)\mathbf{R}{\mathbf{p}}^{\wedge }={(\mathbf{Rp})}^{\wedge }\mathbf{R}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}(\mathbf{R}\Delta ?{)}^{\wedge }\mathbf{R}\mathbf{p}+\mathbf{R}\Delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\Delta \mathbf{\rho },\Delta ?{]}^T}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}?(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{R}\Delta ?+\mathbf{R}\Delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\Delta \mathbf{\rho },\Delta ?{]}^T}\\ & =\left[\begin{array}{cc}\mathbf{R}& ?(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{R}\\ 0^T& 0^T\end{array}\right]\end{array}$$

——————————————

√ 題8

cmake的 find_package指令：

官方文檔

find_package(包名 版本 REQUIRED)

一些包需要 sudo make install后才能被找到

LaTex

$\mathfrak{g}$

$\mathfrak{g}$

$\mathbb{R}$

$\mathbb{R}$

$\times$

$\times$

$\varphi$

$\phi$

$\Phi$

$\Phi$

$\overset{\mathrm{def}}{=}$
$\xlongequal{def}$

$\stackrel{\mathrm{def}}{=}$
$\stackrel{def}{=}$