视觉SLAM十四讲作业练习（3）

花舞の街道

已于 2022-05-03 19:57:54 修改

阅读量662

点赞数

分类专栏：视觉SLAM 文章标签： c++ 开发语言

于 2022-05-03 19:47:35 首次发布

本文链接：https://blog.csdn.net/ASUNAchan/article/details/124560252

版权

视觉SLAM 专栏收录该内容

18 篇文章 9 订阅

订阅专栏

.5 sophus的使用

书中提供的代码

useSophus.cpp

#include <iostream>
#include <cmath>
#include <Eigen/Core>
#include <Eigen/Geometry>

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

using namespace std;
using namespace Eigen;

int main(int argc, char *argv[])
{
    //沿z轴旋转90度的旋转矩阵
    Matrix3d R = AngleAxisd(M_PI / 2, Vector3d(0, 0, 1)).toRotationMatrix();

    //四元数
    Quaterniond q(R);
    Sophus::SO3d SO3_R(R);   //Sophus::SO3d可以直接从旋转矩阵构造
    Sophus::SO3d SO3_q(q);   //也可通过四元数构造

    cout << "SO(3) from matrix: \n" << SO3_R.matrix() << endl;
    cout << "SO(3) from quaternion: \n" << SO3_q.matrix() << endl;
    cout << "上述二者等价！" << endl;

    //使用对数映射获得它的李代数
    Vector3d so3 = SO3_R.log();
    cout << "so3 = " << so3.transpose() << endl;

    //hat为向量到反对称矩阵
    cout << "so3 hat = \n" << Sophus::SO3d::hat(so3) << endl;
    //vee 为反对称到向量
    cout << "so3 hat vee = \n" << Sophus::SO3d::vee(Sophus::SO3d::hat(so3)).transpose() << endl;
    
    //增量扰动模型的更新
    Vector3d updated_so3(1e-4, 0, 0);    //假设的更新量
    Sophus::SO3d SO3_updated = Sophus::SO3d::exp(updated_so3) * SO3_R;
    cout << "SO3 updated = \n" << SO3_updated.matrix() << endl;

    cout << "---------------------------------------------------------------" << endl;
    //对SE(3)操作大同小异
    Vector3d t(1, 0, 0);    //沿x周平移1
    Sophus::SE3d SE3_Rt(R, t);  //由R,t构造SE(3)
    Sophus::SE3d SE3_qt(q, t);

    cout << "SE(3) from matrix: \n" << SE3_Rt.matrix() << endl;
    cout << "SE(3) from quaternion: \n" << SE3_qt.matrix() << endl;

    //李代数为se(3)为一个六维向量
    typedef Eigen::Matrix<double, 6, 1> Vector6d;
    Vector6d se3 = SE3_Rt.log();
    cout << "se3 = " << se3.transpose() << endl;

    cout << "se3 hat = \n" << Sophus::SE3d::hat(se3) << endl;
    cout << "se3 hat vee = \n" << Sophus::SE3d::vee(Sophus::SE3d::hat(se3)).transpose() << endl;


    Vector6d update_se3;    //更新量
    update_se3.setZero();
    update_se3(0, 0) = 1e-4;
    Sophus::SE3d SE3_updated = Sophus::SE3d::exp(update_se3) * SE3_Rt;
    cout << "SE3 updated = \n" << SE3_updated.matrix() << endl;

    return 0;
}

关键：CMakeLists.txt 的编写

按照书中所写的代码，在我的电脑上会报错

1）需要添加eigen3

有点奇怪，重新试了一下发现不需要了。。。。

2）target_link_libraries(useSophus Sophus::Sophus )必需,不然会报超级多的错误。

解决来源：https://blog.csdn.net/weixin_44387324/article/details/118904871

3）target_link_libraries(useSophus ${Sophus_LIBRARIES})感觉最好加上

cmake_minimum_required(VERSION 2.8)
project(useSophus)

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

include_directories(/usr/local/include/eigen3)
add_executable(useSophus useSophus.cpp)
target_link_libraries(useSophus Sophus::Sophus )
#target_link_libraries(useSophus ${Sophus_LIBRARIES})

7.轨迹的描绘

draw_trajectory.cpp

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

数据读取部分代码

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);	//在vector尾部加入一个数据
    }

    reader.close();

    return true;
}

CMakeLists.txt

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})

8.轨迹的误差

与上节相比需添加轨迹误差的计算，以及将两个轨迹画到同一个图中

误差计算（课本上有）：

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;
}

主程序：

#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;
}

总结与反思：此程序总共分为三部分，一是将文件中的数据读入，二是通过pangolin库将读入的轨迹数据画出来，三是误差算法。欠缺的地方是使用pangoin库画出轨迹，因为看视频的时候没认真看，作业也仅仅是现在才开始练习，接下来学习一下这个可视化绘图库吧。

花舞の街道

关注

0
点赞
踩
2

收藏

觉得还不错? 一键收藏
0
评论
视觉SLAM十四讲作业练习（3）

.5 sophus的使用书中提供的代码useSophus.cpp#include <iostream>#include <cmath>#include <Eigen/Core>#include <Eigen/Geometry>#include "sophus/se3.hpp"#include "sophus/so3.hpp"using namespace std;using namespace Eigen;int main(int
复制链接

扫一扫

专栏目录