通过帧间确定动态障碍物，剔除动态3D点云数据后用于生成栅格地图

在使用rtabmap建图时，障碍物层的栅格地图无法清除动态障碍物，对于3D障碍物点云仅仅是简单的累加的过程！
因此尝试通过两帧之间的变化消除动态障碍物点云。

在MapsManager.cpp中剔除动态障碍物

ROS_INFO("x:%fs,y:%fs,z:%fs",iter->second.x(),iter->second.y(),iter->second.z());

首先在该cpp文件中确定了里程计pose，利用该pose进行cropBoxFilter过滤

在更新障碍物的过程中，将之前简单的累加，先改成如下所示，确定一下剔除的范围：

//like
pcl::CropBox<pcl::PointXYZRGB> cropBoxFilter;
double x_min = +iter->second.x()-1;
   double y_min = +iter->second.y()-1;
   double z_min = +iter->second.z()-1;
   double x_max = +iter->second.x()+1;
   double y_max = +iter->second.y()+1;
   double z_max = +iter->second.z()+1;
   cropBoxFilter.setMin(Eigen::Vector4f(x_min, y_min, z_min, 1.0));
   cropBoxFilter.setMax(Eigen::Vector4f(x_max, y_max, z_max, 1.0));
   cropBoxFilter.setNegative(false); 
   cropBoxFilter.setInputCloud(subtractedCloud);
   cropBoxFilter.filter(*subtractedCloud);
*assembledObstacles_=*subtractedCloud;

可以看见，此时只保留了每一帧中的附近二米内的障碍物点云

接下来定义俩点云数据，注意智能指针初始化！

lastObstacles(new pcl::PointCloud<pcl::PointXYZRGB>),
currentObstacles(new pcl::PointCloud<pcl::PointXYZRGB>),

首先简单的确定一下静态障碍物，即在上一帧和当前帧重叠的区域里，上一帧存在的点云在该帧同样存在，简单理解就是该障碍物在这两帧中处于同样的位置，即静止状态！如果要区分动态和静态障碍物，还需要设计更加严格的判断规则。

这里，我使用的kdtree查找最近点，来判断上一帧的点云在该帧同样位置是否存在！由于对PCL库的使用不熟悉，如果有更好的方法，欢迎私信留言！

if(lastObstacles->size()){                            
    //创建kdtree 结构                            
    ROS_INFO("create kdtree");                            
    for(unsigned int i=0; i<lastObstacles->size(); ++i){                                
        pcl::PointXYZRGB searchPoint;                                
        searchPoint.x = lastObstacles->at(i).x;                                
        searchPoint.y = lastObstacles->at(i).y;                                
        searchPoint.z = lastObstacles->at(i).z;
        if(fabs(searchPoint.x - iter->second.x()) <3 &&fabs(searchPoint.y - iter->second.y()) <3 &&fabs(searchPoint.z - iter->second.z()) <3){                                    
          	 //确定是否为静态障碍物
         	  ...
           }                            
     }                           
     //剔除掉不为静态障碍物的点云     
     *currentObstacles+=*lastObstacles;                   
}                        
*lastObstacles = *subtractedCloud;

首先判断上一帧的点云数据，是否在该帧内，即：

fabs(searchPoint.x - iter->second.x()) <3 &&fabs(searchPoint.y - iter->second.y()) <3 &&fabs(searchPoint.z - iter->second.z()) <3

如果在该帧内的话，则在该帧的区域内去查找，上一帧中出现的点云是否依然存在，这里考虑到里程计的误差，将radius设置为0.08，如果发现该点，则认为该点为静态障碍物，则保留，否则将过滤剔除掉！

pcl::KdTreeFLANN<pcl::PointXYZRGB> kdtree;                                    
kdtree.setInputCloud(subtractedCloud);                                    
std::vector<int> pointIdxRadiusSearch;                                    
std::vector<float> pointRadiusSquaredDistance;                                    
float radius = 0.08;                                    
if ( kdtree.radiusSearch (searchPoint, radius, pointIdxRadiusSearch, pointRadiusSquaredDistance) > 0 ){                                            
    inliers->indices.push_back(i);                                    
}                                

当确定好静态障碍物之后，通过ExtractIndices分割算法提取静态部分点云数据。

extract.setInputCloud(lastObstacles);                            
extract.setIndices(inliers);                           
extract.setNegative(false);                            
extract.filter(*lastObstacles);                            
ROS_INFO("lastObstacles point size:%d",(int)lastObstacles->size());                           

在该方法中，每次都是在下一帧中确定上一帧里的障碍物点是否可以加到障碍物层中，而本帧的障碍物点云，则是直接拼接上去：

*assembledObstacles_ = *currentObstacles + *subtractedCloud;

这样则每次保留了最新帧的所有障碍物点云，和之前帧里认为是静态障碍物的点云，此时可以明显的剔除掉大部分的动态障碍物点云。

未剔除动态障碍物点云如下所示，可以明显的看到身边走动的人留下的点：

在经过动态剔除之后，可以看见基本上能去除所有动态的点云：

去除掉动态障碍物之后，便可以使用该点云数据，来生成2D栅格地图，用于路径规划！！
目前只是简单的尝试了一下，细节部分还有待优化。

---

补充:在Lio_livox中也对动态障碍物进行了处理，其中把点云分为前景，背景，面特征，角特征。其中前景点被认为是动态物体，在特征提取过程中被排除。

There are also some parameters in the config file:
Use_seg: choose the segmentation mode for dynamic objects filtering, there are 2 modes:
0 - without using the segmentation method, you can choose this mode if there is few dynamic objects in your data
1 - using the segmentation method to remove dynamic objects

在SegBG函数中，对于地面以上的点云进行处理，从上往下增长，这里种子阈值被设置为4~6m，认为达到这种高度为背景，其他为前景。

SegBG(pLabel,cloud,kdtree,0.5); //背景label=1 前景0

在选择一遍前景后，剩下的都是最高高度在4m以下的点云类。再通过FindACluster进一步确定是否为前景。只有当pLabel仍然等于0的才会被认为时前景，即动态障碍物。

 // cluster 分类
float dx=cf.xmax-cf.xmin;
float dy=cf.ymax-cf.ymin;
float dz=cf.zmax-cf.zmin;
float cx=10000;
for(int ii=0;ii<pnum;ii++){
    if (cx>cloud->points[pid].x)    {
        cx=cloud->points[pid].x;
    }
}
if((dx>15)||(dy>15)||((dx>10)&&(dy>10)))// 太大{
    isBg=2;
}
else if(((dx>6)||(dy>6))&&(cf.zmean < 1.5))//长而过低{
    isBg = 3;//1;//2;
}
else if(((dx<1.5)&&(dy<1.5))&&(cf.zmax>2.5))//小而过高{
    isBg = 4;
}
else if(cf.pnum<5 || (cf.pnum<10 && cx<50)) //点太少{
    isBg=5;
}
else if((cf.zmean>3)||(cf.zmean<0.3))//太高或太低{
    isBg = 6;//1;
}
if(isBg>0){
    for(int ii=0;ii<pnum;ii++)    {
        if(pLabel[ii]==labelId)        {
            pLabel[ii]=isBg;
        }
    }
}
else{
    labelId++;
}

具体代码如上，源码地址:https://github.com/Livox-SDK/LIO-Livox