通過幀間確定動態障礙物，剔除動態3D點雲資料後用於生成柵格地圖

禿頭隊長發表於2020-12-29
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柵格地圖，用於路徑規劃！！
目前只是簡單的嘗試了一下，細節部分還有待優化。

相關文章