注：本人复现的DS-SLAM是这位博主的改进版https://github.com/yubaoliu/DS-SLAM

使用数据集复现时的教程参考：https://blog.csdn.net/m0_43398209/article/details/122279304
环境：ubuntu18.04

1.创建源文件

在/工作空间路径/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM目录下创建源文件，如myD435i_realtime.cc,写入以下内容

#include <geometry_msgs/PoseWithCovarianceStamped.h>
#include <geometry_msgs/Pose.h>
#include <nav_msgs/Odometry.h>
#include <tf/transform_broadcaster.h>

#include <octomap/ColorOcTree.h>
#include <octomap/octomap.h>

#include "../../../include/System.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 octomap;

ros::Publisher CamPose_Pub;
ros::Publisher CamOdom_Pub;
ros::Publisher odom_Pub;

geometry_msgs::PoseStamped Cam_Pose;
geometry_msgs::PoseWithCovarianceStamped Cam_odom;

cv::Mat Camera_Pose;
tf::Transform orb_slam;
tf::TransformBroadcaster* orb_slam_broadcaster;
std::vector<float> Pose_quat(4);
std::vector<float> Pose_trans(3);

ros::Time current_time,last_time;
double lastx=0,lasty=0,lastth=0;
unsigned int a=0,b=0;
octomap::ColorOcTree tree(0.05);

void Pub_CamPose(cv::Mat& pose);

typedef octomap::ColorOcTree::leaf_iterator it_t;

//时间统计
unsigned long nImages=0;                     //统计处理的帧数量
vector<double> vTimesTrack;
vector<double> vOrbTime;
vector<double> vMovingTime;
double segmentationTime = 0;

//为SLAM系统获取RGBD图像帧的类
class ImageGrabber
{
public:

    //构造函数
    ImageGrabber(ORB_SLAM2::System* pSLAM):mpSLAM(pSLAM){}

    //回调函数：订阅图像帧并处理
    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);

    //成员变量
    ORB_SLAM2::System* mpSLAM;
};

int main(int argc,char **argv)
{
    //step1:初始化
    setlocale(LC_ALL,"");

    ROS_INFO("hello world");

    ::google::InitGoogleLogging(argv[0]);
    ros::init(argc,argv,"myD435i");
    ros::start();

    if(argc!=6)
    {
        cerr<<endl
             << "Usage: TUM path_to_vocabulary path_to_settings path_to_prototxt path_to_caffemodel path_to_pascal.png" << endl;
        return 1;
    }
    
    //step2:创建SLAM系统
    ORB_SLAM2::Viewer* viewer;
    viewer=new ORB_SLAM2::Viewer();
    
    ORB_SLAM2::System SLAM(argv[1],argv[2],argv[3],argv[4],argv[5],ORB_SLAM2::System::RGBD,viewer);

    usleep(50);
   
    //step3:创建为SLAM系统获取图像帧的类
    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;                     //创建ROS句柄

    //step4:实例化消息发布者对象
    CamPose_Pub=nh.advertise<geometry_msgs::PoseStamped>("/Camera_Pose",1);
    CamOdom_Pub=nh.advertise<geometry_msgs::PoseWithCovarianceStamped>("/Camera_Odom",1);
    odom_Pub=nh.advertise<nav_msgs::Odometry>("odom",50);

    current_time=ros::Time::now();
    last_time=ros::Time::now();

    //step5:使用message_filter（消息滤波器）订阅realsenseD435i的图像帧
    message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh,"/camera/color/image_raw",1);
    message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh,"/camera/aligned_depth_to_color/image_raw",1);

    //message filter做时间同步（要求时间戳相近）
    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);

    //step6：注册回调
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

    ros::spin();

    SLAM.Shutdown();

    //时间统计
    sort(vTimesTrack.begin(),vTimesTrack.end());
    double totalTime=0;
    for(unsigned long ni=0;ni<nImages;ni++)
    {
        totalTime+=vTimesTrack[ni];
    }
    double orbTotalTime=0;
    for(unsigned long ni=0;ni<nImages;ni++)
    {
        orbTotalTime+=vOrbTime[ni];
    }
    double movingTotalTime=0;
    for(unsigned long ni=0;ni<nImages;ni++)
    {
        movingTotalTime+=vMovingTime[ni];
    }
    
    cout << "-------" << endl
         << endl;
    cout<<"Total frames:"<<nImages<<endl;
    cout<<"vTimesTrack.size:"<<vTimesTrack.size()<<endl;
    cout<<"Total time:"<<totalTime<<endl;
    cout<<"median tracking time:"<<vTimesTrack[nImages/2]<<endl;
    cout<<"mean tracking time:"<<totalTime/nImages<<endl;
    cout<<"mean orb extract time:"<<orbTotalTime/nImages<<endl;
    cout<<"mean moving detection time:"<<movingTotalTime/nImages<<endl;
    cout<<"mean segmentation time:"<<segmentationTime/nImages<<endl;

    //step7:保存相机轨迹
    SLAM.SaveTrajectoryTUM("myD435iCameraTrajectory.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("myD435iKeyFrameTrajectory.txt");

    ros::shutdown();

    return  0;
}

void Pub_CamPose(cv::Mat& pose)
{
    //组织位姿消息
    cv::Mat Rwc(3,3,CV_32F);
    cv::Mat twc(3,1,CV_32F);
    Eigen::Matrix<double,3,3> rotationMat;
    orb_slam_broadcaster=new tf::TransformBroadcaster;
    if(pose.dims<2||pose.rows<3)
    {
        Rwc=Rwc;
        twc=twc;
    }
    else
    {
        Rwc=pose.rowRange(0,3).colRange(0,3).t();
        twc=-Rwc*pose.rowRange(0,3).col(3);

        rotationMat<<   Rwc.at<float>(0,0),Rwc.at<float>(0,1),Rwc.at<float>(0,2),
                        Rwc.at<float>(1,0),Rwc.at<float>(1,1),Rwc.at<float>(1,2),
                        Rwc.at<float>(2,0),Rwc.at<float>(2,1),Rwc.at<float>(2,2);
                    
        Eigen::Quaterniond Q(rotationMat);

        Pose_quat[0]=Q.x();
        Pose_quat[1]=Q.y();
        Pose_quat[2]=Q.z();
        Pose_quat[3]=Q.w();

        Pose_trans[0]=twc.at<float>(0);
        Pose_trans[1]=twc.at<float>(1);
        Pose_trans[2]=twc.at<float>(2);

        orb_slam.setOrigin(tf::Vector3(Pose_trans[2],-Pose_trans[0],-Pose_trans[1]));
        orb_slam.setRotation(tf::Quaternion(Q.z(),-Q.x(),-Q.y(),-Q.w()));
        orb_slam_broadcaster->sendTransform(tf::StampedTransform(orb_slam,ros::Time::now(),"/map","/base_link"));

        //相机位姿
        Cam_Pose.header.stamp=ros::Time::now();
        Cam_Pose.header.frame_id="/map";
        tf::pointTFToMsg(orb_slam.getOrigin(),Cam_Pose.pose.position);
        tf::quaternionTFToMsg(orb_slam.getRotation(),Cam_Pose.pose.orientation);

        //相机里程计（有协方差）
        Cam_odom.header.stamp=ros::Time::now();
        Cam_odom.header.frame_id="/map";
        tf::pointTFToMsg(orb_slam.getOrigin(),Cam_odom.pose.pose.position);
        tf::quaternionTFToMsg(orb_slam.getRotation(),Cam_odom.pose.pose.orientation);
        Cam_odom.pose.covariance={
                                    0.01, 0, 0, 0, 0, 0,
                                    0, 0.01, 0, 0, 0, 0,
                                    0, 0, 0.01, 0, 0, 0,
                                    0, 0, 0, 0.01, 0, 0,
                                    0, 0, 0, 0, 0.01, 0,
                                    0, 0, 0, 0, 0, 0.01
        };

        //发布相机位姿与里程计
        CamPose_Pub.publish(Cam_Pose);
        CamOdom_Pub.publish(Cam_odom);

        //里程计消息（有线速度和角速度）
        nav_msgs::Odometry odom;
        odom.header.stamp=ros::Time::now();
        odom.header.frame_id="/map";

        odom.pose.pose.position=Cam_odom.pose.pose.position;
        odom.pose.pose.orientation=Cam_odom.pose.pose.orientation;

        odom.child_frame_id="/base_link";
        current_time=ros::Time::now();
        double dt=(current_time-last_time).toSec();
        double vx=(Cam_odom.pose.pose.position.x-lastx)/dt;
        double vy=(Cam_odom.pose.pose.position.y-lasty)/dt;
        double vth=(Cam_odom.pose.pose.orientation.z-lastth)/dt;

        odom.twist.twist.linear.x=vx;
        odom.twist.twist.linear.y=vy;
        odom.twist.twist.angular.z=vth;

        odom_Pub.publish(odom);

        last_time=current_time;
        lastx=Cam_odom.pose.pose.position.x;
        lasty=Cam_odom.pose.pose.position.y;
        lastth=Cam_odom.pose.pose.orientation.z;
    }
}

//回调函数：订阅图像帧并处理
void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{

    //step1:获取订阅到的图像帧
    // cout<<"callback"<<endl;
    cv_bridge::CvImageConstPtr cv_ptrRGB;
    try
    {
        cv_ptrRGB=cv_bridge::toCvShare(msgRGB);
    }
    catch(const std::exception& e)
    {
        ROS_ERROR("cv_bridge exception:%s",e.what());
        return;
    }

    cv_bridge::CvImageConstPtr cv_ptrD;
    try
    {
        cv_ptrD=cv_bridge::toCvShare(msgD);
    }
    catch(const std::exception& e)
    {
        ROS_ERROR("cv_bridge exception:%s",e.what());
        return ;
    }
    
    //step2:开始追踪
    std::chrono::steady_clock::time_point t3 = std::chrono::steady_clock::now();
    Camera_Pose=mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
    std::chrono::steady_clock::time_point t4 = std::chrono::steady_clock::now();
    double ttrack = std::chrono::duration_cast<std::chrono::duration<double>>(t4 - t3).count();                                             

    cout << "SLAM TrackRGBD all time =" << ttrack * 1000 << endl
             << endl;
            
    double orbTimeTmp = mpSLAM->mpTracker->orbExtractTime;
    double movingTimeTmp = mpSLAM->mpTracker->movingDetectTime;
    segmentationTime = mpSLAM->mpSegment->mSegmentTime;

    //step3:发布相机位姿信息
    Pub_CamPose(Camera_Pose);
    vTimesTrack.push_back(ttrack);
    vOrbTime.push_back(orbTimeTmp);
    vMovingTime.push_back(movingTimeTmp);

    nImages++;                                                               //更新处理的帧数量

}

2.创建yaml配置文件

在/工作空间路径/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM目录下创建yaml文件，如my_d435i.yaml(可以自己选择其他路径),写入以下代码
注意自己使用的相机的分辨率、内参等需要修改

%YAML:1.0

#--------------------------------------------------------------------------------------------
# Camera Parameters. Adjust them!
#--------------------------------------------------------------------------------------------

# Camera calibration and distortion parameters (OpenCV) 
Camera.fx: 606.8383178710938
Camera.fy: 606.8818359375
Camera.cx: 319.35430908203125
Camera.cy: 237.58682250976562

Camera.k1: 0.0
Camera.k2: 0.0
Camera.p1: 0.0
Camera.p2: 0.0

Camera.width: 640
Camera.height: 480

# Camera frames per second 
Camera.fps: 30.0

# IR projector baseline times fx (aprox.)
Camera.bf: 30.341915893555

# 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: 50.0

# Deptmap values factor   #深度图中的DepthMapFactor为1000.0，代表深度图中的1000.0个单位为1米,即像素值为1代表深度值为1mm。
DepthMapFactor: 1000.0

#--------------------------------------------------------------------------------------------
# ORB Parameters
#--------------------------------------------------------------------------------------------

# 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

PointCloudMapping.Resolution: 0.05

3.修改CMakeLists.txt文件

在/工作空间路径/src/DS-SLAM/Examples/ROS/ORB_SLAM2_PointMap_SegNetM目录下的CMakeLists.txt文件末尾加入以下内容：

#生成可执行文件myD435i
rosbuild_add_executable(myD435i
    myD435i_realtime.cc
)
target_link_libraries(myD435i
    ${LIBS}
)

4.创建launch文件

在/工作空间/src/DS-SLAM目录下创建launch文件，如myD435i_realtime.launch ，写入以下内容：
注：文件中各种参数的路径需要与自己实际的路径匹配，仔细检查

<launch>  

    <!-- param用于 在参数服务器上设置参数 -->
    <!-- /use_sim_time表示 是否使用仿真时间，即回放数据集的时间 ,false表示使用walltime，即实际时间 -->
    <param name ="/use_sim_time" value="false"/>

    <!-- arg 标签用于动态传参，类似于函数的参数，可以增强launch文件的灵活性-->
    
    <!-- ORB词袋 -->
    <arg name="voc" value="$(find ORB_SLAM2_PointMap_SegNetM)/../../../Vocabulary/ORBvoc.bin" />
    
    <!-- 相机配置参数文件 -->
    <arg name="config" value="$(find ORB_SLAM2_PointMap_SegNetM)/my_d435i.yaml" />
    
    <!-- segnet模型文件 -->
    <arg name="prototxt" value="$(find ORB_SLAM2_PointMap_SegNetM)/prototxts/segnet_pascal.prototxt" />
    
    <!-- 训练的segnet模型权重 -->
    <arg name="model" value="$(find ORB_SLAM2_PointMap_SegNetM)/models/segnet_pascal.caffemodel" />
    
    <!-- 调色板信息 -->
    <arg name="color_map" value="$(find ORB_SLAM2_PointMap_SegNetM)/tools/pascal.png" />

    <!-- 功能包                             为节点命名  被运行的节点文件 节点参数：ORB词袋  相机配置文件    运行的数据集    RGB&Depth关联文件  segnet模型文件   语义分割模型权重    彩色地图         日志输出目标 -->
    <!-- <node pkg="ORB_SLAM2_PointMap_SegNetM" name="TUM" type="TUM" args="$(arg voc) $(arg config) $(arg dataset) $(arg associate)  $(arg prototxt) $(arg model) $(arg color_map)" output="screen"  /> -->
    <node pkg="ORB_SLAM2_PointMap_SegNetM" name="myD435i" type="myD435i" args="$(arg voc) $(arg config)  $(arg prototxt) $(arg model) $(arg color_map)" output="screen"  />

    <!-- include 标签用于将另一个xml格式的launch文件导入当前文件 -->
    <!-- Run other launch file -->
    <include file="$(find ORB_SLAM2_PointMap_SegNetM)/launch/Octomap.launch" />
    <include file="$(find ORB_SLAM2_PointMap_SegNetM)/launch/transform.launch" />

    <!-- node标签用于制定ROS节点，注意：roslaunch命令不能保证按照node的声明顺序来启动节点（节点的启动是多进程的） -->

    <!--   包名         节点名称      节点类型（可执行文件名）  传递给节点的参数 -->
    <node pkg="rviz" name="rviz" type="rviz" args="-d $(find ORB_SLAM2_PointMap_SegNetM)/rviz/ds_slam.rviz" />

</launch>

5.开始运行

1.编译

./DS_SLAM_BUILD.sh 

2.启动roscore

roscore

3.启动可以获取D435i图像帧的launch文件
注：首先需要在ROS中配置D435i，参考：https://blog.csdn.net/sinat_16643223/article/details/115386645#commentBox
在配置D435i的工作空间下运行：

roslaunch realsense2_camera rs_rgbd.launch 

4.运行launch文件

roslaunch myD435i_realtime.launch

后续在rviz中的显示就不多说了。

my_d435i_ds_slam.png