realsense鼠标截取彩色深度图区域保存3D信息至csv

// 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 <librealsense2/rsutil.h>
#include <opencv2/opencv.hpp>   // Include OpenCV API

#include <fstream>              // File IO
#include <iostream>             // Terminal IO
#include <sstream>              // Stringstreams
#include<algorithm>
#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 Rect selection;
bool selected = false;
void onMouse(int event, int x, int y, int flags, void* depth)
{
	if (lButtonDown)
	{
		selection.x = MIN(x, p.x);
		selection.y = MIN(y, p.y);
		selection.width = std::abs(x - p.x);
		selection.height = std::abs(y - p.y);
	}
	if (event == EVENT_LBUTTONDOWN)//左键按下，读取初始坐标 
	{
		lButtonDown = true;
		p.x = x;
		p.y = y;
		selection = Rect(x, y, 0, 0);
	}
	if (event == EVENT_LBUTTONUP)
	{
		lButtonDown = false;
		if (selection.width > 0 && selection.height > 0)
		{
			selected = true;
		}
	}
}
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);//指示管道使用所请求的配置启动流
	static int image_num = 1;
	const auto window_depth = "Depth Image";
	namedWindow(window_depth, WINDOW_AUTOSIZE);
	const auto window_color = "color Image";
	namedWindow(window_color, WINDOW_AUTOSIZE);
	char pBuf[255];       //存放路径的变量
	_getcwd(pBuf, 255);   //获取程序的当前目录
	
	while (getWindowProperty(window_depth, WND_PROP_AUTOSIZE) >= 0 && getWindowProperty(window_color, WND_PROP_AUTOSIZE) >= 0)
	{
		rs2::frameset data = pipe.wait_for_frames(); // Wait for next set of frames from the camera
		//rs2::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::depth_frame depth = data.get_depth_frame();
		//rs2::frame depthrgb = color_map.process(depth);//与下句功能一样
		rs2::frame depthrgb = depth.apply_filter(color_map);
		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();*/
		rs2_intrinsics intr = depth.get_profile().as<rs2::video_stream_profile>().get_intrinsics(); // Calibration data
		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_8UC3, (void*)depthrgb.get_data(), Mat::AUTO_STEP);//灰度深度图
		//Mat depthRGBImage(Size(rgbW, rgbH), CV_8UC3, (void*)depthrgb.get_data(), Mat::AUTO_STEP);
		Mat rgbImage(Size(rgbW, rgbH), CV_8UC3, (void*)rgb.get_data(), Mat::AUTO_STEP);
		Mat ir1Image(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);
		//imshow("depthRGB", depthRGBImage);


		//鼠标选择区域
		setMouseCallback(window_depth, onMouse);
		if (selected)
		{
			selected = false;
			selection &= Rect(0, 0, depthImage.cols, depthImage.rows);  //用于确保所选的矩形区域在图片范围内
			//将选择区域对应深度保存csv文件
			std::ofstream csv;
			stringstream depthPointsFile;
			depthPointsFile.clear();
			depthPointsFile << (string)pBuf << "/depthPointsCSV/depthPoints" << image_num << ".csv";

			csv.open(depthPointsFile.str());
			//    std::cout << "Writing metadata to " << filename << endl;
			csv << "Pix_x,Pix_y,3DX,3DY,3DZ\n";
			for (int i = selection.x; i <= selection.x + selection.width; i++)
				for (int j = selection.y; j <= selection.y + selection.height; j++)
				{
					float dist = depth.get_distance(i, j);
					float upoint[3]; //  point (in 3D)
					float upixel[2]; // pixel
					upixel[0] = i;
					upixel[1] = j;
					// Deproject from pixel to point in 3D
					
					rs2_deproject_pixel_to_point(upoint, &intr, upixel, dist);
					// Update the window with new data
					csv << i << "," << j << "," << upoint[0] << "," << upoint[1] << "," << upoint[2] << endl;
				}
			csv.close();

			ofstream depthPointsList;
			depthPointsList.open("depthPointsList.xml", ios::app);
			depthPointsList << depthPointsFile.str() << endl;
			depthPointsList.close();

			Mat selectImg(depthImage, selection);
			//bitwise_not(selectImg, selectImg);
			//rectangle(depthImage,Point(selection.x, selection.y), Point(selection.x + selection.width, selection.y + selection.height),Scalar(255,255,255), 1, 8 );  //画矩形

			stringstream depthFile;
			stringstream selectFile;

			stringstream rgbFile;
			stringstream irFile;
			stringstream ir2File;

			depthFile.clear();
			depthFile << (string)pBuf << "/depthImage/depth" << image_num << ".jpg";
			imwrite(depthFile.str(), depthImage);//保存图片

			selectFile.clear();
			selectFile << (string)pBuf << "/selectImage/select" << image_num << ".jpg";
			imwrite(selectFile.str(), selectImg);//保存图片

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

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

			ofstream depthList;
			depthList.open("depthList.txt", ios::app);
			depthList << depthFile.str() << endl;
			depthList.close();

			ofstream rgbList;
			rgbList.open("rgbList.txt", ios::app);
			rgbList << rgbFile.str() << endl;
			rgbList.close();

			cout << image_num << endl;
			image_num++;
		}

		imshow(window_depth, depthImage);
		imshow(window_color, rgbImage);
		waitKey(1);
	}
	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;
}