【ORB_SLAM2源码解读】TUM RGBD 数据集跑通ORB_SLAM2

下载Tum RGBD数据集

TUM Dataset 数据集下载地址

也可以添加微信＂slamshizhanjiaocheng＂获取fr2/pioneer_360

生成关联文件

生成深度图和彩色图时间戳对其的文件

associate.py 下载地址

也可以添加微信＂slamshizhanjiaocheng＂获取 associate.py

将associate.py文件放在数据集路径下执行下面命令

python associate.py ./rgb.txt ./depth.txt > associations.txt

clion运行命令

rgbd_tum终端运运行命令

 ./Examples/RGB-D/rgbd_tum Vocabulary/ORBvoc.txt Examples/RGB-D/TUM2.yaml /home/q/rgbd_dataset_freiburg2_pioneer_360 /home/q/rgbd_dataset_freiburg2_pioneer_360/associations.txt

实验结果

保存rgbd_tum数据集关键帧轨迹

我们倒着推到一下，最终我们保存的是四元数对吧，四元数来自于哪里？
四元数来自于旋转矩阵的转化，旋转矩阵来自于哪里？
旋转矩阵来自于变换矩阵的分解，变换矩阵来自于哪里？
变换矩阵来自于关键帧的位姿变化，当然是所有的位资都统一到世界坐标系下面，
ORB_SLAM2默认第一个关键帧作为世界坐标系。
剩下就是理解世界坐标系，相机坐标系，之间的关系了

// 按照TUM格式保存相机运行轨迹并保存到指定的文件
void System::SaveTrajectoryTUM(const string &filename){
    cout << endl << "Saving camera trajectory to " << filename << " ..." << endl;
    // 只有在传感器为双目或者RGBD时才可以工作
    if(mSensor==MONOCULAR){
        cerr << "ERROR: SaveTrajectoryTUM cannot be used for monocular." << endl;
        return;
    }

    // 从地图中获取所有的关键帧
    vector<KeyFrame*> vpKFs = mpMap->GetAllKeyFrames();
    // 根据关键帧生成的先后顺序(lId)进行排序
    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());
    //这个可以理解为，在输出浮点数的时候使用0.3141592654这样的方式而不是使用科学计数法
    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();
    // 对于每一个mlRelativeFramePoses中的帧lit
    for(list<cv::Mat>::iterator lit=mpTracker->mlRelativeFramePoses.begin(),
        lend=mpTracker->mlRelativeFramePoses.end();
        lit!=lend;
        lit++, lRit++, lT++, lbL++)		// TODO 为什么是在这里更新参考关键帧？
    {
    	//如果该帧追踪失败，不管它，进行下一个
        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();
        }//查看当前使用的参考关键帧是否为bad
        // TODO 其实我也是挺好奇，为什么在这里就能够更改掉不合适的参考关键帧了呢

        // TODO 这里的函数GetPose()和上面的mTcp有什么不同？
        //最后一个Two是原点校正
        // 最终得到的是参考关键帧相对于世界坐标系的变换
        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;
    }对于每一个mlRelativeFramePoses中的帧lit所进行的操作

    //操作完毕，关闭文件并且输出调试信息
    f.close();
    cout << endl << "trajectory saved!" << endl;
}

rgbd_tum轨迹可视化

视觉SLAM十四讲第二版书中的可视化代码

更换下面的路径就可以了。

SLAM.SaveTrajectoryTUM("CameraTrajectory.txt");
SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
string trajectory_file = "/home/q/projects/ORB_SLAM2/CameraTrajectory.txt";
string trajectory_file = "/home/q/projects/ORB_SLAM2/KeyFrameTrajectory.txt";

rgbd_tum参数文件TUM1.yaml解析

# Camera calibration and distortion parameters (OpenCV) 
# 相机内参数
Camera.fx: 517.306408
Camera.fy: 516.469215
Camera.cx: 318.643040
Camera.cy: 255.313989
# 相机畸变系数
Camera.k1: 0.262383
Camera.k2: -0.953104
Camera.p1: -0.005358
Camera.p2: 0.002628
Camera.k3: 1.163314
# 图像尺度
Camera.width: 640
Camera.height: 480

# Camera frames per second 帧率
Camera.fps: 30.0

# IR projector baseline times fx (aprox.) 基线
Camera.bf: 40.0

# Color order of the images (0: BGR, 1: RGB. It is ignored if images are grayscale)
Camera.RGB: 1

# Close/Far threshold. Baseline times. 
ThDepth: 40.0

# Deptmap values factor 
DepthMapFactor: 5000.0 

# ORB Extractor: Number of features per image 特征点数目
ORBextractor.nFeatures: 1000

# ORB Extractor: Scale factor between levels in the scale pyramid 比例	 
ORBextractor.scaleFactor: 1.2

# ORB Extractor: Number of levels in the scale pyramid 金字塔层数
ORBextractor.nLevels: 8

# 实现特征点均衡化做的处理
# ORB Extractor: Fast threshold
# Image is divided in a grid. At each cell FAST are extracted imposing a minimum response.
# Firstly we impose iniThFAST. If no corners are detected we impose a lower value minThFAST
# You can lower these values if your images have low contrast			
ORBextractor.iniThFAST: 20
ORBextractor.minThFAST: 7


# Viewer Parameters 可视化需要给定的参数
Viewer.KeyFrameSize: 0.05
Viewer.KeyFrameLineWidth: 1
Viewer.GraphLineWidth: 0.9
Viewer.PointSize:2
Viewer.CameraSize: 0.08
Viewer.CameraLineWidth: 3
Viewer.ViewpointX: 0
Viewer.ViewpointY: -0.7
Viewer.ViewpointZ: -1.8
Viewer.ViewpointF: 500

相对于双目和单目没有外参数，因为已经有了深度图，不需要求深度，所以简单很多，都是一些简单算法，输入深度图，根据深度图，和彩色图就相当于建立了3D空间和2D像素平面之间的关系，实际上整个SLAM过程就是围绕着这两者的求解进行展开的，

pose_estimation_2d2d.cpp是已知像素点和像素点之间的关系用对极几何和三角化求解3D点，

pose_estimation_3d2d.cpp用三对点估计位姿P3P
输入３对3D-2D匹配点，3D点世界坐标系的坐标，2D 相机成像平面的投影。

pose_estimation_3d3d.cpp一旦3D点在相机坐标系下面的坐标能够算出来，我们就得到了3D-3D的对应点，把PnP问题转换成了ICP问题。

标定生成yaml参数文件

针孔鱼眼相机camera和惯导IMU标定方法汇总以及详细标定过程