干货！双目相机实测orbslam2 小白一小时可跑通 一篇把orbslam门槛降低到高中毕业的文章

Keywords

双目相机 orbslam2 orbslam orb_slam2 双目广角相机 双目广角相机参数 orbslam用相机跑 双目相机slam 立体相机slam stereo camera slam
orbslam配置 orbslam参数 orb_slam2参数

为什么写这篇文章？
纯slam小白跑用自己的双目也可以轻轻松松跑起来！ 大多数博文要么是针对小觅相机等高级货的，还不甚明了，要么压根就就是跑数据集的，或者压根就跑不通的，误导后来者。
十里桃园写下此博文，愿景是会操作电脑就会跑orbslam2。

本文内容？
接十里桃园的上篇博文《双目相机标定和orbslam2双目参数详解》，让一个小白用很烂的相机也可以跑起来orbslam2双目。

环境？
非ROS系统 纯linux的ORBSLAM2，对了，就那个github上下载下来./build.sh就能编译通过的那个，地址点击 https://github.com/raulmur/ORB_SLAM2

代码及用法
代码在文章最后，将给的代码覆盖掉源码中（ https://github.com/raulmur/ORB_SLAM2 ） stereo_euroc.cc文件中的内容，保存后重新用./build.sh命令编译。

要用小觅相机吗？
不需要，自己粘了俩相机就可以突突突的跑。

如何运行？
替换之后，按官网给的步骤编译robslam2.编译完成之后敲下面的命令，十里桃园用的是linux系统，windows没测想来也差不多把，没有用ros系统：
./Examples/Stereo/stereo_euroc Vocabulary/ORBvoc.txt Examples/Stereo/newbot.yaml

newbot.yaml是个啥？
参考十里的上篇博文《双目相机标定和orbslam2双目参数详解》你改好的配置文件命名为newbot.yaml，放到Examples/Stereo/ 文件夹下面

运行结果
运行后有十里桃园的专属计数器，觉得烦可以进代码去掉shilitaoyuan。正常运行一直刷帧数就对了

经典orbslam结果 必须要上的：

跑起来啥效果啊？
参数正确的话，这个还是蛮稳定的可以突突突的跑（笔记本设置电源选项，最大性能不然不接电源会卡，orbslam会计算的很慢）。
绕桌子一圈回个环，十里桃园在转弯的时侯一路小跑，没丢，赞：

说了这么多，代码在哪啊？
在下面，换掉源码中的stereo_euroc.cc文件中的内容，保存，重新编译即可，记得调下代码中读相机的函数调成你自己的，这个我也想帮你都调好了，但是每个人的相机总有那么一点差异不是：

/**
* This file is part of ORB-SLAM2.
*
* Copyright (C) 2014-2016 Raúl Mur-Artal <raulmur at unizar dot es> (University of Zaragoza)
* For more information see <https://github.com/raulmur/ORB_SLAM2>
*
* ORB-SLAM2 is free software: you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation, either version 3 of the License, or
* (at your option) any later version.
*
* ORB-SLAM2 is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*
* You should have received a copy of the GNU General Public License
* along with ORB-SLAM2. If not, see <http://www.gnu.org/licenses/>.
*/


#include<iostream>
#include<algorithm>
#include<fstream>
#include<iomanip>
#include<chrono>

#include<opencv2/core/core.hpp>

#include<System.h>
using namespace std::chrono;
using namespace std;



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


    // Retrieve paths to images
    vector<string> vstrImageLeft;
    vector<string> vstrImageRight;
    vector<double> vTimeStamp;
    //LoadImages(string(argv[3]), string(argv[4]), string(argv[5]), vstrImageLeft, vstrImageRight, vTimeStamp);

    //if(vstrImageLeft.empty() || vstrImageRight.empty())
   // {
      //  cerr << "ERROR: No images in provided path." << endl;
       // return 1;
    //}

   // if(vstrImageLeft.size()!=vstrImageRight.size())
   // {
     //   cerr << "ERROR: Different number of left and right images." << endl;
   //     return 1;
   // }

    // Read rectification parameters
    cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
    if(!fsSettings.isOpened())
    {
        cerr << "ERROR: Wrong path to settings" << endl;
        return -1;
    }

    cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
    fsSettings["LEFT.K"] >> K_l;
    fsSettings["RIGHT.K"] >> K_r;

    fsSettings["LEFT.P"] >> P_l;
    fsSettings["RIGHT.P"] >> P_r;

    fsSettings["LEFT.R"] >> R_l;
    fsSettings["RIGHT.R"] >> R_r;

    fsSettings["LEFT.D"] >> D_l;
    fsSettings["RIGHT.D"] >> D_r;

    int rows_l = fsSettings["LEFT.height"];
    int cols_l = fsSettings["LEFT.width"];
    int rows_r = fsSettings["RIGHT.height"];
    int cols_r = fsSettings["RIGHT.width"];

    if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
            rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
    {
        cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
        return -1;
    }

    cv::Mat M1l,M2l,M1r,M2r;
    cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,M1l,M2l);
    cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,M1r,M2r);


   // const int nImages = vstrImageLeft.size();

    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM2::System SLAM(argv[1],argv[2],ORB_SLAM2::System::STEREO,true);

    // Vector for tracking time statistics
    vector<float> vTimesTrack;
    cout << endl << "-------" << endl;
    cout << "Start processing camera ..." << endl;

  
    cv::Mat imLeft, imRight, imLeftRect, imRightRect;



//***********************************************************************8
       cv::VideoCapture cap1(0);
    cap1.set(CV_CAP_PROP_FRAME_WIDTH,640);

    cap1.set(CV_CAP_PROP_FRAME_HEIGHT,480);
    cap1.set(CV_CAP_PROP_FPS, 30);
    cv::VideoCapture cap2(1);
    cap2.set(CV_CAP_PROP_FRAME_WIDTH,640);

    cap2.set(CV_CAP_PROP_FRAME_HEIGHT,480);
    cap2.set(CV_CAP_PROP_FPS, 30);
   // cv::VideoCapture cap1(0);

//上面这段按你自己的相机参数修改 双目相机有倆id的 有一个id的 左右别反就行
//***********************************************************************8
	long int nImages = 0;
        int ni=0;
// Main loop
    while(ni>-1)
    {

//***********************************************************************8        
         cap2 >> imRight;
         cap1 >> imLeft;
 
//***********************************************************************8
        if(imLeft.empty())
        {
            cerr << endl << "Check Left Camera!! "<< endl;
            return 1;
        }

        if(imRight.empty())
        {
            cerr << endl << "Check Right Camera!! "<< endl;
            return 1;
        }

        cv::remap(imLeft,imLeftRect,M1l,M2l,cv::INTER_LINEAR);
        cv::remap(imRight,imRightRect,M1r,M2r,cv::INTER_LINEAR);

        time_point<system_clock> now = system_clock::now();
        
        double tframe = now.time_since_epoch().count();
        vTimeStamp.push_back(tframe);

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t1 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t1 = std::chrono::monotonic_clock::now();
#endif

        // Pass the images to the SLAM system
        SLAM.TrackStereo(imLeftRect,imRightRect,tframe);

#ifdef COMPILEDWITHC11
        std::chrono::steady_clock::time_point t2 = std::chrono::steady_clock::now();
#else
        std::chrono::monotonic_clock::time_point t2 = std::chrono::monotonic_clock::now();
#endif

        double ttrack= std::chrono::duration_cast<std::chrono::duration<double> >(t2 - t1).count();
       
        vTimesTrack.push_back(ttrack);

        // Wait to load the next frame
/*        
	double T=0;
        if(ni<nImages-1)
            T = vTimeStamp[ni+1]-tframe;
        else if(ni>0)
            T = tframe-vTimeStamp[ni-1];

       if(ttrack<T)
            usleep((T-ttrack)*1e6);
*/
	nImages++;
	std::cout << "shilitaoyuan_frames: "<<nImages<< std::endl; 
    }

    // Stop all threads
    SLAM.Shutdown();

    // Tracking time statistics
    sort(vTimesTrack.begin(),vTimesTrack.end());
    float totaltime = 0;
    for(int ni=0; ni<nImages; ni++)
    {
        totaltime+=vTimesTrack[ni];
    }
    cout << "-------" << endl << endl;
    cout << "median tracking time: " << vTimesTrack[nImages/2] << endl;
    cout << "mean tracking time: " << totaltime/nImages << endl;

    // Save camera trajectory
    SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");

    return 0;
}