ポイントクラウドの動的障害を検出する方法

Question

ロボットでステレオビジョンを実現したい。私は視差マップと点群を計算しました。今私は場面のダイナミックな障害を検出したいと思います。 誰も私を助けることができますか？ よろしくお願いします。

Answer 1

ここで私は2次元ナビゲーションのためにそれを行います。

まず、2次元アレイとして2つの2次元高度マップを準備します。アレイの1つの組の要素は、2Dマップの同じセルに投影点の高さ分と設定最大高さに他の配列の要素を次のようにする：そして

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

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

ここでは、CvUtils :: hscrollとCvUtils :: vscrollは、2次元配列の内容を 'スクロール'して、スクロールで値を持たないエッジの要素を3番目のパラメータの値で満たす関数です。

今、あなたは、このようなアレイ（c_infとc_neg_inf値を持つ要素についての世話をする）との差を作ることができます。

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

今top_view_elevation_diffのすべての非ゼロ要素があなたの潜在的な障害となっています。あなたはそれらを列挙し、それらの2次元座標を報告し、次に2次元の障害としていくつかの値を報告することができます。

9月中旬まで待つことができれば、奥行き画像と深度カメラ情報を取得するROSノードの完全なコードをパブリックリポジトリに入れ、測定値をdistance to foundに設定した偽のLaserScanメッセージを生成します障害。