视觉SLAM深蓝学院作业解答

最近整理一下之前所做的深蓝学院视觉SLAM课程的作业，其中内容有些遗忘，就当做复习。毕竟做作业的过程中收获很大。

3-6 轨迹的描绘

程序代码：

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

using namespace std;

// path to trajectory file
string trajectory_file = "./trajectory.txt";//注意路径问题，确保文件在build文件夹中

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

    /// implement pose reading code
    // start your code here (5~10 lines)
    ifstream fin(trajectory_file);
//    while(!fin.eof()){
   
//        double data[8]={0};
//        for(auto& d:data)
//            fin>>d;
//        Eigen::Quaterniond q(data[7],data[4],data[5],data[6]);
//        Eigen::Vector3d t(data[1],data[2],data[3]);
//        Sophus::SE3 SE3_q(q,t);
//        poses.push_back(SE3_q);
//    }
    if(!fin)
        cerr<<"ERROR!"<<endl;
    double data[7]={
   0};
    double time=0;
    while(fin>>time){
   
        for(auto& d:data){
   
            fin>>d;
        }
        Eigen::Quaterniond q(data[6],data[3],data[4],data[5]);
        Eigen::Vector3d t(data[0],data[1],data[2]);
//        double q0=data[6],q1=data[3],q2=data[4],q3=data[5];
//        Eigen::Matrix<double,4,4> T;//可以从四元数来求得旋转矩阵，通过先求旋转向量，再用罗德里格斯公式。
//        T<<1-2*q2*q2-2*q3*q3,2*q1*q2+2*q0*q3,2*q1*q3-2*q0*q2,data[0],
//           2*q1*q2-2*q0*q3,1-2*q1*q1-2*q3*q3,2*q2*q3+2*q0*q1,data[1],
//           2*q1*q3+2*q0*q2,2*q2*q3-2*q0*q1,1-2*q1*q1-2*q2*q2,data[2],
//           0,0,0,1;
//        poses.push_back(T);
        Sophus::SE3 SE3_q(q,t);
        poses.push_back(SE3_q);
    }
    // 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(assignment3_6)
set( CMAKE_BUILD_TYPE Release )
set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

add_executable(assignment3_6 "main.cpp")

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


find_package(Sophus REQUIRED)

target_link_libraries(assignment3_6 ${
   Sophus_LIBRARIES})
target_link_libraries(${
   PROJECT_NAME}
${
   OpenCV_LIBS}
${
   EIGEN3_LIBS}
${
   Pangolin_LIBRARIES}
)

4-2图像去畸变

代码：

#include <opencv2/opencv.hpp>
#include <string>

using namespace std;

string image_file = "../test.png";   // 请确保路径正确

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

    // 本程序需要你自己实现去畸变部分的代码。尽管我们可以调用OpenCV的去畸变，但自己实现一遍有助于理解。
    // 畸变参数
    double k1 = -0.28340811, k2 = 0.07395907, p1 = 0.00019359, p2 = 1.76187114e-05;
    // 内参
    double fx = 458.654, fy = 457.296, cx = 367.215, cy = 248.375;

    cv::Mat image = cv::imread(image_file,0);   // 图像是灰度图，CV_8UC1
    int rows = image.rows, cols = image.cols;
    cv::Mat image_undistort = cv::Mat(rows, cols, CV_8UC1);   // 去畸变以后的图

    // 计算去畸变后图像的内容
    for (int v = 0; v < rows; v++)
        for (int u = 0; u < cols; u++) {
   
            double u_distorted = 0, v_distorted = 0