0

我想在机器人中实现立体视觉。我计算了视差图和点云。现在我想检测场景中的动态障碍物。任何人都可以帮助我吗？此致

4

1 回答 1

0

这是我为 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 障碍物。

如果您可以等到 9 月中旬，我会将 ROS 节点的完整代码放入公共存储库中，该节点获取深度图像和深度相机信息并生成伪造的 LaserScan 消息，其中测量设置为发现障碍物的距离。

于 2016-08-29T18:17:14.303 回答