41 #include <visp3/core/vpConfig.h>
43 #if defined(VISP_HAVE_THREADS) && (defined(VISP_HAVE_X11) || defined(VISP_HAVE_GDI))
44 #include <condition_variable>
50 #include <visp3/core/vpImageConvert.h>
51 #include <visp3/core/vpIoException.h>
52 #include <visp3/core/vpIoTools.h>
53 #include <visp3/gui/vpDisplayGDI.h>
54 #include <visp3/gui/vpDisplayX.h>
55 #include <visp3/gui/vpDisplayPCL.h>
56 #include <visp3/io/vpImageIo.h>
57 #include <visp3/io/vpParseArgv.h>
58 #include <visp3/io/vpVideoWriter.h>
60 #if defined(VISP_HAVE_PCL)
61 #include <pcl/pcl_config.h>
62 #if defined(VISP_HAVE_PCL_COMMON)
63 #include <pcl/point_types.h>
64 #include <pcl/point_cloud.h>
66 #if defined(VISP_HAVE_PCL_IO)
67 #include <pcl/io/pcd_io.h>
71 #ifdef VISP_HAVE_MINIZ
72 #define GETOPTARGS "ci:e:jbzodh"
74 #define GETOPTARGS "ci:e:jbodh"
77 #ifdef ENABLE_VISP_NAMESPACE
84 void usage(
const char *name,
const char *badparam)
86 std::cout <<
"\nNAME " << std::endl
88 <<
" - Read data acquired with a Realsense device." << std::endl
90 <<
"SYNOPSIS " << std::endl
92 <<
" [-i <directory>]"
93 <<
" [-e <filename pattern (e.g. %04d)>]"
97 #ifdef VISP_HAVE_MINIZ
104 std::cout <<
"\nOPTIONS " << std::endl
105 <<
" -i <directory>" << std::endl
106 <<
" Input folder that contains the data to read." << std::endl
108 <<
" -e <pattern>" << std::endl
109 <<
" Filename pattern, e.g. %04d." << std::endl
111 <<
" -c" << std::endl
112 <<
" Flag to display data in step by step mode triggered by a user click." << std::endl
114 <<
" -j" << std::endl
115 <<
" Image data are saved in JPEG format, otherwise in PNG." << std::endl
117 <<
" -b" << std::endl
118 <<
" Depth and Pointcloud streams are saved in binary format." << std::endl
120 #ifdef VISP_HAVE_MINIZ
121 <<
" -z" << std::endl
122 <<
" Pointcloud stream is saved in NPZ format." << std::endl
125 <<
" -o" << std::endl
126 <<
" Save color images in PNG format (lossless) in a new folder." << std::endl
128 <<
" -d" << std::endl
129 <<
" Display depth in color." << std::endl
131 <<
" --help, -h" << std::endl
132 <<
" Display this helper message." << std::endl
136 std::cout <<
"\nERROR: Bad parameter " << badparam << std::endl;
140 bool getOptions(
int argc,
const char *argv[], std::string &input_directory, std::string &pattern,
bool &click,
141 bool &force_binary_format,
bool &read_jpeg,
bool &read_npz,
bool &save_video,
bool &color_depth)
144 const char **argv1 = (
const char **)argv;
150 input_directory = optarg;
162 force_binary_format =
true;
164 #ifdef VISP_HAVE_MINIZ
177 usage(argv[0],
nullptr);
182 usage(argv[0], optarg);
188 if ((c == 1) || (c == -1)) {
190 usage(argv[0],
nullptr);
191 std::cerr <<
"ERROR: " << std::endl;
192 std::cerr <<
" Bad argument " << optarg << std::endl << std::endl;
199 bool readData(
int cpt,
const std::string &input_directory,
const std::string &pattern,
vpImage<vpRGBa> &I_color,
200 vpImage<uint16_t> &I_depth_raw,
bool force_binary_format,
bool read_jpeg,
bool read_npz
201 #
if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
202 , pcl::PointCloud<pcl::PointXYZ>::Ptr point_cloud
206 std::string image_filename_ext = read_jpeg ?
".jpg" :
".png";
207 std::string depth_filename_ext = force_binary_format ?
".bin" :
".npz";
208 char buffer[FILENAME_MAX];
209 std::stringstream ss;
210 ss << input_directory <<
"/color_image_" << pattern << image_filename_ext;
211 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
212 std::string filename_color = buffer;
215 ss << input_directory <<
"/depth_image_" << pattern << depth_filename_ext;
216 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
217 std::string filename_depth = buffer;
220 ss << input_directory <<
"/point_cloud_" << pattern << (force_binary_format ?
".bin" :
221 (read_npz ?
".npz" :
".pcd"));
222 snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt);
223 std::string filename_pointcloud = buffer;
227 std::cerr <<
"End of sequence." << std::endl;
238 if (force_binary_format) {
239 std::ifstream file_depth(filename_depth.c_str(), std::ios::in | std::ios::binary);
240 if (file_depth.is_open()) {
241 unsigned int height = 0, width = 0;
244 I_depth_raw.
resize(height, width);
246 uint16_t depth_value = 0;
247 for (
unsigned int i = 0; i < height; i++) {
248 for (
unsigned int j = 0; j < width; j++) {
250 I_depth_raw[i][j] = depth_value;
255 #ifdef VISP_HAVE_MINIZ
265 uint16_t *depth_data_ptr = arr_depth_data.
data<uint16_t>();
266 assert(arr_depth_data.
shape.size() == 3);
267 assert(arr_depth_data.
shape[2] == 1);
269 unsigned int height =
static_cast<unsigned int>(arr_depth_data.
shape[0]);
270 unsigned int width =
static_cast<unsigned int>(arr_depth_data.
shape[1]);
271 const bool copyData =
true;
283 #if defined(VISP_HAVE_PCL)
285 if (force_binary_format) {
286 std::ifstream file_pointcloud(filename_pointcloud.c_str(), std::ios::in | std::ios::binary);
287 if (!file_pointcloud.is_open()) {
288 std::cerr <<
"Cannot read pointcloud file: " << filename_pointcloud << std::endl;
291 uint32_t height = 0, width = 0;
292 const char is_dense = 1;
295 file_pointcloud.read((
char *)(&is_dense),
sizeof(is_dense));
297 point_cloud->width = width;
298 point_cloud->height = height;
299 point_cloud->is_dense = (is_dense != 0);
300 point_cloud->resize((
size_t)width * height);
302 float x = 0.0f, y = 0.0f, z = 0.0f;
303 for (uint32_t i = 0; i < height; i++) {
304 for (uint32_t j = 0; j < width; j++) {
309 point_cloud->points[(size_t)(i * width + j)].x = x;
310 point_cloud->points[(size_t)(i * width + j)].y = y;
311 point_cloud->points[(size_t)(i * width + j)].z = z;
315 #ifdef VISP_HAVE_MINIZ
325 float *pcl_data_ptr = arr_pcl_data.
data<
float>();
326 assert(arr_pcl_data.
shape.size() == 3);
327 assert(arr_pcl_data.
shape[2] == 3);
329 uint32_t height = arr_pcl_data.
shape[0], width = arr_pcl_data.
shape[1];
330 const char is_dense = 1;
332 point_cloud->width = width;
333 point_cloud->height = height;
334 point_cloud->is_dense = (is_dense != 0);
335 point_cloud->resize((
size_t)width * height);
337 for (uint32_t i = 0; i < height; i++) {
338 for (uint32_t j = 0; j < width; j++) {
339 point_cloud->points[(size_t)(i * width + j)].x = pcl_data_ptr[(size_t)(i * width + j)*3 + 0];
340 point_cloud->points[(size_t)(i * width + j)].y = pcl_data_ptr[(size_t)(i * width + j)*3 + 1];
341 point_cloud->points[(size_t)(i * width + j)].z = pcl_data_ptr[(size_t)(i * width + j)*3 + 2];
347 #if defined(VISP_HAVE_PCL_IO)
348 if (pcl::io::loadPCDFile<pcl::PointXYZ>(filename_pointcloud, *point_cloud) == -1) {
349 std::cerr <<
"Cannot read PCD: " << filename_pointcloud << std::endl;
362 int main(
int argc,
const char *argv[])
364 std::string input_directory =
"";
365 std::string pattern =
"%04d";
367 bool force_binary_format =
false;
368 bool save_video =
false;
369 bool color_depth =
false;
370 bool read_jpeg =
false;
371 bool read_npz =
false;
374 if (!getOptions(argc, argv, input_directory, pattern, click, force_binary_format, read_jpeg, read_npz,
375 save_video, color_depth)) {
388 bool init_display =
false;
390 #if defined(VISP_HAVE_PCL)
392 pcl::PointCloud<pcl::PointXYZ>::Ptr pointcloud(
new pcl::PointCloud<pcl::PointXYZ>());
393 #if defined(VISP_HAVE_PCL_VISUALIZATION)
403 writer.
setFileName(output_directory +
"/" + pattern +
".png");
411 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
413 std::lock_guard<std::mutex> lock(mutex);
414 quit = !readData(cpt_frame, input_directory, pattern, I_color, I_depth_raw, force_binary_format, read_jpeg,
415 read_npz, pointcloud);
418 quit = !readData(cpt_frame, input_directory, pattern, I_color, I_depth_raw, force_binary_format, read_jpeg,
429 d1.
init(I_color, 0, 0,
"Color image");
431 d2.
init(I_depth_color, I_color.
getWidth() + 10, 0,
"Depth image");
434 d2.
init(I_depth, I_color.
getWidth() + 10, 0,
"Depth image");
436 #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_VISUALIZATION)
439 pcl_viewer.
startThread(std::ref(mutex), pointcloud);
449 std::stringstream ss;
450 ss <<
"Frame: " << cpt_frame;
502 std::cerr <<
"Enable C++11 or higher (cmake -DUSE_CXX_STANDARD=11) and install X11 or GDI!" << std::endl;
Display for windows using GDI (available on any windows 32 platform).
void setPosition(int posx, int posy)
void startThread(std::mutex &mutex, pcl::PointCloud< pcl::PointXYZ >::Ptr pointcloud)
void setWindowName(const std::string &window_name)
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 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 void read(vpImage< unsigned char > &I, const std::string &filename, int backend=IO_DEFAULT_BACKEND)
Class that defines a 2D point in an image. This class is useful for image processing and stores only ...
unsigned int getWidth() const
void resize(unsigned int h, unsigned int w)
resize the image : Image initialization
void insert(const vpImage< Type > &src, const vpImagePoint &topLeft)
unsigned int getSize() const
unsigned int getHeight() const
Error that can be emitted by the vpIoTools class and its derivatives.
static bool parse(int *argcPtr, const char **argv, vpArgvInfo *argTable, int flags)
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)
std::map< std::string, NpyArray > npz_t
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
std::vector< size_t > shape