經(jīng)典基于外觀的SLAM框架-RTABMAP

1. RTABMAP整體框架

??RTABMAP是采用優(yōu)化算法的方式求解SLAM問題的SLAM框架，本賽題的定位參考輸入信息只有RGB-D相機(jī)的圖像信息，RTABMAP支持RGB-D視覺信息的輸入，并且輸出包括位姿、二維占據(jù)柵格地圖（2D Occupancy）、三維占據(jù)地圖（3D Occupancy）和點(diǎn)云地圖，輸出地圖具有多樣性，RTABMAP有分級(jí)的內(nèi)存管理機(jī)制，能夠?qū)斎霂M(jìn)行圖結(jié)構(gòu)建模和統(tǒng)籌規(guī)劃，將建圖部分的計(jì)算量簡(jiǎn)化到設(shè)備可以承受的范圍內(nèi)。
視覺傳感器有兩個(gè)優(yōu)點(diǎn)，其一是定位的位姿具有魯棒性，不易丟失；其二是感知到的信息量較大，與激光傳感器相比得到的語義更加豐富。里程計(jì)能提供短期運(yùn)動(dòng)信息，這種信息可以給位姿估計(jì)提供預(yù)測(cè)信息，當(dāng)環(huán)境特征缺失時(shí)，里程計(jì)提供的短期運(yùn)動(dòng)信息也可以參考處理。RTABMAP的系統(tǒng)框架圖：

??在RTABMAP中，視覺傳感器是必備的輸入傳感器，另外可以輸入激光雷達(dá)傳感器作為選配輸入傳感器，當(dāng)然也可以將RGB-D的信息通過轉(zhuǎn)換工具轉(zhuǎn)換為激光點(diǎn)數(shù)據(jù)，一般這種方案存在于接入ros-navigation工具包使用，該工具包的必備輸入是激光掃描輸入，RTABMAP接入里程計(jì)的形式可以是輪式里程計(jì)、視覺里程計(jì)、激光里程計(jì)等，由于賽題條件的限制，我們采用視覺里程計(jì)，RTABMAP托管機(jī)器人坐標(biāo)和地圖坐標(biāo)系的轉(zhuǎn)換關(guān)系完成位姿估計(jì)，傳感器數(shù)據(jù)會(huì)存入STM（Short-Term Memory），這段內(nèi)存塊被稱為短期內(nèi)存模塊，由于RTABMAP采用圖結(jié)構(gòu)來組織地圖，圖結(jié)構(gòu)包含節(jié)點(diǎn)和邊，所以每一幀傳感器數(shù)據(jù)的到來都會(huì)創(chuàng)建一個(gè)節(jié)點(diǎn)，該節(jié)點(diǎn)中存儲(chǔ)的內(nèi)容包括該幀對(duì)應(yīng)的里程計(jì)位姿、該幀所有傳感器的視覺觀測(cè)數(shù)據(jù)以及在該幀上提取出的視覺單詞（基于詞袋模型的回環(huán)檢測(cè)）和局部地圖等。節(jié)點(diǎn)之間的連接邊分為三種：

1. 相鄰連接邊。相鄰連接邊里程計(jì)能夠直接獲得相鄰連節(jié)點(diǎn)之間的位姿變換關(guān)系。
2. 閉環(huán)連接邊。閉環(huán)連接邊基于視覺詞袋的閉環(huán)檢測(cè)和多視圖幾何計(jì)算出當(dāng)前節(jié)點(diǎn)與閉環(huán)節(jié)點(diǎn)之間的位姿變換關(guān)系。
3. 相似連接邊。相似連接邊主要用于激光掃描數(shù)據(jù)相關(guān)的閉環(huán)檢測(cè)，因?yàn)榧す鈷呙柙O(shè)備對(duì)于巡視器本身的轉(zhuǎn)向不敏感，更容易在閉環(huán)檢測(cè)上出現(xiàn)問題。

??在閉環(huán)檢測(cè)檢測(cè)到閉環(huán)時(shí)，RTABMAP將所有的節(jié)點(diǎn)和所有的邊進(jìn)行全局優(yōu)化，優(yōu)化的過程通過圖優(yōu)化模塊進(jìn)行，同時(shí)會(huì)對(duì)內(nèi)部的位姿漂移問題進(jìn)行優(yōu)化（主要是修正地圖點(diǎn)到里程計(jì)的變換關(guān)系），經(jīng)過修正后的節(jié)點(diǎn)中存儲(chǔ)的局部地圖通過拼接集成全局地圖。

2.RTABMAP的內(nèi)存管理機(jī)制

??由于視覺傳感器的每一幀到來都會(huì)創(chuàng)建一個(gè)節(jié)點(diǎn)，那么建圖規(guī)模達(dá)到很大之后，節(jié)點(diǎn)的數(shù)量也是同樣非常大的，在此種情況下，對(duì)所有進(jìn)行進(jìn)行全局優(yōu)化的計(jì)算將會(huì)十分的耗費(fèi)資源甚至難以行進(jìn)，為了保證優(yōu)化和回環(huán)檢測(cè)的實(shí)時(shí)性，RTABMAP采用了分級(jí)的內(nèi)存管理機(jī)制。
??RTABMAP采用圖結(jié)構(gòu)維護(hù)地圖結(jié)構(gòu)，將所有的節(jié)點(diǎn)分為三類，即存儲(chǔ)局部地圖節(jié)點(diǎn)的STM、存儲(chǔ)全局地圖節(jié)點(diǎn)的工作內(nèi)存WM（Working Memory）、存儲(chǔ)短期與全局地圖缺乏相關(guān)性的不重要節(jié)點(diǎn)的長(zhǎng)期內(nèi)存LTM（Long-Term Memory），節(jié)點(diǎn)的分類實(shí)例如下圖所示，圖中橫向的箭頭表征相鄰連接邊，豎向的箭頭表征閉環(huán)連接邊，灰色節(jié)點(diǎn)存儲(chǔ)在STM中，白色節(jié)點(diǎn)存儲(chǔ)在WM中，黑色節(jié)點(diǎn)處于LTM中，編號(hào)455節(jié)點(diǎn)外圍有黑色外圈，表征當(dāng)前巡視器位姿，其中每個(gè)節(jié)點(diǎn)都包含視覺傳感器觀測(cè)的數(shù)據(jù)、里程計(jì)的位姿信息和各連接邊信息等。

??RGB-D的視覺傳感器數(shù)據(jù)輸入SM內(nèi)存塊，本內(nèi)存塊對(duì)傳感器的觀測(cè)數(shù)據(jù)進(jìn)行數(shù)據(jù)降維、特征提取、位姿計(jì)算后，加入STM內(nèi)存塊，在STM內(nèi)存塊添加時(shí)序相鄰的節(jié)點(diǎn)若相似度較高，則對(duì)兩個(gè)節(jié)點(diǎn)進(jìn)行權(quán)重更新的融合計(jì)算，當(dāng)STM區(qū)滿后，挑選最早加入STM的節(jié)點(diǎn)移出，并加入到WM進(jìn)行后續(xù)的閉環(huán)檢測(cè)搜索節(jié)點(diǎn)儲(chǔ)備，WM中的閉環(huán)檢測(cè)涉及詞袋模型和貝葉斯濾波，視覺詞袋用于計(jì)算兩個(gè)節(jié)點(diǎn)之間的相似度，貝葉斯濾波器維護(hù)節(jié)點(diǎn)之間的相似性。當(dāng)WM內(nèi)存區(qū)滿后，挑選最早進(jìn)入WM區(qū)的節(jié)點(diǎn)移出，并加入到LTM中。內(nèi)存管理機(jī)制的框圖如下圖所示。

3. 視覺里程計(jì)

??視覺里程計(jì)采用F2M（Frame-to-Map）實(shí)現(xiàn)，F(xiàn)2F（Frame-to-Frame）是利用圖像幀到圖像幀之間的特征點(diǎn)配準(zhǔn)來進(jìn)行位姿變換的計(jì)算，而F2M利用圖像幀到地圖之間的特征點(diǎn)配準(zhǔn)進(jìn)行位姿變換的配準(zhǔn)，RTABMAP的視覺里程計(jì)系統(tǒng)框圖如下圖所示。

4. 局部地圖

??在RTABMAP的STM中插入新節(jié)點(diǎn)后，系統(tǒng)會(huì)利用深度圖像生成對(duì)應(yīng)的局部地圖，局部地圖的參考坐標(biāo)系為巡視器本身的坐標(biāo)系，全局地圖（最終經(jīng)過局部地圖拼接而成）的參考坐標(biāo)系為世界坐標(biāo)系，局部地圖與全局地圖通過機(jī)器人到世界坐標(biāo)系的坐標(biāo)變換完成。視覺里程計(jì)節(jié)點(diǎn)利用閉環(huán)檢測(cè)和全局優(yōu)化，以此種方式維護(hù)全局位姿，全局位姿的核心是地圖到視覺里程計(jì)的坐標(biāo)變換關(guān)系。局部地圖的輸出流程處理框圖如下圖所示。

5. 回環(huán)檢測(cè)與圖優(yōu)化

??對(duì)于局部建圖的累計(jì)誤差，需要回環(huán)檢測(cè)和全局優(yōu)化的介入，視覺詞袋模型和貝葉斯濾波器用于回環(huán)檢測(cè)，視覺詞袋模型屬于視覺SLAM的常規(guī)模型和解決辦法，其主要用于快速匹配相似度高的幀，而貝葉斯濾波器維護(hù)所有候選節(jié)點(diǎn)相似度的概率分布。設(shè)當(dāng)前位姿節(jié)點(diǎn)為$L_t$，隨機(jī)變量$S_t$表示W(wǎng)M中所有待檢測(cè)的候選節(jié)點(diǎn)，隨機(jī)變量$S_t=i$的概率表征$L_t$與$L_i$有回環(huán)的可能性，根據(jù)貝葉斯可得$S_t$更新的公式如下式所示。
P ( S t | L t ) = η P ( L t | S t ) ∑ i = ? 1 t n P ( S t | S t ? 1 = i ) P ( S t ? 1 = i | L t ? 1 ) P\left(S_t\middle| L^t\right)=\eta P\left(L_t\middle| S_t\right)\sum_{i=-1}^{t_n}{P\left(S_t\middle| S_{t-1}=i\right)P\left(S_{t-1}=i\middle| L^{t-1}\right)} P(St? ?Lt)=ηP(Lt?∣St?)i=?1∑tn??P(St?∣St?1?=i)P(St?1?=i ?Lt?1)
??其中$L=L_{?1},\dots ,L_t$表示t時(shí)刻WM中所有的節(jié)點(diǎn)，觀測(cè)模型$P\left(L_t|S_t\right)$可以通過似然函數(shù)$?\left(S_t=j|L_t\right)=P\left(L_t|S_t=j\right)$計(jì)算。歸一化的觀測(cè)模型$P\left(S_t|L^t\right)$與閾值對(duì)比，若低于閾值則回環(huán)檢測(cè)成功，否則取$P\left(S_t|L^t\right)$中概率取值最高的$S_t=i$對(duì)應(yīng)的節(jié)點(diǎn)$L_i$選定為回環(huán)節(jié)點(diǎn)。最后將WM中所有的節(jié)點(diǎn)和約束邊進(jìn)行全局優(yōu)化，同時(shí)對(duì)里程計(jì)位姿進(jìn)行修正。

6. 代碼工程實(shí)踐

??RTABMAP的源碼地址在https://github.com/introlab/rtabmap
??RTAB-Map（Real-Time Appearance-Based Mapping）是一個(gè)開源的RGB-D SLAM（Simultaneous Localization and Mapping）庫(kù)，用于實(shí)時(shí)構(gòu)建環(huán)境地圖。它結(jié)合了視覺識(shí)別和圖像姿態(tài)估計(jì)，并使用回環(huán)檢測(cè)來優(yōu)化地圖的一致性。RTAB-Map的代碼組織結(jié)構(gòu)主要包括以下幾個(gè)主要組件：

1. Core（核心）：這是RTAB-Map的核心模塊，提供了地圖構(gòu)建、回環(huán)檢測(cè)和姿態(tài)估計(jì)等功能。它實(shí)現(xiàn)了基于視覺特征的回環(huán)檢測(cè)算法和基于圖優(yōu)化的姿態(tài)估計(jì)方法。

2. Database（數(shù)據(jù)庫(kù)）：RTAB-Map使用數(shù)據(jù)庫(kù)來存儲(chǔ)和管理地圖數(shù)據(jù)。數(shù)據(jù)庫(kù)模塊提供了對(duì)地圖的讀取、寫入和查詢功能。它可以將地圖數(shù)據(jù)保存到硬盤上的SQLite數(shù)據(jù)庫(kù)中，并且可以根據(jù)需要從數(shù)據(jù)庫(kù)中加載地圖。

3. GUI（圖形用戶界面）：RTAB-Map附帶了一個(gè)基于Qt的圖形用戶界面，用于可視化地圖構(gòu)建的過程和結(jié)果。GUI模塊提供了交互式地圖瀏覽、參數(shù)設(shè)置和結(jié)果導(dǎo)出等功能。

4. Plugins（插件）：RTAB-Map支持插件擴(kuò)展，可以通過插件機(jī)制添加額外的功能。例如，它提供了插件接口來支持不同類型的傳感器、回環(huán)檢測(cè)算法和地圖優(yōu)化算法。

5. Examples（示例）：RTAB-Map提供了一些示例代碼，演示如何使用庫(kù)中的不同功能。示例代碼包括從RGB-D相機(jī)讀取數(shù)據(jù)、構(gòu)建地圖、保存和加載地圖等。

以RGB-D作為輸入的程序參考示例：文章來源地址http://www.zghlxwxcb.cn/news/detail-485554.html

#include <ros/ros.h>
#include <std_msgs/String.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/PointCloud2.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/opencv.hpp>
#include <rtabmap_ros/rgbd_sync.h>
#include <rtabmap_ros/rgbd_odometry.h>
#include <rtabmap_ros/rgbd_mapping.h>

void imageCallback(
    const sensor_msgs::ImageConstPtr& rgbMsg,
    const sensor_msgs::ImageConstPtr& depthMsg,
    const sensor_msgs::CameraInfoConstPtr& infoMsg)
{
    cv_bridge::CvImagePtr cvRgbPtr;
    cv_bridge::CvImagePtr cvDepthPtr;
    try
    {
        cvRgbPtr = cv_bridge::toCvCopy(rgbMsg, sensor_msgs::image_encodings::BGR8);
        cvDepthPtr = cv_bridge::toCvCopy(depthMsg, sensor_msgs::image_encodings::TYPE_32FC1);
    }
    catch (cv_bridge::Exception& e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv::Mat rgbImage = cvRgbPtr->image;
    cv::Mat depthImage = cvDepthPtr->image;

    // Perform RGB-D SLAM with RTAB-Map

    // ...
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "rtabmap_example");
    ros::NodeHandle nh;

    message_filters::Subscriber<sensor_msgs::Image> rgbSub(nh, "/camera/rgb/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::Image> depthSub(nh, "/camera/depth/image_raw", 1);
    message_filters::Subscriber<sensor_msgs::CameraInfo> infoSub(nh, "/camera/rgb/camera_info", 1);

    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo> MySyncPolicy;
    message_filters::Synchronizer<MySyncPolicy> sync(MySyncPolicy(10), rgbSub, depthSub, infoSub);
    sync.registerCallback(boost::bind(&imageCallback, _1, _2, _3));

    ros::spin();

    return 0;
}

到了這里，關(guān)于經(jīng)典基于外觀的SLAM框架-RTABMAP（RGBD視覺輸入方案）的文章就介紹完了。如果您還想了解更多內(nèi)容，請(qǐng)?jiān)谟疑辖撬阉鱐OY模板網(wǎng)以前的文章或繼續(xù)瀏覽下面的相關(guān)文章，希望大家以后多多支持TOY模板網(wǎng)！