一起做RGB-D SLAM (5)

第五講 Visual Odometry (視覺里程計)

2016.11 更新

把原文的SIFT替換成了ORB，這樣你可以在沒有nonfree模塊下使用本程序了。
去掉了cv::cv2Eigen函數，因爲有些讀者找不到這個函數。
檢查了minDis爲零的情況。
之前t的訪問時，行和列顛倒了，會對結果產生一定影響，現在修正了。
請以現在的github上源碼爲準。

　　讀者朋友們大家好，又到了我們開講rgbd slam的時間了。由於前幾天博主在忙開會拍婚紗照等一系列亂七八糟的事情，這一講稍微做的慢了些，先向讀者們道個歉！

　　上幾講中，我們詳細講了兩張圖像間的匹配與運動估計。然而一個實際的機器人總不可能只有兩個圖像數據吧？那該多麼寂寞呀。所以，本講開始，我們要處理一個視頻流，包含八百左右的數據啦。這纔像是在做SLAM嘛！

　　小蘿蔔：那我們去哪裏下載這些數據呢？

　　師兄：可以到我的百度雲裏去：http://yun.baidu.com/s/1i33uvw5

　　因爲有點大（400多M），我就沒有傳到git上。不然運行前四講的代碼就要下一堆東西啦。打開這個數據集，你會看到裏頭有和兩個文件夾，分別是RGB圖與深度圖。前幾講的數據也是取自這裏的哦。

　　小蘿蔔：這算不算師兄你在偷懶呢？

　　師兄：呃，這個，總、總之，我們這裏暫時先用這些數據啦。它們取自nyuv2數據集：http://cs.nyu.edu/~silberman/datasets/nyu_depth_v2.html 這可是一個國際上認可的，相當有名的數據集哦。如果你想要跑自己的數據，當然也可以，不過需要你進行一些預處理啦。

　　本講中，我們利用前幾講寫好的代碼，完成一個視覺里程計(visual odometry)的編寫。什麼是視覺里程計呢？簡而言之，就是把新來的數據與上一幀進行匹配，估計其運動，然後再把運動累加起來的東西。畫成示意圖的話，就是下面這個樣子：

　　師兄：大家看懂了不？這實際上和濾波器很像，通過不斷的兩兩匹配，估計機器人當前的位姿，過去的就給丟棄了。這個思路比較簡單，實際當中也比較有效，能夠保證局部運動的正確性。下面我們來實現一下visual odometry。

　　小蘿蔔：道理我是明白了，可是師兄你這畫風究竟是哪個年代的啊……

準備工作

　　爲了保證代碼的簡潔，我們要把以前做過的東西封裝成函數，寫在slamBase.cpp中，以便將來調用。（不過，由於是算法性質的內容，就不封成c++的對象了）。

　　首先工具函數：將cv的旋轉矢量與位移矢量轉換爲變換矩陣，類型爲Eigen::Isometry3d；

　　src/slamBase.cpp

// cvMat2Eigen
Eigen::Isometry3d cvMat2Eigen( cv::Mat& rvec, cv::Mat& tvec )
{
    cv::Mat R;
    cv::Rodrigues( rvec, R );
    Eigen::Matrix3d r;
    cv::cv2eigen(R, r);
  
    // 將平移向量和旋轉矩陣轉換成變換矩陣
    Eigen::Isometry3d T = Eigen::Isometry3d::Identity();

    Eigen::AngleAxisd angle(r);
    Eigen::Translation<double,3> trans(tvec.at<double>(0,0), tvec.at<double>(0,1), tvec.at<double>(0,2));
    T = angle;
    T(0,3) = tvec.at<double>(0,0); 
    T(1,3) = tvec.at<double>(0,1); 
    T(2,3) = tvec.at<double>(0,2);
    return T;
}

另一個函數：將新的幀合併到舊的點雲裏：

// joinPointCloud 
// 輸入：原始點雲，新來的幀以及它的位姿
// 輸出：將新來幀加到原始幀後的圖像
PointCloud::Ptr joinPointCloud( PointCloud::Ptr original, FRAME& newFrame, Eigen::Isometry3d T, CAMERA_INTRINSIC_PARAMETERS& camera ) 
{
    PointCloud::Ptr newCloud = image2PointCloud( newFrame.rgb, newFrame.depth, camera );

    // 合併點雲
    PointCloud::Ptr output (new PointCloud());
    pcl::transformPointCloud( *original, *output, T.matrix() );
    *newCloud += *output;

    // Voxel grid 濾波降採樣
    static pcl::VoxelGrid<PointT> voxel;
    static ParameterReader pd;
    double gridsize = atof( pd.getData("voxel_grid").c_str() );
    voxel.setLeafSize( gridsize, gridsize, gridsize );
    voxel.setInputCloud( newCloud );
    PointCloud::Ptr tmp( new PointCloud() );
    voxel.filter( *tmp );
    return tmp;
}

另外，在parameters.txt中，我們增加了幾個參數，以便調節程序的性能：

# part 5 
# 數據相關
# 起始與終止索引
start_index=1
end_index=700
# 數據所在目錄
rgb_dir=../data/rgb_png/
rgb_extension=.png
depth_dir=../data/depth_png/
depth_extension=.png
# 點雲分辨率
voxel_grid=0.02
# 是否實時可視化
visualize_pointcloud=yes
# 最小匹配數量
min_good_match=10
# 最小內點
min_inliers=5
# 最大運動誤差
max_norm=0.3

　前面幾個參數是相當直觀的：指定RGB圖與深度圖所在的目錄，起始與終止的圖像索引（也就是第1張到第700張的slam啦）。後面幾個參數，會在後面進行解釋。

實現VO

　　最後，利用之前寫好的工具函數，實現一個VO:

　　src/visualOdometry.cpp

/*************************************************************************
    > File Name: rgbd-slam-tutorial-gx/part V/src/visualOdometry.cpp
    > Author: xiang gao
    > Mail: [email protected]
    > Created Time: 2015年08月01日 星期六 15時35分42秒
 ************************************************************************/

#include <iostream>
#include <fstream>
#include <sstream>
using namespace std;

#include "slamBase.h"

// 給定index，讀取一幀數據
FRAME readFrame( int index, ParameterReader& pd );
// 度量運動的大小
double normofTransform( cv::Mat rvec, cv::Mat tvec );

int main( int argc, char** argv )
{
    ParameterReader pd;
    int startIndex  =   atoi( pd.getData( "start_index" ).c_str() );
    int endIndex    =   atoi( pd.getData( "end_index"   ).c_str() );

    // initialize
    cout<<"Initializing ..."<<endl;
    int currIndex = startIndex; // 當前索引爲currIndex
    FRAME lastFrame = readFrame( currIndex, pd ); // 上一幀數據
    // 我們總是在比較currFrame和lastFrame
    string detector = pd.getData( "detector" );
    string descriptor = pd.getData( "descriptor" );
    CAMERA_INTRINSIC_PARAMETERS camera = getDefaultCamera();
    computeKeyPointsAndDesp( lastFrame, detector, descriptor );
    PointCloud::Ptr cloud = image2PointCloud( lastFrame.rgb, lastFrame.depth, camera );
    
    pcl::visualization::CloudViewer viewer("viewer");

    // 是否顯示點雲
    bool visualize = pd.getData("visualize_pointcloud")==string("yes");

    int min_inliers = atoi( pd.getData("min_inliers").c_str() );
    double max_norm = atof( pd.getData("max_norm").c_str() );

    for ( currIndex=startIndex+1; currIndex<endIndex; currIndex++ )
    {
        cout<<"Reading files "<<currIndex<<endl;
        FRAME currFrame = readFrame( currIndex,pd ); // 讀取currFrame
        computeKeyPointsAndDesp( currFrame, detector, descriptor );
        // 比較currFrame 和 lastFrame
        RESULT_OF_PNP result = estimateMotion( lastFrame, currFrame, camera );
        if ( result.inliers < min_inliers ) //inliers不夠，放棄該幀
            continue;
        // 計算運動範圍是否太大
        double norm = normofTransform(result.rvec, result.tvec);
        cout<<"norm = "<<norm<<endl;
        if ( norm >= max_norm )
            continue;
        Eigen::Isometry3d T = cvMat2Eigen( result.rvec, result.tvec );
        cout<<"T="<<T.matrix()<<endl;
        
        //cloud = joinPointCloud( cloud, currFrame, T.inverse(), camera );
        cloud = joinPointCloud( cloud, currFrame, T, camera );
        
        if ( visualize == true )
            viewer.showCloud( cloud );

        lastFrame = currFrame;
    }

    pcl::io::savePCDFile( "data/result.pcd", *cloud );
    return 0;
}

FRAME readFrame( int index, ParameterReader& pd )
{
    FRAME f;
    string rgbDir   =   pd.getData("rgb_dir");
    string depthDir =   pd.getData("depth_dir");
    
    string rgbExt   =   pd.getData("rgb_extension");
    string depthExt =   pd.getData("depth_extension");

    stringstream ss;
    ss<<rgbDir<<index<<rgbExt;
    string filename;
    ss>>filename;
    f.rgb = cv::imread( filename );

    ss.clear();
    filename.clear();
    ss<<depthDir<<index<<depthExt;
    ss>>filename;

    f.depth = cv::imread( filename, -1 );
    return f;
}

double normofTransform( cv::Mat rvec, cv::Mat tvec )
{
    return fabs(min(cv::norm(rvec), 2*M_PI-cv::norm(rvec)))+ fabs(cv::norm(tvec));
}

其實一個VO也就一百行的代碼，相信大家很快就能讀懂的。我們稍加解釋。

FRAME readFrame( int index, ParameterReader& pd ) 是讀取幀數據的函數。告訴它我要讀第幾幀的數據，它就會乖乖的把數據給找出來，返回一個FRAME結構體。
在得到匹配之後，我們判斷了匹配是否成功，並把失敗的數據丟棄。爲什麼這樣做呢？因爲之前的算法，對於任意兩張圖像都能做出一個結果。對於無關的圖像，就明顯是不對的。所以要去除匹配失敗的情形。
如何檢測匹配失敗呢？我們採用了三個方法：

1. 去掉goodmatch太少的幀，最少的goodmatch定義爲：
```
min_good_match=10
```
2. 去掉solvePnPRASNAC裏，inlier較少的幀，同理定義爲：
```
min_inliers=5
```
3. 去掉求出來的變換矩陣太大的情況。因爲假設運動是連貫的，兩幀之間不會隔的太遠：
```
max_norm=0.3
```

　　如何知道兩幀之間不隔太遠呢？我們計算了一個度量運動大小的值：∥Δt∥+min(2π−∥r∥,∥r∥)。它可以看成是位移與旋轉的範數加和。當這個數大於閾值max_norm時，我們就認爲匹配出錯了。

　　經過這三道工序處理後，vo的結果基本能保持正確啦。下面是一個gif圖片：

　　小蘿蔔：師兄！這效果相當不錯啊！

　　師兄：嗯，至少有點兒像樣啦，雖然問題還是挺多的。具體有哪些問題呢？我們留到下一講裏再說。各位同學也可以運行一下自己的代碼，看看結果哦。

tips：

當點雲出現時，可按5顯示顏色，然後按r重置視角，快速查看點雲；
可以調節parameters.txt中的voxel_grid值來設置點雲分辨率。0.01表示每1cm3的格子裏有一個點。

課後作業

　　請觀察vo的運行狀態並嘗試不同參數，總結它有哪些侷限性。

　　本講代碼： https://github.com/gaoxiang12/rgbd-slam-tutorial-gx/tree/master/part%20V 數據鏈接見前面百度盤。

TIPS

如果在編譯時期出現Link錯誤，請檢查你是否鏈接到了PCL的visualization模塊。如果實在不確定，就照着github源碼抄一遍。
在運動時期，由於存在兩張圖像完全一樣的情況，導致匹配時距離爲0。由於本節程序的設置，這種情況會被當成沒有匹配，導致VO丟失。請你自己fix一下這個bug，我在下一節當中也進行了檢查。

一起做RGB-D SLAM (5)

第五講 Visual Odometry (視覺里程計)

2016.11 更新

準備工作

實現VO

課後作業

TIPS

Stereo Matching文獻筆記之（一）：《Cross-Scale Cost Aggregation for Stereo Matching》讀後感~

一起做RGB-D SLAM(7) （完結篇）

Stereo Matching文獻筆記之（六）：淺談置信度傳播算法（Belief-Propagation）在立體匹配中的應用~

Stereo Matching文獻筆記之（二）：《A Non-Local Cost Aggregation Method for Stereo Matching》讀後感~

編程經驗：關於OpenCV源碼調試的好文章轉載~

Mac下配置sublime實現LaTeX

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結