ORB_SLAM2带回环的稠密建图（包括自己数据集的采集）

稠密建图

按照下面的网站进行安装

https://github.com/xiaobainixi/ORB-SLAM2_RGBD_DENSE_MAP

遇到的问题

Eigen版本

Eigen库的版本要求为3.1.0。可以在usr/include中安装两个版本的EIgen，使用那个用哪个。具体步骤参考

https://blog.csdn.net/weixin_43828675/article/details/118660322?spm=1001.2014.3001.5501

读取字典时，报错：段错误（核心已存储）

修改build.sh为

echo "Configuring and building Thirdparty/DBoW2 ..."

cd Thirdparty/DBoW2
mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j8

cd ../../g2o

echo "Configuring and building Thirdparty/g2o ..."

mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j8

cd ../../../

echo "Uncompress vocabulary ..."

cd Vocabulary
tar -xf ORBvoc.txt.tar.gz
cd ..

echo "Configuring and building ORB_SLAM2 ..."

mkdir build
cd build
cmake .. -DCMAKE_BUILD_TYPE=Release
make -j8
 
cd ..

echo "Converting vocabulary to binary"
./tools/bin_vocabulary

运行

./bin/rgbd_tum ./Vocabulary/ORBvoc.bin Examples/RGB-D/TUM1.yaml /home/dk/桌面/dataset/rgbd_dataset_freiburg1_room /home/dk/桌面/dataset/rgbd_dataset_freiburg1_room/associate.txt

读者需要自行进行修改。

自己数据集的采集

笔者使用RealSenseD435进行数据集的采集。读取相机ros节点发出的消息，使用CV_bridge转变为opencv类型进行存储

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>
#include<fstream>
#include<string>
#include<sstream>
#include<iomanip>
#include<sys/time.h>
#include<opencv2/opencv.hpp>
#include<opencv2/core/core.hpp>

#include<ros/ros.h>
#include<std_msgs/String.h>
#include<cv_bridge/cv_bridge.h>
#include<message_filters/subscriber.h>
#include<message_filters/time_synchronizer.h>
#include<message_filters/sync_policies/approximate_time.h>

using namespace std;
using namespace cv;

int counter = 0;
int flag = 0;

// txt配置文件
ofstream ofs_color;
ofstream ofs_depth;
ofstream association;   // 颜色和深度图根据时间戳进行对齐后的文件

void GrabRGBD(const sensor_msgs::ImageConstPtr &msgRGB, const sensor_msgs::ImageConstPtr &msgD)
{
    // Copy the ros image message to cv::Mat.
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try
    {
        cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD = cv_bridge::toCvShare(msgD);
    }
    catch (cv_bridge::Exception &e)
    {
        ROS_ERROR("cv_bridge exception: %s", e.what());
        return;
    }
    
    imshow("Depth",cv_ptrD->image);
    imshow("RGB",cv_ptrRGB->image);

    if (flag < 10)
    {
        ostringstream ss_color;
        ss_color << "//media//dk//My Passport//self_dataset//camera_pose//rgb//" << 0 << ".png";

        ostringstream ss_depth;
        ss_depth << "//media//dk//My Passport//self_dataset//camera_pose//depth//" << 0 << ".png";

        imwrite(ss_color.str(), cv_ptrRGB->image);
        imwrite(ss_depth.str(), cv_ptrD->image);    
        flag++;
        counter = 0;
    }
    else
    {
        ostringstream ss_color;
        ss_color << "//media//dk//My Passport//self_dataset//camera_pose//rgb//" << to_string(counter) << ".png";
        ofs_color<<to_string(counter)<<" "<<"rgb/"<<to_string(counter)<<".png"<<endl;

        ostringstream ss_depth;
        ss_depth << "//media//dk//My Passport//self_dataset//camera_pose//depth//" << to_string(counter) << ".png";
        ofs_depth<<to_string(counter)<<" "<<"depth/"<<to_string(counter)<<".png"<<endl;

        association<<counter<<" "<<"rgb/"<<counter<<".png"<<" "<<counter<<" "<<"depth/"<<counter<<".png"<<endl;
        imwrite(ss_color.str(), cv_ptrRGB->image);
        imwrite(ss_depth.str(), cv_ptrD->image);
    }

    counter++;
    waitKey(50);
    if(waitKey(50) == 'q')
        return;
    return;
}

int main(int argc, char *argv[])
{
    setlocale(LC_ALL,"");
    ros::init(argc,argv,"RGBD");
    ros::NodeHandle nh;
    ofs_color.open("//media//dk//My Passport//self_dataset//camera_pose//rgb.txt",ios::out|ios::trunc);
    ofs_depth.open("//media//dk//My Passport//self_dataset//camera_pose//depth.txt",ios::out|ios::trunc);
    association.open("//media//dk//My Passport//self_dataset//camera_pose//associate.txt",ios::out|ios::trunc);


    // 订阅的话题名称
    string rgb_topic = "/camera/color/image_raw";
    string depth_topic =  "/camera/aligned_depth_to_color/image_raw";

    cout << endl << "------------------------" << endl;
    cout << "- rgb_topic: " << rgb_topic << endl;
    cout << "- depth_topic: " << depth_topic << endl;

    // 用于多传感器消息的同步
    message_filters::Subscriber <sensor_msgs::Image> rgb_sub(nh, rgb_topic, 1);         // 订阅RGB图像
    message_filters::Subscriber <sensor_msgs::Image> depth_sub(nh, depth_topic, 1);     // 订阅深度图像
    typedef message_filters::sync_policies::ApproximateTime <sensor_msgs::Image, sensor_msgs::Image> sync_pol;  // 根据时间戳进行对齐
    message_filters::Synchronizer <sync_pol> sync(sync_pol(10), rgb_sub, depth_sub);    // 进行信息同步
    sync.registerCallback(boost::bind(&GrabRGBD, _1, _2));  // 将_1, _2传入函数GrabRGBD中

    ros::spin();
    return 0;
}

然后在rgb和depth文件夹中删除0.png即可。


运行数据集

./bin/rgbd_tum Vocabulary/ORBvoc.bin Examples/RGB-D/RealSenseD435i.yaml /media/dk/My\ Passport/self_dataset/camera_pose /media/dk/My\ Passport/self_dataset/camera_pose/associate.txt

读者需要修改为自己保存的数据集的位置

半径滤波后