DS-SLAM代码（动态环境下基于图像分割的SLAM）阅读笔记 （四）

今天我们来说说 DS-SLAM是如何实现语义建图部分的，主要涉及System.cc,Tracking.cc和Pointcloudmapping.cc：

1. **首先我们需要开启建图的线程（thread）。**在System.cc函数里面我们创建一个点云地图的对象：

// 使用智能指针 创建了一个 PointCloudMapping 的对象
    mpPointCloudMapping = boost::make_shared<PointCloudMapping>( resolution );

上面使用到了智能指针，相对于new 了一个对象，然后会调用PointCloudMapping的构造函数，传入参数就是PointCloudMapping需要的参数resolution，一会我们降采样所需要的分辨率：

PointCloudMapping::PointCloudMapping(double resolution_)
{
    this->resolution = resolution_;
    voxel.setLeafSize( resolution, resolution, resolution);
    this->sor.setMeanK(50);                                
    this->sor.setStddevMulThresh(1.0);                    
    globalMap = boost::make_shared< PointCloud >( );
    KfMap = boost::make_shared< PointCloud >( );
    // 这里创建了一个 thread 线程对象，执行 PointCloudMapping::viewer  函数
    viewerThread = boost::make_shared<thread>( bind(&PointCloudMapping::viewer, this ) );
}

在上面的构造函数除了配置滤波器的参数以外，最重要的是我们再次开了一个线程：

viewerThread = boost::make_shared<thread>( bind(&PointCloudMapping::viewer, this ) );

创建了一个thread对象，去执行PointCloudMapping::viewer 函数，这个viewer是一个死循环会显示们的点云数据。

2 我们语义建图的数据是从关键帧传过来的，关键帧在Tracking.cc函数里面的CreatNewKeyframe生成。Tracking.cc代码如下：

// insert Key Frame into point cloud viewer
    mpPointCloudMapping->insertKeyFrame( pKF,   // 关键帧
    this->mImS_C,								//彩色语义分割图
    this->mImS ,								//语义分割图
    this->mImRGB,								//RGB图
     this->mImDepth								//深度图
      );

在PointCloudMapping::insertKeyFrame 里面我们将Track线程传递过来的 keyframe,语义分割图，深度图等放置到对应的容器里面。这里Track的使命也就完成了。

3.通过图片产生点云，并不是来一个关键帧就生成一个，而是由于时间的问题，无法保证生成速度是来一个关键帧就生成一个，所以有可能是好几个关键帧一起去生成点云。程序里遍历idx是 lastKeyframeSize 的关键帧，到当前最新的关键帧去生成点云，通过generatePointCloud函数完成，该函数先遍历每一个关键帧的像素点，看每个像素点周围20*20的小方格内是否有人存在，如果存在的话，那么改像素点将不参与建图，DS-SLAM正是用这种方法来达到在建图中消除人的作用的。flag_exist就是改像素点周围环境是否含有人的标记 如果改像素点没有人的话，那么开启建图。不过DS-SLAM通过查看在语义分割图上该像素的灰度值来判断该像素点是否有语义信息，如果有语义信息就将语义分割图片的颜色赋给点云，颜色信息从2D映射到3D；否则点云的信息就是正常RGBD的颜色，代码如下：

pcl::PointCloud< PointCloudMapping::PointT >::Ptr PointCloudMapping::generatePointCloud(KeyFrame* kf, cv::Mat& semantic_color,cv::Mat& semantic, cv::Mat& color, cv::Mat& depth)
{

    PointCloud::Ptr tmp( new PointCloud() );
    // Point cloud is null ptr
    for ( int m=0; m<depth.rows; m+=1)
    {
        for ( int n=0; n<depth.cols; n+=1)
        {
            float d = depth.ptr<float>(m)[n];
            if (d < 0.01 || d > 8)
                continue;

	            int flag_exist=0;   //人的标志位

	            //在当前像素点20*20的范围内去搜索是否有 人
		        for (int i=-20;i <= 20; i+=3)
		        {
                    for (int j=-20;j <= 20; j+=3)
                    {
                        int tempx = m + i;
                        int tempy = n + j ;
            
                        if( tempx <= 0  ) tempx = 0;
                        if( tempx >= (Camera::height -1)  ) tempx = Camera::height-1;
                        if( tempy  <= 0  ) tempy =  0;
                        if( tempy >= (Camera::width -1) ) tempy = Camera::width -1;
                        if((int)semantic.ptr<uchar>(tempx)[tempy] == PEOPLE_LABLE)
                        {
                            flag_exist=1;
                            break;
                        }
                    }
                    if(flag_exist==1)
                        break;
		        }
	
                if(flag_exist == 1)   // 如果有人存在就不建人那部分的图像
                    continue;

                PointT p;
                p.z = d;
                p.x = ( n - Camera::cx) * p.z / Camera::fx;
                p.y = ( m - Camera::cy) * p.z / Camera::fy;

	            // Deal with color   //语义建图的颜色处理
	            if((int)semantic.ptr<uchar>(m)[n]==0)  //普通颜色
	            {
                    p.b = color.ptr<uchar>(m)[n*3];
                    p.g = color.ptr<uchar>(m)[n*3+1];
                    p.r = color.ptr<uchar>(m)[n*3+2];
	            }
	            else       //语义分割图片的颜色赋给点云，颜色信息从2D映射到3D
	            {
                    p.b = semantic_color.ptr<uchar>(m)[n*3];
                    p.g = semantic_color.ptr<uchar>(m)[n*3+1];
                    p.r = semantic_color.ptr<uchar>(m)[n*3+2]; 
	            }
	            tmp->points.push_back(p);
        }
    }

    Eigen::Isometry3d T = ORB_SLAM2::Converter::toSE3Quat( kf->GetPose() );//得到当前关键帧的位姿
    PointCloud::Ptr cloud(new PointCloud);
    pcl::transformPointCloud( *tmp, *cloud, T.inverse().matrix());  //通过矩阵变换点云
    cloud->is_dense = false;

    cout<<"Generate point cloud for kf "<<kf->mnId<<", size="<<cloud->points.size()<<endl;
    return cloud;
}

4 返回点云后，将该关键帧的点云以topic的形式发布出去。Rviz会自行对于点云信息进行拼接，至此，整个语义建图部分完成。