DS-SLAM代码解读（二）

最新推荐文章于 2024-09-14 08:23:45 发布

原创最新推荐文章于 2024-09-14 08:23:45 发布 · 690 阅读

5 ·

CC 4.0 BY-SA版权

文章标签：

#自动驾驶 #c++

DS-SLAM 专栏收录该内容

2 篇文章

订阅专栏

本文详细介绍了ROS系统中SLAM（Simultaneous Localization And Mapping）模块如何与语义分割线程进行交互。在`TrackRGBD`函数中，RGBD图像被传递给跟踪线程和语义分割线程，通过`GetImg`和`GrabImageRGBD`函数处理。首先，图像被转化为灰度图并计算深度，然后创建`Frame`对象并等待语义分割结果。在语义分割完成后，去除动态点并执行SLAM跟踪，最终返回当前帧的Tcw位姿矩阵。此过程揭示了实时SLAM系统中多线程处理和传感器数据融合的机制。

ros_tum_realtime.cc 的while循环里面：

 Camera_Pose =  SLAM.TrackRGBD(imRGB,imD,tframe);

在System::TrackRGBD 函数中，将RGB图片分别发送给Tracking线程和Semantic Segmantation线程：

// Inform Semantic segmentation thread
mpTracker->GetImg(im);  //图片发送给语义分割线程
return  mpTracker->GrabImageRGBD(im,depthmap,timestamp);  //返回值是当前帧的Tcw的位姿矩阵

（1）调用了Tracking类中的GetImg()函数，将图片发送给语义分割线程。GetImg中：
将新来的RGB(彩色)图片复制给了语义分割线程的mImg 类成员变量，该变量存放需要语义分割的图片

void Tracking::GetImg(const cv::Mat& img)
{
    unique_lock<mutex> lock(mpSegment->mMutexGetNewImg);
    mpSegment->mbNewImgFlag=true;  // 新来图片的标志位
    img.copyTo(mpSegment->mImg);  // 实现了一幅图片分别发送给 Track 线程和 语义分割 Segment线程
}

（2）调用了Tracking类中的GrabImageRGBD()函数，计算当前帧的cv::Mat类型的Tcw位姿矩阵，并return：

//输入RGB图，深度图，时间戳
cv::Mat Tracking::GrabImageRGBD(const cv::Mat &imRGB,const cv::Mat &imD, const double &timestamp)
{
...
//step 1：将RGB（或BGR）或RGBA（BGRA）图像转为灰度图像
cvtColor(mImGray,mImGray,CV_RGB2GRAY); //如果图像是RGB四通道，就转成RGB灰度图
cvtColor(mImGray,mImGray,CV_RGBA2GRAY);//如果图像RGB是四通道，就转成RGBA灰度图
//step 2：将深度相机的disparity转为Depth , 也就是转换成为真正尺度下的深度
if(mDepthMapFactor!=1 || mImDepth.type()!=CV_32F);
    mImDepth.convertTo(mImDepth,CV_32F,mDepthMapFactor);
//step 3: 创建了Frame ,调用Frame构造函数去提取特征点,通过LK光流提取角点，并对生成的角点施加对极约束，极线约束删除动态点
mCurrentFrame = Frame(mImGray,mImDepth,timestamp,mpORBextractorLeft,mpORBVocabulary,mThDepth);
...

帧构造这一步很重要，下一篇博客将对这里进行展开解读

...
//step 4:等待语义分割结果 ,语义分割完成，这个函数isNewSegmentImgArrived 返回 false
while(!isNewSegmentImgArrived()) {
        usleep(1);
    }
...
//step 5 : 结合语义分割结果移除动态的外点
//// 彩色图,灰度图,深度图,mpSegment->mImgSegmentLatest是上面图像的语义分割的结果
mCurrentFrame.CalculEverything(mImRGB,mImGray,mImDepth,mpSegment->mImgSegmentLatest);
...
//step 6 : 语义分割线程已经执行完,移除动态的外点之后进行正常SLAM的Track部分
Track();
...
//step 7 : 得到SLAM计算的位姿
return mCurrentFrame.mTcw.clone();
}