Visual Servoing Platform  version 3.6.1 under development (2024-06-15)
readRealSenseData.cpp

Example that shows how to replay realsense data saved with saveRealSenseData.cpp

/****************************************************************************
*
* ViSP, open source Visual Servoing Platform software.
* Copyright (C) 2005 - 2023 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_THREADS) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
#include <condition_variable>
#include <fstream>
#include <mutex>
#include <queue>
#include <thread>
#include <visp3/core/vpImageConvert.h>
#include <visp3/core/vpIoException.h>
#include <visp3/core/vpIoTools.h>
#include <visp3/gui/vpDisplayGDI.h>
#include <visp3/gui/vpDisplayX.h>
#include <visp3/gui/vpDisplayPCL.h>
#include <visp3/io/vpImageIo.h>
#include <visp3/io/vpParseArgv.h>
#include <visp3/io/vpVideoWriter.h>
#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
#ifdef VISP_HAVE_MINIZ
#define GETOPTARGS "ci:e:jbzodh"
#else
#define GETOPTARGS "ci:e:jbodh"
#endif
#ifdef ENABLE_VISP_NAMESPACE
using namespace VISP_NAMESPACE_NAME;
#endif
namespace
{
void usage(const char *name, const char *badparam)
{
std::cout << "\nNAME " << std::endl
<< " " << vpIoTools::getName(name)
<< " - Read data acquired with a Realsense device." << std::endl
<< std::endl
<< "SYNOPSIS " << std::endl
<< " " << name
<< " [-i <directory>]"
<< " [-e <filename pattern (e.g. %04d)>]"
<< " [-c]"
<< " [-j]"
<< " [-b]"
#ifdef VISP_HAVE_MINIZ
<< " [-z]"
#endif
<< " [-o]"
<< " [-d]"
<< " [--help,-h]"
<< std::endl;
std::cout << "\nOPTIONS " << std::endl
<< " -i <directory>" << std::endl
<< " Input folder that contains the data to read." << std::endl
<< std::endl
<< " -e <pattern>" << std::endl
<< " Filename pattern, e.g. %04d." << std::endl
<< std::endl
<< " -c" << std::endl
<< " Flag to display data in step by step mode triggered by a user click." << std::endl
<< std::endl
<< " -j" << std::endl
<< " Image data are saved in JPEG format, otherwise in PNG." << std::endl
<< std::endl
<< " -b" << std::endl
<< " Depth and Pointcloud streams are saved in binary format." << std::endl
<< std::endl
#ifdef VISP_HAVE_MINIZ
<< " -z" << std::endl
<< " Pointcloud stream is saved in NPZ format." << std::endl
<< std::endl
#endif
<< " -o" << std::endl
<< " Save color images in PNG format (lossless) in a new folder." << std::endl
<< std::endl
<< " -d" << std::endl
<< " Display depth in color." << std::endl
<< std::endl
<< " --help, -h" << std::endl
<< " Display this helper message." << std::endl
<< std::endl;
if (badparam) {
std::cout << "\nERROR: Bad parameter " << badparam << std::endl;
}
}
bool getOptions(int argc, const char *argv[], std::string &input_directory, std::string &pattern, bool &click,
bool &force_binary_format, bool &read_jpeg, bool &read_npz, bool &save_video, bool &color_depth)
{
const char *optarg;
const char **argv1 = (const char **)argv;
int c;
while ((c = vpParseArgv::parse(argc, argv1, GETOPTARGS, &optarg)) > 1) {
switch (c) {
case 'i':
input_directory = optarg;
break;
case 'e':
pattern = optarg;
break;
case 'c':
click = true;
break;
case 'j':
read_jpeg = true;
break;
case 'b':
force_binary_format = true;
break;
#ifdef VISP_HAVE_MINIZ
case 'z':
read_npz = true;
break;
#endif
case 'o':
save_video = true;
break;
case 'd':
color_depth = true;
break;
case 'h':
usage(argv[0], nullptr);
return false;
break;
default:
usage(argv[0], optarg);
return false;
break;
}
}
if ((c == 1) || (c == -1)) {
// standalone param or error
usage(argv[0], nullptr);
std::cerr << "ERROR: " << std::endl;
std::cerr << " Bad argument " << optarg << std::endl << std::endl;
return false;
}
return true;
}
bool readData(int cpt, const std::string &input_directory, const std::string &pattern, vpImage<vpRGBa> &I_color,
vpImage<uint16_t> &I_depth_raw, bool force_binary_format, bool read_jpeg, bool read_npz
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
, pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud
#endif
)
{
std::string image_filename_ext = read_jpeg ? ".jpg" : ".png";
std::string depth_filename_ext = force_binary_format ? ".bin" : ".npz";
char buffer[FILENAME_MAX];
std::stringstream ss;
ss << input_directory << "/color_image_" << pattern << image_filename_ext;
snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
std::string filename_color = buffer;
ss.str("");
ss << input_directory << "/depth_image_" << pattern << depth_filename_ext;
snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
std::string filename_depth = buffer;
ss.str("");
ss << input_directory << "/point_cloud_" << pattern << (force_binary_format ? ".bin" :
(read_npz ? ".npz" : ".pcd"));
snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
std::string filename_pointcloud = buffer;
if (!vpIoTools::checkFilename(filename_color) && !vpIoTools::checkFilename(filename_depth) &&
!vpIoTools::checkFilename(filename_pointcloud)) {
std::cerr << "End of sequence." << std::endl;
return false;
}
// Read color
if (vpIoTools::checkFilename(filename_color)) {
vpImageIo::read(I_color, filename_color);
}
// Read raw depth
if (vpIoTools::checkFilename(filename_depth)) {
if (force_binary_format) {
std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
if (file_depth.is_open()) {
unsigned int height = 0, width = 0;
vpIoTools::readBinaryValueLE(file_depth, height);
vpIoTools::readBinaryValueLE(file_depth, width);
I_depth_raw.resize(height, width);
uint16_t depth_value = 0;
for (unsigned int i = 0; i < height; i++) {
for (unsigned int j = 0; j < width; j++) {
vpIoTools::readBinaryValueLE(file_depth, depth_value);
I_depth_raw[i][j] = depth_value;
}
}
}
}
#ifdef VISP_HAVE_MINIZ
else {
visp::cnpy::npz_t npz_data = visp::cnpy::npz_load(filename_depth);
// Load depth data
visp::cnpy::NpyArray arr_depth_data = npz_data["data"];
if (arr_depth_data.data_holder == nullptr) {
throw vpIoException(vpIoException::ioError, "Loaded NPZ data is null.");
}
uint16_t *depth_data_ptr = arr_depth_data.data<uint16_t>();
assert(arr_depth_data.shape.size() == 3); // H x W x C
assert(arr_depth_data.shape[2] == 1); // Single channel
unsigned int height = arr_depth_data.shape[0], width = arr_depth_data.shape[1];
const bool copyData = true;
I_depth_raw = vpImage<uint16_t>(depth_data_ptr, height, width, copyData);
}
#else
else {
throw(vpIoException(vpIoException::ioError, "Cannot open non-binary depth file when npz I/O functions are disabled."));
}
#endif
}
// Read pointcloud
if (vpIoTools::checkFilename(filename_pointcloud)) {
#if defined(VISP_HAVE_PCL)
// TODO: not tested
if (force_binary_format) {
std::ifstream file_pointcloud(filename_pointcloud.c_str(), std::ios::in | std::ios::binary);
if (!file_pointcloud.is_open()) {
std::cerr << "Cannot read pointcloud file: " << filename_pointcloud << std::endl;
}
uint32_t height = 0, width = 0;
const char is_dense = 1;
vpIoTools::readBinaryValueLE(file_pointcloud, height);
vpIoTools::readBinaryValueLE(file_pointcloud, width);
file_pointcloud.read((char *)(&is_dense), sizeof(is_dense));
point_cloud->width = width;
point_cloud->height = height;
point_cloud->is_dense = (is_dense != 0);
point_cloud->resize((size_t)width * height);
float x = 0.0f, y = 0.0f, z = 0.0f;
for (uint32_t i = 0; i < height; i++) {
for (uint32_t j = 0; j < width; j++) {
vpIoTools::readBinaryValueLE(file_pointcloud, x);
vpIoTools::readBinaryValueLE(file_pointcloud, y);
vpIoTools::readBinaryValueLE(file_pointcloud, z);
point_cloud->points[(size_t)(i * width + j)].x = x;
point_cloud->points[(size_t)(i * width + j)].y = y;
point_cloud->points[(size_t)(i * width + j)].z = z;
}
}
}
#ifdef VISP_HAVE_MINIZ
else if (read_npz) {
visp::cnpy::npz_t npz_data = visp::cnpy::npz_load(filename_pointcloud);
// Load pointcloud data
visp::cnpy::NpyArray arr_pcl_data = npz_data["data"];
if (arr_pcl_data.data_holder == nullptr) {
throw vpIoException(vpIoException::ioError, "Loaded NPZ data is null.");
}
float *pcl_data_ptr = arr_pcl_data.data<float>();
assert(arr_pcl_data.shape.size() == 3); // H x W x C
assert(arr_pcl_data.shape[2] == 3); // 3-channels: X, Y, Z
uint32_t height = arr_pcl_data.shape[0], width = arr_pcl_data.shape[1];
const char is_dense = 1;
point_cloud->width = width;
point_cloud->height = height;
point_cloud->is_dense = (is_dense != 0);
point_cloud->resize((size_t)width * height);
for (uint32_t i = 0; i < height; i++) {
for (uint32_t j = 0; j < width; j++) {
point_cloud->points[(size_t)(i * width + j)].x = pcl_data_ptr[(size_t)(i * width + j)*3 + 0];
point_cloud->points[(size_t)(i * width + j)].y = pcl_data_ptr[(size_t)(i * width + j)*3 + 1];
point_cloud->points[(size_t)(i * width + j)].z = pcl_data_ptr[(size_t)(i * width + j)*3 + 2];
}
}
}
#endif
else {
#if defined(VISP_HAVE_PCL_IO)
if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename_pointcloud, *point_cloud) == -1) {
std::cerr << "Cannot read PCD: " << filename_pointcloud << std::endl;
}
#else
throw(vpIoException(vpIoException::ioError, "Cannot read pcd file without PCL io module"));
#endif
}
#endif
}
return true;
}
} // Namespace
int main(int argc, const char *argv[])
{
std::string input_directory = "";
std::string pattern = "%04d";
bool click = false;
bool force_binary_format = false;
bool save_video = false;
bool color_depth = false;
bool read_jpeg = false;
bool read_npz = false;
// Read the command line options
if (!getOptions(argc, argv, input_directory, pattern, click, force_binary_format, read_jpeg, read_npz,
save_video, color_depth)) {
return EXIT_FAILURE;
}
vpImage<vpRGBa> I_color(480, 640), I_depth_color(480, 640);
vpImage<uint16_t> I_depth_raw(480, 640);
vpImage<unsigned char> I_depth(480, 640);
#ifdef VISP_HAVE_X11
vpDisplayX d1, d2;
#else
vpDisplayGDI d1, d2;
#endif
bool init_display = false;
#if defined(VISP_HAVE_PCL)
std::mutex mutex;
pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(new pcl::PointCloud<pcl::PointXYZ>());
#if defined(VISP_HAVE_PCL_VISUALIZATION)
vpDisplayPCL pcl_viewer;
#endif
#endif
vpVideoWriter writer;
if (save_video) {
std::string output_directory = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
vpIoTools::makeDirectory(output_directory);
writer.setFileName(output_directory + "/" + pattern + ".png");
}
int cpt_frame = 0;
bool quit = false;
while (!quit) {
double t = vpTime::measureTimeMs();
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
{
std::lock_guard<std::mutex> lock(mutex);
quit = !readData(cpt_frame, input_directory, pattern, I_color, I_depth_raw, force_binary_format, read_jpeg,
read_npz, pointcloud);
}
#else
quit = !readData(cpt_frame, input_directory, pattern, I_color, I_depth_raw, force_binary_format, read_jpeg,
read_npz);
#endif
if (color_depth)
vpImageConvert::createDepthHistogram(I_depth_raw, I_depth_color);
else
vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
if (!init_display) {
init_display = true;
d1.init(I_color, 0, 0, "Color image");
if (color_depth) {
d2.init(I_depth_color, I_color.getWidth() + 10, 0, "Depth image");
}
else {
d2.init(I_depth, I_color.getWidth() + 10, 0, "Depth image");
}
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
pcl_viewer.setPosition(I_color.getWidth() + 10, I_color.getHeight() + 70);
pcl_viewer.setWindowName("3D point cloud");
pcl_viewer.startThread(std::ref(mutex), pointcloud);
#endif
}
if (color_depth)
vpDisplay::display(I_depth_color);
else
std::stringstream ss;
ss << "Frame: " << cpt_frame;
vpDisplay::displayText(I_color, 20, 20, ss.str(), vpColor::red);
if (color_depth)
vpDisplay::displayText(I_depth_color, 20, 20, ss.str(), vpColor::red);
else
vpDisplay::displayText(I_depth, 20, 20, ss.str(), vpColor::red);
vpDisplay::flush(I_color);
if (color_depth)
vpDisplay::flush(I_depth_color);
else
vpDisplay::flush(I_depth);
if (save_video) {
if (O.getSize() == 0) {
O.resize(I_color.getHeight(), I_color.getWidth() + I_depth_color.getWidth());
writer.open(O);
}
O.insert(I_color, vpImagePoint());
if (!color_depth)
vpImageConvert::convert(I_depth, I_depth_color);
O.insert(I_depth_color, vpImagePoint(0, I_color.getWidth()));
writer.saveFrame(O);
}
if (vpDisplay::getClick(I_color, button, click)) {
switch (button) {
if (!quit)
quit = !click;
break;
click = !click;
break;
default:
break;
}
}
vpTime::wait(t, 30);
cpt_frame++;
}
return EXIT_SUCCESS;
}
#else
int main()
{
std::cerr << "Enable C++11 or higher (cmake -DUSE_CXX_STANDARD=11) and install X11 or GDI!" << std::endl;
return EXIT_SUCCESS;
}
#endif
static const vpColor red
Definition: vpColor.h:213
Display for windows using GDI (available on any windows 32 platform).
Definition: vpDisplayGDI.h:132
Use the X11 console to display images on unix-like OS. Thus to enable this class X11 should be instal...
Definition: vpDisplayX.h:131
void init(vpImage< unsigned char > &I, int win_x=-1, int win_y=-1, const std::string &win_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
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 read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Definition: vpImageIo.cpp:147
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
Definition: vpImagePoint.h:82
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
Definition: vpImage.h:799
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
Definition: vpImage.h:1610
unsigned int getSize() const
Definition: vpImage.h:225
Error that can be emitted by the vpIoTools class and its derivatives.
Definition: vpIoException.h:55
static bool checkFilename(const std::string &filename)
Definition: vpIoTools.cpp:1218
static void readBinaryValueLE(std::ifstream &file, int16_t &short_value)
Definition: vpIoTools.cpp:2535
static void makeDirectory(const std::string &dirname)
Definition: vpIoTools.cpp:986
static std::string getName(const std::string &pathname)
Definition: vpIoTools.cpp:1996
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
Definition: vpParseArgv.cpp:71
Class that enables to write easily a video file or a sequence of images.
void saveFrame(vpImage< vpRGBa > &I)
void setFileName(const std::string &filename)
void open(vpImage< vpRGBa > &I)
VISP_EXPORT npz_t npz_load(std::string fname)
Definition: vpIoTools.cpp:342
std::map< std::string, NpyArray > npz_t
Definition: vpIoTools.h:127
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()
std::shared_ptr< std::vector< char > > data_holder
Definition: vpIoTools.h:120
std::vector< size_t > shape
Definition: vpIoTools.h:121