Как обнаружить динамические препятствия в облаках точек

Вот как я это делаю для 2D-навигации.

Сначала подготовьте две 2D-карты высот в виде 2D-массивов. Установите для элементов одного из массивов минимальную высоту точек, спроецированных на одну и ту же ячейку 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 — это функции, которые «прокручивают» содержимое двумерного массива, заполняя элементы на краю, не получившие значения в прокрутке, значением третьего параметра.

Теперь вы можете различать массивы (заботясь об элементах со значениями 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 являются вашими потенциальными препятствиями. Вы можете перечислить их и сообщить 2d-координаты тех из них, которые больше, чем некоторое значение, как ваши 2d-препятствия.

Если вы можете подождать до середины сентября, я помещу в общедоступный репозиторий полный код узла ROS, который получает изображение глубины и информацию о камере глубины и генерирует поддельное сообщение LaserScan с измерениями расстояния до найденных препятствий.