So erkennen Sie dynamische Hindernisse in Punktwolken

Question

Ich möchte Stereo Vision in einem Roboter implementieren. Ich habe Disparitätskarten und Punktwolken berechnet. Jetzt möchte ich dynamische Hindernisse in der Szene erkennen. Kann mir bitte jemand helfen? Mit freundlichen Grüßen

Answer 1

Hier ist, wie ich das für 2d Navigation tun.

Bereiten Sie zuerst zwei 2D-Höhenkarten als 2D-Arrays vor. Set Elemente eines der Arrays der Höhe der Punkte auf der gleichen Zelle der 2D-Karte projiziert min und wie diese Elemente der anderen Anordnung zu max Höhen eingestellt:

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); 
     } 
    } 
} 

Dann

//: 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)); 

Hier sind CvUtils :: hscroll und CvUtils :: vscroll Funktionen, die den Inhalt eines 2D-Arrays scrollen, indem sie die Elemente auf der Kante füllen, die keinen Wert in der Rolle haben, mit dem Wert des dritten Parameters.

Jetzt können Sie einen Unterschied zwischen den Anordnungen machen (die Betreuung über Elemente mit c_inf und c_neg_inf Werte) wie folgt aus:

//: 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); 

Nun sind alle Nicht-Null-Elemente von top_view_elevation_diff sind Ihre potenziellen Hindernisse. Sie können sie aufzählen und 2d-Koordinaten von denen von ihnen, die größer als ein Wert sind, als Ihre 2d-Hindernisse melden.

Wenn Sie bis Mitte September warten können, werde ich in einen öffentlichen Repository den vollständigen Code eines ROS-Knotens eingeben, der eine Tiefenbild- und Tiefenkamera-Information erstellt und eine gefälschte LaserScan-Nachricht mit auf Distanz zu gefundenen Messungen generiert Hindernisse.