42 #include <visp3/core/vpConfig.h>
44 #if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
45 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && (VISP_HAVE_OPENCV_VERSION >= 0x030000)
47 #include <visp3/core/vpImage.h>
48 #include <visp3/core/vpImageConvert.h>
49 #include <visp3/core/vpMeterPixelConversion.h>
50 #include <visp3/sensor/vpRealSense2.h>
52 #include <opencv2/core.hpp>
53 #include <opencv2/highgui.hpp>
60 float3() : x(0), y(0), z(0) { }
61 float3(
float x_,
float y_,
float z_) : x(x_), y(y_), z(z_) { }
64 void getPointcloud(
const rs2::depth_frame &depth_frame, std::vector<float3> &pointcloud)
66 auto vf = depth_frame.as<rs2::video_frame>();
67 const int width = vf.get_width();
68 const int height = vf.get_height();
69 pointcloud.resize((
size_t)(width * height));
72 rs2::points points = pc.calculate(depth_frame);
73 auto vertices = points.get_vertices();
74 for (
size_t i = 0; i < points.size(); i++) {
76 if (vertices[i].z > std::numeric_limits<float>::epsilon()) {
77 pcl.x = vertices[i].x;
78 pcl.y = vertices[i].y;
79 pcl.z = vertices[i].z;
86 void createDepthHist(std::vector<uint32_t> &histogram,
const std::vector<float3> &pointcloud,
float depth_scale)
88 std::fill(histogram.begin(), histogram.end(), 0);
90 for (
size_t i = 0; i < pointcloud.size(); i++) {
91 const float3 &pt = pointcloud[i];
92 ++histogram[
static_cast<uint32_t
>(pt.z * depth_scale)];
95 for (
int i = 2; i < 0x10000; i++)
96 histogram[i] += histogram[i - 1];
100 unsigned char getDepthColor(
const std::vector<uint32_t> &histogram,
float z,
float depth_scale)
103 return static_cast<unsigned char>(histogram[
static_cast<uint32_t
>(z * depth_scale)] * 255 / histogram[0xFFFF]);
106 void getNativeFrame(
const rs2::frame &frame,
unsigned char *
const data)
108 auto vf = frame.as<rs2::video_frame>();
109 int size = vf.get_width() * vf.get_height();
111 switch (frame.get_profile().format()) {
112 case RS2_FORMAT_RGB8:
113 case RS2_FORMAT_BGR8:
114 memcpy(data, frame.get_data(), size * 3);
117 case RS2_FORMAT_RGBA8:
118 case RS2_FORMAT_BGRA8:
119 memcpy(data, frame.get_data(), size * 4);
124 memcpy(data, frame.get_data(), size * 2);
128 memcpy(data, frame.get_data(), size);
136 void frame_to_mat(
const rs2::frame &f, cv::Mat &img)
138 auto vf = f.as<rs2::video_frame>();
139 const int w = vf.get_width();
140 const int h = vf.get_height();
141 const int size = w * h;
143 if (f.get_profile().format() == RS2_FORMAT_BGR8) {
144 memcpy(
static_cast<void *
>(img.ptr<cv::Vec3b>()), f.get_data(), size * 3);
146 else if (f.get_profile().format() == RS2_FORMAT_RGB8) {
147 cv::Mat tmp(h, w, CV_8UC3,
const_cast<void *
>(f.get_data()), cv::Mat::AUTO_STEP);
148 cv::cvtColor(tmp, img, cv::COLOR_RGB2BGR);
150 else if (f.get_profile().format() == RS2_FORMAT_Y8) {
151 memcpy(img.ptr<uchar>(), f.get_data(), size);
158 const int width = 640, height = 480, fps = 60;
161 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_BGR8, fps);
162 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
163 config.enable_stream(RS2_STREAM_INFRARED, 1, width, height, RS2_FORMAT_Y8, fps);
164 config.enable_stream(RS2_STREAM_INFRARED, 2, width, height, RS2_FORMAT_Y8, fps);
173 auto color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
174 cv::Mat mat_color(color_profile.height(), color_profile.width(), CV_8UC3);
176 auto depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
177 cv::Mat mat_depth(depth_profile.height(), depth_profile.width(), CV_8UC3);
178 rs2::colorizer color_map;
180 auto infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
181 cv::Mat mat_infrared1(infrared_profile.height(), infrared_profile.width(), CV_8UC1);
182 cv::Mat mat_infrared2(infrared_profile.height(), infrared_profile.width(), CV_8UC1);
184 std::vector<float3> pointcloud;
185 cv::Mat mat_pointcloud(depth_profile.height(), depth_profile.width(), CV_8UC1);
186 std::vector<uint32_t> histogram(0x10000);
190 std::vector<double> time_vector;
195 auto data = pipe.wait_for_frames();
196 frame_to_mat(data.get_color_frame(), mat_color);
197 #if (RS2_API_VERSION >= ((2 * 10000) + (16 * 100) + 0))
198 frame_to_mat(data.get_depth_frame().apply_filter(color_map), mat_depth);
200 frame_to_mat(color_map(data.get_depth_frame()), mat_depth);
203 cv::imshow(
"OpenCV color", mat_color);
204 cv::imshow(
"OpenCV depth", mat_depth);
206 #if (RS2_API_VERSION >= ((2 * 10000) + (10 * 100) + 0))
208 frame_to_mat(data.get_infrared_frame(1), mat_infrared1);
209 frame_to_mat(data.get_infrared_frame(2), mat_infrared2);
211 cv::imshow(
"OpenCV infrared left", mat_infrared1);
212 cv::imshow(
"OpenCV infrared right", mat_infrared2);
215 getPointcloud(data.get_depth_frame(), pointcloud);
216 createDepthHist(histogram, pointcloud, depth_scale);
219 for (
size_t i = 0; i < pointcloud.size(); i++) {
220 const float3 &pt = pointcloud[i];
228 int u = std::min<int>(
static_cast<int>(width - 1),
static_cast<int>(std::max<double>(0.0, imPt.
get_u())));
229 int v = std::min<int>(
static_cast<int>(height - 1),
static_cast<int>(std::max<double>(0.0, imPt.
get_v())));
230 unsigned char depth_viz = getDepthColor(histogram, Z, depth_scale);
231 mat_pointcloud.at<uchar>(v, u) = depth_viz;
234 cv::imshow(
"OpenCV projected pointcloud", mat_pointcloud);
238 if (cv::waitKey(5) == 27 || cv::waitKey(5) == 113) {
243 std::cout <<
"Acquisition - Mean time: " <<
vpMath::getMean(time_vector)
244 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
251 #if !defined(VISP_HAVE_REALSENSE2)
252 std::cout <<
"Install librealsense2 to make this test work." << std::endl;
254 #if !(VISP_HAVE_OPENCV_VERSION >= 0x030000)
255 std::cout <<
"Install OpenCV version >= 3 to make this test work." << std::endl;
Generic class defining intrinsic camera parameters.
void start(bool reset=true)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
static double getMedian(const std::vector< double > &v)
static double getMean(const std::vector< double > &v)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
rs2::pipeline_profile & getPipelineProfile()
Get a reference to rs2::pipeline_profile.
bool open(const rs2::config &cfg=rs2::config())
rs2::pipeline & getPipeline()
Get a reference to rs2::pipeline.