要在每一帧图像中检测高度为 20cm 的台阶，你需要使用计算机视觉技术进行图像处理。以下是一种简单的方法，可以在 OpenCV 中实现：
1. 将 RGB 图像转换为灰度图像。
```cpp
cv::Mat gray_image;
cv::cvtColor(color_image, gray_image, cv::COLOR_BGR2GRAY);
```
2. 对灰度图像进行高斯模糊，以减少噪声。
```cpp
cv::Mat blurred_image;
cv::GaussianBlur(gray_image, blurred_image, cv::Size(5, 5), 0);
```
3. 使用 Canny 边缘检测算法检测图像中的边缘。
```cpp
cv::Mat edges;
cv::Canny(blurred_image, edges, 50, 150);
```
4. 使用霍夫变换检测图像中的直线。
```cpp
std::vector<cv::Vec2f> lines;
cv::HoughLines(edges, lines, 1, CV_PI / 180, 100);
```
5. 对检测到的直线进行筛选，找到符合条件的直线。
```cpp
for (size_t i = 0; i < lines.size(); i++) {
float rho = lines[i][0];
float theta = lines[i][1];
if (theta < CV_PI / 4 || theta > 3 * CV_PI / 4) {
continue;
}
cv::Point pt1(rho / cos(theta), 0);
cv::Point pt2((rho - edges.rows * sin(theta)) / cos(theta), edges.rows);
cv::line(color_image, pt1, pt2, cv::Scalar(0, 0, 255), 2);
}
```
6. 计算台阶距离相机的距离，如果距离小于 0.2m，则输出提醒。
要计算距离，你需要知道相机的内参和深度图像中的像素值。假设你已经将深度图像存储在一个名为 `depth_image` 的 `cv::Mat` 对象中，你可以使用以下代码计算距离：
```cpp
float depth_scale = profile.get_device().first<rs2::depth_sensor>().get_depth_scale();
uint16_t depth_value = depth_image.at<uint16_t>(y, x);
float distance = depth_value * depth_scale;
```
将 `(x, y)` 替换为要计算距离的像素坐标。
完整代码示例：
```cpp
#include <librealsense2/rs.hpp>
#include <opencv2/opencv.hpp>
#include <thread>
void capture_thread(rs2::pipeline pipe, rs2::frame_queue& queue) {
while (true) {
rs2::frameset frameset = pipe.wait_for_frames();
queue.enqueue(frameset);
}
}
void process_thread(rs2_intrinsics color_intrinsics, rs2_intrinsics depth_intrinsics, rs2::frame_queue& queue) {
cv::namedWindow("color", cv::WINDOW_NORMAL);
cv::resizeWindow("color", color_intrinsics.width, color_intrinsics.height);
cv::namedWindow("depth", cv::WINDOW_NORMAL);
cv::resizeWindow("depth", depth_intrinsics.width, depth_intrinsics.height);
while (cv::getWindowProperty("color", cv::WND_PROP_AUTOSIZE) >= 0 && cv::getWindowProperty("depth", cv::WND_PROP_AUTOSIZE) >= 0) {
rs2::frameset frameset;
if (queue.poll_for_frame(&frameset)) {
rs2::frame color_frame = frameset.get_color_frame();
rs2::frame depth_frame = frameset.get_depth_frame();
cv::Mat color_image(cv::Size(color_intrinsics.width, color_intrinsics.height), CV_8UC3, (void*)color_frame.get_data(), cv::Mat::AUTO_STEP);
cv::Mat depth_image(cv::Size(depth_intrinsics.width, depth_intrinsics.height), CV_16UC1, (void*)depth_frame.get_data(), cv::Mat::AUTO_STEP);
// 在这里对每一帧图像进行处理
cv::Mat gray_image;
cv::cvtColor(color_image, gray_image, cv::COLOR_BGR2GRAY);
cv::Mat blurred_image;
cv::GaussianBlur(gray_image, blurred_image, cv::Size(5, 5), 0 cv::Mat edges;
cv::Canny(blurred_image, edges, 50, 150);
std::vector<cv::Vec2f> lines;
cv::HoughLines(edges, lines, 1, CV_PI / 180, 100);
for (size_t i = 0; i < lines.size(); i++) {
float rho = lines[i][0];
float theta = lines[i][1];
if (theta < CV_PI / 4 || theta > 3 * CV_PI / 4) {
continue;
}
cv::Point pt1(rho / cos(theta), 0);
cv::Point pt2((rho - edges.rows * sin(theta)) / cos(theta), edges.rows);
cv::line(color_image, pt1, pt2, cv::Scalar(0, 0, 255), 2);
// 计算台阶距离相机的距离
int step_height = 20; // 台阶高度为20cm
float depth_scale = profile.get_device().first<rs2::depth_sensor>().get_depth_scale();
int x = (pt1.x + pt2.x) / 2; // 取直线中间的像素作为计算距离的像素
int y = (pt1.y + pt2.y) / 2;
uint16_t depth_value = depth_image.at<uint16_t>(y, x);
float distance = depth_value * depth_scale;
if (distance < 0.2 && distance > 0) { // 距离小于0.2m，输出提醒
std::cout << "Step detected at distance " << distance << "m" << std::endl;
}
}
// 显示处理后的图像
cv::imshow("color", color_image);
cv::imshow("depth", depth_image);
}
cv::waitKey(1);
}
}
int main() {
// 初始化 SDK
rs2::context ctx;
// 连接设备
rs2::device_list devices = ctx.query_devices();
if (devices.size() == 0) {
std::cerr << "Error: no device detected" << std::endl;
return 1;
}
rs2::device device = devices[0];
// 配置流
rs2::config cfg;
cfg.enable_stream(rs2_stream::RS2_STREAM_COLOR, 640, 480, rs2_format::RS2_FORMAT_BGR8, 30);
cfg.enable_stream(rs2_stream::RS2_STREAM_DEPTH, 640, 480, rs2_format::RS2_FORMAT_Z16, 30);
// 开始流
rs2::pipeline pipe;
rs2::pipeline_profile profile = pipe.start(cfg);
// 获取相机内参
rs2::video_stream_profile color_profile = profile.get_stream(rs2_stream::RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
rs2_intrinsics color_intrinsics = color_profile.get_intrinsics();
rs2::video_stream_profile depth_profile = profile.get_stream(rs2_stream::RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
rs2_intrinsics depth_intrinsics = depth_profile.get_intrinsics();
// 创建队列并启动线程
rs2::frame_queue queue;
std::thread capture_thread_obj(capture_thread, pipe, std::ref(queue));
std::thread process_thread_obj(process_thread, color_intrinsics, depth_intrinsics, std::ref(queue));
// 等待线程结束
capture_thread_obj.join();
process_thread_obj.join();
// 停止流
pipe.stop();
return 0;
}
```