점 구름에서 동적 장애물을 감지하는 방법

Question

로봇에서 스테레오 비전을 구현하고 싶습니다. 나는 불일치지도와 점 구름을 계산했습니다. 이제는 장면에서 다이내믹 한 장애물을 감지하고 싶습니다. 아무도 도와 줄 수 있습니까? 최고 감사합니다

Answer 1

다음은 2 차원 탐색에 대한 설명입니다.

먼저 두 개의 2 차원 입면도를 2 차원 배열로 준비합니다. 어레이들 중 하나의 집합 요소들은 2 차원지도의 동일 셀에 투영 점의 높이 분 설정할 최대 높이의 다른 배열의 요소를 다음과 같이한다 : 다음

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은 두 번째 배열의 내용을 스크롤하여 세 번째 매개 변수의 값이있는 스크롤의 값이없는 가장자리의 요소를 채우는 함수입니다.

//: 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의 '0'이 아닌 요소가 잠재적 인 장애물 :

이제 이런 (c_inf 및 c_neg_inf 값을 가진 요소에 대해주의하면서) 배열의 차이를 만들 수 있습니다. 당신은 그것들을 열거 할 수 있고 그것들의 2 차원 좌표를보고 당신의 2 차원 장애물로서 어떤 가치를 더할 수 있습니다.

9 월 중순까지 기다릴 수 있다면 깊이 이미지와 깊이 카메라 정보를 사용하는 ROS 노드의 전체 코드를 공공 저장소에 넣고 측정 된 거리를 측정 거리로 설정 한 가짜 레이저 스캔 메시지를 생성합니다 장애물.