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);
145 }
else if (f.get_profile().format() == RS2_FORMAT_RGB8) {
146 cv::Mat tmp(h, w, CV_8UC3, const_cast<void *>(f.get_data()), cv::Mat::AUTO_STEP);
147 cv::cvtColor(tmp, img, cv::COLOR_RGB2BGR);
148 }
else if (f.get_profile().format() == RS2_FORMAT_Y8) {
149 memcpy(img.ptr<uchar>(), f.get_data(), size);
156 const int width = 640, height = 480, fps = 60;
159 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_BGR8, fps);
160 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, fps);
161 config.enable_stream(RS2_STREAM_INFRARED, 1, width, height, RS2_FORMAT_Y8, fps);
162 config.enable_stream(RS2_STREAM_INFRARED, 2, width, height, RS2_FORMAT_Y8, fps);
171 auto color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
172 cv::Mat mat_color(color_profile.height(), color_profile.width(), CV_8UC3);
174 auto depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
175 cv::Mat mat_depth(depth_profile.height(), depth_profile.width(), CV_8UC3);
176 rs2::colorizer color_map;
178 auto infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
179 cv::Mat mat_infrared1(infrared_profile.height(), infrared_profile.width(), CV_8UC1);
180 cv::Mat mat_infrared2(infrared_profile.height(), infrared_profile.width(), CV_8UC1);
182 std::vector<float3> pointcloud;
183 cv::Mat mat_pointcloud(depth_profile.height(), depth_profile.width(), CV_8UC1);
184 std::vector<uint32_t> histogram(0x10000);
188 std::vector<double> time_vector;
193 auto data = pipe.wait_for_frames();
194 frame_to_mat(data.get_color_frame(), mat_color);
195 #if (RS2_API_VERSION >= ((2 * 10000) + (16 * 100) + 0)) 196 frame_to_mat(data.get_depth_frame().apply_filter(color_map), mat_depth);
198 frame_to_mat(color_map(data.get_depth_frame()), mat_depth);
201 cv::imshow(
"OpenCV color", mat_color);
202 cv::imshow(
"OpenCV depth", mat_depth);
204 #if (RS2_API_VERSION >= ((2 * 10000) + (10 * 100) + 0)) 206 frame_to_mat(data.get_infrared_frame(1), mat_infrared1);
207 frame_to_mat(data.get_infrared_frame(2), mat_infrared2);
209 cv::imshow(
"OpenCV infrared left", mat_infrared1);
210 cv::imshow(
"OpenCV infrared right", mat_infrared2);
213 getPointcloud(data.get_depth_frame(), pointcloud);
214 createDepthHist(histogram, pointcloud, depth_scale);
217 for (
size_t i = 0; i < pointcloud.size(); i++) {
218 const float3& pt = pointcloud[i];
226 int u = std::min(static_cast<int>(width-1),
227 static_cast<int>(std::max(0.0, imPt.
get_u())));
228 int v = std::min(static_cast<int>(height-1),
229 static_cast<int>(std::max(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) {
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_CXX_STANDARD >= VISP_CXX_STANDARD_11) 255 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11) " 256 "to make this test work" 259 #if !(VISP_HAVE_OPENCV_VERSION >= 0x030000) 260 std::cout <<
"Install OpenCV version >= 3 to make this test work." << std::endl;
rs2::pipeline_profile & getPipelineProfile()
Get a reference to rs2::pipeline_profile.
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
static double getMedian(const std::vector< double > &v)
static void convertPoint(const vpCameraParameters &cam, const double &x, const double &y, double &u, double &v)
bool open(const rs2::config &cfg=rs2::config())
void start(bool reset=true)
static double getMean(const std::vector< double > &v)
Generic class defining intrinsic camera parameters.
rs2::pipeline & getPipeline()
Get a reference to rs2::pipeline.
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...