Question

我想在机器人中实现立体视觉。我计算了视差图和点云。现在我想检测场景中的动态障碍。有人可以帮我吗？最诚挚的问候

Answer 1

以下是我如何进行二维导航。

首先准备两个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是“滚动”2d数组内容的函数，这些数组填充边缘上没有值的第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的所有非零元素都是你的潜在障碍。你可以枚举它们并报告那些更重要的二维坐标，然后将一些值作为你的二维障碍物。

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

如何检测点云中的动态障碍

1 个答案: