slam十四讲第3章作业

slam十四讲第3章作业_第1张图片

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,Z2Z, 满足 Z1+Z2Z 结合性Z1,Z2,Z3Z, 满足 (Z1+Z2)+Z3=Z1+(Z2+Z3) 幺元Z0=0,Z1Z, 满足 Z1+0=0+Z1=Z1 Z1Z,(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 0N1N,(N1)N, 满足 N1+(N1)=0

slam十四讲第3章作业_第2张图片

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,YR3,X×YR3
  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,ZR3,a,bR, 有:
    ( 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 XR3,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(ac)a(bc)a×(b×c)=b(ac)c(ab) (定理证明如下,就是展开计算)
    slam十四讲第3章作业_第3张图片
    ∀ 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,ZR3,X×(Y×Z)+Y×(Z×X)+Z×(X×Y)=Y(XZ)Z(XY)+Z(YX)X(YZ)+X(ZY)Y(ZX)=0

slam十四讲第3章作业_第4张图片

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} aa=aaTI,aaa=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=0n!1(θa)n=I+θa+2!1θ2aa+3!1θ3aaa+4!1θ4(a)4+=aaTaa+θa+2!1θ2aa3!1θ3a4!1θ4(a)2+=aaT+(θ3!1θ3+5!1θ5)a(12!1θ2+4!1θ4)aa=aa+I+sinθacosθaa=(1cosθ)aa+I+sinθa=cosθI+(1cosθ)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θ24!1θ4+)(a)+θ1(3!1θ351θ5+)(a)2+I=θ1(1cosθ)(a)+θθsinθ(aaTI)+I=θsinθI+(1θsinθ)aaT+θ1cosθ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=0n!1(ϕ)n0Tn=0(n+1)!1(ϕ)nρ1[R0TJρ1]=T

slam十四讲第3章作业_第5张图片slam十四讲第3章作业_第6张图片slam十四讲第3章作业_第7张图片

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} RaRT=(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} vR3,(Ra)v=(Ra)×v=(Ra)×(RR1v)=R[a×(R1v)]=RaR1v

对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=0n!1((Rp))n=n=0n!1(RpRT)n=n=0n!1((RpRT)(RpRT)(RpRT))=n=0n!1(RpRTRpRTRpRT)=n=0n!1(RpppRT)=R(n=0n!1(p)n)RT=Rexp(p)RT

slam十四讲第3章作业_第8张图片

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 
#include 
#include 
#include "sophus/se3.hpp"
#include 

// need pangolin for plotting trajectory
#include 

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
    }

}

运行结果:
slam十四讲第3章作业_第9张图片

slam十四讲第3章作业_第10张图片

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 
#include 
#include 
#include 
#include 

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
    }

}

运行结果:
slam十四讲第3章作业_第11张图片
在这里插入图片描述

你可能感兴趣的:(slam十四讲作业,slam,线性代数,矩阵)