博客

realsense鼠标点击深度图获取对应位置深度

// License: Apache 2.0. See LICENSE file in root directory.
// Copyright(c) 2017 Intel Corporation. All Rights Reserved.

#include <librealsense2/rs.hpp> // Include RealSense Cross Platform API
#include <opencv2/opencv.hpp>   // Include OpenCV API

#include <fstream>              // File IO
#include <iostream>             // Terminal IO
#include <sstream>              // Stringstreams

#include <direct.h>
using namespace std;
using namespace cv;

#define RS_WIDTH 640//1280
#define RS_HEIGHT 480//720
#define RS_FPS 30
bool lButtonDown = false;
static Point p;
static int pointNum = 0;  //个数 

void onMouse(int event, int x, int y, int flags, void* depth)
{
	if (event == EVENT_LBUTTONDOWN)//左键按下，读取初始坐标 
	{
		pointNum++;//统计点击的次数 
		p.x = x;
		p.y = y;
		lButtonDown = 1;
	}
}
int main(int argc, char* argv[]) try
{
	// judge whether devices is exist or not
	rs2::context ctx;
	auto list = ctx.query_devices(); // Get a snapshot of currently connected devices
	if (list.size() == 0)
		throw std::runtime_error("No device detected. Is it plugged in?");
	rs2::device dev = list.front();

	// Declare depth colorizer for pretty visualization of depth data
	rs2::colorizer color_map;
	// Declare RealSense pipeline, encapsulating the actual device and sensors
	rs2::pipeline pipe;

	rs2::config cfg;//创建一个以非默认配置的配置用来配置管道
	 //Add desired streams to configuration
	//彩色和深度可以不配置，按默认值输出；红外图必须进行配置，否则无法显示
	cfg.enable_stream(RS2_STREAM_COLOR, RS_WIDTH, RS_HEIGHT, RS2_FORMAT_BGR8, RS_FPS);//向配置添加所需的流
	cfg.enable_stream(RS2_STREAM_DEPTH, RS_WIDTH, RS_HEIGHT, RS2_FORMAT_Z16, RS_FPS);
	cfg.enable_stream(RS2_STREAM_INFRARED, 1, RS_WIDTH, RS_HEIGHT, RS2_FORMAT_Y8, RS_FPS);
	cfg.enable_stream(RS2_STREAM_INFRARED, 2, RS_WIDTH, RS_HEIGHT, RS2_FORMAT_Y8, RS_FPS);

	//cfg.enable_stream(RS2_STREAM_DEPTH); // Enable default depth
 //  // For the color stream, set format to RGBA
 //  // To allow blending of the color frame on top of the depth frame
	//cfg.enable_stream(RS2_STREAM_COLOR, RS2_FORMAT_RGBA8);

	// Start streaming with default recommended configuration
	pipe.start(cfg);//指示管道使用所请求的配置启动流

	const auto window_name = "Display Image";
	namedWindow(window_name, WINDOW_AUTOSIZE);
	char pBuf[255];       //存放路径的变量
	_getcwd(pBuf, 255);   //获取程序的当前目录
	while (getWindowProperty(window_name, WND_PROP_AUTOSIZE) >= 0)
	{
		rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera
		//rs2::depth_frame depth = data.get_depth_frame().apply_filter(color_map);//Apply color map for visualization of depth// Find and colorize the depth data.如果没有apply_filter(color_map)深度图像为灰度图，对应CV_8UC1
		//rs2::frame depth = data.get_depth_frame().apply_filter(color_map);
		rs2::depth_frame depth = data.get_depth_frame();
		rs2::frame rgb = data.get_color_frame();
		rs2::frame ir1 = data.get_infrared_frame(1);
		rs2::frame ir2 = data.get_infrared_frame(2);

		// Query frame size (width and height)
		//const int depthW = depth.get_width();
		//const int depthH = depth.get_height();
		/*const int depthW = depth.as<rs2::video_frame>().get_width();
		const int depthH = depth.as<rs2::video_frame>().get_height();*/

		const int rgbW = rgb.as<rs2::video_frame>().get_width();
		const int rgbH = rgb.as<rs2::video_frame>().get_height();

		const int irW = ir1.as<rs2::video_frame>().get_width();
		const int irH = ir1.as<rs2::video_frame>().get_height();
		// Create OpenCV matrix of size (w,h) from the colorized depth data
		//depth = depth.apply_filter(color_map);
		//Mat depthImage(Size(depthW, depthH), CV_8UC3, (void*)depth.get_data(), Mat::AUTO_STEP);//彩色深度图
		//Mat depthImage(Size(depthW, depthH), CV_8UC1, (void*)depth.get_data(), Mat::AUTO_STEP);//灰度深度图
		Mat depthImage(Size(RS_WIDTH, RS_HEIGHT), CV_8UC1, (void*)depth.get_data(), Mat::AUTO_STEP);//灰度深度图
		Mat rgbImage(Size(rgbW, rgbH), CV_8UC3, (void*)rgb.get_data(), Mat::AUTO_STEP);

		Mat irImage(Size(irW, irH), CV_8UC1, (void*)ir1.get_data(), Mat::AUTO_STEP);
		Mat ir2Image(Size(irW, irH), CV_8UC1, (void*)ir2.get_data(), Mat::AUTO_STEP);

		// Query the distance from the camera to the object in the center of the image
		setMouseCallback(window_name, onMouse);
		if (lButtonDown)
		{
			float dist = depth.get_distance(p.x, p.y);
			// Update the window with new data
			cout << p.x << "," << p.y << "," << dist << endl;
			lButtonDown = 0;
		}

		stringstream depthFile;
		stringstream rgbFile;
		stringstream irFile;
		stringstream ir2File;
		static int image_num = 1;
		char c = (char)waitKey(1);
		if (c == 27)
			std::cout << "Exit boss" << std::endl;//break;
		switch (c)
		{
		case 'p':
			depthFile.clear();
			depthFile << (string)pBuf << "/depth" << image_num << ".png";
			imwrite(depthFile.str(), depthImage);//保存图片

			rgbFile.clear();
			rgbFile << (string)pBuf << "/rgb" << image_num << ".png";
			imwrite(rgbFile.str(), rgbImage);//保存图片

			irFile.clear();
			irFile << (string)pBuf << "/ir1-" << image_num << ".jpg";
			imwrite(irFile.str(), irImage);//保存图片
			ir2File.clear();
			ir2File << (string)pBuf << "/ir2-" << image_num << ".jpg";
			imwrite(ir2File.str(), ir2Image);//保存图片

			image_num++;
			cout << image_num << endl;
			break;
		default:
			break;
		}

		imshow(window_name, depthImage);

	}
	return EXIT_SUCCESS;
}
catch (const rs2::error& e)
{
	std::cerr << "RealSense error calling " << e.get_failed_function() << "(" << e.get_failed_args() << "):\n    " << e.what() << std::endl;
	return EXIT_FAILURE;
}
catch (const std::exception& e)
{
	std::cerr << e.what() << std::endl;
	return EXIT_FAILURE;
}