Comment détecter les obstacles dynamiques dans les nuages ​​de points

Question

Je souhaite mettre en œuvre une vision stéréo dans un robot. J'ai calculé la carte de disparité et les nuages ​​de points. maintenant je veux détecter les obstacles dynamiques dans la scène. Quelqu'un peut-il m'aider s'il vous plaît? Meilleures salutations

Answer 1

Voici comment je fais cela pour la navigation 2D.

Préparez d'abord deux cartes d'élévation 2D en tant que matrices 2d. Ensemble des éléments de l'un des réseaux à hauteur minimum de points prévus à la même cellule de la carte 2d et définir des éléments de l'autre réseau à des hauteurs max comme ceci:

static const float c_neg_inf = -9999; 
static const float c_inf = 9999; 
int map_pixels_in_m_ = 40; //: for map cell size 2.5 x 2.5 cm 
int map_width = 16 * map_pixels_in_m_; 
int map_height = 16 * map_pixels_in_m_; 
cv::Mat top_view_min_elevation(cv::Size(map_width, map_height), CV_32FC1, cv::Scalar(c_inf)); 
cv::Mat top_view_max_elevation(cv::Size(map_width, map_height), CV_32FC1, cv::Scalar(c_neg_inf)); 

//: prepare elevation maps: 
for (int i = 0, v = 0; v < height; ++v) { 
    for (int u = 0; u < width; ++u, ++i) { 
     if (!pcl::isFinite(point_cloud_->points[i])) 
      continue; 
     pcl::Vector3fMap point_in_laser_frame = point_cloud_->points[i].getVector3fMap(); 
     float z = point_in_laser_frame(2); 
     int map_x = map_width/2 - point_in_laser_frame(1) * map_pixels_in_m_; 
     int map_y = map_height - point_in_laser_frame(0) * map_pixels_in_m_; 
     if (map_x >= 0 && map_x < map_width && map_y >= 0 && map_y < map_width) { 
      //: update elevation maps: 
      top_view_min_elevation.at<float>(map_x, map_y) = std::min(top_view_min_elevation.at<float>(map_x, map_y), z); 
      top_view_max_elevation.at<float>(map_x, map_y) = std::max(top_view_max_elevation.at<float>(map_x, map_y), z); 
     } 
    } 
} 

Puis

//: merge values in neighboring pixels of the elevation maps: 
top_view_min_elevation = cv::min(top_view_min_elevation, CvUtils::hscroll(top_view_min_elevation, -1, c_inf)); 
top_view_max_elevation = cv::max(top_view_max_elevation, CvUtils::hscroll(top_view_max_elevation, -1, c_neg_inf)); 
top_view_min_elevation = cv::min(top_view_min_elevation, CvUtils::hscroll(top_view_min_elevation, 1, c_inf)); 
top_view_max_elevation = cv::max(top_view_max_elevation, CvUtils::hscroll(top_view_max_elevation, 1, c_neg_inf)); 
top_view_min_elevation = cv::min(top_view_min_elevation, CvUtils::vscroll(top_view_min_elevation, -1, c_inf)); 
top_view_max_elevation = cv::max(top_view_max_elevation, CvUtils::vscroll(top_view_max_elevation, -1, c_neg_inf)); 
top_view_min_elevation = cv::min(top_view_min_elevation, CvUtils::vscroll(top_view_min_elevation, 1, c_inf)); 
top_view_max_elevation = cv::max(top_view_max_elevation, CvUtils::vscroll(top_view_max_elevation, 1, c_neg_inf)); 

Ici CvUtils :: hscroll et CvUtils :: vscroll sont des fonctions qui 'font défiler' le contenu d'un tableau 2d en remplissant les éléments du bord qui n'ont pas de valeur dans le scroll avec la valeur du troisième paramètre.

Maintenant, vous pouvez faire une différence entre les tableaux (en prenant soin sur les éléments avec c_inf et les valeurs c_neg_inf) comme ceci:

//: produce the top_view_elevation_diff_: 
cv::Mat top_view_elevation_diff = top_view_max_elevation - top_view_min_elevation; 
cv::threshold(top_view_elevation_diff, top_view_elevation_diff, c_inf, 0, cv::THRESH_TOZERO_INV); 

Maintenant, tous les éléments non nuls de top_view_elevation_diff sont vos obstacles potentiels. Vous pouvez les énumérer et rapporter les coordonnées 2d de ceux d'entre eux qui sont plus rude puis une valeur comme vos obstacles 2d. Si vous pouvez attendre jusqu'à la mi-septembre, je vais mettre dans un référentiel public le code complet d'un nœud ROS qui prend une image de profondeur et des informations de caméra en profondeur et génère un message LaserScan truqué avec des mesures à distance obstacles.