单目视觉里程记代码

在Github上发现了一个简单的单目vo，有接近500星，链接如下：https://github.com/avisingh599/mono-vo 。
这个单目里程计主要依靠opencv实现，提取fast角点并进行光流跟踪，然后求取本质矩阵并恢复两帧位姿。整个里程计都是按照上述流程进行，并没有构建地图，因此也没有使用pnp算法。
我对一些难懂的地方做了注释，分享给大家。
以下是头文件：

#include "opencv2/video/tracking.hpp"
#include "opencv2/imgproc/imgproc.hpp"
#include "opencv2/highgui/highgui.hpp"
#include "opencv2/features2d/features2d.hpp"
#include "opencv2/calib3d/calib3d.hpp"

#include <iostream>
#include <ctype.h>
#include <algorithm> // for copy
#include <iterator> // for ostream_iterator
#include <vector>
#include <ctime>
#include <sstream>
#include <fstream>
#include <string>

using namespace cv;
using namespace std;

// 特征提取
void featureTracking(Mat img_1, Mat img_2, vector<Point2f>& points1, vector<Point2f>& points2, vector<uchar>& status)
{
    //this function automatically gets rid of points for which tracking fails
    vector<float> err;
    Size winSize=Size(21,21);
    TermCriteria termcrit=TermCriteria(TermCriteria::COUNT+TermCriteria::EPS, 30, 0.01);

    // LK光流跟踪
    calcOpticalFlowPyrLK(img_1, img_2, points1, points2, status, err, winSize, 3, termcrit, 0, 0.001);

    //getting rid of points for which the KLT tracking failed or those who have gone outside the frame
    int indexCorrection = 0;
    // 若跟踪失败，状态status为0(假)
    for( int i=0; i<status.size(); i++)
    {
        Point2f pt = points2.at(i- indexCorrection);
        if((status.at(i) == 0)||(pt.x<0)||(pt.y<0))
        {
            if((pt.x<0)||(pt.y<0))
            {
                status.at(i) = 0;
            }
            points1.erase (points1.begin() + (i - indexCorrection));
            points2.erase (points2.begin() + (i - indexCorrection));
            indexCorrection++;
        }

    }
}

// FAST角点检测
void featureDetection(Mat img_1, vector<Point2f>& points1)
{
    //uses FAST as of now, modify parameters as necessary
    vector<KeyPoint> keypoints_1;
    int fast_threshold = 20;
    bool nonmaxSuppression = true;
    FAST(img_1, keypoints_1, fast_threshold, nonmaxSuppression);
    KeyPoint::convert(keypoints_1, points1, vector<int>());
}

以下为cpp文件

#include "vo_features.h"
using namespace cv;
using namespace std;

#define MAX_FRAME 1000
#define MIN_NUM_FEAT 2000

/// 关于尺度 参考：  https://blog.csdn.net/ouyangandy/article/details/89332114

double getAbsoluteScale(int frame_id, int sequence_id, double z_cal)
{
    string line;
    int i = 0;
    //读取位姿真实值pose.txt  ground_truth
    ifstream myfile ("/home/wang/catkin_jg/src/ORB_SLAM2/Data/KITTI/00/00.txt");
    double x =0, y=0, z = 0;
    double x_prev, y_prev, z_prev;
    if (myfile.is_open())
    {
        // 一直循环到i>frame_id时结束while循环，此时x y z 保存的是i=frame_id时读取的数据
        while (( getline (myfile,line) ) && (i<=frame_id))
        {
            z_prev = z;
            x_prev = x;
            y_prev = y;
            std::istringstream in(line);
            //cout << line << '\n';
            //其排列方式为一个增广矩阵：[R|t]  每一行的最后一列数据为平移的t的数据。
            //把位移t提取出来，即第4,8,12列的数据，从0开始，则是3,7,11
            for (int j=0; j<12; j++)
            {
                in >> z;
                if (j==7) y=z;
                if (j==3) x=z;
            }
            i++;
        }
        myfile.close();
    }
    else
    {
        cout << "Unable to open file";
        return 0;
    }

    return sqrt((x-x_prev)*(x-x_prev) + (y-y_prev)*(y-y_prev) + (z-z_prev)*(z-z_prev)) ;

    /**
     从以上代码来看，数据集KITTI-00的真实值pose.txt，里面每行是12列数据，12列数据很容易想到是3个平移量
     和一个3x3的旋转矩阵，这样想没错，但是其排列方式却不是这样的，而是一个3*4的矩阵,
     其排列方式为一个增广矩阵：[R|t]
     也就是说，每一行的最后一列数据为平移的t的数据。
     那么是如何得到尺度因子的呢？众所周知，单目无法得到真实尺度的信息，不具有单位的概念，因此上面的基于单目的
     视觉里程计其尺度信息是来自于groundtruth的，也就是事先知道的真实尺度，
     如何计算：获取当前帧真实位置与前一帧的真实位置的距离作为尺度值。
     也就是最后return的值，其实就是当前帧的（x,y,z）减去上一帧的（x,y,z）这个真实距离作为真实尺度。
     */

}

int main( int argc, char** argv )
{
    Mat img_1, img_2;
    Mat R_f, t_f; //the final rotation and tranlation vectors

    ofstream myfile;
    myfile.open ("results1_1.txt");

    double scale = 1.00;
    char filename1[200];
    char filename2[200];
    // %06d 打印6个字符，不足的用0填充(在前面补)
    // sptintf用法见：https://www.runoob.com/cprogramming/c-function-sprintf.html
    /*
     *  int main()
        {
           char str[80];
           sprintf(str, "Pi 的值 = %f", M_PI);
           puts(str);     return(0);
        }
        让我们编译并运行上面的程序，这将产生以下结果：
        Pi 的值 = 3.141593
     */
    sprintf(filename1, "/home/wang/catkin_jg/src/ORB_SLAM2/Data/KITTI/00/image_0/%06d.png", 0);
    sprintf(filename2, "/home/wang/catkin_jg/src/ORB_SLAM2/Data/KITTI/00/image_0/%06d.png", 1);

    // 用于轨迹pose GUI界面显示的参数
    char text[100];
    int fontFace = FONT_HERSHEY_PLAIN;
    double fontScale = 1;
    int thickness = 1;
    cv::Point textOrg(10, 50);

    // read the first two frames from the dataset
    Mat img_1_c = imread(filename1);
    Mat img_2_c = imread(filename2);

    if ( !img_1_c.data || !img_2_c.data )
    {
        std::cout<< " --(!) Error reading images " << std::endl;
        return -1;
    }

    // we work with grayscale images
    cvtColor(img_1_c, img_1, COLOR_BGR2GRAY);
    cvtColor(img_2_c, img_2, COLOR_BGR2GRAY);

    // feature detection, tracking
    vector<Point2f> points1, points2;        //vectors to store the coordinates of the feature points
    featureDetection(img_1, points1);     //detect features in img_1
    vector<uchar> status;
    featureTracking(img_1,img_2,points1,points2, status); //track those features to img_2

    //TODO: add a fucntion to load these values directly from KITTI's calib files
    // WARNING: different sequences in the KITTI VO dataset have different intrinsic/extrinsic parameters
    double focal = 718.8560;
    cv::Point2d pp(607.1928, 185.2157);
    //recovering the pose and the essential matrix
    Mat E, R, t, mask;
    E = findEssentialMat(points2, points1, focal, pp, RANSAC, 0.999, 1.0, mask);
    recoverPose(E, points2, points1, R, t, focal, pp, mask);

    Mat prevImage = img_2;
    Mat currImage;
    vector<Point2f> prevFeatures = points2;
    vector<Point2f> currFeatures;

    char filename[100];

    R_f = R.clone();
    t_f = t.clone();

    clock_t begin = clock();

    namedWindow( "Road facing camera", WINDOW_AUTOSIZE );// Create a window for display.
    namedWindow( "Trajectory", WINDOW_AUTOSIZE );// Create a window for display.

    Mat traj = Mat::zeros(600, 600, CV_8UC3);

    for(int numFrame=2; numFrame < MAX_FRAME; numFrame++)
    {
        sprintf(filename, "/home/wang/catkin_jg/src/ORB_SLAM2/Data/KITTI/00/image_0/%06d.png", numFrame);
        //cout << numFrame << endl;
        Mat currImage_c = imread(filename);
        cvtColor(currImage_c, currImage, COLOR_BGR2GRAY);
        vector<uchar> status;
        featureTracking(prevImage, currImage, prevFeatures, currFeatures, status);

        E = findEssentialMat(currFeatures, prevFeatures, focal, pp, RANSAC, 0.999, 1.0, mask);
        recoverPose(E, currFeatures, prevFeatures, R, t, focal, pp, mask);

        Mat prevPts(2,prevFeatures.size(), CV_64F), currPts(2,currFeatures.size(), CV_64F);

        for(int i=0;i<prevFeatures.size();i++)
        {
            //this (x,y) combination makes sense as observed from the source code of triangulatePoints on GitHub
            prevPts.at<double>(0,i) = prevFeatures.at(i).x;
            prevPts.at<double>(1,i) = prevFeatures.at(i).y;

            currPts.at<double>(0,i) = currFeatures.at(i).x;
            currPts.at<double>(1,i) = currFeatures.at(i).y;
        }

        scale = getAbsoluteScale(numFrame, 0, t.at<double>(2));

        //cout << "Scale is " << scale << endl;


        if ((scale>0.1)&&(t.at<double>(2) > t.at<double>(0)) && (t.at<double>(2) > t.at<double>(1)))
        {
            t_f = t_f + scale*(R_f*t);
            R_f = R*R_f;
        }
        /// 错误示范
        /**if (scale > 0.1)
        {
            cur_t_ = cur_t_ + scale*t;
            cur_R_ = R*cur_R_;
        }
        之所以错误的原因是，忽略了平移的方向性，因此左乘旋转矩阵，就规定了它朝哪个方向旋转，
        这也符合真实的平移情况
        我的理解是：先旋转后平移再加上原来的平移量才是真实的平移，如上注释。*/

        else {
         //cout << "scale below 0.1, or incorrect translation" << endl;
        }

        // lines for printing results
        // myfile << t_f.at<double>(0) << " " << t_f.at<double>(1) << " " << t_f.at<double>(2) << endl;

        // a redetection is triggered in case the number of feautres being trakced go below a particular threshold
        if (prevFeatures.size() < MIN_NUM_FEAT)
        {
            //cout << "Number of tracked features reduced to " << prevFeatures.size() << endl;
            //cout << "trigerring redection" << endl;
            featureDetection(prevImage, prevFeatures);
            featureTracking(prevImage,currImage,prevFeatures,currFeatures, status);
        }

        prevImage = currImage.clone();
        prevFeatures = currFeatures;

        int x = int(t_f.at<double>(0)) + 300;
        int y = int(t_f.at<double>(2)) + 100;
        // 半径为1,线宽为2,画出来就成了实心的圆。按照traj轨迹一直画圆，就画出了轨迹。
        circle(traj, Point(x, y) ,1, CV_RGB(255,0,0), 2);
        // 画出显示的矩形
        rectangle( traj, Point(10, 30), Point(550, 50), CV_RGB(0,0,0), CV_FILLED);
        sprintf(text, "Coordinates: x = %02fm y = %02fm z = %02fm", t_f.at<double>(0), t_f.at<double>(1), t_f.at<double>(2));
        putText(traj, text, textOrg, fontFace, fontScale, Scalar::all(255), thickness, 8);

        imshow( "Road facing camera", currImage_c );
        imshow( "Trajectory", traj );

        waitKey(1);

    }

    clock_t end = clock();
    double elapsed_secs = double(end - begin) / CLOCKS_PER_SEC;
    cout << "Total time taken: " << elapsed_secs << "s" << endl;
    return 0;
}

以下是运行KITTI数据集00序列时的运行截图