How to detect Dynamic Obstacles in point clouds

Question

I want to implement stereo vision in a robot. I have calculated disparity map and point clouds. now I want to detect Dynamic Obstacles in scene. Can anyone help me please? Best Regards

Answer 1

Here is how I do that for 2d navigation.

First prepare two 2d elevation maps as 2d arrays. Set elements of one of the arrays to min height of points projected to the same cell of the 2d map, and set elements of the other array to max heights like this:

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

Then

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

Here CvUtils::hscroll and CvUtils::vscroll are functions that 'scroll' the content of a 2d array filling the elements on the edge that got no value in the scroll with the value of the third parameter.

Now you can make a difference between the arrays (taking care about elements with c_inf and c_neg_inf values) like this:

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

Now all non-zero elements of top_view_elevation_diff are your potential obstacles. You can enumerate them and report 2d coordinates of those of them that are grater then some value as your 2d obstacles.

If you can wait till the middle of September, I'll put into a public repository full code of a ROS node that takes a depth image and depth camera info and generates a faked LaserScan message with measurements set to distance to found obstacles.