通過幀間確定動態障礙物，剔除動態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柵格地圖，用于路徑規劃！！
目前只是簡單的嘗試了一下，細節部分還有待優化，