realsense鼠标选择区域保存深度信息到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 <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::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 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);
鼠标选择区域
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,depth\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);
// Update the window with new data
csv << i << "," << j << "," << dist << endl;
}
csv.close();
ofstream depthPointsList;
depthPointsList.open("depthPointsList.txt", 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;
}