深蓝学院视觉SLAM第三次习题

最新推荐文章于 2022-03-28 22:30:03 发布

小白的进修之路

最新推荐文章于 2022-03-28 22:30:03 发布

阅读量2k

点赞数 1

分类专栏：视觉SLAM十四讲

本文链接：https://blog.csdn.net/weixin_44218240/article/details/105793559

版权

这篇博客主要涉及视觉SLAM的习题，包括群的性质验证、向量叉乘的李代数性质、SE(3)指数映射的推导、伴随性质证明、轨迹描绘的物理意义解析以及轨迹误差的比较。通过解决这些问题，深入理解SLAM中的数学基础和轨迹表示。

摘要由CSDN通过智能技术生成

今天的习题好像要敲很多代码啊

1.群的性质

请根据群定义，求解以下问题：

{Z,+} 是否为群？若是，验证其满足群定义；若不是，说明理由。
{N,+} 是否为群？若是，验证其满足群定义；若不是，说明理由。
其中 Z 为整数集，N 为自然数集。
1问：（结果是我在word里打出来又粘过来的，公式好麻烦）

2问： {N,+}不为群，不满足逆的条件

2.验证向量叉乘的李代数性质

验证 g = (R 3 ,R,×) 构成李代数
在这里插入图片描述

3.推导 SE(3) 的指数映射

完成 SE(3)指数映射部分，有关左雅可比的详细推导。
在这里插入图片描述

4.伴随

在 SO(3) 和 SE(3) 上，有一个东西称为伴随（Adjoint）。下面请你证明 SO(3) 伴随的性质。
在这里插入图片描述

5.轨迹的描绘

记世界坐标系为 W，机器人坐标系为 C，那么机器人的运动可以用 Twc或 Tcw来描述。现在，我们希望画出机器人在世界当中的运动轨迹，请回答以下问题：

事实上，Twc的平移部分即构成了机器人的轨迹。它的物理意义是什么？为何画出 Twc的平移部分就得到了机器人的轨迹？

Twc的旋转部分表示机器人的移动方向，平移部分代表二维平面上的移动，即机器人的轨迹。Twc表示由相机坐标系观察世界坐标系，世界坐标系是不动的，即机器人相对世界坐标系原点运动，在观察者看来即是机器人的轨迹。

我为你准备了一个轨迹文件（code/trajectory.txt）。该文件的每一行由若干个数据组成，格式为
[t,t x ,t y ,t z ,q x ,q y ,q z ,q w ],其中 t 为时间，t x ,t y ,t z 为 T WC 的平移部分，q x ,q y ,q z ,q w 是四元数表示的 Twc的旋转部分，q w为四元数实部。

draw_trajectory.cpp

#include <sophus/se3.hpp>
#include <string>
#include <iostream>
#include <fstream>
#include <Eigen/Core>
#include <Eigen/Geometry>
// need pangolin for plotting trajectory
#include <pangolin/pangolin.h>

using namespace std;
using namespace Eigen;
// 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::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)
    {
   
      cout<<"can't find file at "<<trajectory_file<<endl;
      return 1;
    }
    while(!fin.eof())
    {
   
      double t,tx,ty,tz,qx,qy,qz,qw;
      fin>>t>>tx>>ty>>tz>>qx>>qy>>qz>>qw;
      Quaterniond q(qw,qx,qy,qz);
      Vector3d v(tx,ty,tz);
      Sophus::SE3 SE3_qt(q,v);
      poses.push_back(SE3_qt);
    }
    cout<<"read total "<<poses.size()<<"pose entries"<<endl;

    // 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