P71

什么样的相机位姿 最符合 当前观测数据。

求解最优的 $\mathbf{R},\mathbf{t}$， 使得误差最小化。

4.1 李群与李代数基础

群： 只有一个(良好的)运算的集合。



封结幺逆 、 丰俭由你

李群： 具有连续(光滑)性质的群。

在 t = 0 附近，旋转矩阵可以由$exp({\varphi }_0^{\wedge }t)$计算得到

4.1.3 李代数的定义

李代数 描述了李群的局部性质

• 单位元 附近的正切空间。

$\mathfrak{g}=({\mathbb{R}}^3,\mathbb{R},\times )$构成了一个李代数

4.1.4 李代数 so(3)

$\mathfrak{so}(3)$: 一个由三维向量组成的集合，每个向量对应一个反对称矩阵，可以用于表达旋转矩阵的导数。
s o ( 3 ) = { ϕ ∈ R 3 , Φ = ϕ ∧ ∈ R 3 × 3 } \mathfrak{so}(3)=\{\bm{\phi}\in\mathbb{R}^3,\bm{\Phi}=\bm{\phi}^{\land}\in\mathbb{R}^{3\times3}\} so(3)={ϕ∈R3,Φ=ϕ∧∈R3×3}

4.1.5 李代数 se(3)

李群 $SE(3)$ 对应的李代数 $\mathfrak{se}(3)$

4.2 指数与对数映射

4.2.1 SO(3)上的指数映射

$\mathfrak{so}(3)$： 旋转向量 组成的空间
指数映射： 罗德里格斯公式

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

对数映射： 李群$SO(3)$ 中的元素 ——> 李代数$\mathfrak{so}(3)$

把旋转角固定在 $\pm \pi$ 之间，则李群和李代数 元素 一一对应。

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

罗德里格斯公式推导

$$\begin{array}{rl}exp({\mathbf{\varphi }}^{\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+\cdot \cdot \cdot \\ & 将{\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+\cdot \cdot \cdot \\ & =\mathbf{a}{\mathbf{a}}^T+(\theta -\frac{1}{3!}{\theta }^3+\frac{1}{5!}{\theta }^5-\cdot \cdot \cdot ){\mathbf{a}}^{\wedge }-(1-\frac{1}{2!}{\theta }^2+\frac{1}{4!}{\theta }^4-\cdot \cdot \cdot ){\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) 上的指数映射

——————
推导：
$$\begin{array}{rl}\sum _{n=0}^{\infty }\frac{1}{(n+1)!}({\mathbf{\varphi }}^{\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+\cdot \cdot \cdot \\ & =\frac{1}{\theta }(\frac{1}{2!}{\theta }^2-\frac{1}{4!}{\theta }^4+\cdot \cdot \cdot ){\mathbf{a}}^{\wedge }+\frac{1}{\theta }(\frac{1}{3!}{\theta }^3-\frac{1}{5!}{\theta }^5+\cdot \cdot \cdot )({\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 }$
平移部分发生了一次以 $\mathbf{J}$ 为系数矩阵的 线性变换。

SO(3),SE(3),so(3),se(3)的对应关系

4.3 李代数求导与扰动模型

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

4.3.2 SO(3)上的李代数求导

位姿由SO(3)上的旋转矩阵 或 SE(3)上的变换矩阵 描述

设某时刻机器人的位姿为 $\mathbf{T}$， 观察到了一个世界坐标位于 $\mathbf{p}$ 的点，产生了一个观测数据 $\mathbf{z}$
计算理想的观测与实际数据之间的误差： $\mathbf{e}=\mathbf{z}-\mathbf{Tp}$
假设一共有 $N$ 个这样的路标点和观测，对机器人进行位姿估计，相当于寻找一个最优的 $\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$$

求解上述问题，需要计算目标函数 $J$ 关于变换矩阵 $\mathbf{T}$ 的导数。

使用 李代数 解决 求导问题 的2种思路：
1、用李代数表示姿态，然后根据李代数加法对李代数求导。
2、对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。

4.3.3 李代数求导

——————

推导：
$$\begin{array}{rl}\frac{\partial (\mathbf{Rp})}{\partial \mathbf{R}}& \stackrel{R对应的李代数为\mathbf{\varphi }}{=}\frac{\partial (\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p})}{\partial \mathbf{\varphi }}\\ & =\underset{\Delta \mathbf{\varphi }\to 0}{\lim }\frac{\exp ((\mathbf{\varphi }+\Delta \mathbf{\varphi }{)}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\Delta \mathbf{\varphi }}\\ & 由式(4.35)\\ & =\underset{\Delta \varphi \to 0}{\lim }\frac{\exp (({\mathbf{J}}_l\Delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\Delta \mathbf{\varphi }}\\ & =\underset{\Delta \mathbf{\varphi }\to 0}{\lim }\frac{(\mathbf{I}+({\mathbf{J}}_l\Delta \mathbf{\varphi }{)}^{\wedge })\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\Delta \mathbf{\varphi }}\\ & =\underset{\Delta \mathbf{\varphi }\to 0}{\lim }\frac{({\mathbf{J}}_l\Delta \mathbf{\varphi }{)}^{\wedge }\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\Delta \mathbf{\varphi }}\\ & a^{\wedge }等效于a\times ,因此根据叉乘的性质\\ & =\underset{\Delta \mathbf{\varphi }\to 0}{\lim }\frac{-(\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\Delta \mathbf{\varphi }}{\Delta \mathbf{\varphi }}\\ & =-(\mathbf{Rp}{)}^{\wedge }{\mathbf{J}}_l\end{array}$$

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

4.3.4 扰动模型(左乘)【更简单 的导数计算模型】

对 $\mathbf{R}$ 进行一次扰动 $\Delta \mathbf{R}$ ，看结果对于 扰动的变化率。

设左扰动 $\Delta \mathbf{R}$ 对应的李代数 为 $\mathbf{\phi }$

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

4.3.5 SE(3)上的李代数求导

假设某空间点 $\mathbf{p}$ 经过一次变换 $\mathbf{T}$ (对应李代数为 $\mathbf{\xi }$)， 得到 $\mathbf{Tp}$
现在给 $\mathbf{T}$ 左乘一个扰动 $\Delta \mathbf{T}=\exp (\Delta {\mathbf{\xi }}^{\wedge })$
设扰动项的李代数为 $\Delta \mathbf{\xi }=[\Delta \mathbf{\rho },\Delta \mathbf{\varphi }{]}^T$，则

$$\begin{array}{rl}\frac{\partial (\mathbf{Tp})}{\partial \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 {\mathbf{\varphi }}^{\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 {\mathbf{\varphi }}^{\wedge }(\mathbf{Rp}+\mathbf{t})+\Delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\Delta \mathbf{\rho },\Delta \mathbf{\varphi }{]}^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应用 【Code】

SO(3)、SE(3)
二维运动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轴  转90° 的旋转矩阵Matrix3d R = AngleAxisd(M_PI/2, Vector3d(0, 0, 1)).toRotationMatrix();/* 四元数 */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){/* 使用 对数映射 获得  李代数*/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  向量 ——> 反对称矩阵cout << "so3 hat = \n" << Sophus::SO3::hat(so3)<< endl;// vee  反对称 ——> 向量cout << "so3 hat vee = " << Sophus::SO3::vee(Sophus::SO3::hat(so3)).transpose() << endl;Vector3d update_so3(1e-4, 0, 0);// 假设更新量为这么多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轴 旋转 90° 的旋转矩阵Vector3d t(1, 0, 0);  // 沿 X 轴平移1Sophus::SE3 SE3_Rt(R, t);  // 从R,t 构造 SE(3)Quaterniond q(R);Sophus::SE3 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) 是一个 6 维 向量*/typedef Eigen::Matrix<double, 6, 1> Vector6d;Vector6d se3 = SE3_Rt.log();cout << "se3 = " << se3.transpose() << endl;// hat  向量 ——> 反对称矩阵cout << "se3 hat = \n" << Sophus::SE3::hat(se3)<< endl;// vee  反对称 ——> 向量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 评估轨迹的误差 【Code】

————————
考虑一条估计轨迹 ${\mathbf{T}}_{esti,i}$ 和真实轨迹 ${\mathbf{T}}_{gt,i}$ ，其中 $i=1，\cdot \cdot \cdot ，N$

1、绝对误差轨迹(Absolute Trajectory Error, ATE) 旋转和平移误差
$$AT{E}_{all}=\sqrt{\frac{1}{N}\sum _{i=1}^N||log({\mathbf{T}}_{gt,i}^{-1}{\mathbf{T}}_{esti,i}{)}^{\vee }|{|}_2^2}$$

• 每个位姿 李代数 的均方根误差 (Root-Mean-Squared Error，RMSE)

2、绝对平移误差(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 表示 取括号内部变量的平移部分。

• 从整条轨迹上看，旋转出现偏差后，随后的轨迹在平移上也会出现误差。

3、相对误差
考虑 $i$ 时刻到 $i+\Delta t$ 的运动，相对位姿误差(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 相似变换群 与 李代数

单目视觉 相似变换群Sim(3)
尺度不确定性 与 尺度漂移
对位于空间的点 $\mathbf{p}$ ，在相机坐标系下要经过一个相似变换。
$${\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}$$

对于尺度因子，李群中的 $s$ 即为李代数中 $\sigma$ 的指数函数

4.6 小结
李群 SO(3) 和 SE(3) 以及对应的李代数 $\mathfrak{so}(3)$ 和 $\mathfrak{se}(3)$

BCH 线性近似， 对位姿进行扰动并求导

习题

题1

验证SO(3)、SE(3)、Sim(3)关于乘法成群

特殊正交群SO(n) 旋转矩阵群
特殊欧式群SE(n) n维欧式变换

题2

题4

√ 题5

证明：
$$\begin{array}{rl}原式等效于证明& \\ \mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}\mathbf{R}& ={(\mathbf{Rp})}^{\wedge }\mathbf{R}\\ \mathbf{R}{\mathbf{p}}^{\wedge }\mathbf{I}& ={(\mathbf{Rp})}^{\wedge }\mathbf{R}\\ \mathbf{R}{\mathbf{p}}^{\wedge }& ={(\mathbf{Rp})}^{\wedge }\mathbf{R}\\ 对于向量\mathbf{v}\in {\mathbb{R}}^3& \\ \mathbf{R}{\mathbf{p}}^{\wedge }\mathbf{v}& ={(\mathbf{Rp})}^{\wedge }\mathbf{R}\mathbf{v}\\ 上式等号左边& 表示向量\mathbf{p},\mathbf{v}叉乘后所得向量根据\mathbf{R}旋转。\\ 等号右边表示& 向量\mathbf{p},\mathbf{v}分别根据\mathbf{R}旋转后叉乘，显然得到同一个向量。\\ 证毕。& \end{array}$$

√ 题6

$$\begin{array}{rl}根据题5的结论& ：{(\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}})\\ 级数展开& \\ & =\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}}\cdot \mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}\cdot ，\cdot \cdot \cdot ，\cdot \mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}\cdot \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)伴随性质

$$\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}\cdot \mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{-1}\cdot \mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{-1}\cdot \mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{-1}\cdot ,\cdot \cdot \cdot ,\mathbf{T}{\mathbf{\xi }}^{\wedge }{\mathbf{T}}^{-1}\cdot \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 }\\ \mathbf{\varphi }\end{array}\right]，{\mathbf{\xi }}^{\wedge }=\left[\begin{array}{cc}{\mathbf{\varphi }}^{\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}{\mathbf{\varphi }}^{\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}{\mathbf{\varphi }}^{\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}{\mathbf{\varphi }}^{\wedge }{\mathbf{R}}^T& -\mathbf{R}{\mathbf{\varphi }}^{\wedge }{\mathbf{R}}^T\mathbf{t}+\mathbf{R\rho }\\ 0^T& 0\end{array}\right]\\ 由题5的结论& \mathbf{R}{\mathbf{p}}^{\wedge }{\mathbf{R}}^{\mathbf{T}}={(\mathbf{Rp})}^{\wedge }\\ & =\left[\begin{array}{cc}(\mathbf{R}\mathbf{\varphi }{)}^{\wedge }& -(\mathbf{R}\mathbf{\varphi }{)}^{\wedge }\mathbf{t}+\mathbf{R\rho }\\ 0^T& 0\end{array}\right]\\ & 对比\mathbf{\xi },{\mathbf{\xi }}^{\wedge }进行转换\\ & ={\left[\begin{array}{c}-(\mathbf{R}\mathbf{\varphi }{)}^{\wedge }\mathbf{t}+\mathbf{R\rho }\\ \mathbf{R}\mathbf{\varphi }\end{array}\right]}^{\wedge }\\ 由叉乘性质，& \\ & ={\left[\begin{array}{c}{\mathbf{t}}^{\wedge }\mathbf{R}\mathbf{\varphi }+\mathbf{R\rho }\\ \mathbf{R}\mathbf{\varphi }\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 }\\ \mathbf{\varphi }\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):
设右扰动 $\Delta \mathbf{R}$ 对应的李代数 为 $\phi$

$$\begin{array}{rl}\frac{\partial (\mathbf{Rp})}{\partial \mathbf{\phi }}& =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp ({\mathbf{\varphi }}^{\wedge })\exp ({\mathbf{\phi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp ({\mathbf{\varphi }}^{\wedge })(\mathbf{I}+{\mathbf{\phi }}^{\wedge })\mathbf{p}-\exp ({\mathbf{\varphi }}^{\wedge })\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\exp ({\mathbf{\varphi }}^{\wedge }){\mathbf{\phi }}^{\wedge }\mathbf{p}}{\mathbf{\phi }}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{\mathbf{R}{\mathbf{\phi }}^{\wedge }\mathbf{p}}{\mathbf{\phi }}\\ 由题5:\mathbf{R}{\mathbf{p}}^{\wedge }& ={(\mathbf{Rp})}^{\wedge }\mathbf{R}\\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{(\mathbf{R}\mathbf{\phi }{)}^{\wedge }\mathbf{Rp}}{\mathbf{\phi }}\\ 由叉乘性质& \\ & =\underset{\mathbf{\phi }\to 0}{\lim }\frac{-(\mathbf{Rp}{)}^{\wedge }\mathbf{R}\mathbf{\phi }}{\mathbf{\phi }}\\ & =-(\mathbf{Rp}{)}^{\wedge }\mathbf{R}\end{array}$$

SE(3):

假设某空间点 $\mathbf{p}$ 经过一次变换 $\mathbf{T}$ (对应李代数为 $\mathbf{\xi }$)， 得到 $\mathbf{Tp}$
现在给 $\mathbf{T}$ 右乘一个扰动 $\Delta \mathbf{T}=\exp (\Delta {\mathbf{\xi }}^{\wedge })$
设扰动项的李代数为 $\Delta \mathbf{\xi }=[\Delta \mathbf{\rho },\Delta \mathbf{\varphi }{]}^T$，则

$$\begin{array}{rl}\frac{\partial (\mathbf{Tp})}{\partial \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 {\mathbf{\varphi }}^{\wedge }& \Delta \mathbf{\rho }\\ 0^T& 0\end{array}\right]\mathbf{p}}{\Delta \mathbf{\xi }}\\ 把\mathbf{p}前的& 矩阵当做旋转矩阵，结合向量旋转的性质，\\ & =\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 {\mathbf{\varphi }}^{\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 {\mathbf{\varphi }}^{\wedge }\mathbf{p}+\mathbf{R}\Delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\Delta \mathbf{\rho },\Delta \mathbf{\varphi }{]}^T}\\ 将& \Delta \mathbf{\rho },\Delta \mathbf{\varphi }提出来，方便求导\\ & 根据\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 \mathbf{\varphi }{)}^{\wedge }\mathbf{R}\mathbf{p}+\mathbf{R}\Delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\Delta \mathbf{\rho },\Delta \mathbf{\varphi }{]}^T}\\ & =\underset{\Delta \mathbf{\xi }\to 0}{\lim }\frac{\left[\begin{array}{c}-(\mathbf{R}\mathbf{p}{)}^{\wedge }\mathbf{R}\Delta \mathbf{\varphi }+\mathbf{R}\Delta \mathbf{\rho }\\ 0^T\end{array}\right]}{[\Delta \mathbf{\rho },\Delta \mathbf{\varphi }{]}^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}{=}$