【视觉SLAM十四讲】李群、李代数（第三章实践作业）理论推导+代码实现+过程问题解决

2021-06-19 17:33:12 阅读：190 来源： 互联网

标签：wedge right 推导李群 kern SLAM Sophus 1pt left

视觉SLAM十四讲李群、李代数-理论推导+代码实现+过程问题解决

文章目录

视觉SLAM十四讲李群、李代数-理论推导+代码实现+过程问题解决

2. 群的性质(2分，约1小时)

课上我们讲解了什么是群。请根据群定义，求解以下问题：
群（Group）是一种集合加上一种运算的代数结构。我们把集合记作 A A A，运算记作 ⋅ \cdot ⋅ ，那么群可以记作 G = ( A , ⋅ ) G = \left( {A, \cdot } \right) G=(A,⋅)
。群要求这个运算满足以下几个条件：

封闭性: ∀ a 1 , a 2 ∈ A , a 1 ⋅ a 2 ∈ A . \forall {a_1},{a_2} \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {a_1} \cdot {a_2} \in A. ∀a1,a2∈A,a1⋅a2∈A.
结合律: ∀ a 1 , a 2 , a 3 ∈ A , ( a 1 ⋅ a 2 ) ⋅ a 3 = a 1 ⋅ ( a 2 ⋅ a 3 ) . \forall {a_1},{a_2},{a_3} \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} ({a_1} \cdot {a_2}) \cdot {a_3} = {a_1} \cdot ({a_2} \cdot {a_3}). ∀a1,a2,a3∈A,(a1⋅a2)⋅a3=a1⋅(a2⋅a3).
幺元: ∃ a 0 ∈ A , s . t . ∀ a ∈ A , a 0 ⋅ a = a ⋅ a 0 = a . \exists {a_0} \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} s.t.{\kern 1pt} {\kern 1pt} \forall a \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {a_0} \cdot a = a \cdot {a_0} = a. ∃a0∈A,s.t.∀a∈A,a0⋅a=a⋅a0=a.
逆: ∀ a ∈ A , ∃ a − 1 ∈ A , s . t . a ⋅ a − 1 = a 0 . \forall a \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \exists {a^{ - 1}} \in A,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} s.t.{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} a \cdot {a^{ - 1}} = {a_0}. ∀a∈A,∃a−1∈A,s.t.a⋅a−1=a0.

1.{Z,+} 是否为群？若是，验证其满⾜群定义；若不是，说明理由。

答：
{Z,+}是群；
对于{Z,+}，设 a 1 , a 2 , a 3 ∈ Z {a_1},{a_2},{a_3} \in Z a1,a2,a3∈Z。

封闭性：两个整数相加依然是整数——成立
对于 ∀ a 1 , a 2 , ∈ Z \forall{a_1},{a_2}, \in Z ∀a1,a2,∈Z 有 a 1 + a 2 ∈ Z , {a_1} + {a_2} \in Z, a1+a2∈Z, 因此满足封闭性。
结合律：整数的两元加法满足结合律——成立
对于 ∀ a 1 , a 2 , a 3 ∈ Z , ( a 1 + a 2 ) + a 3 = a 1 + ( a 2 + a 3 ) , \forall {a_1},{a_2},{a_3} \in Z,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} ({a_1} + {a_2}) + {a_3} = {a_1} + ({a_2} + {a_3}), ∀a1,a2,a3∈Z,(a1+a2)+a3=a1+(a2+a3), 因此满足结合律
幺元：任何整数加上0，等于它本身——成立
对于 ∃ 0 ∈ Z , \exists {\kern 1pt} 0 \in Z, ∃0∈Z, 对于 ∀ a ∈ Z , \forall a \in Z, ∀a∈Z, 有 a + 0 = 0 + a = a , {\kern 1pt} {\kern 1pt} a + 0 = 0 + a = a, a+0=0+a=a,因此满足幺元
逆:任何整数加上它的相反数等于幺元0，所以逆是其相反数——成立
对于 ∀ a ∈ Z , \forall a \in Z, ∀a∈Z, ∃ − a ∈ Z , \exists {\kern 1pt} - a \in Z, ∃−a∈Z, 使得 a + ( − a ) = 0 , {\kern 1pt} {\kern 1pt} a + \left( { - a} \right) = 0, a+(−a)=0, 因此满足逆
{Z,+} 满足以上四条性质，因此是群。

2. {N,+}是否为群？若是，验证其满⾜群定义；若不是，说明理由。其中Z 为整数集，N 为自然数集。
答： {N,+} 不是群
1.封闭性：两个自然数相加依然是自然数——成立
2.结合律：两个自然数相加可以互换位置——成立
3.幺元：任何自然数与0相加仍然是自然数本身——成立
4.逆: 自然数都是非负数，不存在逆元素与其相加为幺元——不成立
对于 ∀ a ∈ N , \forall a \in N, ∀a∈N, 且 a ≠ 0 , − a ∉ N {\kern 1pt} {\kern 1pt} a \ne 0, - a \notin N a=0,−a∈/N不满足逆的性质要求
因此 {N,+}不是群。

3. 验证向量叉乘的李代数性质(2分，约1小时)

（详解版）链接

4. 推导 S E ( 3 ) {\bf{SE}}\left( 3 \right) SE(3) 的指数映射(2分，约1小时)

（详解版）链接

5. 伴随(2分，约1小时)

在 S O ( 3 ) {\mathbf{SO}}\left( 3 \right) SO(3) 和 S E ( 3 ) {\mathbf{SE}}\left( 3 \right) SE(3) 上，有⼀个东西称为伴随（Adjoint）。下⾯请你证明 S O ( 3 ) {\mathbf{SO}}\left( 3 \right) SO(3) 伴随的性质。对于 S O ( 3 ) {\mathbf{SO}}\left( 3 \right) SO(3) ，有：
R ⁡ exp ⁡ ( p ∧ ) R T = exp ⁡ ( ( R p ) ∧ ) . \operatorname{R} \exp \left( {{p^ \wedge }} \right){{\mathbf{R}}^T} = \exp \left( {{{\left( {{\mathbf{R}}p} \right)}^ \wedge }} \right). Rexp(p∧)RT=exp((Rp)∧).

此时称 A d ( R ) = R Ad\left( R \right)=R Ad(R)=R 。提⽰：⾸先你需要证明 ∀ a ∈ R 3 , \forall a \in {{\mathbf{R}}^3}, ∀a∈R3, R a ∧ R T = ( R a ) ∧ R{a^ \wedge }{R^T}={\left( {Ra} \right)^ \wedge } Ra∧RT=(Ra)∧ 页⾯https://math.stackexchange.com/questions/2190603/derivation-of-adjoint-for-so3提⽰了⼀种简洁的途径。
对于 S E ( 3 ) {\mathbf{SE}}\left( 3 \right) SE(3) ，有：
T exp ⁡ ( ξ ∧ ) T − 1 = exp ⁡ ( ( A d ( T ) ξ ) ∧ ) T\exp \left( {{\xi ^ \wedge }} \right){T^{ - 1}} = \exp \left( {{{\left( {Ad\left( T \right)\xi } \right)}^ \wedge }} \right) Texp(ξ∧)T−1=exp((Ad(T)ξ)∧)
其中 A d ( T ) Ad\left( T \right) Ad(T) 定义为：
在这里插入图片描述

这个性质将在后⽂的Pose Graph优化中⽤到。但是 S E ( 3 ) {\mathbf{SE}}\left( 3 \right) SE(3) 的证明较为复杂，不作要求。完整的 S O ( 3 ) {\mathbf{SO}}\left( 3 \right) SO(3) 和 S E ( 3 ) {\mathbf{SE}}\left( 3 \right) SE(3)性质见1和2。
答：证明
我们先来证明 R a ∧ R T = ( R a ) ∧ R{a^ \wedge }{R^T}={\left( {Ra} \right)^ \wedge } Ra∧RT=(Ra)∧
a ∧ v = a × v {a^ \wedge }v = a \times v a∧v=a×v
我们可以通过使等式的RHS作用于任意向量v来证明该等式：
( R a ) ∧ v = ( R a ) × v = ( R a ) × ( R R − 1 v ) ( R R − 1 = I ) = R [ a × ( R − 1 v ) ] ( d i s t r i b u t i o n l a w ) = R a ∧ R − 1 v ( a s s o c i a t i v e l a w ) \begin{gathered} {\left( {Ra} \right)^ \wedge }v = \left( {Ra} \right) \times v \\ = \left( {Ra} \right) \times (R{R^{ - 1}}v){\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} (R{R^{ - 1}} = I) \\ = R[a \times ({R^{ - 1}}v)]{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} (distribution{\text{ }}law) \\ = R{a^ \wedge }{R^{ - 1}}v{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} (associative{\text{ }}law) \\ \end{gathered} (Ra)∧v=(Ra)×v=(Ra)×(RR−1v)(RR−1=I)=R[a×(R−1v)](distribution law)=Ra∧R−1v(associative law)

此得到：
( R a ) ∧ = R a ∧ R − 1 {\left( {Ra} \right)^ \wedge } = R{a^ \wedge }{R^{ - 1}} (Ra)∧=Ra∧R−1
而 R R R是正交矩阵，因此
( R a ) ∧ = R a ∧ R T {\left( {Ra} \right)^ \wedge } = R{a^ \wedge }{R^T} (Ra)∧=Ra∧RT
法一：

exp ⁡ ( ( R ρ ) ∧ ) = ∑ n = 0 ∞ 1 n ! ( ( R ρ ) ∧ ) n = ∑ n = 0 ∞ 1 n ! ( R ρ ∧ R T ) n = ∑ n = 0 ∞ 1 n ! ( ( R ρ ∧ R T ) ⋅ ( R ρ ∧ R T ) ⋅ ⋯ ⋅ ( R ρ ∧ R T ) ) = ∑ n = 0 ∞ 1 n ! ( R ρ ∧ R T R ρ ∧ R T ⋯ R ρ ∧ R T ) = ∑ n = 0 ∞ 1 n ! ( R ρ ∧ ρ ∧ ⋯ ρ ∧ R T ) = R ( ∑ n = 0 ∞ 1 n ! ( ρ ∧ ) n ) R T = R exp ⁡ ( ρ ∧ ) R T \begin{array}{l} \exp \left( {{{\left( {R\rho } \right)}^ \wedge }} \right) = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {{{\left( {R\rho } \right)}^ \wedge }} \right)}^n}} = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {R{\rho ^ \wedge }{R^T}} \right)}^n}} \\ = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}\left( {\left( {R{\rho ^ \wedge }{R^T}} \right) \cdot \left( {R{\rho ^ \wedge }{R^T}} \right) \cdot {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \cdots {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} \cdot \left( {R{\rho ^ \wedge }{R^T}} \right)} \right)} \\ = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}\left( {R{\rho ^ \wedge }{R^T}R{\rho ^ \wedge }{R^T} \cdots R{\rho ^ \wedge }{R^T}} \right)} \\ = \sum\limits_{n = 0}^\infty {\frac{1}{{n!}}\left( {R{\rho ^ \wedge }{\rho ^ \wedge } \cdots {\rho ^ \wedge }{R^T}} \right)} = R\left( {\sum\limits_{n = 0}^\infty {\frac{1}{{n!}}{{\left( {{\rho ^ \wedge }} \right)}^n}} } \right){R^T} = {\mathop{\rm R}\nolimits} \exp \left( {{\rho ^ \wedge }} \right){R^T} \end{array} exp((Rρ)∧)=n=0∑∞n!1((Rρ)∧)n=n=0∑∞n!1(Rρ∧RT)n=n=0∑∞n!1((Rρ∧RT)⋅(Rρ∧RT)⋅⋯⋅(Rρ∧RT))=n=0∑∞n!1(Rρ∧RTRρ∧RT⋯Rρ∧RT)=n=0∑∞n!1(Rρ∧ρ∧⋯ρ∧RT)=R(n=0∑∞n!1(ρ∧)n)RT=Rexp(ρ∧)RT
法二：
令 ρ = θ a , a \rho = \theta a,{\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} {\kern 1pt} a ρ=θa,a为单位向量
罗德里格斯公式：

exp ⁡ ( ρ ∧ ) = cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + s i n θ a ∧ \exp \left( {{\rho ^ \wedge }} \right) = \cos \theta I + (1 - \cos \theta )a{a^T} + sin\theta {a^ \wedge } exp(ρ∧)=cosθI+(1−cosθ)aaT+sinθa∧
R exp ⁡ ( θ a ∧ ) R T = R ( cos ⁡ θ I + ( 1 − cos ⁡ θ ) a a T + s i n θ a ∧ ) R T = cos ⁡ θ I + ( 1 − cos ⁡ θ ) R a ( R a ) T + sin ⁡ θ R a ∧ R T = cos ⁡ θ I + ( 1 − cos ⁡ θ ) R a ( R a ) T + sin ⁡ θ ( R a ) ∧ = exp ⁡ ( θ ( R a ) ∧ ) = exp ⁡ ( ( R ρ ) ∧ ) \begin{array}{c} {\mathop{\rm R}\nolimits} \exp \left( {\theta {a^ \wedge }} \right){R^T} = R(\cos \theta I + (1 - \cos \theta )a{a^T} + sin\theta {a^ \wedge }){R^T}\\ = \cos \theta I + (1 - \cos \theta )Ra{\left( {Ra} \right)^T} + \sin \theta R{a^ \wedge }{R^T}\\ = \cos \theta I + (1 - \cos \theta )Ra{\left( {Ra} \right)^T} + \sin \theta {\left( {Ra} \right)^ \wedge }\\ = \exp \left( {\theta {{\left( {Ra} \right)}^ \wedge }} \right)\\ = \exp \left( {{{\left( {R\rho } \right)}^ \wedge }} \right) \end{array} Rexp(θa∧)RT=R(cosθI+(1−cosθ)aaT+sinθa∧)RT=cosθI+(1−cosθ)Ra(Ra)T+sinθRa∧RT=cosθI+(1−cosθ)Ra(Ra)T+sinθ(Ra)∧=exp(θ(Ra)∧)=exp((Rρ)∧)
证毕。
在数学中，一个李群 G G G的伴随表示（adjoint representation）或伴随作用（adjoint action）是 G G G 在它自身的李代数上的自然表示。这个表示是群 G G G 在自身上的共轭作用的线性化形式。

6. 轨迹的描绘(2分，约1小时)

我们通常会记录机器⼈的运动轨迹，来观察它的运动是否符合预期。⼤部分数据集都会提供标准轨迹以供参考，如kitti、TUM-RGBD等。这些⽂件会有各⾃的格式，但⾸先你要理解它的内容。记世界坐标系为 W {W} W，机器⼈坐标系为 C {C} C，那么机器⼈的运动可以⽤ T W C {T_{WC}} TWC或 T C W {T_{CW}} TCW来描述。现在，我们希望画出机器⼈在世界当中的运动轨迹，请回答以下问题：

1.事实上，的平移部分即构成了机器⼈的轨迹。它的物理意义是什么？为何画出 T W C {T_{WC}} TWC的平移部分就得到了机器⼈的轨迹？

2.我为你准备了⼀个轨迹⽂件（code/trajectory.txt）。该⽂件的每⼀⾏由若⼲个数据组成，格式为 [ t , t x , t y , t z , q x , q y , q z , q w ] \left[ {t,{t_x},{t_y},{t_z},{q_x},{q_y},{q_z},{q_w}} \right] [t,tx,ty,tz,qx,qy,qz,qw]

其中 t t t为时间， t x , t y , t z {t_x},{t_y},{t_z} tx,ty,tz为的平移部分， q x , q y , q z , q w {q_x},{q_y},{q_z},{q_w} qx,qy,qz,qw是四元数表⽰的 T W C {T_{WC}} TWC的旋转部分， q w {q_w} qw为四元数实部。同时，我为你提供了画图程序draw_trajectory.cpp⽂件。该⽂件提供了画图部分的代码，请你完成数据读取部分的代码，然后书写CMakeLists.txt以让此程序运⾏起来。注意我们需要⽤到Pangolin库来画图，所以你需要事先安装Pangolin（如果你做了第⼀次作业，那么现在已经安装了）。CMakeLists.txt可以参照ORB-SLAM2部分。

答：把draw_trajectory.cpp程序中增加两个头件#include <unistd.h>和#include <Eigen/Core>。

draw_trajectory.cpp对应代码如下：

#include "sophus/so3.h"
#include "sophus/se3.h"
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <Eigen/Core>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>
using namespace std;
// path to trajectory file
string trajectory_file = "/home/zhe/1/lianxi/3/plotTrajectory/trajectory.txt";
class SE3d;
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>>);
int main(int argc, char **argv) {
    vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> poses;
    //文件读取器
    ifstream fin(trajectory_file);
    if (!fin) {
        cerr << "trajectory " << trajectory_file << " not found." << endl;
    }
    //如果eof()返回0，就没读完
    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));
        poses.push_back(p1);
    }
    fin.close();
    // end your code here
    // draw trajectory in pangolin
    DrawTrajectory(poses);
    return 0;
}
/************************************************************************/
void DrawTrajectory(vector<Sophus::SE3, Eigen::aligned_allocator<Sophus::SE3>> 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
    }
}

CmakeLists.txt对应代码如下：

cmake_minimum_required(VERSION 2.8)
project(plotTrajectory)
#添加库
#sophus
#  为使用 sophus，需要使用find_package命令找到它并赋给Sophus_INCLUDE_DIRS
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
#Pangolin生成一个libPangolin动态链接库
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})
include_directories("/usr/include/eigen3")
#编译
add_executable(plotTrajectory draw_trajectory.cpp)
#链接
#target_link_libraries(plotTrajectory Sophus::Sophus)
target_link_libraries(plotTrajectory ${Sophus_LIBRARIES} )
target_link_libraries(plotTrajectory ${Pangolin_LIBRARIES})

运行结果如下所示：

在这里插入图片描述

该图中:轨迹首尾颜色不一样,通过观察,发现是着色函数设置的颜色随位置变化.

7*. 轨迹的误差(2分，约1小时)

本题为附加题。除了画出真实轨迹以外，我们经常需要把SLAM估计的轨迹与真实轨迹相⽐较。下⾯说明⽐较的原理，请你完成⽐较部分的代码实现。设真实轨迹（ground-truth）为 T g {T_g} Tg，估计轨迹 T e {T_e} Te。它们都以 T W C {T_WC} TWC的形式存储，格式同上题。现在，你需要计算估计轨迹的误差。我们假设每⼀个 T g {T_g} Tg都与给定的 T e {T_e} Te对应。那么，对于任意第 i i i个位姿，它的误差可定义为：

e i = ∥ l o g ( T g i − 1 T e i ) ∨ ∥ 2 . {e_i} = \parallel log{(T_{gi}^{ - 1}{T_{ei}})^ \vee }{\parallel _{{\kern 1pt} {\kern 1pt} 2}}. ei=∥log(Tgi−1Tei)∨∥2.

即两个位姿之差的李代数⼆范数。于是，可以定义两条轨迹的均⽅根（Root-Mean-Square-Error,RMSE）误差为：
R M S E ( g , e ) = 1 n ∑ i = 1 n e i 2 . RMSE\left( {g,e} \right) = \sqrt {\frac{1}{n}\sum\limits_{i = 1}^n {e_i^2} } . RMSE(g,e)=n1i=1∑nei2 .

我为你准备了code/ground-truth.txt和code/estimate.txt两条轨迹。请你根据上⾯公式，实现RMSE的计算代码，给出最后的RMSE结果。作为验算，参考答案为：2.207。

注：

1.实际当中的轨迹⽐较还要更复杂⼀些。通常ground-truth由其他传感器记录（如vicon），它的采样频率通常⾼于相机的频率，所以在处理之前还需要按照时间戳对齐。另外，由于传感器坐标系不⼀致，还需要计算两个坐标系之间的差异。这件事也可以⽤ICP解得，我们将在后⾯的课程中讲到。

2.你可以⽤上题的画图程序将两条轨迹画在同⼀个图⾥，看看它们相差多少。

安装Sophus及问题解决

    git clone https://github.com/strasdat/Sophus.git
    cd Sophus
    git checkout a621ff
    mkdir build
    cd build
    cmake ..
    make

安装过程中若报如下错误

安装sophus存在的问题：

CMakeFiles/Sophus.dir/build.make:65: recipe for target ‘CMakeFiles/Sophus.dir/sophus/so2.cpp.o’ failed
make[2]: *** [CMakeFiles/Sophus.dir/sophus/so2.cpp.o] Error 1
make[2]: Leaving directory ‘/home/drew/svo/workspace/Sophus/build’
CMakeFiles/Makefile2:107: recipe for target ‘CMakeFiles/Sophus.dir/all’ failed
make[1]: *** [CMakeFiles/Sophus.dir/all] Error 2

二、解决办法

1、针对cc1: all warnings being treated as errors

则：在 makefile 中找到 -Werror 将其注释掉或者删除

2、修改Sophus下的so2.cpp文件
将下面这个修改一下：

unit_complex_.real() = 1.;
unit_complex_.imag() = 0.;

修改为：

unit_complex_.real(1.);
unit_complex_.imag(0.);

编译时报：

CMakeFiles/plotTrajectory.dir/build.make:62: recipe for target 'CMakeFiles/plotTrajectory.dir/draw_trajectory.cpp.o' failed
make[2]: *** [CMakeFiles/plotTrajectory.dir/draw_trajectory.cpp.o] Error 1
CMakeFiles/Makefile2:67: recipe for target 'CMakeFiles/plotTrajectory.dir/all' failed
make[1]: *** [CMakeFiles/plotTrajectory.dir/all] Error 2
Makefile:83: recipe for target 'all' failed
make: *** [all] Error 2

解决：

1、修改包含的头文件名

#include "sophus/se3.hpp"

改为：

#include "sophus/so3.h"
#include "sophus/se3.h"

2、修改代码

把各个地方的：

Sophus::SO3d
Sophus::SE3d

的“d”都去掉，改为：

Sophus::SO3
Sophus::SE3

3、修改CMakelists.txt

将：

target_link_libraries(useSophus Sophus::Sophus)

改成：

target_link_libraries( useSophus ${Sophus_LIBRARIES} )

**答：**添加的代码主要包括三部分:

读取两个文件ground-truth.txt和estimate.txt。

// path to file
//真实轨迹结果
string trajectory_gd = "/home/zhe/1/lianxi/3/wucha/groundtruth.txt";
//估计轨迹结果
string trajectory_est = "/home/zhe/1/lianxi/3/wucha/estimated.txt";
//利用typedef定义一个容器类型，里面装的是位姿(time 平移，四元数【旋转】)

typedef vector<Sophus::SE3,Eigen::aligned_allocator<Sophus::SE3>> TrajectoryType;
TrajectoryType ReadTrajectory(const string &path);

   TrajectoryType groundtruth=ReadTrajectory(trajectory_gd);
    TrajectoryType estimated=ReadTrajectory(trajectory_est);

TrajectoryType ReadTrajectory(const string &path)
{
    ifstream fin(path);//流入
    TrajectoryType trajectory;
    if(!fin)
    {
        cerr<<"could not find a trajectory file"<<endl;//cerr
        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(qx,qy,qz,qw),Eigen::Vector3d(tx,ty,tz));//得到位姿
        trajectory.push_back(p1);
    }
    return trajectory;
}

2.计算rmse，对于已经得到的两个pose集合，可以通过以下代码计算。

void DrawTrajectory(const TrajectoryType &gt,const TrajectoryType &esti);
assert(!groundtruth.empty()&&!estimated.empty());
    assert(groundtruth.size()==estimated.size());

double rmse=0;
    for(std::size_t i=0;i<estimated.size();i++)
    {
        Sophus::SE3 p1=estimated[i],p2=groundtruth[i];//estimated and groundtruth : vector
        double error =(p2.inverse()*p1).log().norm();//模长，见slam十四讲89页 公式4.44
        rmse+=error*error;//得到模长的平方
    }
    rmse=rmse/double(estimated.size());
    rmse=sqrt(rmse);
    cout<<"绝对误差 rmse= "<< rmse<<endl;
    DrawTrajectory(groundtruth,estimated);
    return  0;
}

3.画图，修改轨迹绘制代码，函数部分代码如下所示：

   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();i++)
        {
            glColor3f(0.0f, 0.0f, 1.0f);//blue  for 真实轨迹
            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();i++)
        {
            glColor3f(1.0f, 0.0f, 0.0f);  // red for 估计轨迹
            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);//休眠5ms

最后运行结果如下：

在这里插入图片描述

/home/zhe/1anxi/3/wucha/cmake-build-debug/plotError
绝对误差 rmse= 2.20727
Process finished with exit code 0

其中，蓝色轨迹表示真实值，红色轨迹表示估计值

完整代码如下：

1、draw_trajectory_error.cpp对应代码


#include "sophus/so3.h"
#include "sophus/se3.h"
#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>
#include <Eigen/Core>
#include <Eigen/Geometry>


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

using namespace Sophus;
using namespace std;

// path to file
//真实轨迹结果
string trajectory_gd = "/home/zhe/1/lianxi/3/wucha/groundtruth.txt";
//估计轨迹结果
string trajectory_est = "/home/zhe/1/lianxi/3/wucha/estimated.txt";

//利用typedef定义一个容器类型，里面装的是位姿(time 平移，四元数【旋转】)
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(trajectory_gd);
    TrajectoryType estimated=ReadTrajectory(trajectory_est);
    //assert宏的原型定义在<assert.h>中，其作用是如果它的条件返回错误，则终止程序执行，原型定义:
    //#include <assert.h>
    //void assert( int expression );
    //　assert的作用是现计算表达式 expression ，如果其值为假（即为0）
    // 那么它先向stderr打印一条出错信息，然后通过调用 abort 来终止程序运行
    assert(!groundtruth.empty()&&!estimated.empty());
    assert(groundtruth.size()==estimated.size());
    //计算 绝对轨迹误差 rmse
    //size_t它是一种“整型”类型，里面保存的是一个整数，就像int, long那样。
    // 这种整数用来记录一个大小(size)。size_t的全称应该是size type，就是说“一种用来记录大小的数据类型”
    //通常我们用sizeof(XXX)操作，这个操作所得到的结果就是size_t类型。
    // 因为size_t类型的数据其实是保存了一个整数，所以它也可以做加减乘除，也可以转化为int并赋值给int类型的变量
    //示例代码：
    //int i;                   // 定义一个int类型的变量i
    //size_t size = sizeof(i); // 用sizeof操作得到变量i的大小，这是一个size_t类型的值
    //                         // 可以用来对一个size_t类型的变量做初始化
    //i = (int)size;           // size_t类型的值可以转化为int类型的值
    double rmse=0;
    for(std::size_t i=0;i<estimated.size();i++)
    {
        Sophus::SE3 p1=estimated[i],p2=groundtruth[i];//estimated and groundtruth : vector
        double error =(p2.inverse()*p1).log().norm();//模长，见slam十四讲89页 公式4.44
        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<<"could not find a trajectory file"<<endl;//cerr
        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(qx,qy,qz,qw),Eigen::Vector3d(tx,ty,tz));//得到位姿
        trajectory.push_back(p1);
    }
    return trajectory;
}
void DrawTrajectory(const TrajectoryType &gt,const TrajectoryType &esti)
{
// 创建 pangolin 窗口，画轨迹
    //creat and set titles 、 width 、height of windows
    pangolin::CreateWindowAndBind(" Trajectory form truth and estimated ", 1024, 768);
    //glEnable(GL_DEPTH_TEST)启用了之后，OpenGL在绘制的时候就会检查，当前像素前面是否有别的像素，如果别的像素挡道了它，那它就不会绘制，也就是说，OpenGL就只绘制最前面的一层。
    //当我们需要绘制透明图片时，就需要关闭它glDisable(GL_DEPTH_TEST);
    //并且打开混合 glEnable(GL_BLEND);
    glEnable(GL_DEPTH_TEST);
    glEnable(GL_BLEND);
    //glBlendFunc(GLenum sfactor,GLenum dfactor);
    //源因子和目标因子是可以通过glBlendFunc函数来进行设置的。
    // glBlendFunc有两个参数，前者sfactor表示源因子，后者dfactor表示目标因子。
    //前者sfactor表示源颜色，后者dfactor表示目标颜色
    //GL_SRC_ALPHA：表示使用源颜色的alpha值来作为因子
    //GL_ONE_MINUS_SRC_ALPHA：表示用1.0减去源颜色的alpha值来作为因子（1-alpha)
    glBlendFunc(GL_SRC_ALPHA,GL_ONE_MINUS_SRC_ALPHA);

    //OpenGlRenderState 创建一个相机的观察视图，模拟相机
    pangolin::OpenGlRenderState s_cam(//定义投影和初始模型视图矩阵
            //ProjectionMatrix 前两个参数是相机的宽高，紧接着四个参数是相机的内参，最后两个是最近和最远视距
            pangolin::ProjectionMatrix(1024,768,500,500,512,389,0.1,1000),
            //ModelViewLookAt 前三个参数是相机的位置，紧接着三个是相机所看的视点的位置，最后三个参数是一个向量，表示相机的的朝向
            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(1.0),1.0,-1024.0f/768.0f).
            //SetBounds() 前四个参数是视图在视窗中的范围（下 上 左 右），建议动手改改参数就明白了
            SetHandler(new pangolin::Handler3D(s_cam));
    //     SetHandle设置相机的视图句柄，需要用它来显示前面设置的 “相机” 所 “拍摄” 的内容
    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();i++)
        {
            glColor3f(0.0f, 0.0f, 1.0f);//blue  for 真实轨迹
            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();i++)
        {
            glColor3f(1.0f, 0.0f, 0.0f);  // red for 估计轨迹
            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);//休眠5ms
    }
}

2、CMakeLists.txt对应代码

cmake_minimum_required(VERSION 2.8)
project(wucha)

set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")

#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)
#添加库
#sophus
#  为使用 sophus，需要使用find_package命令找到它并赋给Sophus_INCLUDE_DIRS
find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIRS})
#Pangolin生成一个libPangolin动态链接库
find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

include_directories("/usr/include/eigen3")
#编译
add_executable(plotError draw_trajectory_error.cpp)
#链接
#target_link_libraries(plotError Sophus::Sophus)
target_link_libraries(plotError ${Sophus_LIBRARIES} )

target_link_libraries(plotError ${Pangolin_LIBRARIES})

标签：wedge,right,推导,李群,kern,SLAM,Sophus,1pt,left
来源： https://blog.csdn.net/weixin_38493195/article/details/118053754

本站声明： 1. iCode9 技术分享网（下文简称本站）提供的所有内容，仅供技术学习、探讨和分享；
2. 关于本站的所有留言、评论、转载及引用，纯属内容发起人的个人观点，与本站观点和立场无关；
3. 关于本站的所有言论和文字，纯属内容发起人的个人观点，与本站观点和立场无关；
4. 本站文章均是网友提供，不完全保证技术分享内容的完整性、准确性、时效性、风险性和版权归属；如您发现该文章侵犯了您的权益，可联系我们第一时间进行删除；
5. 本站为非盈利性的个人网站，所有内容不会用来进行牟利，也不会利用任何形式的广告来间接获益，纯粹是为了广大技术爱好者提供技术内容和技术思想的分享性交流网站。

ICode9