41#include <visp3/core/vpConfig.h>
42#if (defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2)) && (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11) && \
43 (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
45#include <condition_variable>
51#if defined(VISP_HAVE_PCL)
52#include <pcl/common/common.h>
53#include <pcl/io/pcd_io.h>
56#include <visp3/core/vpImageConvert.h>
57#include <visp3/core/vpIoTools.h>
58#include <visp3/core/vpXmlParserCamera.h>
59#include <visp3/gui/vpDisplayGDI.h>
60#include <visp3/gui/vpDisplayX.h>
61#include <visp3/io/vpImageIo.h>
62#include <visp3/io/vpParseArgv.h>
63#include <visp3/sensor/vpRealSense.h>
64#include <visp3/sensor/vpRealSense2.h>
67#if defined(VISP_HAVE_REALSENSE2)
71#define GETOPTARGS "so:acdpiCf:bh"
75void usage(
const char *name,
const char *badparam)
78 Save RealSense data.\n\
89 Use aligned streams.\n\
101 Save infrared stream.\n\
110 Save point cloud in binary format.\n\
113 Print the help.\n\n",
117 fprintf(stdout,
"\nERROR: Bad parameter [%s]\n", badparam);
120bool getOptions(
int argc,
char **argv,
bool &save, std::string &output_directory,
bool &use_aligned_stream,
121 bool &save_color,
bool &save_depth,
bool &save_pointcloud,
bool &save_infrared,
bool &click_to_save,
122 int &stream_fps,
bool &save_pointcloud_binary_format)
125 const char **argv1 = (
const char **)argv;
134 output_directory = optarg;
137 use_aligned_stream =
true;
146 save_pointcloud =
true;
149 save_infrared =
true;
152 click_to_save =
true;
155 stream_fps = atoi(optarg);
158 save_pointcloud_binary_format =
true;
162 usage(argv[0], NULL);
167 usage(argv[0], optarg);
173 if ((c == 1) || (c == -1)) {
175 usage(argv[0], NULL);
176 std::cerr <<
"ERROR: " << std::endl;
177 std::cerr <<
" Bad argument " << optarg << std::endl
192 : m_cancelled(false), m_cond(), m_queueColor(), m_queueDepth(), m_queuePointCloud(), m_queueInfrared(),
193 m_maxQueueSize(1024 * 8), m_mutex()
198 std::lock_guard<std::mutex> lock(m_mutex);
206 const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
208 const std::vector<vpColVector> &pointCloud,
212 std::lock_guard<std::mutex> lock(m_mutex);
214 m_queueColor.push(colorImg);
215 m_queueDepth.push(depthImg);
216 m_queuePointCloud.push(pointCloud);
217 m_queueInfrared.push(infraredImg);
220 while (m_queueColor.size() > m_maxQueueSize) {
225 while (m_queueDepth.size() > m_maxQueueSize) {
230 while (m_queuePointCloud.size() > m_maxQueueSize) {
231 m_queuePointCloud.pop();
235 while (m_queueInfrared.size() > m_maxQueueSize) {
236 m_queueInfrared.pop();
245 pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
247 std::vector<vpColVector> &pointCloud,
251 std::unique_lock<std::mutex> lock(m_mutex);
253 while (m_queueColor.empty() || m_queueDepth.empty() || m_queuePointCloud.empty() || m_queueInfrared.empty()) {
265 colorImg = m_queueColor.front();
266 depthImg = m_queueDepth.front();
267 pointCloud = m_queuePointCloud.front();
268 infraredImg = m_queueInfrared.front();
272 m_queuePointCloud.pop();
273 m_queueInfrared.pop();
276 void setMaxQueueSize(
const size_t max_queue_size) { m_maxQueueSize = max_queue_size; }
280 std::condition_variable m_cond;
281 std::queue<vpImage<vpRGBa>> m_queueColor;
282 std::queue<vpImage<uint16_t>> m_queueDepth;
284 std::queue<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_queuePointCloud;
286 std::queue<std::vector<vpColVector>> m_queuePointCloud;
288 std::queue<vpImage<unsigned char>> m_queueInfrared;
289 size_t m_maxQueueSize;
296 StorageWorker(FrameQueue &queue,
const std::string &directory,
bool save_color,
bool save_depth,
bool save_pointcloud,
297 bool save_infrared,
bool save_pointcloud_binary_format,
308 : m_queue(queue), m_directory(directory), m_cpt(0), m_save_color(save_color), m_save_depth(save_depth),
309 m_save_pointcloud(save_pointcloud), m_save_infrared(save_infrared),
310 m_save_pointcloud_binary_format(save_pointcloud_binary_format)
313 m_size_height(height), m_size_width(width)
324 pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud;
326 std::vector<vpColVector> pointCloud;
330 char buffer[FILENAME_MAX];
332 m_queue.pop(colorImg, depthImg, pointCloud, infraredImg);
334 if (!m_directory.empty()) {
335 std::stringstream ss;
338 ss << m_directory <<
"/color_image_%04d.jpg";
339 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
341 std::string filename_color = buffer;
347 ss << m_directory <<
"/depth_image_%04d.bin";
348 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
349 std::string filename_depth = buffer;
351 std::ofstream file_depth(filename_depth.c_str(), std::ios::out | std::ios::binary);
352 if (file_depth.is_open()) {
358 for (
unsigned int i = 0; i < height; i++) {
359 for (
unsigned int j = 0; j < width; j++) {
360 value = depthImg[i][j];
367 if (m_save_pointcloud) {
369 ss << m_directory <<
"/point_cloud_%04d" << (m_save_pointcloud_binary_format ?
".bin" :
".pcd");
370 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
371 std::string filename_point_cloud = buffer;
373 if (m_save_pointcloud_binary_format) {
374 std::ofstream file_pointcloud(filename_point_cloud.c_str(), std::ios::out | std::ios::binary);
376 if (file_pointcloud.is_open()) {
378 uint32_t width = pointCloud->width;
379 uint32_t height = pointCloud->height;
381 char is_dense = pointCloud->is_dense;
385 file_pointcloud.write((
char *)(&is_dense),
sizeof(is_dense));
387 for (uint32_t i = 0; i < height; i++) {
388 for (uint32_t j = 0; j < width; j++) {
389 pcl::PointXYZ pt = (*pointCloud)(j, i);
397 uint32_t width = m_size_width;
398 uint32_t height = m_size_height;
404 file_pointcloud.write((
char *)(&is_dense),
sizeof(is_dense));
406 for (uint32_t i = 0; i < height; i++) {
407 for (uint32_t j = 0; j < width; j++) {
408 float x = (float)pointCloud[i * width + j][0];
409 float y = (float)pointCloud[i * width + j][1];
410 float z = (float)pointCloud[i * width + j][2];
422 pcl::io::savePCDFileBinary(filename_point_cloud, *pointCloud);
427 if (m_save_infrared) {
429 ss << m_directory <<
"/infrared_image_%04d.jpg";
430 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
432 std::string filename_infrared = buffer;
440 catch (
const FrameQueue::cancelled &) {
441 std::cout <<
"Receive cancel FrameQueue." << std::endl;
447 std::string m_directory;
451 bool m_save_pointcloud;
452 bool m_save_infrared;
453 bool m_save_pointcloud_binary_format;
461int main(
int argc,
char *argv[])
465 std::string output_directory_custom =
"";
466 bool use_aligned_stream =
false;
467 bool save_color =
false;
468 bool save_depth =
false;
469 bool save_pointcloud =
false;
470 bool save_infrared =
false;
471 bool click_to_save =
false;
473 bool save_pointcloud_binary_format =
false;
476 if (!getOptions(argc, argv, save, output_directory_custom, use_aligned_stream, save_color, save_depth,
477 save_pointcloud, save_infrared, click_to_save, stream_fps, save_pointcloud_binary_format)) {
481 if (!output_directory_custom.empty())
482 output_directory = output_directory_custom +
"/" + output_directory;
485 save_pointcloud_binary_format =
true;
488 std::cout <<
"save: " << save << std::endl;
489 std::cout <<
"output_directory: " << output_directory << std::endl;
490 std::cout <<
"use_aligned_stream: " << use_aligned_stream << std::endl;
491 std::cout <<
"save_color: " << save_color << std::endl;
492 std::cout <<
"save_depth: " << save_depth << std::endl;
493 std::cout <<
"save_pointcloud: " << save_pointcloud << std::endl;
494 std::cout <<
"save_infrared: " << save_infrared << std::endl;
495 std::cout <<
"stream_fps: " << stream_fps << std::endl;
496 std::cout <<
"save_pointcloud_binary_format: " << save_pointcloud_binary_format << std::endl;
497 std::cout <<
"click_to_save: " << click_to_save << std::endl;
499 int width = 640, height = 480;
504 config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, stream_fps);
505 config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, stream_fps);
506 config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, stream_fps);
507 realsense.
open(config);
533 d1.
init(I_gray, 0, 0,
"RealSense color stream");
534 d2.
init(I_depth, I_gray.getWidth() + 80, 0,
"RealSense depth stream");
535 d3.
init(I_infrared, I_gray.getWidth() + 80, I_gray.getHeight() + 10,
"RealSense infrared stream");
538 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, NULL, NULL);
561 xml_camera.
save(cam_color, output_directory +
"/camera.xml",
"color_camera", width, height);
563 if (use_aligned_stream) {
564 xml_camera.
save(cam_color, output_directory +
"/camera.xml",
"depth_camera", width, height);
568 xml_camera.
save(cam_depth, output_directory +
"/camera.xml",
"depth_camera", width, height);
572 xml_camera.
save(cam_infrared, output_directory +
"/camera.xml",
"infrared_camera", width, height);
574 if (!use_aligned_stream) {
580 xml_camera.
save(cam_color, output_directory +
"/camera.xml",
"color_camera", width, height);
583 xml_camera.
save(cam_color_rectified, output_directory +
"/camera.xml",
"color_camera_rectified", width, height);
585 if (use_aligned_stream) {
587 xml_camera.
save(cam_depth, output_directory +
"/camera.xml",
"depth_camera", width, height);
590 xml_camera.
save(cam_color, output_directory +
"/camera.xml",
"depth_camera", width, height);
595 xml_camera.
save(cam_depth_aligned_to_rectified_color, output_directory +
"/camera.xml",
596 "depth_camera_aligned_to_rectified_color", width, height);
599 xml_camera.
save(cam_infrared, output_directory +
"/camera.xml",
"infrared_camera", width, height);
601 if (!use_aligned_stream) {
602 depth_M_color = realsense.
getTransformation(rs::stream::color, rs::stream::depth);
605 std::ofstream file(std::string(output_directory +
"/depth_M_color.txt"));
606 depth_M_color.
save(file);
610 FrameQueue save_queue;
611 StorageWorker storage(std::ref(save_queue), std::cref(output_directory), save_color, save_depth, save_pointcloud,
612 save_infrared, save_pointcloud_binary_format, width, height);
613 std::thread storage_thread(&StorageWorker::run, &storage);
616 rs2::align align_to(RS2_STREAM_COLOR);
617 if (use_aligned_stream && save_infrared) {
618 std::cerr <<
"Cannot use aligned streams with infrared acquisition currently."
619 "\nInfrared stream acquisition is disabled!"
627 pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud(
new pcl::PointCloud<pcl::PointXYZ>);
629 std::vector<vpColVector> pointCloud;
632 if (use_aligned_stream) {
635 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, NULL, pointCloud, NULL,
638 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, &pointCloud, NULL,
643 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, NULL, pointCloud,
644 (
unsigned char *)I_infrared.bitmap, NULL, rs::stream::rectified_color,
645 rs::stream::depth_aligned_to_rectified_color);
647 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, &pointCloud,
648 (
unsigned char *)I_infrared.bitmap, NULL, rs::stream::rectified_color,
649 rs::stream::depth_aligned_to_rectified_color);
655 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, NULL, pointCloud,
656 (
unsigned char *)I_infrared.bitmap, NULL);
658 realsense.
acquire((
unsigned char *)I_color.bitmap, (
unsigned char *)I_depth_raw.bitmap, &pointCloud,
659 (
unsigned char *)I_infrared.bitmap);
670 if (!click_to_save) {
674 std::stringstream ss;
675 ss <<
"Images saved:" << nb_saves;
683 if (save && !click_to_save) {
685 pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy = pointCloud->makeShared();
686 save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared);
688 save_queue.push(I_color, I_depth_raw, pointCloud, I_infrared);
694 if (!click_to_save) {
704 pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy = pointCloud->makeShared();
705 save_queue.push(I_color, I_depth_raw, pointCloud_copy, I_infrared);
707 save_queue.push(I_color, I_depth_raw, pointCloud, I_infrared);
723 storage_thread.join();
730 std::cerr <<
"Need libRealSense or libRealSense2 and C++11 and displayX or displayGDI!" << std::endl;
Generic class defining intrinsic camera parameters.
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 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)
Implementation of an homogeneous matrix and operations on such kind of matrices.
void save(std::ofstream &f) const
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 void write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition of the vpImage class member functions.
unsigned int getWidth() const
unsigned int getHeight() const
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
bool open(const rs2::config &cfg=rs2::config())
void setStreamSettings(const rs::stream &stream, const rs::preset &preset)
vpCameraParameters getCameraParameters(const rs::stream &stream, vpCameraParameters::vpCameraParametersProjType type=vpCameraParameters::perspectiveProjWithDistortion) const
void acquire(std::vector< vpColVector > &pointcloud)
vpHomogeneousMatrix getTransformation(const rs::stream &from, const rs::stream &to) const
XML parser to load and save intrinsic camera parameters.
int save(const vpCameraParameters &cam, const std::string &filename, const std::string &camera_name, unsigned int image_width=0, unsigned int image_height=0, const std::string &additionalInfo="", bool verbose=true)
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")