43 #include <visp3/core/vpConfig.h>
45 #if defined(VISP_HAVE_REALSENSE2) && defined(VISP_HAVE_THREADS) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
47 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
49 #include <pcl/visualization/cloud_viewer.h>
50 #include <pcl/visualization/pcl_visualizer.h>
54 #include <visp3/core/vpImage.h>
55 #include <visp3/core/vpImageConvert.h>
56 #include <visp3/gui/vpDisplayGDI.h>
57 #include <visp3/gui/vpDisplayX.h>
58 #include <visp3/sensor/vpRealSense2.h>
60 #if VISP_HAVE_OPENCV_VERSION >= 0x030000
61 #include <opencv2/core.hpp>
62 #include <opencv2/highgui.hpp>
67 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
69 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
70 pcl::PointCloud<pcl::PointXYZRGB>::Ptr pointcloud_color(
new pcl::PointCloud<pcl::PointXYZRGB>());
71 bool cancelled =
false, update_pointcloud =
false;
76 explicit ViewerWorker(
bool color_mode, std::mutex &mutex) : m_colorMode(color_mode), m_mutex(mutex) { }
81 pcl::visualization::PCLVisualizer::Ptr viewer(
new pcl::visualization::PCLVisualizer(
"3D Viewer " + date));
82 pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb(pointcloud_color);
83 pcl::PointCloud<pcl::PointXYZ>::Ptr local_pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
84 pcl::PointCloud<pcl::PointXYZRGB>::Ptr local_pointcloud_color(
new pcl::PointCloud<pcl::PointXYZRGB>());
86 viewer->setBackgroundColor(0, 0, 0);
87 viewer->initCameraParameters();
88 viewer->setPosition(640 + 80, 480 + 80);
89 viewer->setCameraPosition(0, 0, -0.25, 0, -1, 0);
90 viewer->setSize(640, 480);
93 bool local_update =
false, local_cancelled =
false;
94 while (!local_cancelled) {
96 std::unique_lock<std::mutex> lock(m_mutex, std::try_to_lock);
98 if (lock.owns_lock()) {
99 local_update = update_pointcloud;
100 update_pointcloud =
false;
101 local_cancelled = cancelled;
105 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,
124 viewer->addPointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
125 viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1,
"sample cloud");
131 viewer->updatePointCloud<pcl::PointXYZRGB>(local_pointcloud_color, rgb,
"RGB sample cloud");
134 viewer->updatePointCloud<pcl::PointXYZ>(local_pointcloud,
"sample cloud");
142 std::cout <<
"End of point cloud display thread" << std::endl;
150 void getPointcloud(
const rs2::depth_frame &depth_frame, std::vector<vpColVector> &point_cloud)
152 auto vf = depth_frame.as<rs2::video_frame>();
153 const int width = vf.get_width();
154 const int height = vf.get_height();
155 point_cloud.resize((
size_t)(width * height));
158 rs2::points points = pc.calculate(depth_frame);
159 auto vertices = points.get_vertices();
161 for (
size_t i = 0; i < points.size(); i++) {
163 v[0] = vertices[i].x;
164 v[1] = vertices[i].y;
165 v[2] = vertices[i].z;
180 void getNativeFrame(
const rs2::frame &frame,
unsigned char *
const data)
182 auto vf = frame.as<rs2::video_frame>();
183 int size = vf.get_width() * vf.get_height();
185 switch (frame.get_profile().format()) {
186 case RS2_FORMAT_RGB8:
187 case RS2_FORMAT_BGR8:
188 memcpy(data, (
void *)frame.get_data(), size * 3);
191 case RS2_FORMAT_RGBA8:
192 case RS2_FORMAT_BGRA8:
193 memcpy(data, (
void *)frame.get_data(), size * 4);
198 memcpy(data, (
unsigned char *)frame.get_data(), size * 2);
202 memcpy(data, (
unsigned char *)frame.get_data(), size);
210 #if VISP_HAVE_OPENCV_VERSION >= 0x030000
211 void frame_to_mat(
const rs2::frame &f, cv::Mat &img)
213 auto vf = f.as<rs2::video_frame>();
214 const int w = vf.get_width();
215 const int h = vf.get_height();
216 const int size = w * h;
218 if (f.get_profile().format() == RS2_FORMAT_BGR8) {
219 memcpy(
static_cast<void *
>(img.ptr<cv::Vec3b>()), f.get_data(), size * 3);
221 else if (f.get_profile().format() == RS2_FORMAT_RGB8) {
222 cv::Mat tmp(h, w, CV_8UC3, (
void *)f.get_data(), cv::Mat::AUTO_STEP);
223 cv::cvtColor(tmp, img, cv::COLOR_RGB2BGR);
225 else if (f.get_profile().format() == RS2_FORMAT_Y8) {
226 memcpy(img.ptr<uchar>(), f.get_data(), size);
232 int main(
int argc,
char *argv[])
234 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
235 bool pcl_color =
false;
237 bool show_info =
false;
239 for (
int i = 1; i < argc; i++) {
240 if (std::string(argv[i]) ==
"--show_info") {
243 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
244 else if (std::string(argv[i]) ==
"--pcl_color") {
248 else if (std::string(argv[i]) ==
"--help" || std::string(argv[i]) ==
"-h") {
249 std::cout << argv[0] <<
" [--show_info]"
250 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
262 std::cout <<
"RealSense:\n" << rs << std::endl;
268 config.enable_stream(RS2_STREAM_COLOR, 1920, 1080, RS2_FORMAT_RGBA8, 30);
269 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 30);
270 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y16, 30);
274 auto color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
275 vpImage<vpRGBa> color((
unsigned int)color_profile.height(), (
unsigned int)color_profile.width());
277 auto depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
278 vpImage<vpRGBa> depth_color((
unsigned int)depth_profile.height(), (
unsigned int)depth_profile.width());
279 vpImage<uint16_t> depth_raw((
unsigned int)depth_profile.height(), (
unsigned int)depth_profile.width());
281 auto infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
282 vpImage<unsigned char> infrared((
unsigned int)infrared_profile.height(), (
unsigned int)infrared_profile.width());
283 vpImage<uint16_t> infrared_raw((
unsigned int)infrared_profile.height(), (
unsigned int)infrared_profile.width());
290 d1.
init(color, 0, 0,
"Color");
291 d2.
init(depth_color, color.getWidth(), 0,
"Depth");
292 d3.
init(infrared, 0, color.getHeight() + 100,
"Infrared");
294 std::vector<vpColVector> pointcloud_colvector;
295 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
297 ViewerWorker viewer_colvector(
false, mutex);
298 std::thread viewer_colvector_thread(&ViewerWorker::run, &viewer_colvector);
303 std::cout <<
"Color intrinsics:\n"
306 std::cout <<
"Color intrinsics:\n"
309 std::cout <<
"Depth intrinsics:\n"
312 std::cout <<
"Depth intrinsics:\n"
315 std::cout <<
"Infrared intrinsics:\n"
318 std::cout <<
"Infrared intrinsics:\n"
322 std::cout <<
"depth_M_color:\n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl;
323 std::cout <<
"color_M_infrared:\n" << rs.
getTransformation(RS2_STREAM_INFRARED, RS2_STREAM_COLOR) << std::endl;
325 std::vector<double> time_vector;
330 auto data = pipe.wait_for_frames();
331 auto color_frame = data.get_color_frame();
332 getNativeFrame(color_frame, (
unsigned char *)color.bitmap);
334 auto depth_frame = data.get_depth_frame();
335 getNativeFrame(depth_frame, (
unsigned char *)depth_raw.bitmap);
337 auto infrared_frame = data.first(RS2_STREAM_INFRARED);
338 getNativeFrame(infrared_frame, (
unsigned char *)infrared_raw.bitmap);
340 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
341 getPointcloud(depth_frame, pointcloud_colvector);
344 std::lock_guard<std::mutex> lock(mutex);
346 pointcloud->width = depth_profile.width();
347 pointcloud->height = depth_profile.height();
348 pointcloud->points.resize(pointcloud_colvector.size());
349 for (
size_t i = 0; i < pointcloud_colvector.size(); i++) {
350 pointcloud->points[(size_t)i].x = pointcloud_colvector[i][0];
351 pointcloud->points[(size_t)i].y = pointcloud_colvector[i][1];
352 pointcloud->points[(size_t)i].z = pointcloud_colvector[i][2];
355 update_pointcloud =
true;
379 d2.
close(depth_color);
382 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
384 std::lock_guard<std::mutex> lock(mutex);
388 viewer_colvector_thread.join();
390 std::cout <<
"Acquisition1 - Mean time: " <<
vpMath::getMean(time_vector)
391 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
393 config.disable_all_streams();
394 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 60);
395 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);
396 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
399 color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
400 color.init((
unsigned int)color_profile.height(), (
unsigned int)color_profile.width());
402 depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
403 depth_color.init((
unsigned int)depth_profile.height(), (
unsigned int)depth_profile.width());
404 depth_raw.init((
unsigned int)depth_profile.height(), (
unsigned int)depth_profile.width());
406 infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
407 infrared.init((
unsigned int)infrared_profile.height(), (
unsigned int)infrared_profile.width());
409 d1.
init(color, 0, 0,
"Color");
410 d2.
init(depth_color, color.getWidth(), 0,
"Depth");
411 d3.
init(infrared, 0, color.getHeight() + 100,
"Infrared");
413 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
415 ViewerWorker viewer(pcl_color, mutex);
416 std::thread viewer_thread(&ViewerWorker::run, &viewer);
419 std::cout <<
"\n" << std::endl;
420 std::cout <<
"Color intrinsics:\n"
423 std::cout <<
"Color intrinsics:\n"
426 std::cout <<
"Depth intrinsics:\n"
429 std::cout <<
"Depth intrinsics:\n"
432 std::cout <<
"Infrared intrinsics:\n"
435 std::cout <<
"Infrared intrinsics:\n"
439 std::cout <<
"depth_M_color:\n" << rs.
getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH) << std::endl;
440 std::cout <<
"color_M_infrared:\n" << rs.
getTransformation(RS2_STREAM_INFRARED, RS2_STREAM_COLOR) << std::endl;
447 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
449 std::lock_guard<std::mutex> lock(mutex);
452 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth_raw.bitmap,
nullptr, pointcloud_color,
453 (
unsigned char *)infrared.bitmap);
456 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth_raw.bitmap,
nullptr, pointcloud,
457 (
unsigned char *)infrared.bitmap);
460 update_pointcloud =
true;
463 rs.
acquire((
unsigned char *)color.bitmap, (
unsigned char *)depth_raw.bitmap,
nullptr,
464 (
unsigned char *)infrared.bitmap);
484 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
486 std::lock_guard<std::mutex> lock(mutex);
490 viewer_thread.join();
494 d2.
close(depth_color);
496 std::cout <<
"Acquisition2 - Mean time: " <<
vpMath::getMean(time_vector)
497 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
499 #if VISP_HAVE_OPENCV_VERSION >= 0x030000
501 config.disable_all_streams();
502 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_BGR8, 60);
503 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);
504 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
507 color_profile = profile.get_stream(RS2_STREAM_COLOR).as<rs2::video_stream_profile>();
508 cv::Mat mat_color(color_profile.height(), color_profile.width(), CV_8UC3);
510 depth_profile = profile.get_stream(RS2_STREAM_DEPTH).as<rs2::video_stream_profile>();
511 cv::Mat mat_depth(depth_profile.height(), depth_profile.width(), CV_8UC3);
512 rs2::colorizer color_map;
514 infrared_profile = profile.get_stream(RS2_STREAM_INFRARED).as<rs2::video_stream_profile>();
515 cv::Mat mat_infrared(infrared_profile.height(), infrared_profile.width(), CV_8U);
522 auto data = pipe.wait_for_frames();
523 frame_to_mat(data.get_color_frame(), mat_color);
524 #if (RS2_API_VERSION >= ((2 * 10000) + (16 * 100) + 0))
525 frame_to_mat(data.get_depth_frame().apply_filter(color_map), mat_depth);
527 frame_to_mat(color_map(data.get_depth_frame()), mat_depth);
529 frame_to_mat(data.first(RS2_STREAM_INFRARED), mat_infrared);
531 cv::imshow(
"OpenCV color", mat_color);
532 cv::imshow(
"OpenCV depth", mat_depth);
533 cv::imshow(
"OpenCV infrared", mat_infrared);
536 if (cv::waitKey(10) == 27)
540 std::cout <<
"Acquisition3 - Mean time: " <<
vpMath::getMean(time_vector)
541 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
544 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
547 ViewerWorker viewer_colvector2(
false, mutex);
548 std::thread viewer_colvector_thread2(&ViewerWorker::run, &viewer_colvector2);
552 config.disable_all_streams();
553 config.enable_stream(RS2_STREAM_COLOR, 640, 480, RS2_FORMAT_RGBA8, 60);
554 config.enable_stream(RS2_STREAM_DEPTH, 640, 480, RS2_FORMAT_Z16, 60);
555 config.enable_stream(RS2_STREAM_INFRARED, 640, 480, RS2_FORMAT_Y8, 60);
562 rs.
acquire(
nullptr,
nullptr, &pointcloud_colvector,
nullptr,
nullptr);
565 std::lock_guard<std::mutex> lock(mutex);
566 pointcloud->width = 640;
567 pointcloud->height = 480;
568 pointcloud->points.resize(pointcloud_colvector.size());
569 for (
size_t i = 0; i < pointcloud_colvector.size(); i++) {
570 pointcloud->points[(size_t)i].x = pointcloud_colvector[i][0];
571 pointcloud->points[(size_t)i].y = pointcloud_colvector[i][1];
572 pointcloud->points[(size_t)i].z = pointcloud_colvector[i][2];
575 update_pointcloud =
true;
583 std::lock_guard<std::mutex> lock(mutex);
587 viewer_colvector_thread2.join();
589 std::cout <<
"Acquisition4 - Mean time: " <<
vpMath::getMean(time_vector)
590 <<
" ms ; Median time: " <<
vpMath::getMedian(time_vector) <<
" ms" << std::endl;
599 #if !defined(VISP_HAVE_REALSENSE2)
600 std::cout <<
"Install librealsense2 to make this test work." << std::endl;
602 #if !defined(VISP_HAVE_X11) && !defined(VISP_HAVE_GDI)
603 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="") vp_override
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)
static double getMedian(const std::vector< double > &v)
static double getMean(const std::vector< double > &v)
vpCameraParameters getCameraParameters(const rs2_stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion, int index=-1) const
void acquire(vpImage< unsigned char > &grey, double *ts=nullptr)
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.
vpHomogeneousMatrix getTransformation(const rs2_stream &from, const rs2_stream &to, int from_index=-1) const
void init(vpImage< unsigned char > &Iinput, vpImage< unsigned char > &IcannyVisp, vpImage< unsigned char > *p_dIx, vpImage< unsigned char > *p_dIy, vpImage< unsigned char > *p_IcannyimgFilter)
Initialize the different displays.
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()