43#include <visp3/core/vpConfig.h>
45#if defined(VISP_HAVE_REALSENSE2) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
46 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
50#include <pcl/visualization/cloud_viewer.h>
51#include <pcl/visualization/pcl_visualizer.h>
55#include <visp3/core/vpImage.h>
56#include <visp3/core/vpImageConvert.h>
57#include <visp3/gui/vpDisplayGDI.h>
58#include <visp3/gui/vpDisplayX.h>
59#include <visp3/sensor/vpRealSense2.h>
61#if VISP_HAVE_OPENCV_VERSION >= 0x030000
62#include <opencv2/core.hpp>
63#include <opencv2/highgui.hpp>
70pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
71pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(
new pcl::PointCloud<pcl::PointXYZRGB>());
72bool cancelled =
false, update_pointcloud =
false;
77 explicit ViewerWorker(
bool color_mode, std::mutex &mutex) : m_colorMode(color_mode), m_mutex(mutex) {}
82 pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer(
"3D Viewer " + date));
83 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_color);
84 pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
85 pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_pointcloud_color(
new pcl::PointCloud<pcl::PointXYZRGB>());
87 viewer->setBackgroundColor(0, 0, 0);
88 viewer->initCameraParameters();
89 viewer->setPosition(640 + 80, 480 + 80);
90 viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
91 viewer->setSize(640, 480);
94 bool local_update =
false, local_cancelled =
false;
95 while (!local_cancelled) {
97 std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
99 if (lock.owns_lock()) {
100 local_update = update_pointcloud;
101 update_pointcloud =
false;
102 local_cancelled = cancelled;
106 local_pointcloud_color = pointcloud_color->makeShared();
108 local_pointcloud = pointcloud->makeShared();
114 if (local_update && !local_cancelled) {
115 local_update =
false;
119 viewer->addPointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb,
"RGB sample cloud");
120 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
123 viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
124 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
129 viewer->updatePointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb,
"RGB sample cloud");
131 viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
139 std::cout <<
"End of point cloud display thread" << std::endl;
147void getPointcloud(
const rs2::depth_frame &depth_frame, std::vector<vpColVector> &point_cloud)
149 auto vf = depth_frame.as<rs2::video_frame>();
150 const int width = vf.get_width();
151 const int height = vf.get_height();
152 point_cloud.resize((
size_t)(width * height));
155 rs2::points points = pc.calculate(depth_frame);
156 auto vertices = points.get_vertices();
158 for (
size_t i = 0; i < points.size(); i++) {
160 v[0] = vertices[i].x;
161 v[1] = vertices[i].y;
162 v[2] = vertices[i].z;
176void getNativeFrame(
const rs2::frame &frame,
unsigned char *
const data)
178 auto vf = frame.as<rs2::video_frame>();
179 int size = vf.get_width() * vf.get_height();
181 switch (frame.get_profile().format()) {
182 case RS2_FORMAT_RGB8:
183 case RS2_FORMAT_BGR8:
184 memcpy(data, (
void *)frame.get_data(), size * 3);
187 case RS2_FORMAT_RGBA8:
188 case RS2_FORMAT_BGRA8:
189 memcpy(data, (
void *)frame.get_data(), size * 4);
194 memcpy(data, (
unsigned char *)frame.get_data(), size * 2);
198 memcpy(data, (
unsigned char *)frame.get_data(), size);
206#if VISP_HAVE_OPENCV_VERSION >= 0x030000
207void frame_to_mat(
const rs2::frame &f, cv::Mat &img)
209 auto vf = f.as<rs2::video_frame>();
210 const int w = vf.get_width();
211 const int h = vf.get_height();
212 const int size = w * h;
214 if (f.get_profile().format() == RS2_FORMAT_BGR8) {
215 memcpy(
static_cast<void *
>(img.ptr<cv::Vec3b>()), f.get_data(), size * 3);
216 }
else if (f.get_profile().format() == RS2_FORMAT_RGB8) {
217 cv::Mat tmp(h, w, CV_8UC3, (
void *)f.get_data(), cv::Mat::AUTO_STEP);
218 cv::cvtColor(tmp, img, cv::COLOR_RGB2BGR);
219 }
else if (f.get_profile().format() == RS2_FORMAT_Y8) {
220 memcpy(img.ptr<uchar>(), f.get_data(), size);
226int main(
int argc,
char *argv[])
229 bool pcl_color =
false;
231 bool show_info =
false;
233 for (
int i = 1; i < argc; i++) {
234 if (std::string(argv[i]) ==
"--show_info") {
238 else if (std::string(argv[i]) ==
"--pcl_color") {
242 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
243 std::cout << argv[0] <<
" [--show_info]"
256 std::cout <<
"RealSense:\n" << rs << std::endl;
262 config.enable_stream(RS2_STREAM_COLOR, 1920, 1080, RS2_FORMAT_RGBA8, 30);
263 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
264 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y16, 30);
268 auto color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
269 vpImage<vpRGBa> color((
unsigned int)color_profile.height(), (
unsigned int)color_profile.width());
271 auto depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
272 vpImage<vpRGBa> depth_color((
unsigned int)depth_profile.height(), (
unsigned int)depth_profile.width());
273 vpImage<uint16_t> depth_raw((
unsigned int)depth_profile.height(), (
unsigned int)depth_profile.width());
275 auto infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
276 vpImage<unsigned char> infrared((
unsigned int)infrared_profile.height(), (
unsigned int)infrared_profile.width());
277 vpImage<uint16_t> infrared_raw((
unsigned int)infrared_profile.height(), (
unsigned int)infrared_profile.width());
284 d1.
init(color, 0, 0,
"Color");
285 d2.
init(depth_color, color.getWidth(), 0,
"Depth");
286 d3.
init(infrared, 0, color.getHeight() + 100,
"Infrared");
288 std::vector<vpColVector> pointcloud_colvector;
291 ViewerWorker viewer_colvector(
false, mutex);
292 std::thread viewer_colvector_thread(&ViewerWorker::run, &viewer_colvector);
297 std::cout <<
"Color intrinsics:\n"
300 std::cout <<
"Color intrinsics:\n"
303 std::cout <<
"Depth intrinsics:\n"
306 std::cout <<
"Depth intrinsics:\n"
309 std::cout <<
"Infrared intrinsics:\n"
312 std::cout <<
"Infrared intrinsics:\n"
316 std::cout <<
"depth_M_color:\n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl;
317 std::cout <<
"color_M_infrared:\n" << rs.
getTransformation(RS2_STREAM_INFRARED, RS2_STREAM_COLOR) << std::endl;
319 std::vector<double> time_vector;
324 auto data = pipe.wait_for_frames();
325 auto color_frame = data.get_color_frame();
326 getNativeFrame(color_frame, (
unsigned char *)color.bitmap);
328 auto depth_frame = data.get_depth_frame();
329 getNativeFrame(depth_frame, (
unsigned char *)depth_raw.bitmap);
331 auto infrared_frame = data.first(RS2_STREAM_INFRARED);
332 getNativeFrame(infrared_frame, (
unsigned char *)infrared_raw.bitmap);
335 getPointcloud(depth_frame, pointcloud_colvector);
338 std::lock_guard<std::mutex> lock(mutex);
340 pointcloud->width = depth_profile.width();
341 pointcloud->height = depth_profile.height();
342 pointcloud->points.resize(pointcloud_colvector.size());
343 for (
size_t i = 0; i < pointcloud_colvector.size(); i++) {
344 pointcloud->points[(size_t)i].x = pointcloud_colvector[i][0];
345 pointcloud->points[(size_t)i].y = pointcloud_colvector[i][1];
346 pointcloud->points[(size_t)i].z = pointcloud_colvector[i][2];
349 update_pointcloud =
true;
373 d2.
close(depth_color);
378 std::lock_guard<std::mutex> lock(mutex);
382 viewer_colvector_thread.join();
384 std::cout <<
"Acquisition1 - Mean time: " <<
vpMath::getMean(time_vector)
385 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
387 config.disable_all_streams();
388 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 60);
389 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);
390 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
393 color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
394 color.init((
unsigned int)color_profile.height(), (
unsigned int)color_profile.width());
396 depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
397 depth_color.init((
unsigned int)depth_profile.height(), (
unsigned int)depth_profile.width());
398 depth_raw.init((
unsigned int)depth_profile.height(), (
unsigned int)depth_profile.width());
400 infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
401 infrared.init((
unsigned int)infrared_profile.height(), (
unsigned int)infrared_profile.width());
403 d1.
init(color, 0, 0,
"Color");
404 d2.
init(depth_color, color.getWidth(), 0,
"Depth");
405 d3.
init(infrared, 0, color.getHeight() + 100,
"Infrared");
409 ViewerWorker viewer(pcl_color, mutex);
410 std::thread viewer_thread(&ViewerWorker::run, &viewer);
413 std::cout <<
"\n" << std::endl;
414 std::cout <<
"Color intrinsics:\n"
417 std::cout <<
"Color intrinsics:\n"
420 std::cout <<
"Depth intrinsics:\n"
423 std::cout <<
"Depth intrinsics:\n"
426 std::cout <<
"Infrared intrinsics:\n"
429 std::cout <<
"Infrared intrinsics:\n"
433 std::cout <<
"depth_M_color:\n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl;
434 std::cout <<
"color_M_infrared:\n" << rs.
getTransformation(RS2_STREAM_INFRARED, RS2_STREAM_COLOR) << std::endl;
443 std::lock_guard<std::mutex> lock(mutex);
446 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth_raw.bitmap, NULL, pointcloud_color,
447 (
unsigned char *)infrared.bitmap);
449 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth_raw.bitmap, NULL, pointcloud,
450 (
unsigned char *)infrared.bitmap);
453 update_pointcloud =
true;
456 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth_raw.bitmap, NULL,
457 (
unsigned char *)infrared.bitmap);
479 std::lock_guard<std::mutex> lock(mutex);
483 viewer_thread.join();
487 d2.
close(depth_color);
489 std::cout <<
"Acquisition2 - Mean time: " <<
vpMath::getMean(time_vector)
490 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
492#if VISP_HAVE_OPENCV_VERSION >= 0x030000
494 config.disable_all_streams();
495 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 60);
496 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);
497 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
500 color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
501 cv::Mat mat_color(color_profile.height(), color_profile.width(), CV_8UC3);
503 depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
504 cv::Mat mat_depth(depth_profile.height(), depth_profile.width(), CV_8UC3);
505 rs2::colorizer color_map;
507 infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
508 cv::Mat mat_infrared(infrared_profile.height(), infrared_profile.width(), CV_8U);
515 auto data = pipe.wait_for_frames();
516 frame_to_mat(data.get_color_frame(), mat_color);
517#if (RS2_API_VERSION >= ((2 * 10000) + (16 * 100) + 0))
518 frame_to_mat(data.get_depth_frame().apply_filter(color_map), mat_depth);
520 frame_to_mat(color_map(data.get_depth_frame()), mat_depth);
522 frame_to_mat(data.first(RS2_STREAM_INFRARED), mat_infrared);
524 cv::imshow(
"OpenCV color", mat_color);
525 cv::imshow(
"OpenCV depth", mat_depth);
526 cv::imshow(
"OpenCV infrared", mat_infrared);
529 if (cv::waitKey(10) == 27)
533 std::cout <<
"Acquisition3 - Mean time: " <<
vpMath::getMean(time_vector)
534 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
540 ViewerWorker viewer_colvector2(
false, mutex);
541 std::thread viewer_colvector_thread2(&ViewerWorker::run, &viewer_colvector2);
545 config.disable_all_streams();
546 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 60);
547 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);
548 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
555 rs.
acquire(NULL, NULL, &pointcloud_colvector, NULL, NULL);
558 std::lock_guard<std::mutex> lock(mutex);
559 pointcloud->width = 640;
560 pointcloud->height = 480;
561 pointcloud->points.resize(pointcloud_colvector.size());
562 for (
size_t i = 0; i < pointcloud_colvector.size(); i++) {
563 pointcloud->points[(size_t)i].x = pointcloud_colvector[i][0];
564 pointcloud->points[(size_t)i].y = pointcloud_colvector[i][1];
565 pointcloud->points[(size_t)i].z = pointcloud_colvector[i][2];
568 update_pointcloud =
true;
576 std::lock_guard<std::mutex> lock(mutex);
580 viewer_colvector_thread2.join();
582 std::cout <<
"Acquisition4 - Mean time: " <<
vpMath::getMean(time_vector)
583 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
592#if !defined(VISP_HAVE_REALSENSE2)
593 std::cout <<
"Install librealsense2 to make this test work." << std::endl;
595#if !(VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
596 std::cout <<
"Build ViSP with c++11 or higher compiler flag (cmake -DUSE_CXX_STANDARD=11) "
597 "to make this test work"
600#if !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_GDI)
601 std::cout <<
"X11 or GDI are needed." << std::endl;
@ perspectiveProjWithDistortion
Perspective projection with distortion model.
@ perspectiveProjWithoutDistortion
Perspective projection without distortion model.
Implementation of column vector and the associated operations.
Display for windows using GDI (available on any windows 32 platform).
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_title="")
static void close(vpImage< unsigned char > &I)
static bool getClick(const vpImage< unsigned char > &I, bool blocking=true)
static void display(const vpImage< unsigned char > &I)
static void flush(const vpImage< unsigned char > &I)
static void displayText(const vpImage< unsigned char > &I, const vpImagePoint &ip, const std::string &s, const vpColor &color)
static void createDepthHistogram(const vpImage< uint16_t > &src_depth, vpImage< vpRGBa > &dest_rgba)
static void convert(const vpImage< unsigned char > &src, vpImage< vpRGBa > &dest)
Definition of the vpImage class member functions.
static double getMedian(const std::vector< double > &v)
static double getMean(const std::vector< double > &v)
void acquire(vpImage< unsigned char > &grey, double *ts=NULL)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
rs2::pipeline & getPipeline()
Get a reference to rs2::pipeline.
bool open(const rs2::config &cfg=rs2::config())
rs2::pipeline_profile & getPipelineProfile()
Get a reference to rs2::pipeline_profile.
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const
VISP_EXPORT int wait(double t0, double t)
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
VISP_EXPORT double measureTimeMs()