ORB-SLAM3中如何实现双目稠密建图

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

---

前言

在ORB-SLAM3系统的基础上，增加动态点去除和双目稠密建图线程，并引入语义信息，构建稠密语义地图，最后转为八叉树地图存储。

---

一、过程示意图

二、双目稠密建图在ros_stereo中的改动

本小节以ORB-SLAM3源代码中的/Examples_old/ROS/ORB_SLAM3/src/ros_stereo.cc为基础进行改动。

1.引入库

代码如下（示例）：

#include<iostream>
#include<algorithm>
#include<fstream>
#include<chrono>

#include<ros/ros.h>     // 编写ROS节点
#include <cv_bridge/cv_bridge.h>        // 用于在ROS中将ROS图像消息和OpenCV图像格式之间的转换
#include <message_filters/subscriber.h>     // 订阅ROS消息
#include <message_filters/time_synchronizer.h>      // 用于同步多个ROS消息的时间戳
#include <message_filters/sync_policies/approximate_time.h>     // 用于近似时间同步

#include<opencv2/core/core.hpp>     //

#include"../../../include/System.h"

#include <sensor_msgs/Image.h>      // 用于传递传感器数据的图像消息
#include <sensor_msgs/CameraInfo.h>     // 相机信息
#include <sensor_msgs/image_encodings.h>    // 图像编码信息
#include <stereo_msgs/DisparityImage.h>     // 视差信息
#include <message_filters/sync_policies/exact_time.h>

#include <image_transport/subscriber_filter.h>  // ROS中用于图像传输
#include <image_geometry/stereo_camera_model.h>     // ROS中处理立体相机模型的头文件
#include <pcl_ros/point_cloud.h>    // ROS中处理点云数据的功能
#include <pcl/point_types.h>        // 定义点云数据的类型
#include <pcl_conversions/pcl_conversions.h>        // 提供点云数据与其他数据格式的转换功能

#include "../include/program_communication.h"	// 用于C++端与python端的通信，方便与引入深度学习方法计算双目视差图

2.主函数分析、改动

对ros_stereo.cc中的主函数进行分析，并根据需要增加改动代码。
主函数代码如下：

// 主函数入口
int main(int argc, char **argv)
{
    // 初始化ROS节点
    ros::init(argc, argv, "Stereo"); // ROS节点名称在这里定义
    ros::start();

    // 检查命令行参数数量是否正确
    if(argc != 4)
    {
        cerr << endl << "Usage: rosrun ORB_SLAM3 Stereo path_to_vocabulary path_to_settings do_rectify" << endl;
        ros::shutdown();
        return 1;
    }    

    // 创建ORB_SLAM3系统，初始化SLAM系统的所有线程，并准备处理帧。
    // 这里以RGB-D为相机模型创建系统，便于接收深度图。
    ORB_SLAM3::System SLAM(argv[1],argv[2],ORB_SLAM3::System::RGBD, true);

    // 创建图像抓取器对象，并传入SLAM系统的指针
    ImageGrabber igb(&SLAM);

    // 从命令行参数中解析do_rectify参数值，并赋给图像抓取器对象的do_rectify成员变量
    stringstream ss(argv[3]);
    ss >> boolalpha >> igb.do_rectify;

    if(igb.do_rectify)
    {      
        // Load settings related to stereo calibration
        cv::FileStorage fsSettings(argv[2], cv::FileStorage::READ);
        if(!fsSettings.isOpened())
        {
            cerr << "ERROR: Wrong path to settings" << endl;
            return -1;
        }

        cv::Mat K_l, K_r, P_l, P_r, R_l, R_r, D_l, D_r;
        fsSettings["LEFT.K"] >> K_l;
        fsSettings["RIGHT.K"] >> K_r;

        fsSettings["LEFT.P"] >> P_l;
        fsSettings["RIGHT.P"] >> P_r;

        fsSettings["LEFT.R"] >> R_l;
        fsSettings["RIGHT.R"] >> R_r;

        fsSettings["LEFT.D"] >> D_l;
        fsSettings["RIGHT.D"] >> D_r;

        int rows_l = fsSettings["LEFT.height"];
        int cols_l = fsSettings["LEFT.width"];
        int rows_r = fsSettings["RIGHT.height"];
        int cols_r = fsSettings["RIGHT.width"];

        if(K_l.empty() || K_r.empty() || P_l.empty() || P_r.empty() || R_l.empty() || R_r.empty() || D_l.empty() || D_r.empty() ||
                rows_l==0 || rows_r==0 || cols_l==0 || cols_r==0)
        {
            cerr << "ERROR: Calibration parameters to rectify stereo are missing!" << endl;
            return -1;
        }

        cv::initUndistortRectifyMap(K_l,D_l,R_l,P_l.rowRange(0,3).colRange(0,3),cv::Size(cols_l,rows_l),CV_32F,igb.M1l,igb.M2l);
        cv::initUndistortRectifyMap(K_r,D_r,R_r,P_r.rowRange(0,3).colRange(0,3),cv::Size(cols_r,rows_r),CV_32F,igb.M1r,igb.M2r);
    }
	
	// 创建一个ROS节点句柄
	ros::NodeHandle nh;
	
	// 根据数据集rosbag中的话题需要，指定话题名称和队列大小。
	// 创建用于订阅左侧相机图像消息的消息过滤器订阅者，并指定话题名称和队列大小
	message_filters::Subscriber<sensor_msgs::Image> left_sub(nh, "/camera/left/image_rect", 1);
	
	// 创建用于订阅右侧相机图像消息的消息过滤器订阅者，并指定话题名称和队列大小
	message_filters::Subscriber<sensor_msgs::Image> right_sub(nh, "/camera/right/image_rect", 1);
	
	// 创建用于订阅左侧相机相机信息消息的消息过滤器订阅者，并指定话题名称和队列大小
	message_filters::Subscriber<sensor_msgs::CameraInfo> left_info(nh, "/camera/left/camera_info", 1);
	
	// 创建用于订阅右侧相机相机信息消息的消息过滤器订阅者，并指定话题名称和队列大小
	message_filters::Subscriber<sensor_msgs::CameraInfo> right_info(nh, "/camera/right/camera_info", 1);

	/*
	 * 使用了时间同步策略 ApproximateTime 对左右相机图像和相机信息进行同步，
	 * 设置了一个时间窗口大小为10个消息，并且注册了回调函数，回调函数绑定到了
	 * ImageGrabber 对象的 process 方法上。
	 */
    typedef message_filters::sync_policies::ApproximateTime<sensor_msgs::Image, sensor_msgs::Image, sensor_msgs::CameraInfo, sensor_msgs::CameraInfo> sync_pol;
    message_filters::Synchronizer<sync_pol> sync(sync_pol(10), left_sub,right_sub,  left_info,  right_info);
    sync.registerCallback(boost::bind(&ImageGrabber::process ,&igb,_1,_2,_3,_4));

    ros::spin();        // 进入ROS事件循环,等待消息到达并调用相应的回调函数

    // Stop all threads
    // 结束全部线程
    SLAM.Shutdown();

    // Save camera trajectory
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryTUM("FrameTrajectory_TUM_Format.txt");
    SLAM.SaveTrajectoryKITTI("FrameTrajectory_KITTI_Format.txt");

    ros::shutdown();

    return 0;
}	

3.修改ImageGrabber对象

class ImageGrabber
{
private:    // 私有成员变量
    ImageTransfer imageTransfer;    // 用于实现C++向python发送图像和从python接收图像
    image_geometry::StereoCameraModel model_;       // 用于存储立体相机模型的对象
    boost::shared_ptr<image_transport::Publisher> depth_pub_;   // 管理图像传输中的发布者对象
    
public:
    // 构造函数，初始化图像抓取器对象，设置ORB_SLAM3系统指针和图像传输
    ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM),imageTransfer("127.0.0.1", 1234){
        ros::NodeHandle local_nh("~");    // 创建一个ROS节点句柄，用于处理私有命名空间参数
        image_transport::ImageTransport local_it(local_nh);   // 创建图像传输对象
        depth_pub_.reset(new image_transport::Publisher(local_it.advertise("depth", 1)));   // 创建深度图像的发布者对象
    }
    
    // 抓取RGB-D图像的函数，接收RGB和深度图像的消息指针
    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);
    
    // 处理图像的函数，接收左右图像和相机信息的消息指针
    void process(const sensor_msgs::ImageConstPtr &l_image_msg, const sensor_msgs::ImageConstPtr &r_image_msg, const sensor_msgs::CameraInfoConstPtr &l_info_msg, const sensor_msgs::CameraInfoConstPtr &r_info_msg);
    
    ORB_SLAM3::System* mpSLAM; // 指向ORB_SLAM3系统的指针
    bool do_rectify;    // 是否进行校正的标志
    cv::Mat M1l,M2l,M1r,M2r;    // 左右相机的校正矩阵
};

4.声明GrabRGBD函数

void ImageGrabber::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;
    }

    mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
}

5.声明process函数

void ImageGrabber::process(const sensor_msgs::ImageConstPtr &l_image_msg, const sensor_msgs::ImageConstPtr &r_image_msg,const sensor_msgs::CameraInfoConstPtr &l_info_msg, const sensor_msgs::CameraInfoConstPtr &r_info_msg)
  {
        std::cout<<"Received images and camera info."<<std::endl;
        model_.fromCameraInfo(l_info_msg, r_info_msg);      // 根据左右相机信息建立立体相机模型

        // 转换左右图像消息为MAT形式,便于发送到python端进行视差图计算。
        cv_bridge::CvImageConstPtr l_cv_ptrs, r_cv_ptrs;
        try
        {
            l_cv_ptrs = cv_bridge::toCvShare(l_image_msg, "bgr8");
            r_cv_ptrs = cv_bridge::toCvShare(r_image_msg, "bgr8");
        }
        catch (cv_bridge::Exception& e)
        {
          ROS_ERROR("cv_bridge exception: %s", e.what());
          return;
        }

        cv::Mat l_image = l_cv_ptrs->image;
        cv::Mat r_image = r_cv_ptrs->image;

        imageTransfer.sendMatImage(l_image);
        imageTransfer.sendMatImage(r_image);

        // Stereo parameters
        float f = model_.right().fx();
        float T = model_.baseline();
        if (T > 10){ T=T*0.001f;}
        float depth_fact = T * f * 820.0f;
        cout << "fx and T and depth_fact: " << f << "   "<< T << "    " << depth_fact << endl;
        uint16_t bad_point = std::numeric_limits<uint16_t>::max();
        
        ROS_ASSERT(l_image_msg->width == r_image_msg->width);
        ROS_ASSERT(l_image_msg->height == r_image_msg->height);

        int32_t width = l_image_msg->width;
        int32_t height = l_image_msg->height;

        float *l_disp_data = imageTransfer.receiveFloatImageData();

        cv_bridge::CvImage out_depth_msg;
        out_depth_msg.header = l_image_msg->header;
        out_depth_msg.encoding = sensor_msgs::image_encodings::MONO16;
        out_depth_msg.image = cv::Mat(height, width, CV_16UC1);
        //创建out_depth_msg_image_data 的指针，它指向 out_depth_msg 的图像数据
        uint16_t *out_depth_msg_image_data = reinterpret_cast<uint16_t *>(&out_depth_msg.image.data[0]);
        std::vector<int32_t> inliers;
        int i = 0;
        for (int32_t i = 0; i < width * height; i++)
        {
            float disp = l_disp_data[i];
            out_depth_msg_image_data[i] = disp <= 1.0f ? bad_point : (uint16_t)(depth_fact / disp);

            if (l_disp_data[i] > 0)
                inliers.push_back(i);
        }
         // Publish
        depth_pub_->publish(out_depth_msg.toImageMsg());
        // Convert CvImage to Mat
        // 存储深度图为png以验证运行过程中深度图是否合理。
        //cv::Mat depth_image = out_depth_msg.image;
        //std::string depth_image_filename = "/home/cai/图片/depth_image.png";
        //cv::imwrite(depth_image_filename, depth_image);
        
        sensor_msgs::ImageConstPtr msgD;
        GrabRGBD(l_image_msg ,out_depth_msg.toImageMsg());
        
        return;
  }

---

总结

在C++与python之间完成了socket通信, 引入深度学习方法CRE计算双目视差图,实现了双目的稠密建图. 但所建出来的稠密地图存在一些滤波问题. 未实现引入语义信息,构建出语义地图.