视觉SLAM理论与实践第三讲习题

1、是

2、是

（都满足封闭性、结合率、么元、逆）

证明：

 

证明：

 

 

证明：

 

 

1、

物理意义：中旋转部分表示机器人方向，平移部分表示机器人的位移

是相机坐标系相对于世界坐标系的变换矩阵，世界坐标系是不发生变化的，即机器人绕世界坐标系原点运动，平移部分就是机器人从一个地方沿某个方向移动的距离，把这些距离都记录下来，就是机器人的轨迹了。

2、

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

project(draw_trajectory)

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

#include_directories("/usr/include/eigen3")
find_package(Eigen3 3.1.0 REQUIRED)
include_directories(${Eigen3_INCLUDE_DIRS})



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

add_executable(draw_trajectory draw_trajectory.cpp)

target_link_libraries(draw_trajectory ${Pangolin_LIBRARIES} ${Sophus_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS})

draw_trajectory.cpp

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)
    {
            cout << "trajectory_file not found" << 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(qx, qy, qz,qw);
            Vector3d v(tx, ty, tz);
            Sophus::SE3d p(q, v);
            poses.push_back(p);
    }

轨迹图

 

答案：RMSE = 2.20727

 

CMakeLists.txt

cmake_minimum_required(VERSION 2.8)

#project(draw_trajectory)
project(compute_rmse)

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

#include_directories("/usr/include/eigen3")
find_package(Eigen3 3.1.0 REQUIRED)
include_directories(${Eigen3_INCLUDE_DIRS})



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

#add_executable(draw_trajectory draw_trajectory.cpp)
add_executable(compute_rmse draw_trajectory2.cpp)

#target_link_libraries(draw_trajectory ${Pangolin_LIBRARIES} ${Sophus_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS})
target_link_libraries(compute_rmse ${Pangolin_LIBRARIES} ${Sophus_INCLUDE_DIRS} ${Eigen3_INCLUDE_DIRS})

compute_rmse.cpp

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

// path to trajectory file
string estimated_file = "./../estimated.txt";
string groundtruth_file = "./../groundtruth.txt";

// function for plotting trajectory, don't edit this code
// start point is red and end point is blue

typedef vector<Sophus::SE3d, Eigen::aligned_allocator<Sophus::SE3d>> TrajectoryType;

void DrawTrajectory(const TrajectoryType &gru, const TrajectoryType &est);

TrajectoryType ReadTrajectory(const string &path);

int main(int argc, char **argv) {

    TrajectoryType estimated = ReadTrajectory(estimated_file);
    TrajectoryType groundtruth = ReadTrajectory(groundtruth_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;


    // draw trajectory in pangolin
    DrawTrajectory(groundtruth, estimated);
    return 0;
}

/*******************************************************************************************/
void DrawTrajectory(const TrajectoryType &gru, const TrajectoryType &est)
{

    // 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 < gru.size() - 1; i++)
        {
            glColor3f(0.0f, 0.0f, 1.0f);
            glBegin(GL_LINES);
            auto p1 = gru[i], p2 = gru[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 < est.size() - 1; i++)
        {
                glColor3f(1.0f, 0.0f, 0.0f); //red for estimated
                glBegin(GL_LINES);
                auto p1 = est[i], p2 = est[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
    }
}

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;
                Eigen::Quaterniond q(qx, qy, qz, qw);
                Eigen::Vector3d v(tx, ty, tz);
                Sophus::SE3d p(q, v);
                trajectory.push_back(p);
        }

        return trajectory;
}