具体关于D435得相关资料在网上自行百度吧。
.h
#ifndef D435CAMERA_H
#define D435CAMERA_H
#include <iostream>
#include <opencv2/opencv.hpp>
#include <boost/thread.hpp>
#include <librealsense2/rs.hpp>
using namespace std;
using namespace cv;
class D435Camera
{
public:
D435Camera();
void start();
Mat readColorCamera();
Mat readDepthCamera();
void stop();
private:
Mat frame_to_mat(const rs2::frame& f);
rs2::config cfg;
rs2::pipeline pipe;
rs2::frameset frame;
rs2_intrinsics depth_intrinsics; //深度摄像头内参
rs2_intrinsics color_intrinsics; //彩色摄像头内参
};
#endif // D435CAMERA_H
.cpp
#include "d435camera.h"
D435Camera::D435Camera()
{
}
void D435Camera::start()
{
//读取设备型号和序列号
rs2::context ctx;
auto devicelist = ctx.query_devices();
rs2::device dev = *devicelist.begin(); // 单个设备
cout << "RS2_CAMERA_NAME: " << dev.get_info(RS2_CAMERA_INFO_NAME) << endl;
cout << "RS2_CAMERA_SERIAL_NUMBER: " << dev.get_info(RS2_CAMERA_INFO_SERIAL_NUMBER) << endl;
cout << "RS2_FIRMWARE_VERSION: " << dev.get_info(RS2_CAMERA_INFO_RECOMMENDED_FIRMWARE_VERSION) << endl;
//摄像头工作模式设置
cfg.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 30);
cfg.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
//启动设备
rs2::pipeline_profile profile = pipe.start(cfg);
//声明数据流
auto _stream_depth = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
auto _stream_color = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
//获得摄像头内参
depth_intrinsics = _stream_depth.get_intrinsics();
color_intrinsics = _stream_color.get_intrinsics();
//打印内参
char str[124] = {0};
snprintf(str, 124, "%d_%d_%f_%f_%f_%f_%d_%f_%f_%f_%f_%f",
depth_intrinsics.width, depth_intrinsics.height, depth_intrinsics.ppx, depth_intrinsics.ppy,
depth_intrinsics.fx, depth_intrinsics.fy, depth_intrinsics.model, depth_intrinsics.coeffs[0],
depth_intrinsics.coeffs[1],depth_intrinsics.coeffs[2],depth_intrinsics.coeffs[3],depth_intrinsics.coeffs[4]);
printf("depth intrinsics: %s:%d\n", str, strlen(str));
memset(str, 0x00, 124);
snprintf(str, 124, "%d_%d_%f_%f_%f_%f_%d_%f_%f_%f_%f_%f",
color_intrinsics.width, color_intrinsics.height, color_intrinsics.ppx, color_intrinsics.ppy,
color_intrinsics.fx, color_intrinsics.fy, color_intrinsics.model, color_intrinsics.coeffs[0],
color_intrinsics.coeffs[1],color_intrinsics.coeffs[2],color_intrinsics.coeffs[3],color_intrinsics.coeffs[4]);
printf("color intrinsics: %s:%d\n", str, strlen(str));
//丢掉前几帧
for (int i = 0; i<10; i++)
pipe.wait_for_frames();
}
Mat D435Camera::readColorCamera()
{
frame = pipe.wait_for_frames();
auto color_frame = frame.get_color_frame();
Mat color = frame_to_mat(color_frame);
// imwrite("./D435rgb.png", color);
return color;
}
Mat D435Camera::readDepthCamera()
{
frame = pipe.wait_for_frames();
auto depth_frame = frame.get_depth_frame();
Mat depth = frame_to_mat(depth_frame);
// imwrite("./D435depth.png", depth);
return depth;
}
void D435Camera::stop()
{
pipe.stop();
}
Mat D435Camera::frame_to_mat(const rs2::frame &f)
{
auto vf = f.as<rs2::video_frame>();
const int w = vf.get_width();
const int h = vf.get_height();
if (f.get_profile().format() == RS2_FORMAT_BGR8)
{
return Mat(Size(w, h), CV_8UC3, (void*)f.get_data(), Mat::AUTO_STEP);
}
else if (f.get_profile().format() == RS2_FORMAT_RGB8)
{
auto r = Mat(Size(w, h), CV_8UC3, (void*)f.get_data(), Mat::AUTO_STEP);
cvtColor(r, r, CV_RGB2BGR);
return r;
}
else if (f.get_profile().format() == RS2_FORMAT_Z16)
{
return Mat(Size(w, h), CV_16UC1, (void*)f.get_data(), Mat::AUTO_STEP);
}
else if (f.get_profile().format() == RS2_FORMAT_Y8)
{
return Mat(Size(w, h), CV_8UC1, (void*)f.get_data(), Mat::AUTO_STEP);
}
throw std::runtime_error("Frame format is not supported yet!");
}
main.cpp
#include <iostream>
#include <unistd.h>
#include "d435camera.h"
using namespace std;
int main()
{
cout << "Hello World!" << endl;
D435Camera d435Camera;
d435Camera.start();
while (true) {
Mat color = d435Camera.readColorCamera();
Mat depth = d435Camera.readDepthCamera();
imshow("D435Color", color);
imshow("D435Depth", depth);
waitKey(30);
}
d435Camera.stop();
return 0;
}