3.ORB-SLAM3中如何实现稠密建图（一）：进入追踪线程前

这里以我的代码为例跟大家讲述，感兴趣的同学可以向我要代码奥

目录

1 如何开启稠密建图

1.1 传入参数控制纯定位/稠密建图/正常SLAM模式

1.2 系统构造函数System::System

2 稠密建图线程的构造

2.1 追踪线程的构造

2.2 ImageGrabber::GrabRGBD

---

1 如何开启稠密建图

1.1 传入参数控制纯定位/稠密建图/正常SLAM模式

        由于我的代码是依赖稠密建图进行导航的，因此程序执行的参数有两个额外输入的变量：

/**
 * @brief 主函数
 * @param argc
 * @param argv 指定参数 3 稠密建图  4 纯定位模式
 * @return
 */
int main(int argc, char **argv)
{
    ros::init(argc, argv, "RGBD");
    ros::start();
    bool mode1 = false;
    bool mode2 = false;
    // 稠密建图
    std::string dense(argv[3]);
    // 纯定位模式
    std::string purelocation(argv[4]);

    if(dense == "true")
        mode1 = true;
    else
        mode1 = false;
    if(purelocation == "true")
        mode2 = true;
    else
        mode2 = false;
    // Create SLAM system. It initializes all system threads and gets ready to process frames.
    ORB_SLAM3::System SLAM(argv[1],argv[2],mode1, mode2, ORB_SLAM3::System::RGBD,true);

    ImageGrabber igb(&SLAM);

    ros::NodeHandle nh;
    // pub track_image
    image_transport::ImageTransport image_transport(nh);
    message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 100);
    message_filters::Subscriber depth_sub(nh, "/camera/depth/image_raw", 100);
    typedef message_filters::sync_policies::ApproximateTime sync_pol;
    message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));


    CamPose_Pub = nh.advertise("/Camera_Pose",100);
    Camodom_Pub = nh.advertise("/Camera_Odom", 100);
    tracking_img_pub = image_transport.advertise( "/ORB_SLAM3/tracking_image", 1);
    tracked_mappoints_pub = nh.advertise( "/ORB_SLAM3/tracked_points", 1);
    all_mappoints_pub = nh.advertise( "ORB_SLAM3/all_points", 1);
    ros::ServiceServer service = nh.advertiseService("deactivate_localization", deactivateLocalizationCallback);
    ros::spin();

    // Stop all threads
    SLAM.Shutdown();
    SLAM.SaveTrajectoryTUM("FrameTrajectory.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    // Save camera trajectory
    ros::shutdown();

    return 0;
}

        如果传入参数3为'true'就是稠密建图，如果传入参数4为'true'就是纯定位。

        这里我们传入命令：表示进入稠密建图模式

rosrun akm_location Akm_RGBD Vocabulary/ORBvoc.bin Examples/RGB-D/akm_slam.yaml true false

        这时我们的mode1为true，mode2为false。

        我们进入SLAM的构造函数1.2节

        现在我们来看这个ImageGrabber这个类：

class ImageGrabber
{
public:
    ros::NodeHandle nh;
    ros::Publisher pub_camerapath;
    nav_msgs::Path  camerapath;

    ImageGrabber(ORB_SLAM3::System* pSLAM):mpSLAM(pSLAM),nh("~")
    {
        pub_camerapath= nh.advertise ("Path", 100);
    }

    void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD);

    ORB_SLAM3::System* mpSLAM;
};

1. 成员变量：

   • nh：一个 ROS 节点的句柄，用于与 ROS 系统通信。
   • pub_camerapath：一个 ROS 发布器（Publisher），用于发布 nav_msgs::Path 类型的消息。
   • camerapath：一个 nav_msgs::Path 类型的路径信息。

2. 构造函数：

   • ImageGrabber(ORB_SLAM3::System* pSLAM)：构造函数接受一个指向 ORB_SLAM3::System 对象的指针 pSLAM 作为参数。在构造函数中完成了一些初始化操作：
     • mpSLAM(pSLAM)：将构造函数参数 pSLAM 的值赋给类成员变量 mpSLAM。
     • nh("~")：使用 ROS 节点句柄进行初始化，波浪号 "~" 表示使用私有命名空间。
     • pub_camerapath= nh.advertise ("Path", 100)：初始化 pub_camerapath，将其设置为一个发布名为 "Path" 的 nav_msgs::Path 类型消息的发布器，缓冲区大小为 100。

3. 成员函数：

   • void GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB, const sensor_msgs::ImageConstPtr& msgD)：这是一个未定义的函数，它似乎意在从 RGB 和深度传感器接收图像数据。

4. 成员变量：

   • mpSLAM：一个指向 ORB_SLAM3::System 类型对象的指针。

        这段代码的目的是创建一个能够与ROS通信的类，用于接收RGB和深度图像数据，并使用ORB_SLAM3系统处理这些数据。类中的 pub_camerapath 可以用于发布相机路径信息。       

        GrabRGBD函数解析我写在了2.2节。

    image_transport::ImageTransport image_transport(nh);
    message_filters::Subscriber rgb_sub(nh, "/camera/rgb/image_raw", 100);
    message_filters::Subscriber depth_sub(nh, "/camera/depth/image_raw", 100);
    typedef message_filters::sync_policies::ApproximateTime sync_pol;
    message_filters::Synchronizer sync(sync_pol(10), rgb_sub,depth_sub);
    sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD,&igb,_1,_2));

        这里做了一个时间同步（RGB图像与深度图像的同步），将图像成对送入GrabRGBD函数中进行追踪。

    CamPose_Pub = nh.advertise("/Camera_Pose",100);
    Camodom_Pub = nh.advertise("/Camera_Odom", 100);
    tracking_img_pub = image_transport.advertise( "/ORB_SLAM3/tracking_image", 1);
    tracked_mappoints_pub = nh.advertise( "/ORB_SLAM3/tracked_points", 1);
    all_mappoints_pub = nh.advertise( "ORB_SLAM3/all_points", 1);

        这里发布了一些信息。

        相机的里程计数据，显示数据，当前追踪的地图点数据，所有的地图点数据。

        最后系统关闭：

    // Stop all threads
    SLAM.Shutdown();
    SLAM.SaveTrajectoryTUM("FrameTrajectory.txt");
    SLAM.SaveKeyFrameTrajectoryTUM("KeyFrameTrajectory.txt");
    // Save camera trajectory
    ros::shutdown();

1.2 系统构造函数System::System

        这里和ORBSLAM3一样的就不再阐述了，只说改动的部分～

        1.构造函数：

System::System(const string &strVocFile, const string &strSettingsFile, const bool mode1, const bool mode2, const eSensor sensor,
               const bool bUseViewer, const int initFr, const string &strSequence):
    mSensor(sensor), mpViewer(static_cast(NULL)), mbReset(false), mbResetActiveMap(false),
    mbActivateLocalizationMode(false), mbDeactivateLocalizationMode(false), mbShutDown(false)

        这里添加了两个标志位：const bool mode1, const bool mode2代表是否进行稠密建图/重定位的标志。

        2.读取先前建立的地图（这里第一篇、第二篇博客讲述了如何加载、保存建立好的地图）

    // 查看配置文件版本，不同版本有不同处理方法
    cv::FileNode node = fsSettings["File.version"];
    if(!node.empty() && node.isString() && node.string() == "1.0")
    {
        settings_ = new Settings(strSettingsFile,mSensor);

        // 保存及加载地图的名字
        mStrLoadAtlasFromFile = settings_->atlasLoadFile();
        mStrSaveAtlasToFile = settings_->atlasSaveFile();

        cout << (*settings_) << endl;
    }

        3.开启稠密建图线程：

    if(mode1)
    {
        densemapping = true;
        mpPointCloudMapping = make_shared (0.01);
        cout << "选择为进行ROS下RGBD稠密重建！正在运行，请等待！"  << endl;
    }
    else
    {
        densemapping = false;
        cout << "选择为不进行稠密重建！正在运行，请等待！" << endl;
    }

        如果mode1为true，则置densemapping标志位为true。建立了稠密建图线程的指针mpPointCloudMapping。

        如果进行稠密建图，则执行下列部分：

    if(densemapping)
    {
        //Initialize the Tracking thread
        //(it will live in the main thread of execution, the one that called this constructor)
        // 创建跟踪线程（主线程）,不会立刻开启,会在对图像和imu预处理后在main主线程种执行
        cout << "Seq. Name: " << strSequence << endl;
        mpTracker = new Tracking(this,
                                 mpVocabulary,
                                 mpFrameDrawer,
                                 mpMapDrawer,
                                 mpAtlas,
                                 mpKeyFrameDatabase,
                                 strSettingsFile,
                                 mSensor,
                                 mpPointCloudMapping,
                                 settings_,
                                 strSequence);
        //Initialize the Local Mapping thread and launch
        //创建并开启local mapping线程
        mpLocalMapper = new LocalMapping(this, mpAtlas, mSensor==MONOCULAR || mSensor==IMU_MONOCULAR,
                                         mSensor==IMU_MONOCULAR || mSensor==IMU_STEREO || mSensor==IMU_RGBD, strSequence);
        mptLocalMapping = new thread(&ORB_SLAM3::LocalMapping::Run,mpLocalMapper);
        mpLocalMapper->mInitFr = initFr;

        // 设置最远3D地图点的深度值，如果超过阈值，说明可能三角化不太准确，丢弃
        if(settings_)
            mpLocalMapper->mThFarPoints = settings_->thFarPoints();
        else
            mpLocalMapper->mThFarPoints = fsSettings["thFarPoints"];
        // ? 这里有个疑问,C++中浮点型跟0比较是否用精确?
        if(mpLocalMapper->mThFarPoints!=0)
        {
            cout << "Discard points further than " << mpLocalMapper->mThFarPoints << " m from current camera" << endl;
            mpLocalMapper->mbFarPoints = true;
        }
        else
            mpLocalMapper->mbFarPoints = false;

        //Initialize the Loop Closing thread and launch
        // mSensor!=MONOCULAR && mSensor!=IMU_MONOCULAR
        // 创建并开启闭环线程
        mpLoopCloser = new LoopClosing(mpAtlas, mpKeyFrameDatabase, mpVocabulary, mSensor!=MONOCULAR, activeLC); // mSensor!=MONOCULAR);
        mptLoopClosing = new thread(&ORB_SLAM3::LoopClosing::Run, mpLoopCloser);

        //Set pointers between threads
        // 设置线程间的指针
        mpTracker->SetLocalMapper(mpLocalMapper);
        mpTracker->SetLoopClosing(mpLoopCloser);

        mpLocalMapper->SetTracker(mpTracker);
        mpLocalMapper->SetLoopCloser(mpLoopCloser);

        mpLoopCloser->SetTracker(mpTracker);
        mpLoopCloser->SetLocalMapper(mpLocalMapper);

        //usleep(10*1000*1000);

        //Initialize the Viewer thread and launch
        // 创建并开启显示线程
        if(false)
            //if(false) // TODO
        {
            mpViewer = new Viewer(this, mpFrameDrawer,mpMapDrawer,mpTracker,strSettingsFile,settings_);
            mptViewer = new thread(&Viewer::Run, mpViewer);
            mpTracker->SetViewer(mpViewer);
            mpLoopCloser->mpViewer = mpViewer;
            mpViewer->both = mpFrameDrawer->both;
        }
        // Choose to use slam mode
//        if(mode2){
//            ActivateLocalizationMode();
//            mpTracker->mbActivateLocalizationMode = true;
//        } else
        cout << "正常SLAM模式！"  << endl;
        mpTracker->mbActivateLocalizationMode = false;
        // Fix verbosity
        // 打印输出中间的信息，设置为安静模式
        Verbose::SetTh(Verbose::VERBOSITY_QUIET);
    }

        首先构造追踪线程的指针（见2.1节），只有这个和ORB-SLAM3不一样其余部分全部一样。

2 稠密建图线程的构造

2.1 追踪线程的构造

        这里传入了稠密建图线程的指针：mpPointCloudMapping

        mpTracker = new Tracking(this,
                                 mpVocabulary,
                                 mpFrameDrawer,
                                 mpMapDrawer,
                                 mpAtlas,
                                 mpKeyFrameDatabase,
                                 strSettingsFile,
                                 mSensor,
                                 mpPointCloudMapping,
                                 settings_,
                                 strSequence);

        我们看看这个追踪线程有何不同： 

shared_ptr mpPointCloudMapping;

        只是把稠密建图的指针给到了追踪线程里面，其他并没有什么不同哦。

2.2 ImageGrabber::GrabRGBD

        这里很详细的都标注出来啦！主要作用就是把ROS格式的彩色RGB图像和深度图像转换成CV::Mat的格式然后进入追踪，获得追踪结果（未经过优化）并发布供后续节点使用。

/**
 * @brief
 * @param msgRGB ROS格式的图像 -- 彩色图像
 * @param msgD ROS格式的图像 -- 深度图像
 */
void ImageGrabber::GrabRGBD(const sensor_msgs::ImageConstPtr& msgRGB,const sensor_msgs::ImageConstPtr& msgD)
{
     1. ROS格式的彩色深度图像转化成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;
    }

     2.调用追踪线程获得位姿
    geometry_msgs::Pose sensor_pose;
    Sophus::SE3f sophus_Tcw;
    sophus_Tcw = mpSLAM->TrackRGBD(cv_ptrRGB->image,cv_ptrD->image,cv_ptrRGB->header.stamp.toSec());
    cv::Mat Tcw = cv::Mat(4, 4, CV_32F);
    cv::eigen2cv(sophus_Tcw.matrix(), Tcw);

    orb_slam_broadcaster = new tf::TransformBroadcaster;

     3.发布正在追踪的图像(有特征点哪个)
    publish_tracking_img(mpSLAM->GetCurrentFrame(), cv_ptrRGB->header.stamp);

     4.发布正在追踪的地图点
    publish_tracked_points(mpSLAM->GetTrackedMapPoints(), cv_ptrRGB->header.stamp);

     5.发布所有的地图点
    publish_all_points(mpSLAM->GetAllMapPoints(), cv_ptrRGB->header.stamp);
    if (!Tcw.empty())
    {
        cv::Mat Twc =Tcw.inv();
        cv::Mat RWC= Twc.rowRange(0,3).colRange(0,3);
        cv::Mat tWC=  Twc.rowRange(0,3).col(3);
        cv::Mat twc(3,1,CV_32F);
        twc = tWC;
        Eigen::Matrix eigMat ;
        eigMat <(0,0),RWC.at(0,1),RWC.at(0,2),
                RWC.at(1,0),RWC.at(1,1),RWC.at(1,2),
                RWC.at(2,0),RWC.at(2,1),RWC.at(2,2);
        Eigen::Quaterniond q(eigMat);

        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(0);
        //Pose_trans[1] = twc.at(1);
        Pose_trans[1] = 0;
        Pose_trans[2] = twc.at(2);

         6.sensor_pose存放camera的位姿
        sensor_pose.position.x = twc.at(0);
        sensor_pose.position.y = twc.at(1);
        sensor_pose.position.z = twc.at(2);
        sensor_pose.orientation.x = q.x();
        sensor_pose.orientation.y = q.y();
        sensor_pose.orientation.z = q.z();
        sensor_pose.orientation.w = q.w();

        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(), "/odom", "/orb_cam_link"));

        Cam_Pose.header.stamp =ros::Time::now();
        //Cam_Pose.header.seq = msgRGB->header.seq;
        Cam_Pose.header.frame_id = "/odom";
        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 = "/odom";
        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};

         7.发布相机单帧位姿
        CamPose_Pub.publish(Cam_Pose);
        Camodom_Pub.publish(Cam_odom);
        std_msgs::Header header ;
        header.stamp =msgRGB->header.stamp;
        header.seq = msgRGB->header.seq;
        header.frame_id="/odom";
        camerapath.header =header;
        camerapath.poses.push_back(Cam_Pose);

         7.发布相机轨迹
        pub_camerapath.publish(camerapath);

    }

}