Visual Servoing Platform  version 3.6.1 under development (2024-11-05)
saveRealSenseData.cpp

Example that shows how to save realsense data that can be replayed with readRealSenseData.cpp

/*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2024 by Inria. All rights reserved.
*
* This software is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License as published by
* the Free Software Foundation; either version 2 of the License, or
* (at your option) any later version.
* See the file LICENSE.txt at the root directory of this source
* distribution for additional information about the GNU GPL.
*
* For using ViSP with software that can not be combined with the GNU
* GPL, please contact Inria about acquiring a ViSP Professional
* Edition License.
*
* See https://visp.inria.fr for more information.
*
* This software was developed at:
* Inria Rennes - Bretagne Atlantique
* Campus Universitaire de Beaulieu
* 35042 Rennes Cedex
* France
*
* If you have questions regarding the use of this file, please contact
* Inria at visp@inria.fr
*
* This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
* WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
*/
#include <iostream>
#include <visp3/core/vpConfig.h>
#if (defined(VISP_HAVE_REALSENSE) || defined(VISP_HAVE_REALSENSE2)) && defined(VISP_HAVE_THREADS) \
&& (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI)) && defined(VISP_HAVE_PUGIXML) \
&& ((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
#include <condition_variable>
#include <fstream>
#include <mutex>
#include <queue>
#include <thread>
#if defined(VISP_HAVE_PCL)
#include <pcl/pcl_config.h>
#if defined(VISP_HAVE_PCL_COMMON)
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#endif
#if defined(VISP_HAVE_PCL_IO)
#include <pcl/io/pcd_io.h>
#endif
#endif
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpIoException.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/core/vpXmlParserCamera.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/sensor/vpRealSense.h>
#include <visp3/sensor/vpRealSense2.h>
// Priority to libRealSense2
#if defined(VISP_HAVE_REALSENSE2)
#define USE_REALSENSE2
#endif
#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
#define GETOPTARGS "se:o:acdpzijCf:bvh"
#else
#define GETOPTARGS "se:o:acdpijCf:bvh"
#endif
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
namespace
{
void usage(const char *name, const char *badparam, int fps)
{
std::cout << "\nSYNOPSIS " << std::endl
<< " " << name
<< " [-s]"
<< " [-e <filename pattern (e.g. %06d)>]"
<< " [-a]"
<< " [-c]"
<< " [-d]"
<< " [-p]"
<< " [-b]"
#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
<< " [-z]"
#endif
<< " [-i]"
<< " [-j]"
<< " [-C]"
<< " [-f <fps>]"
<< " [-v]"
<< " [-o <directory>]"
<< " [--help,-h]"
<< std::endl;
std::cout << "\nOPTIONS " << std::endl
<< " -s" << std::endl
<< " Flag to enable data saving." << std::endl
<< std::endl
<< " -e <pattern>" << std::endl
<< " Filename pattern when saving data." << std::endl
<< std::endl
<< " -a" << std::endl
<< " Color and depth are aligned." << std::endl
<< std::endl
<< " -c" << std::endl
<< " Add color stream to saved data when -s option is enable." << std::endl
<< std::endl
<< " -d" << std::endl
<< " Add depth stream to saved data when -s option is enable." << std::endl
<< std::endl
<< " -p" << std::endl
<< " Add point cloud stream to saved data when -s option is enabled." << std::endl
<< " By default (if available), the point cloud is saved in Point Cloud Data file format (.PCD extension file)."
<< std::endl
<< " You can also use the -z option to save the point cloud in .npz (NumPy)." << std::endl
<< std::endl
<< " -b" << std::endl
<< " Force depth and pointcloud to be saved in (little-endian) binary format." << std::endl
<< std::endl
#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
<< " -z" << std::endl
<< " Pointcloud is saved in NPZ format." << std::endl
<< std::endl
#endif
<< " -i" << std::endl
<< " Add infrared stream to saved data when -s option is enabled." << std::endl
<< std::endl
<< " -j" << std::endl
<< " Save image data using JPEG format (otherwise PNG is used)." << std::endl
<< std::endl
<< " -C" << std::endl
<< " Trigger one shot data saver after each user click." << std::endl
<< std::endl
<< " -f <fps>" << std::endl
<< " Set camera framerate." << std::endl
<< " Default: " << fps << std::endl
<< std::endl
<< " -v" << std::endl
<< " Display depth using a cumulative histogram." << std::endl
<< " Warning: this operation is time consuming" << std::endl
<< std::endl
<< " -o <directory>" << std::endl
<< " Output directory that will host saved data." << std::endl
<< std::endl
<< " --help, -h" << std::endl
<< " Display this helper message." << std::endl
<< std::endl;
std::cout << "\nEXAMPLE " << std::endl
<< "- Save aligned color + depth + point cloud in data folder" << std::endl
<< " " << name << " -s -a -c -d -p -o data" << std::endl
<< "- Save color + IR + depth + point cloud in NPZ format in data folder" << std::endl
<< " " << name << " -s -c -d -i -p -z -o data" << std::endl
<< std::endl;
if (badparam) {
std::cout << "\nERROR: Bad parameter " << badparam << std::endl;
}
}
bool getOptions(int argc, const char *argv[], bool &save, std::string &pattern, std::string &output_directory,
bool &use_aligned_stream, bool &save_color, bool &save_depth, bool &save_pointcloud,
bool &save_infrared, bool &click_to_save, int &stream_fps, bool &save_pcl_npz_format,
bool &save_force_binary_format, bool &save_jpeg, bool &depth_hist_visu)
{
const char *optarg;
const char **argv1 = (const char **)argv;
int c;
while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
switch (c) {
case 's':
save = true;
break;
case 'e':
pattern = optarg;
break;
case 'o':
output_directory = optarg;
break;
case 'a':
use_aligned_stream = true;
break;
case 'c':
save_color = true;
break;
case 'd':
save_depth = true;
break;
case 'p':
save_pointcloud = true;
break;
case 'i':
save_infrared = true;
break;
case 'j':
save_jpeg = true;
break;
case 'C':
click_to_save = true;
break;
case 'f':
stream_fps = atoi(optarg);
break;
case 'v':
depth_hist_visu = true;
break;
#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
case 'z':
save_pcl_npz_format = true;
break;
#endif
case 'b':
save_force_binary_format = true;
break;
case 'h':
usage(argv[0], nullptr, stream_fps);
return false;
break;
default:
usage(argv[0], optarg, stream_fps);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], nullptr, stream_fps);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg << std::endl
<< std::endl;
return false;
}
return true;
}
#ifndef DOXYGEN_SHOULD_SKIP_THIS
// Code adapted from: https://stackoverflow.com/a/37146523
class vpFrameQueue
{
public:
struct vpCancelled_t
{ };
vpFrameQueue()
: m_cancelled(false), m_cond(), m_queueColor(), m_queueDepth(), m_queuePointCloud(), m_queueInfrared(),
m_maxQueueSize(1024 * 8), m_mutex()
{ }
void cancel()
{
std::lock_guard<std::mutex> lock(m_mutex);
m_cancelled = true;
m_cond.notify_all();
}
// Push data to save in the queue (FIFO)
void push(const std::unique_ptr<vpImage<vpRGBa>> &ptr_colorImg,
const std::unique_ptr<vpImage<uint16_t>> &ptr_depthImg,
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
const pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
#else
const std::unique_ptr<std::vector<vpColVector>> &ptr_pointCloud,
#endif
const std::unique_ptr<vpImage<unsigned char>> &ptr_infraredImg)
{
std::lock_guard<std::mutex> lock(m_mutex);
if (ptr_colorImg) {
m_queueColor.push(*ptr_colorImg);
}
if (ptr_depthImg) {
m_queueDepth.push(*ptr_depthImg);
}
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
if (pointCloud) {
m_queuePointCloud.push(pointCloud);
}
#else
if (ptr_pointCloud) {
m_queuePointCloud.push(*ptr_pointCloud);
}
#endif
if (ptr_infraredImg) {
m_queueInfrared.push(*ptr_infraredImg);
}
// Pop extra data in the queue
while (m_queueColor.size() > m_maxQueueSize) {
m_queueColor.pop();
}
// Pop extra data in the queue
while (m_queueDepth.size() > m_maxQueueSize) {
m_queueDepth.pop();
}
// Pop extra data in the queue
while (m_queuePointCloud.size() > m_maxQueueSize) {
m_queuePointCloud.pop();
}
// Pop extra data in the queue
while (m_queueInfrared.size() > m_maxQueueSize) {
m_queueInfrared.pop();
}
m_cond.notify_one();
}
// Pop the image to save from the queue (FIFO)
void pop(std::unique_ptr<vpImage<vpRGBa>> &ptr_colorImg,
std::unique_ptr<vpImage<uint16_t>> &ptr_depthImg,
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
pcl::PointCloud<pcl::PointXYZ>::Ptr &pointCloud,
#else
std::unique_ptr<std::vector<vpColVector>> &ptr_pointCloud,
#endif
std::unique_ptr<vpImage<unsigned char>> &ptr_infraredImg)
{
std::unique_lock<std::mutex> lock(m_mutex);
// Since we push all 4 data at a time, there should be no situation where a queue size is different from the others
while (m_queueColor.empty() && m_queueDepth.empty() && m_queuePointCloud.empty() && m_queueInfrared.empty()) {
if (m_cancelled) {
throw vpCancelled_t();
}
m_cond.wait(lock);
if (m_cancelled) {
throw vpCancelled_t();
}
}
if (!m_queueColor.empty()) {
ptr_colorImg = std::make_unique<vpImage<vpRGBa>>(m_queueColor.front());
m_queueColor.pop();
}
if (!m_queueDepth.empty()) {
ptr_depthImg = std::make_unique<vpImage<uint16_t>>(m_queueDepth.front());
m_queueDepth.pop();
}
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
if (!m_queuePointCloud.empty()) {
pointCloud = m_queuePointCloud.front();
m_queuePointCloud.pop();
}
#else
if (!m_queuePointCloud.empty()) {
ptr_pointCloud = std::make_unique<std::vector<vpColVector>>(m_queuePointCloud.front());
m_queuePointCloud.pop();
}
#endif
if (!m_queueInfrared.empty()) {
ptr_infraredImg = std::make_unique<vpImage<unsigned char>>(m_queueInfrared.front());
m_queueInfrared.pop();
}
}
bool empty()
{
std::lock_guard<std::mutex> lock(m_mutex);
return m_queueColor.empty() && m_queueDepth.empty() && m_queuePointCloud.empty() && m_queueInfrared.empty();
}
void setMaxQueueSize(const size_t max_queue_size) { m_maxQueueSize = max_queue_size; }
private:
bool m_cancelled;
std::condition_variable m_cond;
std::queue<vpImage<vpRGBa>> m_queueColor;
std::queue<vpImage<uint16_t>> m_queueDepth;
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
std::queue<pcl::PointCloud<pcl::PointXYZ>::Ptr> m_queuePointCloud;
#else
std::queue<std::vector<vpColVector>> m_queuePointCloud;
#endif
std::queue<vpImage<unsigned char>> m_queueInfrared;
size_t m_maxQueueSize;
std::mutex m_mutex;
};
class vpStorageWorker
{
public:
vpStorageWorker(vpFrameQueue &queue, const std::string &save_pattern, const std::string &directory, bool save_color,
bool save_depth, bool save_pointcloud, bool save_infrared, bool save_pcl_npz_format,
bool save_force_binary_format, bool save_jpeg,
int
#ifndef VISP_HAVE_PCL
width
#endif
,
int
#ifndef VISP_HAVE_PCL
height
#endif
)
: m_queue(queue), m_save_pattern(save_pattern), m_directory(directory), m_cpt(0), m_save_color(save_color), m_save_depth(save_depth),
m_save_pointcloud(save_pointcloud), m_save_infrared(save_infrared), m_save_pcl_npz_format(save_pcl_npz_format),
m_save_force_binary_format(save_force_binary_format), m_save_jpeg(save_jpeg)
#ifndef VISP_HAVE_PCL
,
m_size_height(height), m_size_width(width)
#endif
{ }
// Thread main loop
void run()
{
try {
std::unique_ptr<vpImage<vpRGBa>> ptr_colorImg;
std::unique_ptr<vpImage<uint16_t>> ptr_depthImg;
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
pcl::PointCloud<pcl::PointXYZ>::Ptr ptr_pointCloud;
#else
std::unique_ptr<std::vector<vpColVector>> ptr_pointCloud;
#endif
std::unique_ptr<vpImage<unsigned char>> ptr_infraredImg;
std::vector<float> vec_pcl;
char buffer[FILENAME_MAX];
std::string image_filename_ext = m_save_jpeg ? ".jpg" : ".png";
for (;;) {
m_queue.pop(ptr_colorImg, ptr_depthImg, ptr_pointCloud, ptr_infraredImg);
if (!m_directory.empty()) {
std::string current_time = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
std::stringstream ss;
if (m_save_color && ptr_colorImg) {
ss << m_directory << "/color_image_" << m_save_pattern << image_filename_ext;
snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
std::string filename_color = buffer;
vpImageIo::write(*ptr_colorImg, filename_color);
}
if (m_save_depth && ptr_depthImg) {
ss.str("");
if (m_save_force_binary_format) {
ss << m_directory << "/depth_image_" << m_save_pattern << ".bin";
snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
std::string filename_depth = buffer;
std::ofstream file_depth(filename_depth.c_str(), std::ios::out | std::ios::binary);
if (file_depth.is_open()) {
unsigned int height = ptr_depthImg->getHeight(), width = ptr_depthImg->getWidth();
vpIoTools::writeBinaryValueLE(file_depth, height);
vpIoTools::writeBinaryValueLE(file_depth, width);
uint16_t value;
for (unsigned int i = 0; i < height; i++) {
for (unsigned int j = 0; j < width; j++) {
value = (*ptr_depthImg)[i][j];
vpIoTools::writeBinaryValueLE(file_depth, value);
}
}
}
}
#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
else {
ss << m_directory << "/depth_image_" << m_save_pattern << ".npz";
snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
std::string filename_depth = buffer;
// Write Npz headers
std::vector<char> vec_filename(filename_depth.begin(), filename_depth.end());
// Null-terminated character is handled at reading
// For null-terminated character handling, see:
// https://stackoverflow.com/a/8247804
// https://stackoverflow.com/a/45491652
visp::cnpy::npz_save(filename_depth, "filename", &vec_filename[0], { vec_filename.size() }, "w");
std::vector<char> vec_current_time(current_time.begin(), current_time.end());
visp::cnpy::npz_save(filename_depth, "timestamp", &vec_current_time, { vec_current_time.size() }, "a");
unsigned int height = ptr_depthImg->getHeight();
unsigned int width = ptr_depthImg->getWidth();
unsigned int channel = 1;
visp::cnpy::npz_save(filename_depth, "height", &height, { 1 }, "a");
visp::cnpy::npz_save(filename_depth, "width", &width, { 1 }, "a");
visp::cnpy::npz_save(filename_depth, "channel", &channel, { 1 }, "a");
// Write data
std::vector<uint16_t> I_depth_raw_vec(ptr_depthImg->bitmap, ptr_depthImg->bitmap + ptr_depthImg->getSize());
visp::cnpy::npz_save(filename_depth, "data", I_depth_raw_vec.data(), { height, width }, "a");
}
#else
else {
throw(vpIoException(vpIoException::ioError, "Cannot manage non-binary files when npz I/O functions are disabled."));
}
#endif
}
if (m_save_pointcloud && ptr_pointCloud) {
ss.str("");
std::string pcl_extension = m_save_force_binary_format ? ".bin" : (m_save_pcl_npz_format ? ".npz" : ".pcd");
ss << m_directory << "/point_cloud_" << m_save_pattern << pcl_extension;
snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
std::string filename_point_cloud = buffer;
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
uint32_t width = ptr_pointCloud->width;
uint32_t height = ptr_pointCloud->height;
#else
uint32_t width = m_size_width;
uint32_t height = m_size_height;
#endif
if (m_save_force_binary_format) {
std::ofstream file_pointcloud(filename_point_cloud.c_str(), std::ios::out | std::ios::binary);
if (file_pointcloud.is_open()) {
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
// true if ptr_pointCloud does not contain NaN or Inf, not handled currently
char is_dense = ptr_pointCloud->is_dense;
vpIoTools::writeBinaryValueLE(file_pointcloud, height);
vpIoTools::writeBinaryValueLE(file_pointcloud, width);
file_pointcloud.write((char *)(&is_dense), sizeof(is_dense));
for (uint32_t i = 0; i < height; i++) {
for (uint32_t j = 0; j < width; j++) {
pcl::PointXYZ pt = (*ptr_pointCloud)(j, i);
vpIoTools::writeBinaryValueLE(file_pointcloud, pt.x);
vpIoTools::writeBinaryValueLE(file_pointcloud, pt.y);
vpIoTools::writeBinaryValueLE(file_pointcloud, pt.z);
}
}
#else
// to be consistent with PCL version
const char is_dense = 1;
vpIoTools::writeBinaryValueLE(file_pointcloud, height);
vpIoTools::writeBinaryValueLE(file_pointcloud, width);
file_pointcloud.write((char *)(&is_dense), sizeof(is_dense));
for (uint32_t i = 0; i < height; i++) {
for (uint32_t j = 0; j < width; j++) {
float x = (float)(*ptr_pointCloud)[i * width + j][0];
float y = (float)(*ptr_pointCloud)[i * width + j][1];
float z = (float)(*ptr_pointCloud)[i * width + j][2];
vpIoTools::writeBinaryValueLE(file_pointcloud, x);
vpIoTools::writeBinaryValueLE(file_pointcloud, y);
vpIoTools::writeBinaryValueLE(file_pointcloud, z);
}
}
#endif
}
}
else if (m_save_pcl_npz_format) {
#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
// Write Npz headers
std::vector<char> vec_filename(filename_point_cloud.begin(), filename_point_cloud.end());
// Null-terminated character is handled at reading
// For null-terminated character handling, see:
// https://stackoverflow.com/a/8247804
// https://stackoverflow.com/a/45491652
visp::cnpy::npz_save(filename_point_cloud, "filename", &vec_filename[0], { vec_filename.size() }, "w");
std::vector<char> vec_current_time(current_time.begin(), current_time.end());
visp::cnpy::npz_save(filename_point_cloud, "timestamp", &vec_current_time, { vec_current_time.size() }, "a");
const uint32_t channels = 3;
visp::cnpy::npz_save(filename_point_cloud, "height", &height, { 1 }, "a");
visp::cnpy::npz_save(filename_point_cloud, "width", &width, { 1 }, "a");
visp::cnpy::npz_save(filename_point_cloud, "channel", &channels, { 1 }, "a");
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
// can probably be optimized by assuming channel=1 and use data of type XYZ
// but this should probably not work with the Python script for display
for (uint32_t i = 0; i < height; i++) {
for (uint32_t j = 0; j < width; j++) {
pcl::PointXYZ pt = (*ptr_pointCloud)(j, i);
vec_pcl[channels * (i*width + j) + 0] = pt.x;
vec_pcl[channels * (i*width + j) + 1] = pt.y;
vec_pcl[channels * (i*width + j) + 2] = pt.z;
}
}
#else
vec_pcl.resize(height * width * channels);
for (uint32_t i = 0; i < height; i++) {
for (uint32_t j = 0; j < width; j++) {
vec_pcl[channels * (i*width + j) + 0] = (*ptr_pointCloud)[i*width + j][0];
vec_pcl[channels * (i*width + j) + 1] = (*ptr_pointCloud)[i*width + j][1];
vec_pcl[channels * (i*width + j) + 2] = (*ptr_pointCloud)[i*width + j][2];
}
}
#endif
// Write data
visp::cnpy::npz_save(filename_point_cloud, "data", vec_pcl.data(), { height, width, channels }, "a");
#else
throw(vpIoException(vpIoException::fatalError, "Cannot save in npz format when npz I/O functions are disabled."));
#endif
}
else {
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_IO) && defined(VISP_HAVE_PCL_COMMON)
pcl::io::savePCDFileBinary(filename_point_cloud, *ptr_pointCloud);
#endif
}
}
if (m_save_infrared && ptr_infraredImg) {
ss.str("");
ss << m_directory << "/infrared_image_" << m_save_pattern << image_filename_ext;
snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt);
std::string filename_infrared = buffer;
vpImageIo::write(*ptr_infraredImg, filename_infrared);
}
m_cpt++;
}
}
}
catch (const vpFrameQueue::vpCancelled_t &) {
std::cout << "Receive cancel vpFrameQueue." << std::endl;
}
}
private:
vpFrameQueue &m_queue;
std::string m_save_pattern;
std::string m_directory;
unsigned int m_cpt;
bool m_save_color;
bool m_save_depth;
bool m_save_pointcloud;
bool m_save_infrared;
bool m_save_pcl_npz_format;
bool m_save_force_binary_format;
bool m_save_jpeg;
#ifndef VISP_HAVE_PCL
int m_size_height;
int m_size_width;
#endif
};
} // Namespace
#endif // DOXYGEN_SHOULD_SKIP_THIS
int main(int argc, const char *argv[])
{
bool save = false;
std::string save_pattern = "%04d";
std::string output_directory = vpTime::getDateTime("%Y_%m_%d_%H.%M.%S");
std::string output_directory_custom = "";
bool use_aligned_stream = false;
bool save_color = false;
bool save_depth = false;
bool save_pointcloud = false;
bool save_infrared = false;
bool click_to_save = false;
int stream_fps = 30;
bool save_pcl_npz_format = false;
bool save_force_binary_format = false;
bool save_jpeg = false;
bool depth_hist_visu = false;
// Read the command line options
if (!getOptions(argc, argv, save, save_pattern, output_directory_custom, use_aligned_stream, save_color, save_depth,
save_pointcloud, save_infrared, click_to_save, stream_fps, save_pcl_npz_format,
save_force_binary_format, save_jpeg, depth_hist_visu)) {
return EXIT_FAILURE;
}
if (!output_directory_custom.empty())
output_directory = output_directory_custom + "/" + output_directory;
#ifndef VISP_HAVE_PCL
save_pcl_npz_format = !save_force_binary_format ? true : false;
#endif
std::cout << "save: " << save << std::endl;
std::cout << "save_pattern: " << save_pattern << std::endl;
std::cout << "output_directory: " << output_directory << std::endl;
std::cout << "use_aligned_stream: " << use_aligned_stream << std::endl;
std::cout << "save_color: " << save_color << std::endl;
std::cout << "save_depth: " << save_depth << std::endl;
std::cout << "save_pointcloud: " << save_pointcloud << std::endl;
std::cout << "save_infrared: " << save_infrared << std::endl;
std::cout << "save_jpeg: " << save_jpeg << std::endl;
std::cout << "stream_fps: " << stream_fps << std::endl;
std::cout << "depth_hist_visu: " << depth_hist_visu << std::endl;
#if defined(VISP_HAVE_MINIZ) && defined(VISP_HAVE_WORKING_REGEX)
std::cout << "save_pcl_npz_format: " << save_pcl_npz_format << std::endl;
#endif
std::cout << "save_force_binary_format: " << save_force_binary_format << std::endl;
std::cout << "click_to_save: " << click_to_save << std::endl;
int width = 640, height = 480;
#ifdef USE_REALSENSE2
vpRealSense2 realsense;
rs2::config config;
config.enable_stream(RS2_STREAM_COLOR, width, height, RS2_FORMAT_RGBA8, stream_fps);
config.enable_stream(RS2_STREAM_DEPTH, width, height, RS2_FORMAT_Z16, stream_fps);
config.enable_stream(RS2_STREAM_INFRARED, width, height, RS2_FORMAT_Y8, stream_fps);
realsense.open(config);
#else
vpRealSense realsense;
realsense.setStreamSettings(rs::stream::color,
vpRealSense::vpRsStreamParams(width, height, rs::format::rgba8, stream_fps));
realsense.setStreamSettings(rs::stream::depth,
vpRealSense::vpRsStreamParams(width, height, rs::format::z16, stream_fps));
realsense.setStreamSettings(rs::stream::infrared,
vpRealSense::vpRsStreamParams(width, height, rs::format::y8, stream_fps));
realsense.setStreamSettings(rs::stream::infrared2,
vpRealSense::vpRsStreamParams(width, height, rs::format::y8, stream_fps));
realsense.open();
#endif
vpImage<vpRGBa> I_color(height, width);
vpImage<unsigned char> I_depth(height, width);
vpImage<uint16_t> I_depth_raw(height, width);
vpImage<unsigned char> I_infrared(height, width);
#ifdef VISP_HAVE_X11
vpDisplayX d1, d2, d3;
#else
vpDisplayGDI d1, d2, d3;
#endif
d1.init(I_color, 0, 0, "RealSense color stream");
d2.init(I_depth, I_color.getWidth() + 80, 0, "RealSense depth stream");
d3.init(I_infrared, I_color.getWidth() + 80, I_color.getHeight() + 70, "RealSense infrared stream");
while (true) {
double start = vpTime::measureTimeMs();
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, nullptr);
vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
double delta_time = vpTime::measureTimeMs() - start;
std::ostringstream oss_time;
oss_time << delta_time << " ms ; fps=" << 1000/delta_time;
vpDisplay::displayText(I_color, 40, 20, oss_time.str(), vpColor::red);
vpDisplay::displayText(I_color, 20, 20, "Click when ready.", vpColor::red);
vpDisplay::flush(I_color);
vpDisplay::flush(I_depth);
if (vpDisplay::getClick(I_color, false)) {
break;
}
}
if (save) {
// Create output directory
vpIoTools::makeDirectory(output_directory);
// Save intrinsics
#ifdef USE_REALSENSE2
vpCameraParameters cam_color = realsense.getCameraParameters(RS2_STREAM_COLOR);
vpXmlParserCamera xml_camera;
xml_camera.save(cam_color, output_directory + "/camera.xml", "color_camera", width, height);
if (use_aligned_stream) {
xml_camera.save(cam_color, output_directory + "/camera.xml", "depth_camera", width, height);
}
else {
vpCameraParameters cam_depth = realsense.getCameraParameters(RS2_STREAM_DEPTH);
xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
}
vpCameraParameters cam_infrared = realsense.getCameraParameters(RS2_STREAM_INFRARED);
xml_camera.save(cam_infrared, output_directory + "/camera.xml", "infrared_camera", width, height);
vpHomogeneousMatrix depth_M_color;
if (!use_aligned_stream) {
depth_M_color = realsense.getTransformation(RS2_STREAM_COLOR, RS2_STREAM_DEPTH);
}
#else
vpCameraParameters cam_color = realsense.getCameraParameters(rs::stream::color);
vpXmlParserCamera xml_camera;
xml_camera.save(cam_color, output_directory + "/camera.xml", "color_camera", width, height);
vpCameraParameters cam_color_rectified = realsense.getCameraParameters(rs::stream::rectified_color);
xml_camera.save(cam_color_rectified, output_directory + "/camera.xml", "color_camera_rectified", width, height);
if (use_aligned_stream) {
vpCameraParameters cam_depth = realsense.getCameraParameters(rs::stream::depth);
xml_camera.save(cam_depth, output_directory + "/camera.xml", "depth_camera", width, height);
}
else {
xml_camera.save(cam_color, output_directory + "/camera.xml", "depth_camera", width, height);
}
vpCameraParameters cam_depth_aligned_to_rectified_color =
realsense.getCameraParameters(rs::stream::depth_aligned_to_rectified_color);
xml_camera.save(cam_depth_aligned_to_rectified_color, output_directory + "/camera.xml",
"depth_camera_aligned_to_rectified_color", width, height);
vpCameraParameters cam_infrared = realsense.getCameraParameters(rs::stream::infrared);
xml_camera.save(cam_infrared, output_directory + "/camera.xml", "infrared_camera", width, height);
vpHomogeneousMatrix depth_M_color;
if (!use_aligned_stream) {
depth_M_color = realsense.getTransformation(rs::stream::color, rs::stream::depth);
}
#endif
std::ofstream file(std::string(output_directory + "/depth_M_color.txt"));
depth_M_color.save(file);
file.close();
}
vpFrameQueue save_queue;
vpStorageWorker storage(std::ref(save_queue), save_pattern, std::cref(output_directory), save_color, save_depth,
save_pointcloud, save_infrared, save_pcl_npz_format, save_force_binary_format, save_jpeg, width, height);
std::thread storage_thread(&vpStorageWorker::run, &storage);
#ifdef USE_REALSENSE2
rs2::align align_to(RS2_STREAM_COLOR);
if (use_aligned_stream && save_infrared) {
std::cerr << "Cannot use aligned streams with infrared acquisition currently."
<< "\nInfrared stream acquisition is disabled!"
<< std::endl;
}
#endif
int nb_saves = 0;
bool quit = false;
// If PCL is available, always use PCL datatype even if we will save in NPZ or BIN file format
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud(new pcl::PointCloud<pcl::PointXYZ>);
#else
std::vector<vpColVector> pointCloud;
#endif
std::unique_ptr<vpImage<vpRGBa>> ptr_colorImg;
std::unique_ptr<vpImage<uint16_t>> ptr_depthImg;
std::unique_ptr<std::vector<vpColVector>> ptr_pointCloud;
std::unique_ptr<vpImage<unsigned char>> ptr_infraredImg;
std::vector<double> vec_delta_time;
while (!quit) {
double start = vpTime::measureTimeMs();
if (use_aligned_stream) {
#ifdef USE_REALSENSE2
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud, nullptr,
&align_to);
#else
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud, nullptr,
&align_to);
#endif
#else
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
(unsigned char *)I_infrared.bitmap, nullptr, rs::stream::rectified_color,
rs::stream::depth_aligned_to_rectified_color);
#else
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud,
(unsigned char *)I_infrared.bitmap, nullptr, rs::stream::rectified_color,
rs::stream::depth_aligned_to_rectified_color);
#endif
#endif
}
else {
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, nullptr, pointCloud,
(unsigned char *)I_infrared.bitmap, nullptr);
#else
realsense.acquire((unsigned char *)I_color.bitmap, (unsigned char *)I_depth_raw.bitmap, &pointCloud,
(unsigned char *)I_infrared.bitmap);
#endif
}
if (depth_hist_visu) {
vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
}
else {
// Seems like sometimes using createDepthHistogram takes lots of time?
// so we simply perform bit shift from uint16_t to uint8_t
for (unsigned int i = 0; i < I_depth_raw.getRows(); i++) {
for (unsigned int j = 0; j < I_depth_raw.getCols(); j++) {
I_depth[i][j] = I_depth_raw[i][j] >> 8;
}
}
}
vpDisplay::display(I_infrared);
if (!click_to_save) {
vpDisplay::displayText(I_color, 20, 20, "Click to quit.", vpColor::red);
}
else {
std::stringstream ss;
ss << "Images saved: " << nb_saves;
vpDisplay::displayText(I_color, 20, 20, ss.str(), vpColor::red);
}
if (save && !click_to_save) {
if (save_color) {
ptr_colorImg = std::make_unique<vpImage<vpRGBa>>(I_color);
}
if (save_depth) {
ptr_depthImg = std::make_unique<vpImage<uint16_t>>(I_depth_raw);
}
if (save_infrared) {
ptr_infraredImg = std::make_unique<vpImage<unsigned char>>(I_infrared);
}
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy;
if (save_pointcloud) {
pointCloud_copy = pointCloud->makeShared();
}
save_queue.push(ptr_colorImg, ptr_depthImg, pointCloud_copy, ptr_infraredImg);
#else
if (save_pointcloud) {
ptr_pointCloud = std::make_unique<std::vector<vpColVector>>(pointCloud);
}
save_queue.push(ptr_colorImg, ptr_depthImg, ptr_pointCloud, ptr_infraredImg);
#endif
}
double delta_time = vpTime::measureTimeMs() - start;
vec_delta_time.push_back(delta_time);
std::ostringstream oss_time;
oss_time << delta_time << " ms ; fps=" << 1000/delta_time;
vpDisplay::displayText(I_color, 40, 20, oss_time.str(), vpColor::red);
vpDisplay::flush(I_color);
vpDisplay::flush(I_depth);
vpDisplay::flush(I_infrared);
if (vpDisplay::getClick(I_color, button, false)) {
if (!click_to_save) {
save = false;
quit = true;
save_queue.cancel();
}
else {
switch (button) {
if (save) {
nb_saves++;
if (save_color) {
ptr_colorImg = std::make_unique<vpImage<vpRGBa>>(I_color);
}
if (save_depth) {
ptr_depthImg = std::make_unique<vpImage<uint16_t>>(I_depth_raw);
}
if (save_infrared) {
ptr_infraredImg = std::make_unique<vpImage<unsigned char>>(I_infrared);
}
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
pcl::PointCloud<pcl::PointXYZ>::Ptr pointCloud_copy;
if (save_pointcloud) {
pointCloud_copy = pointCloud->makeShared();
}
save_queue.push(ptr_colorImg, ptr_depthImg, pointCloud_copy, ptr_infraredImg);
#else
if (save_pointcloud) {
ptr_pointCloud = std::make_unique<std::vector<vpColVector>>(pointCloud);
}
save_queue.push(ptr_colorImg, ptr_depthImg, ptr_pointCloud, ptr_infraredImg);
#endif
}
break;
default:
save = false;
quit = true;
save_queue.cancel();
break;
}
}
}
}
double mean_vec_delta_time = vpMath::getMean(vec_delta_time);
double median_vec_delta_time = vpMath::getMedian(vec_delta_time);
double std_vec_delta_time = vpMath::getStdev(vec_delta_time);
std::cout << "Acquisition time, mean=" << mean_vec_delta_time << " ms ; median="
<< median_vec_delta_time << " ms ; std=" << std_vec_delta_time << " ms" << std::endl;
std::cout << "FPS, mean=" << 1000/mean_vec_delta_time << " fps ; median="
<< 1000/median_vec_delta_time << " fps" << std::endl;
storage_thread.join();
return EXIT_SUCCESS;
}
#else
int main()
{
std::cerr << "Need libRealSense or libRealSense2 and C++11 and displayX or displayGDI!" << std::endl;
#if !defined(VISP_HAVE_PUGIXML)
std::cout << "pugixml built-in 3rdparty is requested." << std::endl;
#elif !((__cplusplus >= 201402L) || (defined(_MSVC_LANG) && (_MSVC_LANG >= 201402L)))
std::cout << "At least c++14 standard is requested." << std::endl;
#endif
return EXIT_SUCCESS;
}
#endif
Generic class defining intrinsic camera parameters.
static const vpColor red
Definition: vpColor.h:217
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:130
void init(vpImage< unsigned char > &I, int winx=-1, int winy=-1, const std::string &title="") VP_OVERRIDE
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)
@ ioError
I/O error.
Definition: vpException.h:67
@ fatalError
Fatal error.
Definition: vpException.h:72
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 write(const vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:291
Error that can be emitted by the vpIoTools class and its derivatives.
Definition: vpIoException.h:55
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:550
static void writeBinaryValueLE(std::ofstream &file, const int16_t short_value)
static double getMedian(const std::vector< double > &v)
Definition: vpMath.cpp:322
static double getStdev(const std::vector< double > &v, bool useBesselCorrection=false)
Definition: vpMath.cpp:353
static double getMean(const std::vector< double > &v)
Definition: vpMath.cpp:302
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:70
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)
void npz_save(std::string zipname, std::string fname, const T *data, const std::vector< size_t > &shape, std::string mode="w")
Definition: vpIoTools.h:235
VISP_EXPORT std::string getDateTime(const std::string &format="%Y/%m/%d %H:%M:%S")
VISP_EXPORT double measureTimeMs()