【视觉SLAM 14讲】搭建 VO 0.3 代码讲解，问题总结

一、代码解释：

0.3版的vo在0.2的基础上加上了g2o优化

在visual_odometry.cpp中通过调用cv::solvePnPRansac()方法求出R，t过后再用g2o，位姿进行优化。

在g2o_types.h定义了三种边但是只有一种符合3D到2D的重投影误差。

g2o_types.h部分代码

#ifndef MYSLAM_G2O_TYPES_H
#define MYSLAM_G2O_TYPES_H

#include "myslam/common_include.h"
#include "camera.h"

#include <g2o/core/base_vertex.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_levenberg.h>
#include <g2o/types/sba/types_six_dof_expmap.h>
#include <g2o/solvers/dense/linear_solver_dense.h>
#include <g2o/core/robust_kernel.h>
#include <g2o/core/robust_kernel_impl.h>

namespace myslam {
    ...
    //第三种
    class EdgeProjectXYZ2UVPoseOnly
            : public g2o::BaseUnaryEdge<2, Eigen::Vector2d, g2o::VertexSE3Expmap> {
    public:
        //Eigen自动内存对齐
        EIGEN_MAKE_ALIGNED_OPERATOR_NEW
        //计算误差函数
        virtual void computeError();
        //线性优化方法(计算6 * 2 雅克比矩阵)
        virtual void linearizeOplus();
        //读写功能函数
        virtual bool read(std::istream &in) {}
        virtual bool write(std::ostream &os) const {};
        //定义三维点坐标和相机模型(计算雅克比矩阵需要用到x,y,z)
        Vector3d point_;
        Camera *camera_;
    };
}

#endif // MYSLAM_G2O_TYPES_H

g2o_types.cpp部分代码

#include "myslam/g2o_types.h"

namespace myslam {
    ...
    //计算误差
    void EdgeProjectXYZ2UVPoseOnly::computeError() {
        //取出顶点转换成位姿
        const g2o::VertexSE3Expmap *pose = static_cast<const g2o::VertexSE3Expmap *> ( _vertices[0] );
        //误差计算 ： 测量值 - 估计值 (重投影误差)
        //原理：由位姿转换为四元数在由四元素映射成相机坐标，在利用camera类中的方法映射到像素坐标
        _error = _measurement - camera_->camera2pixel(
                pose->estimate().map(point_));
    }
    //线性优化方法
    void EdgeProjectXYZ2UVPoseOnly::linearizeOplus() {
        //顶点数组中取出顶点，转换成位姿(顶点虽然里面存的是位姿但是还是涉及到类型转换)
        g2o::VertexSE3Expmap *pose = static_cast<g2o::VertexSE3Expmap *> ( _vertices[0] );
        //这由位姿构造一个四元数形式T
        g2o::SE3Quat T(pose->estimate());
        //用 T求得变换后的3D点坐标
        Vector3d xyz_trans = T.map(point_);
        //拿到了3D点的x,y,z方便接下来计算雅克比矩阵
        double x = xyz_trans[0];
        double y = xyz_trans[1];
        double z = xyz_trans[2];
        double z_2 = z * z;

        //这里的雅克比矩阵和14讲里面有点不一样
        _jacobianOplusXi(0, 0) = x * y / z_2 * camera_->fx_;
        _jacobianOplusXi(0, 1) = -(1 + (x * x / z_2)) * camera_->fx_;
        _jacobianOplusXi(0, 2) = y / z * camera_->fx_;
        _jacobianOplusXi(0, 3) = -1. / z * camera_->fx_;
        _jacobianOplusXi(0, 4) = 0;
        _jacobianOplusXi(0, 5) = x / z_2 * camera_->fx_;

        _jacobianOplusXi(1, 0) = (1 + y * y / z_2) * camera_->fy_;
        _jacobianOplusXi(1, 1) = -x * y / z_2 * camera_->fy_;
        _jacobianOplusXi(1, 2) = -x / z * camera_->fy_;
        _jacobianOplusXi(1, 3) = 0;
        _jacobianOplusXi(1, 4) = -1. / z * camera_->fy_;
        _jacobianOplusXi(1, 5) = y / z_2 * camera_->fy_;
    }
}

接下来看最重要的visual_odometry.h及其实现

#ifndef VISUALODOMETRY_H
#define VISUALODOMETRY_H

#include "myslam/common_include.h"
#include "myslam/map.h"
#include <opencv2/features2d/features2d.hpp>

namespace myslam {
    class VisualOdometry {
    public:
        typedef shared_ptr <VisualOdometry> Ptr;
        //枚举表征VO状态，初始化、正常、丢失
        enum VOState {
            INITIALIZING = -1,
            OK = 0,
            LOST
        };
        //VO状态、地图（关键帧和特征点）、参考帧、当前帧
        VOState state_;     // current VO status
        Map::Ptr map_;       // map with all frames and map points
        Frame::Ptr ref_;       // reference key-frame
        Frame::Ptr curr_;      // current frame
        //orb、前一帧 3D点、当前帧关键点、前一帧和当前帧的描述子矩阵、前一帧后一帧的关键点匹配关系
        cv::Ptr <cv::ORB> orb_;  // orb detector and computer
        vector <cv::Point3f> pts_3d_ref_;        // 3d points in reference frame
        vector <cv::KeyPoint> keypoints_curr_;    // keypoints in current frame
        Mat descriptors_curr_;  // descriptor in current frame
        Mat descriptors_ref_;   // descriptor in reference frame
        vector <cv::DMatch> feature_matches_;   // feature matches
        //特征匹配提取器
        cv::FlannBasedMatcher matcher_flann_;     // flann matcher
        //位姿估计T矩阵、内点数、丢失数
        SE3 T_c_r_estimated_;    // the estimated pose of current frame
        int num_inliers_;        // number of inlier features in icp
        int num_lost_;           // number of lost times
        // parameters
        //特征点数量、尺度、金字塔等级数、特征点好的匹配数量、最大持续时间丢失、最小内点数量、
        int num_of_features_;   // number of features
        double scale_factor_;   // scale in image pyramid
        int level_pyramid_;     // number of pyramid levels
        float match_ratio_;     // ratio for selecting  good matches
        int max_num_lost_;      // max number of continuous lost times
        int min_inliers_;       // minimum inliers
        double key_frame_min_rot;   // minimal rotation of two key-frames
        double key_frame_min_trans; // minimal translation of two key-frames
        double map_point_erase_ratio_; // remove map point ratio
        //构造函数析构函数
    public: // functions
        VisualOdometry();
        ~VisualOdometry();
        //核心！！！添加新关键帧
        bool addFrame(Frame::Ptr frame);      // add a new frame
    protected:
        // inner operation.
        void extractKeyPoints(); //提取特征点
        void computeDescriptors();//计算描述子
        void featureMatching();//计算特征匹配
        void setRef3DPoints();//还原深度信息
        void poseEstimationPnP();//PNP
        //关键帧相关的函数
        void addKeyFrame();
        bool checkEstimatedPose();
        bool checkKeyFrame();
    };
}
#endif // VISUALODOMETRY_H

visual_odometry.cpp

#include <opencv2/highgui/highgui.hpp>
#include <opencv2/imgproc/imgproc.hpp>
#include <opencv2/calib3d/calib3d.hpp>
#include <algorithm>
#include <boost/timer.hpp>
#include "myslam/config.h"
#include "myslam/visual_odometry.h"
#include "myslam/g2o_types.h"

namespace myslam {

    //初始化列表，生成对象的时候自动调用
    VisualOdometry::VisualOdometry() :
            state_(INITIALIZING), ref_(nullptr), curr_(nullptr), map_(new Map), num_lost_(0), num_inliers_(0),
            matcher_flann_(new cv::flann::LshIndexParams(5, 10, 2)) {
        //从配置文件中加载VO配置
        num_of_features_ = Config::get<int>("number_of_features");
        scale_factor_ = Config::get<double>("scale_factor");
        level_pyramid_ = Config::get<int>("level_pyramid");
        match_ratio_ = Config::get<float>("match_ratio");
        max_num_lost_ = Config::get<float>("max_num_lost");
        min_inliers_ = Config::get<int>("min_inliers");
        key_frame_min_rot = Config::get<double>("keyframe_rotation");
        key_frame_min_trans = Config::get<double>("keyframe_translation");
        map_point_erase_ratio_ = Config::get<double>("map_point_erase_ratio");
        orb_ = cv::ORB::create(num_of_features_, scale_factor_, level_pyramid_);
    }

    VisualOdometry::~VisualOdometry() {

    }

    //添加帧，同时进行
    bool VisualOdometry::addFrame(Frame::Ptr frame) {
        switch (state_) {
            case INITIALIZING: {//只有第一帧会走这个case
                //VO系统初始化部分，将第一帧位姿作为世界坐标系
                state_ = OK;//修改 VO 状态 OK
                curr_ = ref_ = frame;//第一帧既是当前帧也是前一帧
                map_->insertKeyFrame(frame);//将第一帧作为关键帧
                // extract features from first frame and add them into map
                extractKeyPoints();//提取关键帧
                computeDescriptors();//提取描述子
                setRef3DPoints();//还原深度坐标
                break;
            }
            case OK: {
                curr_ = frame;//更新当前帧
                extractKeyPoints();//提取关键帧
                computeDescriptors();//提取描述子
                featureMatching(); //特征值匹配
                poseEstimationPnP();   //相机位姿估计PNP

                if (checkEstimatedPose() == true) // a good estimation
                {
                    curr_->T_c_w_ = T_c_r_estimated_ * ref_->T_c_w_;  // T_c_w = T_c_r*T_r_w
                    ref_ = curr_;//更新前一帧
                    setRef3DPoints();//还原深度
                    num_lost_ = 0;//没有丢失
                    if (checkKeyFrame() == true) // is a key-frame
                    {
                        addKeyFrame();
                    }
                } else {// bad estimation due to various reasons
                    num_lost_++;
                    if (num_lost_ > max_num_lost_) {
                        state_ = LOST;
                    }
                    return false;
                }
                break;
            }
            case LOST: {
                cout << "vo has lost." << endl;
                break;
            }
        }

        return true;
    }

    //提取特征点
    void VisualOdometry::extractKeyPoints() {
        boost::timer timer;
        orb_->detect(curr_->color_, keypoints_curr_);
        cout << "extract keypoints cost time: " << timer.elapsed() << endl;
    }
    //计算描述子
    void VisualOdometry::computeDescriptors() {
        boost::timer timer;
        orb_->compute(curr_->color_, keypoints_curr_, descriptors_curr_);
        cout << "descriptor computation cost time: " << timer.elapsed() << endl;
    }
    //特征匹配
    void VisualOdometry::featureMatching() {
        boost::timer timer;
        vector <cv::DMatch> matches;
        //通过前后两帧的描述子得到特征点匹配集合
        matcher_flann_.match(descriptors_ref_, descriptors_curr_, matches);
        // select the best matches

        //找到 matches数组中最小距离，赋值给min_dist
        float min_dis = std::min_element(
                matches.begin(), matches.end(),
                [](const cv::DMatch &m1, const cv::DMatch &m2) {
                    return m1.distance < m2.distance;
                })->distance;

        feature_matches_.clear();//清理上一帧用过的特征
        //根据最小匹配距离筛选特征匹配中比较好的匹配点
        for (cv::DMatch &m: matches) {
            if (m.distance < max<float>(min_dis * match_ratio_, 30.0)) {
                feature_matches_.push_back(m);
            }
        }
        cout << "good matches: " << feature_matches_.size() << endl;
        cout << "match cost time: " << timer.elapsed() << endl;
    }

    //由于PNP条件为前一帧的3D坐标 和当前帧的2D坐标 所以在当前帧变为前一帧时  需要还原深度信息
    void VisualOdometry::setRef3DPoints() {
        // select the features with depth measurements
        pts_3d_ref_.clear();//前前帧的3D点
        descriptors_ref_ = Mat();
        //遍历还原所有特征点的深度
        for (size_t i = 0; i < keypoints_curr_.size(); i++) {
            //找到深度信息
            double d = ref_->findDepth(keypoints_curr_[i]);
            //深度为负说明特征点无效
            if (d > 0) {
                //构造像素坐标到相机坐标点的3D坐标
                Vector3d p_cam = ref_->camera_->pixel2camera(
                        Vector2d(keypoints_curr_[i].pt.x, keypoints_curr_[i].pt.y), d
                );
                //构造Point3f点按行放入前一帧3D点存储器中
                pts_3d_ref_.push_back(cv::Point3f(p_cam(0, 0), p_cam(1, 0), p_cam(2, 0)));
                //将描述子按行加入前一帧描述子容器
                descriptors_ref_.push_back(descriptors_curr_.row(i));
            }
        }
    }
    //PNP
    void VisualOdometry::poseEstimationPnP() {
        // construct the 3d 2d observations
        vector <cv::Point3f> pts3d;//将参考帧的3D
        vector <cv::Point2f> pts2d;//当前帧的2d

        //将参考帧的3D坐标和当前帧的2D坐标加入容器
        for (cv::DMatch m: feature_matches_) {
            pts3d.push_back(pts_3d_ref_[m.queryIdx]);
            pts2d.push_back(keypoints_curr_[m.trainIdx].pt);
        }

        //内参矩阵
        Mat K = (cv::Mat_<double>(3, 3) <<
                                        ref_->camera_->fx_, 0, ref_->camera_->cx_,
                0, ref_->camera_->fy_, ref_->camera_->cy_,
                0, 0, 1
        );

        //rvec :前一帧坐标系到当前帧的相机坐标系的旋转矩阵
        //tvec :前一帧坐标系到当前帧的相机坐标系的平移向量(矩阵形式)
        //inlers : 内点数组
        Mat rvec, tvec, inliers;
        //pnp求解 可以看文档了解参数：https://docs.opencv.org/4.x/d9/d0c/group__calib3d.html#ga549c2075fac14829ff4a58bc931c033d
        cv::solvePnPRansac(pts3d, pts2d, K, Mat(), rvec, tvec, false, 100, 4.0, 0.99, inliers);
        num_inliers_ = inliers.rows;
        cout << "pnp inliers: " << num_inliers_ << endl;
        //由pnp求解出来的rvec个tvex求出 T
        T_c_r_estimated_ = SE3(
                SO3(rvec.at<double>(0, 0), rvec.at<double>(1, 0), rvec.at<double>(2, 0)),
                Vector3d(tvec.at<double>(0, 0), tvec.at<double>(1, 0), tvec.at<double>(2, 0))
        );

        // using bundle adjustment to optimize the pose
        //BA优化位姿g2o 有关g2o使用教程参考：
        typedef g2o::BlockSolver <g2o::BlockSolverTraits<6, 2>> Block;
        typedef g2o::LinearSolverDense<Block::PoseMatrixType>()
        LinearSolver;
        //初始化
        Block::LinearSolverType *linearSolver = new LinearSolver();//线性求解器
        Block *solver_ptr = new Block(linearSolver);
        g2o::OptimizationAlgorithmLevenberg *solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
        //优化器对象SparseOptimizer
        g2o::SparseOptimizer optimizer;
        //将优化算法设置给SparseOptimizer
        optimizer.setAlgorithm(solver);

        //添加顶点
        g2o::VertexSE3Expmap *pose = new g2o::VertexSE3Expmap();
        pose->setId(0);
        pose->setEstimate(g2o::SE3Quat(
                T_c_r_estimated_.rotation_matrix(),
                T_c_r_estimated_.translation()
        ));
        optimizer.addVertex(pose);


        //添加边(一个内点一条边)
        for (int i = 0; i < inliers.rows; i++) {
            int index = inliers.at<int>(i, 0);
            // 3D -> 2D projection
            //新建一条边第三种边
            EdgeProjectXYZ2UVPoseOnly *edge = new EdgeProjectXYZ2UVPoseOnly();
            //设置对应边的参数
            edge->setId(i);//设置ID
            edge->setVertex(0, pose);//设置边对应的优化顶点
            edge->camera_ = curr_->camera_.get();//设置相机模型
            edge->point_ = Vector3d(pts3d[index].x, pts3d[index].y, pts3d[index].z);//设置3D点
            //设置边的测量值
            edge->setMeasurement(Vector2d(pts2d[index].x, pts2d[index].y));
            edge->setInformation(Eigen::Matrix2d::Identity());
            optimizer.addEdge(edge);
        }

        //执行优化
        optimizer.initializeOptimization();//初始化整个优化器
        optimizer.optimize(10);//开始执行优化，迭代 10次

        //将结果转成 T
        T_c_r_estimated_ = SE3(
                pose->estimate().rotation(),
                pose->estimate().translation()
        );
    }

    bool VisualOdometry::checkEstimatedPose() {
        // check if the estimated pose is good
        //
        if (num_inliers_ < min_inliers_) {
            cout << "reject because inlier is too small: " << num_inliers_ << endl;
            return false;
        }
        // if the motion is too large, it is probably wrong
        Sophus::Vector6d d = T_c_r_estimated_.log();
        if (d.norm() > 5.0) {
            cout << "reject because motion is too large: " << d.norm() << endl;
            return false;
        }
        return true;
    }

    //T中 R或 t比较大都是关键帧
    bool VisualOdometry::checkKeyFrame() {
        Sophus::Vector6d d = T_c_r_estimated_.log();
        Vector3d trans = d.head<3>();
        Vector3d rot = d.tail<3>();
        if (rot.norm() > key_frame_min_rot || trans.norm() > key_frame_min_trans)
            return true;
        return false;
    }
    void VisualOdometry::addKeyFrame() {
        cout << "adding a key-frame" << endl;
        map_->insertKeyFrame(curr_);
    }

}

run_vo.cpp

// -------------- test the visual odometry -------------
#include <fstream>
#include <boost/timer.hpp>
#include <opencv2/imgcodecs.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv2/viz.hpp>

#include "myslam/config.h"
#include "myslam/visual_odometry.h"
#include "../include/myslam/camera.h"

int main(int argc, char **argv) {
    if (argc != 2) {
        cout << "usage: run_vo parameter_file" << endl;
        return 1;
    }

    myslam::Config::setParameterFile(argv[1]);//加载配置文件default.yaml
    myslam::VisualOdometry::Ptr vo(new myslam::VisualOdometry);//创建VO框架对象

    string dataset_dir = myslam::Config::get<string>("dataset_dir");//数据集绝对路径
    cout << "dataset: " << dataset_dir << endl;
    ifstream fin(dataset_dir + "/associate.txt");
    if (!fin) {
        cout << "please generate the associate file called associate.txt!" << endl;
        return 1;
    }

    vector <string> rgb_files, depth_files;
    vector<double> rgb_times, depth_times;
    while (!fin.eof()) {
        string rgb_time, rgb_file, depth_time, depth_file;
        fin >> rgb_time >> rgb_file >> depth_time >> depth_file;//从associate.txt中按顺序读取 rgb, depth

        //以下四行把rgb,depth全部存入对应的vector容器
        rgb_times.push_back(atof(rgb_time.c_str()));
        depth_times.push_back(atof(depth_time.c_str()));
        rgb_files.push_back(dataset_dir + "/" + rgb_file);//绝对路径
        depth_files.push_back(dataset_dir + "/" + depth_file);//绝对路径

        if (fin.good() == false)
            break;
    }

    myslam::Camera::Ptr camera(new myslam::Camera);//创建相机对象，在config/default.txt中获取相机参数

    // visualization
    cv::viz::Viz3d vis("Visual Odometry");//生成3D图像可视化对象vis
    cv::viz::WCoordinateSystem world_coor(1.0), camera_coor(0.5);//设置世界坐标系，相机坐标系
    cv::Point3d cam_pos(0, -1.0, -1.0), cam_focal_point(0, 0, 0), cam_y_dir(0, 1, 0);//设置相机的朝向(光轴朝向)
    cv::Affine3d cam_pose = cv::viz::makeCameraPose(cam_pos, cam_focal_point, cam_y_dir);//设置相机的位置与朝向
    vis.setViewerPose(cam_pose);//在vis中显示出来

    world_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 2.0);//设置世界坐标系渲染属性
    camera_coor.setRenderingProperty(cv::viz::LINE_WIDTH, 1.0);//设置相机坐标系渲染属性

    //显示世界、相机部件
    vis.showWidget("World", world_coor);
    vis.showWidget("Camera", camera_coor);

    //rgb文件数量
    cout << "read total " << rgb_files.size() << " entries" << endl;
    //读取所有rgb文件
    for (int i = 0; i < rgb_files.size(); i++) {
        //将rgb图像数据转化为 rgb图像矩阵
        Mat color = cv::imread(rgb_files[i]);
        //将深度图数据转化为深度矩阵
        Mat depth = cv::imread(depth_files[i], -1);

        if (color.data == nullptr || depth.data == nullptr)
            break;

        //生成帧对象,将遍历到的图像信息存放到帧里面去
        myslam::Frame::Ptr pFrame = myslam::Frame::createFrame();
        pFrame->camera_ = camera;//相机对象早在生成时就初始化好了
        pFrame->color_ = color;
        pFrame->depth_ = depth;
        pFrame->time_stamp_ = rgb_times[i];

        //将当前帧加入VO系统
        boost::timer timer;
        vo->addFrame(pFrame);
        cout << "VO costs time: " << timer.elapsed() << endl;

        //如果帧丢失直接结束
        if (vo->state_ == myslam::VisualOdometry::LOST)
            break;

        //这里视第一帧的相机坐标系为世界坐标系,现在求出的是世界坐标系到相机坐标系的变换矩阵
        SE3 Tcw = pFrame->T_c_w_.inverse();

        // show the map and the camera pose
        //求出世界坐标系到相机坐标系的欧式变换矩阵
        cv::Affine3d M(
                cv::Affine3d::Mat3(
                        Tcw.rotation_matrix()(0, 0), Tcw.rotation_matrix()(0, 1), Tcw.rotation_matrix()(0, 2),
                        Tcw.rotation_matrix()(1, 0), Tcw.rotation_matrix()(1, 1), Tcw.rotation_matrix()(1, 2),
                        Tcw.rotation_matrix()(2, 0), Tcw.rotation_matrix()(2, 1), Tcw.rotation_matrix()(2, 2)
                ),
                cv::Affine3d::Vec3(
                        Tcw.translation()(0, 0), Tcw.translation()(1, 0), Tcw.translation()(2, 0)
                )
        );

        cv::imshow("image", color);
        cv::waitKey(1);
        vis.setWidgetPose("Camera", M);
        vis.spinOnce(1, false);
    }

    return 0;
}

二、搭建：

请确保在看完0.2版本之后再开始以下内容

编译工程

在0.3目录下打开终端：

mkdir build
cd build
cmake ..
make

出现以下错误

In file included from /usr/local/include/g2o/core/base_unary_edge.h:30:0,
                 from /home/zqh/1/slambook-master/project/0.3/include/myslam/g2o_types.h:27,
                 from /home/zqh/1/slambook-master/project/0.3/src/g2o_types.cpp:1:
/usr/local/include/g2o/core/base_fixed_sized_edge.h:200:32: error: ‘index_sequence’ is not a member of ‘std’
....

原因分析：

slambook1版本g2o 版本太老了 使用的是c++11版本的g2o，我用的是最新的g2o，使用的是C++14标准

解决：

让CMake编译在C++14标准下进行，在CMakeLists.txt添加：

set(CMAKE_CXX_STANDARD 14)

修改完后重新编译出现：

In file included from /home/zqh/1/slambook-master/project/0.3/src/g2o_types.cpp:1:0:
/home/zqh/1/slambook-master/project/0.3/include/myslam/g2o_types.h:37:80: error: ‘VertexSBAPointXYZ’ is not a member of ‘g2o’
 rojectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>
                                                                     ^~~~~~~~~~~~~~~~~
/home/zqh/1/slambook-master/project/0.3/include/myslam/g2o_types.h:37:80: note: suggested alternative: ‘VertexPointXYZ’
 rojectXYZRGBD : public g2o::BaseBinaryEdge<3, Eigen::Vector3d, g2o::VertexSBAPointXYZ, g2o::VertexSE3Expmap>
                                                                     ^~~~~~~~~~~~~~~~~
                                                                                VertexPointXYZ
...

原因分析：

新版g2o库中不再存在原先的VertexSBAPointXYZ，将所有报错的地方改为VertexPointXYZ即可