源码地址

源码链接：https://github.com/jingpang/LearnVIORB

电脑配置

Ubuntu 18.04 + ROS Melodic + GTSAM 4.0.2 + CERES 1.14.0
pcl1.8+vtk8.2.0+opencv3.2.0

环境配置

之前已经配置过VINS-MOON的环境（可参考之前的博客）

编译

 cd ~/catkin_ws/src
//下载解压https://github.com/jingpang/LearnVIORB
 cd VI_ORBSLAM2
chmod +x build.sh

将build.sh文件中的make -j改为make
将下列文件加入头文件代码

#include <unistd.h>
#include <stdio.h>
#include <stdlib.h>

Examples/Monocular/mono_euroc.cc
Examples/Monocular/mono_kitti.cc
Examples/Monocular/mono_tum.cc
Examples/RGB-D/rgbd_tum.cc
Examples/Stereo/stereo_euroc.cc
Examples/Stereo/stereo_kitti.cc
Examples/Stereo/ORB_SLAM2/src/AR/ViewerAR.cc
Examples/Ros/stereo_kitti.cc
Examples/ROS/ORB_SLAM2/src/AR/ViewerAR.cc
src/Converter.cc
src/Frame.cc
src/FrameDrawer.cc
src/Initializer.cc
src/KeyFrame.cc
src/KeyFrameDatabase.cc
src/LocalMapping.cc
src/LoopClosing.cc
src/Map.cc
src/MapDrawer.cc
src/MapPoint.cc
src/Optimizer.cc
src/ORBextractor.cc
src/ORBmatcher.cc
src/PnPsolver.cc
src/Sim3Solver.cc
src/System.cc
src/Tracking.cc
src/Viewer.cc

添加原因：
（遇到同样的错误，可以添加头文件）
修改所有CMakeLists.txt文件，在set（LIBS xxxxx 的后面加上

-lboost_system
//）

参考链接：https://blog.csdn.net/Robert_Q/article/details/121592896

所有CMakeLists.txt文件,将find_package中的opencv版本号改成自己安装的opencv版本号，EiGen3也改成自己安装的版本号
opencv版本号查看：

pkg-config --modversion opencv

参考链接：https://blog.csdn.net/qin_liang/article/details/127419612

报错

usr/local/include/eigen3/Eigen/src/Core/util/StaticAssert.h:33:40: error: static assertion failed: YOU_MIXED_DIFFERENT_NUMERIC_TYPES__YOU_NEED_TO_USE_THE_CAST_METHOD_OF_MATRIXBASE_TO_CAST_NUMERIC_TYPES_EXPLICITLY
#define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);

解决方法：
打开VI-ORBSLAM2文件夹下的thirdparty/g2o/g2o/solvers/linear_solver_eigen.h

把typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, SparseMatrix::Index> PermutationMatrix;
修改为typedef Eigen::PermutationMatrix<Eigen::Dynamic, Eigen::Dynamic, int> PermutationMatrix;

参考链接：https://blog.csdn.net/weixin_44401286/article/details/102319719

编译

./build.sh

删除build文件夹，再次编译

mkdir build
cd build
cmake ..
make -j2

编译ROS

 cd ~/catkin_ws2/src/VI-ORBSLAM2/Examples/ROS/ORB_VIO/
 mkdir build
 cd build
 cmake ..
 make -j2

修改.bashrc文件

sudo  gedit ~/.bashrc

在文件尾添加

export ROS_PACKAGE_PATH=${ROS_PACKAGE_PATH}:~/catkin_ws2/src/VI-ORBSLAM2/Examples/ROS

修改后在终端输入

source ~/.bashrc

轨迹保存为tum格式

在include/System.h 中添加

    void SaveTrajectory(const string &filename);

在src/System.cc中添加

 void System::SaveTrajectory(const string &filename)
    {
        cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
        
        vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
        sort(vpKFs.begin(),vpKFs.end(),KeyFrame::lId);
        
        // Transform all keyframes so that the first keyframe is at the origin.
        // After a loop closure the first keyframe might not be at the origin.
        cv::Mat Two = vpKFs[0]->GetPoseInverse();
        
        ofstream f;
        f.open(filename.c_str());
        f << fixed;
        
        // Frame pose is stored relative to its reference keyframe (which is optimized by BA and pose graph).
        // We need to get first the keyframe pose and then concatenate the relative transformation.
        // Frames not localized (tracking failure) are not saved.
        
        // For each frame we have a reference keyframe (lRit), the timestamp (lT) and a flag
        // which is true when tracking failed (lbL).
        list<ORB_SLAM2::KeyFrame*>::iterator lRit = mpTracker->mlpReferences.begin();
        list<double>::iterator lT = mpTracker->mlFrameTimes.begin();
        list<bool>::iterator lbL = mpTracker->mlbLost.begin();
        for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
            lend=mpTracker->mlRelativeFramePoses.end();lit!=lend;lit++, lRit++, lT++, lbL++)
        {
            if(*lbL)
                continue;
            
            KeyFrame* pKF = *lRit;
            
            cv::Mat Trw = cv::Mat::eye(4,4,CV_32F);
            
            // If the reference keyframe was culled, traverse the spanning tree to get a suitable keyframe.
            while(pKF->isBad())
            {
                Trw = Trw*pKF->mTcp;
                pKF = pKF->GetParent();
            }
            
            Trw = Trw*pKF->GetPose()*Two;
            
            cv::Mat Tcw = (*lit)*Trw;
            cv::Mat Rwc = Tcw.rowRange(0,3).colRange(0,3).t();
            cv::Mat twc = -Rwc*Tcw.rowRange(0,3).col(3);
            
            vector<float> q = Converter::toQuaternion(Rwc);
            
            f << setprecision(6) << *lT << " " <<  setprecision(9) << twc.at<float>(0) << " " << twc.at<float>(1) << " " << twc.at<float>(2) << " " << q[0] << " " << q[1] << " " << q[2] << " " << q[3] << endl;
        }
        f.close();
        cout << endl << "trajectory saved!" << endl;
    }

在Examples/ROS/ORB_VIO/src的主函数中添加

   SLAM.SaveTrajectory(config._tmpFilePath+"trajectory.txt");

参考：https://blog.csdn.net/qq_40464599/article/details/113482823

运行结果

Euroc数据集

下载地址：https://projects.asl.ethz.ch/datasets/doku.php?id=kmavvisualinertialdatasets

roslaunch ORB_VIO testeuroc.launch 
rosbag play MH_01_easy.bag  

evo_traj tum trajectory.txt  -p --plot_mode=xyz