一定把vslam拿下！！！-Ch3李群李代数（小白版）

已于 2022-11-06 10:27:38 修改
阅读量194 收藏
点赞数
文章标签： 线性代数 计算机视觉
于 2022-11-02 13:30:05 首次发布
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/monexxxxx/article/details/127649685
版权

一定把vslam拿下！！！（小白版）// 部分内容转载自他人博客！侵删

深蓝学院视觉十四讲课后作业——ch3 李群李代数
本文记录了学习的部分笔记心得与课后习题 主要用途为学习巩固、方便自己查阅
部分内容参阅互联网或者书籍知识，部分图片来自网络，欢迎批评指正
如有问题，可及时与我沟通讨论！

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

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

1、{Z,+} 是否为群？若是，验证其满足群定义；若不是，说明理由。
2、{N,+} 是否为群？若是，验证其满足群定义；若不是，说明理由。
其中 Z 为整数集，N 为自然数集。
3、解释什么是阿贝尔群。并说明矩阵及乘法构成的群是否为阿贝尔群。
请添加图片描述

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

)
请添加图片描述

推导 SE(3) 的指数映射 (1 分，约 1 小时)

在这里插入图片描述
请添加图片描述

伴随 (2 分，约 1 小时)

在这里插入图片描述
请添加图片描述

常见函数的求导应用 （2 分，约 2 小时）

在这里插入图片描述
暂时未做…

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

在这里插入图片描述
代码部分如下：//模板类sophus

#include <string>
#include <iostream>
#include <fstream>
#include <unistd.h>

#include <sophus/se3.hpp>

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

bool ReadTrajectory(const string &trajectory_file_path, vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses);

int main(int argc, char **argv) {
    /*
        vector<Sophus::SE3, Eigen::aligned_allocator< Sophus::SE3>> poses;
        //这句实际上表达的是vector< Sophus::SE3> poses的意思
        但是在Eigen管理内存和C++11中的方法是不一样的，所以需要单独强调元素的内存分配和管理。
        (特别注意Eigen库数据结构内存对齐问题)
    */
    vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses; //vector容器

    // 读取文件
    if (!ReadTrajectory(trajectory_file, poses)) {
        cout << "读取失败...\n";
    } 
    else {
        cout << "读取成功!\n";
    }

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

bool ReadTrajectory(const string &trajectory_file_path, vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses){
    // 读文件
    // 2创建流对象
    ifstream reader;

    // 3.1打开文件
    reader.open(trajectory_file_path, ios::in);
    
    // 3.2判断是否打开成功
    if (!reader.is_open()) {
        cout << "文件打开失败！！！ " << trajectory_file_path << '\n';
        return false;
    }

    // 4读数据
    // 将读到的数据储存在SE(3)矩阵中
    Eigen::Quaterniond q(1,0,0,0);  //四元数
    Eigen::Vector3d t(0,0,0);       //平移
    double time;

    while (!reader.eof())   //eof() 判断文件是否为空或者是否读到文件结尾
    {   //文件存储的格式为[t,tx,ty,tz,qx,qy,qz]
        reader >> time;
        // read t
        reader >> t[0];
        reader >> t[1];
        reader >> t[2];
        // read q
        reader >> q.x();
        reader >> q.y();
        reader >> q.z();
        reader >> q.w();
        Sophus::SE3d SE3 (q,t);
        poses.push_back(SE3);
    }

    reader.close();

    return true;
}

void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses) {
    if (poses.empty()) {
        cerr << "Trajectory is empty!\n";
        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
    }
}

CMakeList如下：

cmake_minimum_required(VERSION 3.1)
project(trajectory)

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIR})

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

运行结果如下：
在这里插入图片描述

代码实现过程中出了一些意外的问题，本身使用的是vscode来进行代码编写编译，结果一直运行不起来 显示如下图所示错误，进行了 许多修改依然这般，重新尝试了usesophus也无法运行，试着直接用cmake进行编译均运行成功 ，说明是vscode中的相关设置不成功，但仍未找到原因…
在这里插入图片描述

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

本题为附加题。
除了画出真实轨迹以外，我们经常需要把 SLAM 估计的轨迹与真实轨迹相比较。下面说明比较的原
理，请你完成比较部分的代码实现。
在这里插入图片描述
2. 实际当中的轨迹比较还要更复杂一些。通常 ground-truth 由其他传感器记录（如 vicon），它的采
样频率通常高于相机的频率，所以在处理之前还需要按照时间戳对齐。另外，由于传感器坐标系不
一致，还需要计算两个坐标系之间的差异。这件事也可以用 ICP 解得，我们将在后面的课程中讲
到。
3. 你可以用上题的画图程序将两条轨迹画在同一个图里，看看它们相差多少。
运行结果如下：
在这里插入图片描述
代码如下：

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

using namespace std;

string estimation_file_path = "../estimated.txt";
string groundtruth_file_path = "../groundtruth.txt";

bool ReadTrajectory(const string &trajectory_file_path, std::vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses){
    // 读文件
    // 2创建流对象
    ifstream reader;

    // 3.1打开文件
    reader.open(trajectory_file_path, ios::in);
    
    // 3.2判断是否打开成功
    if (!reader.is_open()) {
        cout << "文件打开失败！！！ " << trajectory_file_path << '\n';
        return false;
    }

    // 4读数据
    // 将读到的数据储存在SE(3)矩阵中
    Eigen::Quaterniond q(1,0,0,0);  //四元数
    Eigen::Vector3d t(0,0,0);       //平移
    double time;

    while (!reader.eof())   //eof() 判断文件是否为空或者是否读到文件结尾
    {   //文件存储的格式为[t,tx,ty,tz,qx,qy,qz]
        reader >> time;
        // read t
        reader >> t[0];
        reader >> t[1];
        reader >> t[2];
        // read q
        reader >> q.x();
        reader >> q.y();
        reader >> q.z();
        reader >> q.w();
        Sophus::SE3d SE3 (q,t);
        poses.push_back(SE3);
    }

    reader.close();

    return true;
}

double ComputeRMSE( const vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>>& poses_estimated,
                    const vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>>& poses_groundtruth) {
    //算法参考课本
    double rmse = 0.0;
    for (int i = 0; i < poses_estimated.size(); i++) {
        Sophus::SE3d p_true = poses_groundtruth[i];
        Sophus::SE3d p_esti = poses_estimated[i];
        // Compute norm of error
        double error = (p_true.inverse() * p_esti).log().norm();
        rmse += error * error;
    }

    rmse = sqrt(rmse / double(poses_estimated.size()));

    return rmse;
}

void DrawTrajectory(vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses_estimated,
                    vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> &poses_groundtruth) 
{
    if (poses_estimated.empty()) {
        cerr << "Trajectory_esti is empty!\n";
        return;
    }
    if (poses_groundtruth.empty()) {
        cerr << "Trajectory_true is empty!\n";
        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_estimated.size() - 1; i++) {
            glColor3f(1 - (float) i / poses_estimated.size(), 0.0f, (float) i / poses_estimated.size());
            glBegin(GL_LINES);
            auto p1 = poses_estimated[i], p2 = poses_estimated[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 < poses_groundtruth.size() - 1; i++) {
            glColor3f(1 - (float) i / poses_groundtruth.size(), 0.0f, (float) i / poses_groundtruth.size());
            glBegin(GL_LINES);
            auto p1_t = poses_groundtruth[i], p2_t = poses_groundtruth[i + 1];
            glVertex3d(p1_t.translation()[0], p1_t.translation()[1], p1_t.translation()[2]);
            glVertex3d(p2_t.translation()[0], p2_t.translation()[1], p2_t.translation()[2]);
            glEnd();
        }
        pangolin::FinishFrame();
        usleep(5000);   // sleep 5 ms
    }
}

int main(int argc, char** argv) {
    vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_estimated;
    vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> poses_groundtruth;

    // read estimation file
    if (!ReadTrajectory(estimation_file_path, poses_estimated)) {
        cout << "Reading estimation file fails...\n";
    } 
    else {
        cout << "Reading estimation file succeeds!\n";
    }

    // read groundtruth file
    if (!ReadTrajectory(groundtruth_file_path, poses_groundtruth)) {
        cout << "Reading groundtruth file fails...\n";
    } 
    else {
        cout << "Reading groundtruth file succeeds!\n";
    }

    cout << ComputeRMSE(poses_estimated, poses_groundtruth) << '\n';

    DrawTrajectory(poses_estimated,poses_groundtruth);

    return 0;
}

CMakelist如下：

cmake_minimum_required(VERSION 3.1)
project(trajectory_error)

find_package(Pangolin REQUIRED)
include_directories(${Pangolin_INCLUDE_DIRS})

find_package(Sophus REQUIRED)
include_directories(${Sophus_INCLUDE_DIR})

add_executable(trajectory_error trajectory_error.cpp)
target_link_libraries(trajectory_error Sophus::Sophus )
target_link_libraries(trajectory_error ${Pangolin_LIBRARIES} ${Sophus_LIBRARIES})
monexxxxx
关注
  • 0
    点赞
  • 踩
  • 0
    收藏
    觉得还不错? 一键收藏
  • 0
    评论
CV之VSLAM：SLAM/VSLAM的简介、视觉SLAM的五大流程之详细攻略
近期请国内外头部出版社可尽快私信博主！——心比天高，仗剑走天涯，保持热爱，奔赴向梦想！低调，谦虚，自律，反思，成长，还算是比较正能量的博主，公益免费传播……内心特别想在AI界做出一些可以推进历史进程影响力的东西(兴趣使然，有点小情怀，也有点使命感呀)…
02-29 4076
SLAM：simultaneous localization and mapping, 即时定位与地图构建。1、思路：SLAM是同步定位与地图构建，是指根据传感器的信息，一边计算自身位置，一边构建环境地图的过程。2、目的：解决在未知环境下，运动时的定位与地图构建问题。3、问题描述：将一个机器人，放入未知环境中的未知位置，是否有办法让机器人一边移动，一边逐步描绘出此环境完全的地图。(1)、其中完全的地图，是指不受障碍行进到房间可进入的每个角落。VSLAM则更为高级，是基于。
视觉slam学习笔记以及课后习题《第三讲李群李代数》
m0_61238051的博客
10-12 758
前言 这篇博客主要记录了我在深蓝学院视觉slam课程中的课后习题，因为是为了统计知识点来方便自己以后查阅，所以有部分知识可能不太严谨，如果给大家造成了困扰请见谅，大家发现了问题也可以私信或者评论给我及时改正，欢迎大家一起探讨。其他章的笔记可以在我的首页文章中查看。 整个作业的代码和文档都可以参考我的GitHub存储库GitHub - 1481588643/vslam 如果想要了解视觉slam其他章节的内容，也可以查阅以下链接 https://blog.csdn.net/m0_61238051/ar.
SLAM: VSLAM扫盲之旅
alppkk4545的博客
10-26 490
在《机器人手册》 第2卷，这本书里面，第23章讲述了：三维视觉和基于视觉的实时定位于地图重建。在第37章里面，讲述了 同时定位与建图。指出了SLAM的三种基本方法。 一种是EKF的方法，但由于性能的约束，逐渐变得非主流；第二种基于图表示，成功使用稀疏非线性优化方法解决SLAM问题，成为主要方法；第三种也是...
SLAM: SLAM基本流程—VSLAM扫盲之旅
热门推荐
wishchinYang的专栏
10-26 1万+
在很多机器人的论文和书籍里面，劈头第一页即是，经典的SLAM视觉框架是过去十几年前已经成熟的研究结果，这个框架和算法本身已经没有太多理论可以操作的空间。封杀了很多人的SLAM科研之路，把SLAM当一个工程看待。 在《机器人手册》 第2卷，这本书里面，第23章讲述了：三维视觉和基于视觉的实时定位于地图重建。在第37章里面，讲述了 同时定位与建图。指出了SLAM的三种基本...
SLAM 介绍
武溪嵌人
10-25 5194
http://www.slamcn.org/index.php/%E9%A6%96%E9%A1%B5 首页 目录  [隐藏]  1 SLAM 介绍 1.1 什么是SLAM1.2 SLAM与视觉里程计（Visual Odometry）1.3 SLAM和SfM 2 主流开源SLAM方案 2.1 视觉传感器2.2 激光传感器2.3
vslam: 扫盲之旅(转载)
rainbowbirds_aes的博客
07-24 349
https://blog.csdn.net/oSunBo/article/details/70306009
vslam-orbbec-camera-astra-pro
07-03
在视觉slam领域中，奥比中光的体感深度相机应用较广，但是它的安装资源整合比较耗时，为此，我特地将所有需要的安装包压缩在一起，也写了对应博客，方便大家进行安装，希望大家能够顺利进行slam导航及避障学习。
stella-vslam所有依赖包
03-30
VSLAM（Visual Simultaneous Localization and Mapping，视觉同时定位与建图）是机器人和自动驾驶领域中的关键技术之一，它允许设备在未知环境中通过摄像头输入实时地构建地图并确定自身位置。Stella VSLAM是一个...
SLAM-良好线切割-转为PL-SLAM等线辅助VSLAM设计的增强模块-辅助姿态优化-优质项目实战.zip
03-17
在这个名为"SLAM-良好线切割-转为PL-SLAM等线辅助VSLAM设计的增强模块-辅助姿态优化-优质项目实战"的压缩包中，我们看到的是一个针对SLAM算法进行优化和实践的项目，特别是关注了线特征的提取、PL-SLAM方法以及姿态...
论文研究-An Omni-directional vSLAM based on Spherical Camera Model and 3D Modeling.pdf
08-19
一种基于球形摄像机模型和3D建模的全向机器人SLAM，佟国峰，吴子章，本文针对未知环境的机器人自主定位与地图绘制，利用全向图像采集设备，获得球面全景图像，以球形摄像机模型的视图理论和激光建模
基于Intel Movidius Myriad 2(MA2150)的双目VSLAM空间定位方案-电路方案
04-20
但随着最近几年计算机视觉技术的快速发展，SLAM技术越来越多的应用于家用机器人、无人机、AR设备，基于视觉的Visual SLAM（简称VSLAM）逐渐开始崭露头角。VSLAM的技术框架主要包括传感器数据预处理、前端、后端、...
SLAM学习笔记——（一）SLAM分类
bingoplus的博客
12-02 1万+
SLAM（ Simultaneous Localization and Mapping）是指同步定位与地图构建。定位和建图两个问题相互依赖，准确的定位依赖于正确的地图，而构建正确的地图又需要准确的定位，这是一个迭代的过程。 目前的SLAM根据硬件设备的不同主要有两种： 基于激光雷达的SLAM 基于激光雷达构建场景是古老并且可信的方法，激光雷达也是精确场景使用最多的SLAM传感器。它们提供机器
vslam资源
zjguilai的博客
04-28 2597
之前曾经总结过关于SLAM的资源，但感觉有些零零散散，在此进行补充与完善，分享给大家。 SLAM，全称叫做Simultaneous Localization and Mapping，中文叫做同时定位与建图。 二 干货 1.PnP算法简介与代码解析（视频+PPT课件+代码解析） 链接https://pan.baidu.com/s/1d9e0FaIvK_s8m1pMJvNXtg 密码：hf22 2.L...
SLAM入门学习资料分享
weixin_33877885的博客
12-02 287
参考：http://blog.csdn.net/xidianzhimeng/article/details/52447335 介绍 随着信息技术的飞速发展，SLAM（即时定位与地图构建）技术已经在多个领域发挥着不可替代的作用，包括无人驾驶、增强现实、三维重建等等。常言道“师父领进门，修行在个人”，但是对于还没有入门的小白来讲，SLAM怎样入门是一...
VSLAM就是视觉SLAM
诗筱涵的博客
06-10 479
现在你明白为什么有一本书叫《视觉SLAM十四讲》《移动机器人的SLAM与VSLAM》
vSLAM 经典算法简述
学习交流
01-15 3479
初稿。。。。。。。。。。。。。 vSLAM 的核心算法流程 预备阶段，包括传感器的选型和各种标定。Visual SLAM 自 PTAM 算法以来，框架基本趋于固定。通常包括 3 个线程，前端 tracking 线程、后端 mapping 优化线程、闭环检测（loop closure）线程。 前端 tracking 线程主要涉及到： 特征的提取、特征的匹配；（核心：特征点描述子 SURF SIFT...
麻省理工学院 - MIT - 线性代数学习笔记
最新发布
m0_52695557的博客
07-20 439
由上式可知，我们把左边的矩阵（初等矩阵）当成3个1*3的行向量，右边的结果的第一行=左边第一行的第一个数 * 中间第一行 + 左边第一行的第二个数 * 中间第二行 + 左边第一行的第三个数 *中间第三行，所以结果为：1,0,0，以此类推第二行为-3,1,0（-3 * 中间第一行+1 * 中间第二行），第三行为0,0,1。如果还是不明白，因为我们已经知道A * A逆=A逆 * A=E，所以下面那个式子还可以写成，A转置的逆*A的转置=E，逆矩阵又是唯一的，所以A逆的转置=A转置的逆。（同理也可以去掉列一）
【线性代数】矩阵变换
m0_56997192的博客
07-20 218
1，什么是对角矩阵表示将矩阵进行伸缩（反射）变换，仅沿坐标轴方向伸缩（反射）变换。
微积分-微分应用2（平均值定理）
weixin_45911156的博客
07-20 533
平均值定理
VSLAM RANSAC
09-02
VSLAM（Visual Simultaneous Localization and Mapping）是一种同时进行定位和地图构建的技术。RANSAC（Random Sample Consensus）是一种用于拟合模型的迭代方法，常用于计算机视觉和图像处理中的特征匹配和参数估计...

“相关推荐”对你有帮助么？

  • 非常没帮助
  • 没帮助
  • 一般
  • 有帮助
  • 非常有帮助
提交
评论
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值