深蓝学院视觉slam十四讲第3章作业

2.1

{ Z , + } \{\mathbb Z,+\} {Z,+}是群

 封闭性:  ∀ Z 1 , Z 2 ∈ Z ,  满足  Z 1 + Z 2 ∈ Z  结合性:  ∀ Z 1 , Z 2 , Z 3 ∈ Z ,  满足  ( Z 1 + Z 2 ) + Z 3 = Z 1 + ( Z 2 + Z 3 )  幺元:  ∃ Z 0 = 0 , ∀ Z 1 ∈ Z ,  满足  Z 1 + 0 = 0 + Z 1 = Z 1  逆:  ∀ Z 1 ∈ Z , ∃ ( − Z 1 ) ∈ Z ,  满足  Z 1 + ( − Z 1 ) = 0 \begin{aligned} &\text { 封闭性: } \forall Z_{1}, Z_{2} \in \mathbb Z, \text { 满足 } Z_{1}+Z_{2} \in \mathbb Z\\ &\text { 结合性: } \forall Z_{1}, Z_{2}, Z_{3} \in \mathbb Z, \text { 满足 }\left(Z_{1}+Z_{2}\right)+Z_{3}=Z_{1}+\left(Z_{2}+Z_{3}\right)\\ &\text { 幺元: } \exists Z_{0}=0, \forall Z_{1} \in \mathbb Z, \text { 满足 } Z_{1}+0=0+Z_{1}=Z_{1}\\ &\text { 逆: } \forall Z_{1} \in \mathbb Z, \exists(-Z_{1} )\in \mathbb Z, \text { 满足 } Z_{1}+\left(-Z_{1}\right)=0 \end{aligned} ​ 封闭性: ∀Z1​,Z2​∈Z, 满足 Z1​+Z2​∈Z 结合性: ∀Z1​,Z2​,Z3​∈Z, 满足 (Z1​+Z2​)+Z3​=Z1​+(Z2​+Z3​) 幺元: ∃Z0​=0,∀Z1​∈Z, 满足 Z1​+0=0+Z1​=Z1​ 逆: ∀Z1​∈Z,∃(−Z1​)∈Z, 满足 Z1​+(−Z1​)=0​

2.2

{ N , + } \{\mathbb N,+\} {N,+}不是群，自然数集里面没有负数，不满足逆条件

除 0 以 外 ， ∀ N 1 ∈ N , 都 不 存 在 ( − N 1 ) ∈ N ,  满足  N 1 + ( − N 1 ) = 0 除0以外，\forall N_{1} \in \mathbb N, 都不存在(-N_{1} )\in \mathbb N, \text { 满足 } N_{1}+\left(-N_{1}\right)=0 除0以外，∀N1​∈N,都不存在(−N1​)∈N, 满足 N1​+(−N1​)=0

3.

1. 封闭性 ∀ X , Y ∈ R 3 , X × Y ∈ R 3 \quad \forall X, Y \in \mathbb{R}^3,X\times Y \in \mathbb{R}^3 ∀X,Y∈R3,X×Y∈R3

2. 双线性 ∀ X , Y , Z ∈ R 3 , a , b ∈ R , \quad \forall X, Y, Z \in \mathbb{R}^3, a, b \in \mathbb{R}, ∀X,Y,Z∈R3,a,b∈R, 有:

   ( a X + b Y ) × Z = a ( X × Z ) + b ( Y × Z ) , Z × ( a X + b Y ) = a ( Z × X ) + b ( Z × Y ) (a X+b Y)\times Z=a(X\times Z)+b(Y\times Z), \quad Z\times (a X+b Y)=a(Z\times X)+b(Z\times Y) (aX+bY)×Z=a(X×Z)+b(Y×Z),Z×(aX+bY)=a(Z×X)+b(Z×Y)

3. 自反性 ∀ X ∈ R 3 , X × X = 0 \quad \forall X \in \mathbb{R}^3,X\times X=0 ∀X∈R3,X×X=0

4. 雅可比等价

   由向量叉乘的拉格朗日公式： ( a × b ) × c = b ( a ⋅ c ) − a ( b ⋅ c ) a × ( b × c ) = b ( a ⋅ c ) − c ( a ⋅ b ) (a\times b)\times c=b(a\cdot c)-a(b\cdot c) \quad a\times (b\times c)=b(a\cdot c)-c(a\cdot b) (a×b)×c=b(a⋅c)−a(b⋅c)a×(b×c)=b(a⋅c)−c(a⋅b) （定理证明如下，就是展开计算）

   

   ∀ X , Y , Z ∈ R 3 , X × ( Y × Z ) + Y × ( Z × X ) + Z × ( X × Y ) = Y ( X ⋅ Z ) − Z ( X ⋅ Y ) + Z ( Y ⋅ X ) − X ( Y ⋅ Z ) + X ( Z ⋅ Y ) − Y ( Z ⋅ X ) = 0 \quad \forall X, Y, Z \in \mathbb{R}^3,X\times(Y\times Z)+Y\times(Z\times X)+Z\times(X\times Y)\\=Y(X\cdot Z)-Z(X\cdot Y) + Z(Y\cdot X)-X(Y\cdot Z)+ X(Z\cdot Y)-Y(Z\cdot X)=0 ∀X,Y,Z∈R3,X×(Y×Z)+Y×(Z×X)+Z×(X×Y)=Y(X⋅Z)−Z(X⋅Y)+Z(Y⋅X)−X(Y⋅Z)+X(Z⋅Y)−Y(Z⋅X)=0

4.

题中小错误，应该为令 ϕ = θ a \phi=\theta \bm a ϕ=θa

有 a ∧ a ∧ = a a T − I , a ∧ a ∧ a ∧ = − a ∧ \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}=\boldsymbol{a}\boldsymbol{a}^{T}-\boldsymbol{I}, \quad \boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}\boldsymbol{a}^{\wedge}=-\boldsymbol{a}^{\wedge} a∧a∧=aaT−I,a∧a∧a∧=−a∧

∑ n = 0 ∞ 1 n ! ( θ a ∧ ) n = I + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ + 1 3 ! θ 3 a ∧ a ∧ a ∧ + 1 4 ! θ 4 ( a ∧ ) 4 + … = a a T − a ∧ a ∧ + θ a ∧ + 1 2 ! θ 2 a ∧ a ∧ − 1 3 ! θ 3 a ∧ − 1 4 ! θ 4 ( a ∧ ) 2 + … = a a T + ( θ − 1 3 ! θ 3 + 1 5 ! θ 5 − … ) a ∧ − ( 1 − 1 2 ! θ 2 + 1 4 ! θ 4 − … ) a ∧ a ∧ = a ∧ a ∧ + I + sin ⁡ θ a ∧ − cos ⁡ θ a ∧ a ∧ = ( 1 − cos ⁡ θ ) a ∧ a ∧ + I + sin ⁡ θ a ∧ = cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + sin ⁡ θ a ∧ = d e f R \begin{aligned} \sum_{n=0} ^{\infty}\frac{1}{n !}\left(\theta \boldsymbol{a}^{\wedge}\right)^{n} &=\boldsymbol{I}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}-\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\theta \boldsymbol{a}^{\wedge}+\frac{1}{2 !} \theta^{2} \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}-\frac{1}{3 !} \theta^{3} \boldsymbol{a}^{\wedge}-\frac{1}{4 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\ldots \\ &=\boldsymbol{a} \boldsymbol{a}^{T}+\left(\theta-\frac{1}{3 !} \theta^{3}+\frac{1}{5 !} \theta^{5}-\ldots\right) \boldsymbol{a}^{\wedge}-\left(1-\frac{1}{2 !} \theta^{2}+\frac{1}{4 !} \theta^{4}-\ldots\right) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge}-\cos \theta \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge} \\ &=(1-\cos \theta) \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}+\boldsymbol{I}+\sin \theta \boldsymbol{a}^{\wedge} \\ &=\cos \theta \boldsymbol{I}+(1-\cos \theta) \boldsymbol{a} \boldsymbol{a}^{T}+\sin \theta \boldsymbol{a}^{\wedge}\stackrel{\mathrm{def}}{=} \boldsymbol{R} \end{aligned} n=0∑∞​n!1​(θa∧)n​=I+θa∧+2!1​θ2a∧a∧+3!1​θ3a∧a∧a∧+4!1​θ4(a∧)4+…=aaT−a∧a∧+θa∧+2!1​θ2a∧a∧−3!1​θ3a∧−4!1​θ4(a∧)2+…=aaT+(θ−3!1​θ3+5!1​θ5−…)a∧−(1−2!1​θ2+4!1​θ4−…)a∧a∧=a∧a∧+I+sinθa∧−cosθa∧a∧=(1−cosθ)a∧a∧+I+sinθa∧=cosθI+(1−cosθ)aaT+sinθa∧=defR​

∑ n = 0 ∞ 1 ( n + 1 ) ! ( θ a ∧ ) n = I + 1 2 ! θ a ∧ + 1 3 ! θ 2 ( a ∧ ) 2 + 1 4 ! θ 3 ( a ∧ ) 3 + 1 5 ! θ 4 ( a ∧ ) 4 ⋯ = 1 θ ( 1 2 ! θ 2 − 1 4 ! θ 4 + ⋯   ) ( a ∧ ) + 1 θ ( 1 3 ! θ 3 − 1 5 θ 5 + ⋯   ) ( a ∧ ) 2 + I = 1 θ ( 1 − cos ⁡ θ ) ( a ∧ ) + θ − sin ⁡ θ θ ( a a T − I ) + I = sin ⁡ θ θ I + ( 1 − sin ⁡ θ θ ) a a T + 1 − cos ⁡ θ θ a ∧ = d e f J \begin{aligned} \sum_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\theta \boldsymbol{a}^{\wedge}\right)^{n} &=\boldsymbol{I}+\frac{1}{2 !} \theta \boldsymbol{a}^{\wedge}+\frac{1}{3 !} \theta^{2}\left(\boldsymbol{a}^{\wedge}\right)^{2}+\frac{1}{4 !} \theta^{3}\left(\boldsymbol{a}^{\wedge}\right)^{3}+\frac{1}{5 !} \theta^{4}\left(\boldsymbol{a}^{\wedge}\right)^{4} \cdots \\ &=\frac{1}{\theta}\left(\frac{1}{2 !} \theta^{2}-\frac{1}{4 !} \theta^{4}+\cdots\right)\left(\boldsymbol{a}^{\wedge}\right)+\frac{1}{\theta}\left(\frac{1}{3 !} \theta^{3}-\frac{1}{5} \theta^{5}+\cdots\right)\left(\boldsymbol{a}^{\wedge}\right)^{2}+\boldsymbol{I} \\ &=\frac{1}{\theta}(1-\cos \theta)\left(\boldsymbol{a}^{\wedge}\right)+\frac{\theta-\sin \theta}{\theta}\left(\boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}-\boldsymbol{I}\right)+\boldsymbol{I} \\ &=\frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^{\mathrm{T}}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \stackrel{\mathrm{def}}{=} \boldsymbol{J} \end{aligned} n=0∑∞​(n+1)!1​(θa∧)n​=I+2!1​θa∧+3!1​θ2(a∧)2+4!1​θ3(a∧)3+5!1​θ4(a∧)4⋯=θ1​(2!1​θ2−4!1​θ4+⋯)(a∧)+θ1​(3!1​θ3−51​θ5+⋯)(a∧)2+I=θ1​(1−cosθ)(a∧)+θθ−sinθ​(aaT−I)+I=θsinθ​I+(1−θsinθ​)aaT+θ1−cosθ​a∧=defJ​

故最终有：

exp ⁡ ( ξ ∧ ) = [ ∑ n = 0 ∞ 1 n ! ( ϕ ∧ ) n ∑ n = 0 ∞ 1 ( n + 1 ) ! ( ϕ ∧ ) n ρ 0 T 1 ] ≜ [ R J ρ 0 T 1 ] = T \begin{aligned} \exp \left(\boldsymbol{\xi}^{\wedge}\right) &= \left[ \begin{array}{cc} \sum\limits_{n=0}^{\infty} \frac{1}{n !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} & \sum\limits_{n=0}^{\infty} \frac{1}{(n+1) !}\left(\boldsymbol{\phi}^{\wedge}\right)^{n} \boldsymbol{\rho} \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array} \right] \\ & \triangleq\left[\begin{array}{cc} \boldsymbol{R} & \boldsymbol{J} \rho \\ \mathbf{0}^{\mathrm{T}} & 1 \end{array}\right]=\boldsymbol{T} \end{aligned} exp(ξ∧)​=⎣⎡​n=0∑∞​n!1​(ϕ∧)n0T​n=0∑∞​(n+1)!1​(ϕ∧)nρ1​⎦⎤​≜[R0T​Jρ1​]=T​

5.

参考链接 https://math.stackexchange.com/questions/2190603/derivation-of-adjoint-for-so3

先证： R a ∧ R T = ( R a ) ∧ \bm R\bm a^{\wedge} \bm R^T = (\bm R \bm a)^{\wedge} Ra∧RT=(Ra)∧

∀ v ∈ R 3 , ( R a ) ∧ v = ( R a ) × v = ( R a ) × ( R R − 1 v ) = R [ a × ( R − 1 v ) ] = R a ∧ R − 1 v \forall \bm{v} \in \mathbb{R}^3,(\bm{R} \bm a)^{\wedge}\bm{v}=(\bm{R} \bm a) \times \bm{v}=(\bm{R} \bm a) \times\left(\bm{R} \bm{R}^{-1} \bm{v}\right)=\bm{R}\left[\boldsymbol{\bm a} \times\left(\bm{R}^{-1} \mathbf{v}\right)\right]=\bm{R} \bm a^{\wedge} \bm{R}^{-1} \bm{v} ∀v∈R3,(Ra)∧v=(Ra)×v=(Ra)×(RR−1v)=R[a×(R−1v)]=Ra∧R−1v

对exp进行泰勒展开再合并

exp ⁡ ( ( R p ) ∧ ) = ∑ n = 0 ∞ 1 n ! ( ( R p ) ∧ ) n = ∑ n = 0 ∞ 1 n ! ( R p ∧ R T ) n = ∑ n = 0 ∞ 1 n ! ( ( R p ∧ R T ) ⋅ ( R p ∧ R T ) ⋅ ⋯ ⋅ ( R p ∧ R T ) ) = ∑ n = 0 ∞ 1 n ! ( R p ∧ R T R p ∧ R T ⋯ R p ∧ R T ) = ∑ n = 0 ∞ 1 n ! ( R p ∧ p ∧ ⋯ p ∧ R T ) = R ( ∑ n = 0 ∞ 1 n ! ( p ∧ ) n ) R T = R exp ⁡ ( p ∧ ) R T \exp \left((\boldsymbol{R} \boldsymbol{p})^{\wedge}\right)=\sum_{n=0} ^{\infty}\frac{1}{n !}\left((\boldsymbol{R} \boldsymbol{p})^{\wedge}\right)^{n} =\sum_{n=0} ^{\infty}\frac{1}{n !}\left(\bm R\bm p^{\wedge} \bm R^T \right)^{n} \\ =\sum_{n=0} ^{\infty}\frac{1}{n !} \left( \left(\bm R\bm p^{\wedge} \bm R^T \right) \cdot \left(\bm R\bm p^{\wedge} \bm R^T \right)\cdot \quad \cdots \quad \cdot \left(\bm R\bm p^{\wedge} \bm R^T \right) \right) \\ =\sum_{n=0} ^{\infty}\frac{1}{n !} \left(\bm R\bm p^{\wedge} \bm R^T\bm R\bm p^{\wedge} \bm R^T \cdots \bm R\bm p^{\wedge} \bm R^T \right) \\ =\sum_{n=0} ^{\infty}\frac{1}{n !} \left(\bm R\bm p^{\wedge} \bm p^{\wedge} \cdots\bm p^{\wedge} \bm R^T \right) =\bm R \left(\sum_{n=0} ^{\infty}\frac{1}{n !}\left(\bm p^{\wedge} \right)^{n} \right) \bm R^T =\boldsymbol{R} \exp \left(\boldsymbol{p}^{\wedge}\right) \boldsymbol{R}^{\mathrm{T}} exp((Rp)∧)=n=0∑∞​n!1​((Rp)∧)n=n=0∑∞​n!1​(Rp∧RT)n=n=0∑∞​n!1​((Rp∧RT)⋅(Rp∧RT)⋅⋯⋅(Rp∧RT))=n=0∑∞​n!1​(Rp∧RTRp∧RT⋯Rp∧RT)=n=0∑∞​n!1​(Rp∧p∧⋯p∧RT)=R(n=0∑∞​n!1​(p∧)n)RT=Rexp(p∧)RT

6.1

T W C \bm T_{WC} TWC​的物理意义：某点从相机坐标系到世界坐标系的变换矩阵，当该点为相机坐标系原点时该矩阵表示了相机在世界坐标系中的位姿，其平移部分即为相机在世界坐标系中的坐标。画出 T W C \bm T_{WC} TWC​的平移部分（相机在世界坐标系下坐标）便画出了机器人的轨迹。

6.2

cmake_minimum_required(VERSION 3.10)
project(plotTrajectory)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)

find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)
include_directories("/usr/include/eigen3" ${Sophus_INCLUDE_DIRS} ${Pangolin_INCLUDE_DIRS})

add_executable(plotTrajectory draw_trajectory.cpp)
target_link_libraries(plotTrajectory Sophus::Sophus ${Pangolin_LIBRARIES})
           

#include <string>
#include <iostream>
#include <fstream>
#include "sophus/se3.hpp"
#include <unistd.h>

// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;

// path to trajectory file
string trajectory_file = "../trajectory.txt";


// function for plotting trajectory, don't edit this code
// start point is red and end point is blue
void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>>);

int main(int argc, char **argv) {

    vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses;

    /// implement pose reading code
    // start your code here (5~10 lines)
    ifstream fin(trajectory_file);
    if (!fin) {
        cerr << "trajectory " << trajectory_file << " not found." << endl;
    }
    while (!fin.eof()) {
        double time, tx, ty, tz, qx, qy, qz, qw;
        fin >> time >> tx >> ty >> tz >> qx >> qy >> qz >> qw;
        Sophus::SE3d p1(Eigen::Quaterniond(qw, qx, qy, qz), Eigen::Vector3d(tx, ty, tz));
        poses.push_back(p1);
    }
    fin.close();

    // end your code here

    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}

/*******************************************************************************************/
void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses) {
    if (poses.empty()) {
        cerr << "Trajectory is empty!" << endl;
        return;
    }

    // create pangolin window and plot the trajectory
    pangolin::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 < poses.size() - 1; i++) {
            glColor3f(1 - (float) i / poses.size(), 0.0f, (float) i / poses.size());
            glBegin(GL_LINES);
            auto p1 = poses[i], p2 = poses[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
    }

}
           

运行结果：

7.

cmake_minimum_required(VERSION 3.10)
project(TrajectoryError)

set(CMAKE_CXX_STANDARD 11)
set(CMAKE_BUILD_TYPE "Release")
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
set(CMAKE_LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib)

find_package(Sophus REQUIRED)
find_package(Pangolin REQUIRED)

include_directories("/usr/include/eigen3" ${Pangolin_INCLUDE_DIRS}) #${Sophus_INCLUDE_DIRS}

add_executable(TrajectoryError TrajectoryError.cpp)
target_link_libraries(TrajectoryError Sophus::Sophus ${Pangolin_LIBRARIES}) # ${Sophus_LIBRARIES} Sophus::Sophus
           

#include <iostream>
#include <fstream>
#include <unistd.h>
#include <pangolin/pangolin.h>
#include <sophus/se3.hpp>

using namespace Sophus;
using namespace std;

string groundtruth_file = "../groundtruth.txt";
string estimated_file = "../estimated.txt";

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> 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 rmse
    double rmse = 0;
    for (size_t i = 0; i < estimated.size(); i++) {
        Sophus::SE3d 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::SE3d 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 trajectory
    pangolin::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 truth
            glBegin(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 estimated
            glBegin(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
    }

}
           

运行结果：