diff --git a/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp b/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp index cd4c3b7aaf..329db5b559 100644 --- a/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp +++ b/example/device/framegrabber/grabV4l2MultiCpp11Thread.cpp @@ -133,119 +133,8 @@ bool getOptions(int argc, char **argv, unsigned int &deviceCount, bool &saveVide return true; } -/* -// Code adapted from the original author Dan MaĊĦek to be compatible with ViSP -// image -class vpFrameQueue -{ - -public: - struct vpCancelled_t - { }; - - vpFrameQueue() - : m_cancelled(false), m_cond(), m_queueColor(), m_maxQueueSize(std::numeric_limits::max()), m_mutex() - { } - - void cancel() - { - std::lock_guard lock(m_mutex); - m_cancelled = true; - m_cond.notify_all(); - } - - // Push the image to save in the queue (FIFO) - void push(const vpImage &image) - { - std::lock_guard lock(m_mutex); - - m_queueColor.push(image); - - // Pop extra images in the queue - while (m_queueColor.size() > m_maxQueueSize) { - m_queueColor.pop(); - } - - m_cond.notify_one(); - } - - // Pop the image to save from the queue (FIFO) - vpImage pop() - { - std::unique_lock lock(m_mutex); - - while (m_queueColor.empty()) { - if (m_cancelled) { - throw vpCancelled_t(); - } - - m_cond.wait(lock); - - if (m_cancelled) { - throw vpCancelled_t(); - } - } - - vpImage image(m_queueColor.front()); - m_queueColor.pop(); - - return image; - } - - void setMaxQueueSize(const size_t max_queue_size) { m_maxQueueSize = max_queue_size; } - -private: - bool m_cancelled; - std::condition_variable m_cond; - std::queue > m_queueColor; - size_t m_maxQueueSize; - std::mutex m_mutex; -}; -*/ - -/* -class vpStorageWorker -{ - -public: - vpStorageWorker(vpConcurrentQueue> &queue, const std::string &filename, unsigned int width, unsigned int height) - : m_queue(queue), m_filename(filename), m_width(width), m_height(height) - { } - - // Thread main loop - void run() - { - vpImage O_color(m_height, m_width); - - vpVideoWriter writer; - if (!m_filename.empty()) { - writer.setFileName(m_filename); - writer.open(O_color); - } - - try { - for (;;) { - vpImage image(m_queue.pop()); - - if (!m_filename.empty()) { - writer.saveFrame(image); - } - } - } - catch (vpConcurrentQueue>::vpCancelled_t &) { - } - } - -private: - vpConcurrentQueue> &m_queue; - std::string m_filename; - unsigned int m_width; - unsigned int m_height; -};*/ - class vpShareImage { - private: bool m_cancelled; std::condition_variable m_cond; @@ -470,8 +359,6 @@ int main(int argc, char *argv[]) // Synchronized queues for each camera stream std::vector>> save_queues(grabbers.size()); - // std::vector> storages; - // std::vector storage_threads; std::vector> storages; std::vector storage_threads; @@ -505,10 +392,6 @@ int main(int argc, char *argv[]) } if (saveVideo) { - // for (auto &s : storages) { - // // Start the storage thread for the current camera stream - // storage_threads.emplace_back(&vpVideoStorageWorker::run, &s); - // } storage_threads.emplace_back(storages); } diff --git a/example/device/framegrabber/readRealSenseData.cpp b/example/device/framegrabber/readRealSenseData.cpp index f557e9d06c..8eb5cd2673 100644 --- a/example/device/framegrabber/readRealSenseData.cpp +++ b/example/device/framegrabber/readRealSenseData.cpp @@ -33,7 +33,7 @@ /*! \example readRealSenseData.cpp - \brief Example that show how to replay realsense data saved with saveRealSenseData.cpp + \brief Example that shows how to replay realsense data saved with saveRealSenseData.cpp */ #include @@ -68,7 +68,7 @@ #endif #endif -#define GETOPTARGS "ci:bodh" +#define GETOPTARGS "ci:jbnodh" namespace { @@ -83,7 +83,9 @@ void usage(const char *name, const char *badparam) << " " << name << " [-i ]" << " [-c]" + << " [-j]" << " [-b]" + << " [-n]" << " [-o]" << " [-d]" << " [--help,-h]" @@ -95,11 +97,17 @@ void usage(const char *name, const char *badparam) << " -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 + << " -n" << std::endl + << " Binary data (depth or pointcloud) are saved in NPZ format." << std::endl + << std::endl << " -b" << std::endl - << " Point cloud stream is saved in binary format." << std::endl + << " Point cloud stream is saved in binary format, otherwise in .pcd." << std::endl << std::endl << " -o" << std::endl - << " Save color images in png format in a new folder." << 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 @@ -114,7 +122,7 @@ void usage(const char *name, const char *badparam) } bool getOptions(int argc, const char *argv[], std::string &input_directory, bool &click, bool &pointcloud_binary_format, - bool &save_video, bool &color_depth) + bool &read_jpeg, bool &read_npz, bool &save_video, bool &color_depth) { const char *optarg; const char **argv1 = (const char **)argv; @@ -128,6 +136,12 @@ bool getOptions(int argc, const char *argv[], std::string &input_directory, bool case 'c': click = true; break; + case 'j': + read_jpeg = true; + break; + case 'n': + read_npz = true; + break; case 'b': pointcloud_binary_format = true; break; @@ -162,25 +176,27 @@ bool getOptions(int argc, const char *argv[], std::string &input_directory, bool } bool readData(int cpt, const std::string &input_directory, vpImage &I_color, vpImage &I_depth_raw, - bool pointcloud_binary_format + bool pointcloud_binary_format, bool read_jpeg, bool read_npz #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) , pcl::PointCloud::Ptr point_cloud #endif ) { + std::string image_filename_ext = read_jpeg ? ".jpg" : ".png"; + std::string binary_filename_ext = read_npz ? ".npz" : ".bin"; char buffer[FILENAME_MAX]; std::stringstream ss; - ss << input_directory << "/color_image_%04d.jpg"; + ss << input_directory << "/color_image_%04d" << image_filename_ext; snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt); std::string filename_color = buffer; ss.str(""); - ss << input_directory << "/depth_image_%04d.bin"; + ss << input_directory << "/depth_image_%04d" << binary_filename_ext; snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt); std::string filename_depth = buffer; ss.str(""); - ss << input_directory << "/point_cloud_%04d" << (pointcloud_binary_format ? ".bin" : ".pcd"); + ss << input_directory << "/point_cloud_%04d" << (read_npz ? ".npz" : (pointcloud_binary_format ? ".bin" : ".pcd")); snprintf(buffer, FILENAME_MAX, ss.str().c_str(), cpt); std::string filename_pointcloud = buffer; @@ -196,32 +212,82 @@ bool readData(int cpt, const std::string &input_directory, vpImage &I_co } // Read raw depth - 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; + if (read_npz) { + 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(); + 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(depth_data_ptr, height, width, copyData); + } + else { + 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; + } } } } // Read pointcloud #if defined(VISP_HAVE_PCL) - if (pointcloud_binary_format) { + if (read_npz) { + // TODO: not tested + 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(); + 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); + + 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++) { + 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]; + } + } + } + else if (pointcloud_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; - char is_dense = 1; + const char is_dense = 1; vpIoTools::readBinaryValueLE(file_pointcloud, height); vpIoTools::readBinaryValueLE(file_pointcloud, width); file_pointcloud.read((char *)(&is_dense), sizeof(is_dense)); @@ -266,9 +332,12 @@ int main(int argc, const char *argv[]) bool pointcloud_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, click, pointcloud_binary_format, save_video, color_depth)) { + if (!getOptions(argc, argv, input_directory, click, pointcloud_binary_format, read_jpeg, read_npz, + save_video, color_depth)) { return EXIT_FAILURE; } @@ -307,10 +376,10 @@ int main(int argc, const char *argv[]) #if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON) { std::lock_guard lock(mutex); - quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format, pointcloud); + quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format, read_jpeg, read_npz, pointcloud); } #else - quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format); + quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format, read_jpeg, read_npz); #endif if (color_depth) diff --git a/example/device/framegrabber/saveRealSenseData.cpp b/example/device/framegrabber/saveRealSenseData.cpp index 13b03dde2f..d52a450db9 100644 --- a/example/device/framegrabber/saveRealSenseData.cpp +++ b/example/device/framegrabber/saveRealSenseData.cpp @@ -33,7 +33,7 @@ /*! \example saveRealSenseData.cpp - \brief Example that show how to save realsense data that can be replayed with readRealSenseData.cpp + \brief Example that shows how to save realsense data that can be replayed with readRealSenseData.cpp */ #include @@ -69,12 +69,12 @@ #include #include - // Priority to libRealSense2 +// Priority to libRealSense2 #if defined(VISP_HAVE_REALSENSE2) #define USE_REALSENSE2 #endif -#define GETOPTARGS "so:acdpiCf:bh" +#define GETOPTARGS "so:acdpijCf:bh" namespace { @@ -89,6 +89,7 @@ void usage(const char *name, const char *badparam, int fps) << " [-p]" << " [-b]" << " [-i]" + << " [-j]" << " [-C]" << " [-f ]" << " [-o ]" @@ -118,6 +119,9 @@ void usage(const char *name, const char *badparam, int fps) << " -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 @@ -143,7 +147,7 @@ void usage(const char *name, const char *badparam, int fps) bool getOptions(int argc, const char *argv[], bool &save, 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_pointcloud_binary_format) + int &stream_fps, bool &save_pointcloud_binary_format, bool &save_jpeg) { const char *optarg; const char **argv1 = (const char **)argv; @@ -172,6 +176,9 @@ bool getOptions(int argc, const char *argv[], bool &save, std::string &output_di case 'i': save_infrared = true; break; + case 'j': + save_jpeg = true; + break; case 'C': click_to_save = true; break; @@ -206,6 +213,11 @@ bool getOptions(int argc, const char *argv[], bool &save, std::string &output_di return true; } +// Code adapted from: https://stackoverflow.com/a/37146523 +// See example/device/framegrabber/saveRealSenseData2.cpp which uses ViSP: +// - vpConcurrentQueue +// - vpWriterWorker +// - vpWriterExecutor class vpFrameQueue { public: @@ -318,7 +330,7 @@ class vpStorageWorker { public: vpStorageWorker(vpFrameQueue &queue, const std::string &directory, bool save_color, bool save_depth, bool save_pointcloud, - bool save_infrared, bool save_pointcloud_binary_format, + bool save_infrared, bool save_pointcloud_binary_format, bool save_jpeg, int #ifndef VISP_HAVE_PCL width @@ -331,7 +343,7 @@ class vpStorageWorker ) : m_queue(queue), 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_pointcloud_binary_format(save_pointcloud_binary_format) + m_save_pointcloud_binary_format(save_pointcloud_binary_format), m_save_jpeg(save_jpeg) #ifndef VISP_HAVE_PCL , m_size_height(height), m_size_width(width) @@ -352,6 +364,7 @@ class vpStorageWorker vpImage infraredImg; char buffer[FILENAME_MAX]; + std::string image_filename_ext = m_save_jpeg ? ".jpg" : ".png"; for (;;) { m_queue.pop(colorImg, depthImg, pointCloud, infraredImg); @@ -359,7 +372,7 @@ class vpStorageWorker std::stringstream ss; if (m_save_color) { - ss << m_directory << "/color_image_%04d.jpg"; + ss << m_directory << "/color_image_%04d" << image_filename_ext; snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt); std::string filename_color = buffer; @@ -421,7 +434,7 @@ class vpStorageWorker uint32_t width = m_size_width; uint32_t height = m_size_height; // to be consistent with PCL version - char is_dense = 1; + const char is_dense = 1; vpIoTools::writeBinaryValueLE(file_pointcloud, height); vpIoTools::writeBinaryValueLE(file_pointcloud, width); @@ -450,7 +463,7 @@ class vpStorageWorker if (m_save_infrared) { ss.str(""); - ss << m_directory << "/infrared_image_%04d.jpg"; + ss << m_directory << "/infrared_image_%04d" << image_filename_ext; snprintf(buffer, FILENAME_MAX, ss.str().c_str(), m_cpt); std::string filename_infrared = buffer; @@ -475,6 +488,7 @@ class vpStorageWorker bool m_save_pointcloud; bool m_save_infrared; bool m_save_pointcloud_binary_format; + bool m_save_jpeg; #ifndef VISP_HAVE_PCL int m_size_height; int m_size_width; @@ -495,10 +509,12 @@ int main(int argc, const char *argv[]) bool click_to_save = false; int stream_fps = 30; bool save_pointcloud_binary_format = false; + bool save_jpeg = false; // Read the command line options if (!getOptions(argc, argv, save, output_directory_custom, use_aligned_stream, save_color, save_depth, - save_pointcloud, save_infrared, click_to_save, stream_fps, save_pointcloud_binary_format)) { + save_pointcloud, save_infrared, click_to_save, stream_fps, save_pointcloud_binary_format, + save_jpeg)) { return EXIT_FAILURE; } @@ -516,6 +532,7 @@ int main(int argc, const char *argv[]) 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 << "save_pointcloud_binary_format: " << save_pointcloud_binary_format << std::endl; std::cout << "click_to_save: " << click_to_save << std::endl; @@ -631,7 +648,7 @@ int main(int argc, const char *argv[]) vpFrameQueue save_queue; vpStorageWorker storage(std::ref(save_queue), std::cref(output_directory), save_color, save_depth, save_pointcloud, - save_infrared, save_pointcloud_binary_format, width, height); + save_infrared, save_pointcloud_binary_format, save_jpeg, width, height); std::thread storage_thread(&vpStorageWorker::run, &storage); #ifdef USE_REALSENSE2 diff --git a/example/device/framegrabber/saveRealSenseData2.cpp b/example/device/framegrabber/saveRealSenseData2.cpp index 16fe311700..791f1182b9 100644 --- a/example/device/framegrabber/saveRealSenseData2.cpp +++ b/example/device/framegrabber/saveRealSenseData2.cpp @@ -64,7 +64,7 @@ // If set, use a single thread to save the acquisition data #define TEST_SINGLE_THREAD 1 -#define GETOPTARGS "so:acdpiCf:h" +#define GETOPTARGS "so:acdpijCf:h" namespace { @@ -78,6 +78,7 @@ void usage(const char *name, const char *badparam, int fps) << " [-d]" << " [-p]" << " [-i]" + << " [-j]" << " [-C]" << " [-f ]" << " [-o ]" @@ -102,6 +103,9 @@ void usage(const char *name, const char *badparam, int fps) << " -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 @@ -123,7 +127,7 @@ void usage(const char *name, const char *badparam, int fps) bool getOptions(int argc, const char *argv[], bool &save, 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) + int &stream_fps, bool &save_jpeg) { const char *optarg; const char **argv1 = (const char **)argv; @@ -152,6 +156,9 @@ bool getOptions(int argc, const char *argv[], bool &save, std::string &output_di case 'i': save_infrared = true; break; + case 'j': + save_jpeg = true; + break; case 'C': click_to_save = true; break; @@ -223,6 +230,8 @@ template >> class vpNPZStor while (runOnce()); } + // TODO: currently data are saved using the default platform endianness (most likely little-endian) + // this will not work using different platform endianness bool runOnce() override { try { @@ -235,6 +244,10 @@ template >> class vpNPZStor // Write Npz headers std::vector vec_filename(str_filename.begin(), str_filename.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, "filename", &vec_filename[0], { vec_filename.size() }, "w"); std::string current_time = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S"); @@ -296,10 +309,11 @@ int main(int argc, const char *argv[]) bool save_infrared = false; bool click_to_save = false; int stream_fps = 30; + bool save_jpeg = false; // Read the command line options if (!getOptions(argc, argv, save, output_directory_custom, use_aligned_stream, save_color, save_depth, - save_pointcloud, save_infrared, click_to_save, stream_fps)) { + save_pointcloud, save_infrared, click_to_save, stream_fps, save_jpeg)) { return EXIT_FAILURE; } @@ -313,6 +327,7 @@ int main(int argc, const char *argv[]) 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 << "click_to_save: " << click_to_save << std::endl; @@ -397,19 +412,21 @@ int main(int argc, const char *argv[]) std::vector storage_threads; #endif + std::string image_filename_ext = save_jpeg ? ".jpg" : ".png"; + if (save) { std::ostringstream oss; oss << output_directory; std::cout << "Create directory: " << oss.str() << std::endl; vpIoTools::makeDirectory(oss.str()); - oss << "/color_image_%04d.jpg"; + oss << "/color_image_%04d" << image_filename_ext; std::string rgb_output = oss.str(); storages.emplace_back(std::make_shared>(rgb_queue, rgb_output)); oss.str(""); oss.clear(); - oss << output_directory << "/infrared_image_%04d.jpg"; + oss << output_directory << "/infrared_image_%04d" << image_filename_ext; std::string ir_output = oss.str(); storages.emplace_back(std::make_shared>(infrared_queue, ir_output)); diff --git a/modules/core/include/visp3/core/vpConcurrentQueue.h b/modules/core/include/visp3/core/vpConcurrentQueue.h index 5f7acde9e5..ffd87578f6 100644 --- a/modules/core/include/visp3/core/vpConcurrentQueue.h +++ b/modules/core/include/visp3/core/vpConcurrentQueue.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -28,12 +28,12 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * C++ std::queue with mutex for concurrent IO operations. + * C++ std::queue with mutex for concurrent I/O operations. */ /*! \file vpConcurrentQueue.h - \brief C++ std::queue with mutex for concurrent IO operations, see https://stackoverflow.com/a/37146523. + \brief C++ std::queue with mutex for concurrent IO operations, see https://stackoverflow.com/a/37146523 */ #ifndef vpConcurrentQueue_h diff --git a/modules/core/include/visp3/core/vpWriterExecutor.h b/modules/core/include/visp3/core/vpWriterExecutor.h index 3b21438876..bee877d807 100644 --- a/modules/core/include/visp3/core/vpWriterExecutor.h +++ b/modules/core/include/visp3/core/vpWriterExecutor.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -28,12 +28,12 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * TODO: . + * Single or multi-threaded executor, which will call the runOnce() method of the different input workers. */ /*! \file vpWriterExecutor.h - \brief TODO: . + \brief Single or multi-threaded executor, which will call the runOnce() method of the different input workers. */ #ifndef _vpWriterExecutor_h_ diff --git a/modules/core/include/visp3/core/vpWriterWorker.h b/modules/core/include/visp3/core/vpWriterWorker.h index 18e293090a..0b2a7206b1 100644 --- a/modules/core/include/visp3/core/vpWriterWorker.h +++ b/modules/core/include/visp3/core/vpWriterWorker.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -28,12 +28,12 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * TODO: . + * Virtual base class providing different methods to be adapted. */ /*! \file vpWriterWorker.h - \brief TODO: . + \brief Virtual base class providing different methods to be adapted. */ #ifndef _vpWriterWorker_h_ @@ -43,9 +43,8 @@ /*! \class vpWriterWorker - + Virtual base class providing different methods to be adapted. */ - class VISP_EXPORT vpWriterWorker { public: diff --git a/modules/core/src/tools/file/vpIoTools.cpp b/modules/core/src/tools/file/vpIoTools.cpp index c6321c1c39..42cbd30975 100644 --- a/modules/core/src/tools/file/vpIoTools.cpp +++ b/modules/core/src/tools/file/vpIoTools.cpp @@ -360,34 +360,35 @@ visp::cnpy::npz_t visp::cnpy::npz_load(std::string fname) if ((local_header[2] != 0x03) || (local_header[3] != 0x04)) { quit = true; } + else { + //read in the variable name + uint16_t name_len = *(uint16_t *)&local_header[26]; + std::string varname(name_len, ' '); + size_t vname_res = fread(&varname[0], sizeof(char), name_len, fp); + if (vname_res != name_len) { + throw std::runtime_error("npz_load: failed fread"); + } - //read in the variable name - uint16_t name_len = *(uint16_t *)&local_header[26]; - std::string varname(name_len, ' '); - size_t vname_res = fread(&varname[0], sizeof(char), name_len, fp); - if (vname_res != name_len) { - throw std::runtime_error("npz_load: failed fread"); - } - - //erase the lagging .npy - varname.erase(varname.end()-4, varname.end()); + //erase the lagging .npy + varname.erase(varname.end()-4, varname.end()); - //read in the extra field - uint16_t extra_field_len = *(uint16_t *)&local_header[28]; - if (extra_field_len > 0) { - std::vector buff(extra_field_len); - size_t efield_res = fread(&buff[0], sizeof(char), extra_field_len, fp); - if (efield_res != extra_field_len) { - throw std::runtime_error("npz_load: failed fread"); + //read in the extra field + uint16_t extra_field_len = *(uint16_t *)&local_header[28]; + if (extra_field_len > 0) { + std::vector buff(extra_field_len); + size_t efield_res = fread(&buff[0], sizeof(char), extra_field_len, fp); + if (efield_res != extra_field_len) { + throw std::runtime_error("npz_load: failed fread"); + } } - } - uint16_t compr_method = *reinterpret_cast(&local_header[0]+8); - uint32_t compr_bytes = *reinterpret_cast(&local_header[0]+18); - uint32_t uncompr_bytes = *reinterpret_cast(&local_header[0]+22); + uint16_t compr_method = *reinterpret_cast(&local_header[0]+8); + uint32_t compr_bytes = *reinterpret_cast(&local_header[0]+18); + uint32_t uncompr_bytes = *reinterpret_cast(&local_header[0]+22); - if (compr_method == 0) { arrays[varname] = load_the_npy_file(fp); } - else { arrays[varname] = load_the_npz_array(fp, compr_bytes, uncompr_bytes); } + if (compr_method == 0) { arrays[varname] = load_the_npy_file(fp); } + else { arrays[varname] = load_the_npz_array(fp, compr_bytes, uncompr_bytes); } + } } fclose(fp); @@ -423,33 +424,34 @@ visp::cnpy::NpyArray visp::cnpy::npz_load(std::string fname, std::string varname if ((local_header[2] != 0x03) || (local_header[3] != 0x04)) { quit = true; } + else { + //read in the variable name + uint16_t name_len = *(uint16_t *)&local_header[26]; + std::string vname(name_len, ' '); + size_t vname_res = fread(&vname[0], sizeof(char), name_len, fp); + if (vname_res != name_len) { + throw std::runtime_error("npz_load: failed fread"); + } + vname.erase(vname.end()-4, vname.end()); //erase the lagging .npy - //read in the variable name - uint16_t name_len = *(uint16_t *)&local_header[26]; - std::string vname(name_len, ' '); - size_t vname_res = fread(&vname[0], sizeof(char), name_len, fp); - if (vname_res != name_len) { - throw std::runtime_error("npz_load: failed fread"); - } - vname.erase(vname.end()-4, vname.end()); //erase the lagging .npy - - //read in the extra field - uint16_t extra_field_len = *(uint16_t *)&local_header[28]; - fseek(fp, extra_field_len, SEEK_CUR); //skip past the extra field + //read in the extra field + uint16_t extra_field_len = *(uint16_t *)&local_header[28]; + fseek(fp, extra_field_len, SEEK_CUR); //skip past the extra field - uint16_t compr_method = *reinterpret_cast(&local_header[0]+8); - uint32_t compr_bytes = *reinterpret_cast(&local_header[0]+18); - uint32_t uncompr_bytes = *reinterpret_cast(&local_header[0]+22); + uint16_t compr_method = *reinterpret_cast(&local_header[0]+8); + uint32_t compr_bytes = *reinterpret_cast(&local_header[0]+18); + uint32_t uncompr_bytes = *reinterpret_cast(&local_header[0]+22); - if (vname == varname) { - NpyArray array = (compr_method == 0) ? load_the_npy_file(fp) : load_the_npz_array(fp, compr_bytes, uncompr_bytes); - fclose(fp); - return array; - } - else { - //skip past the data - uint32_t size = *(uint32_t *)&local_header[22]; - fseek(fp, size, SEEK_CUR); + if (vname == varname) { + NpyArray array = (compr_method == 0) ? load_the_npy_file(fp) : load_the_npz_array(fp, compr_bytes, uncompr_bytes); + fclose(fp); + return array; + } + else { + //skip past the data + uint32_t size = *(uint32_t *)&local_header[22]; + fseek(fp, size, SEEK_CUR); + } } } @@ -2119,16 +2121,16 @@ std::string vpIoTools::toLowerCase(const std::string &input) out += std::tolower(*it); } return out; - } +} - /** - * @brief Return a upper-case version of the string \b input . - * Numbers and special characters stay the same - * - * @param input The input string for which we want to ensure that all the characters are in upper case. - * @return std::string A upper-case version of the string \b input, where - * numbers and special characters stay the same - */ +/** + * @brief Return a upper-case version of the string \b input . + * Numbers and special characters stay the same + * + * @param input The input string for which we want to ensure that all the characters are in upper case. + * @return std::string A upper-case version of the string \b input, where + * numbers and special characters stay the same + */ std::string vpIoTools::toUpperCase(const std::string &input) { std::string out; @@ -2140,16 +2142,16 @@ std::string vpIoTools::toUpperCase(const std::string &input) out += std::toupper(*it); } return out; - } +} - /*! - Returns the absolute path using realpath() on Unix systems or - GetFullPathName() on Windows systems. \return According to realpath() - manual, returns an absolute pathname that names the same file, whose - resolution does not involve '.', '..', or symbolic links for Unix systems. - According to GetFullPathName() documentation, retrieves the full path of the - specified file for Windows systems. - */ +/*! + Returns the absolute path using realpath() on Unix systems or + GetFullPathName() on Windows systems. \return According to realpath() + manual, returns an absolute pathname that names the same file, whose + resolution does not involve '.', '..', or symbolic links for Unix systems. + According to GetFullPathName() documentation, retrieves the full path of the + specified file for Windows systems. + */ std::string vpIoTools::getAbsolutePathname(const std::string &pathname) { diff --git a/modules/io/include/visp3/io/vpFileStorageWorker.h b/modules/io/include/visp3/io/vpFileStorageWorker.h index 89dbca61c8..412a1e170d 100644 --- a/modules/io/include/visp3/io/vpFileStorageWorker.h +++ b/modules/io/include/visp3/io/vpFileStorageWorker.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -28,12 +28,17 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * TODO: . + * Binary file worker to execute code inside runOnce(): + * - pop data from the concurrent queue (thread-safe queue) + * - write data to a file in binary format */ /*! \file vpFileStorageWorker.h - \brief TODO:, see https://stackoverflow.com/a/37146523. + \brief See https://stackoverflow.com/a/37146523 for inspiration + Binary file worker to execute code inside runOnce(): + - pop data from the concurrent queue (thread-safe queue) + - write data to a file in binary format */ #ifndef vpFileStorageWorker_h @@ -101,7 +106,7 @@ template >> class VISP_EXPO if (m_write_header) { // Write data header - // WARNING: header data must already be stored in little-endian format + // WARNING: header data must already be stored in little-endian format if endianness is needed writer.write(m_ptr_header_vec[i], m_header_size_vec[i]); } diff --git a/modules/io/include/visp3/io/vpVideoStorageWorker.h b/modules/io/include/visp3/io/vpVideoStorageWorker.h index 83a52f9c56..ad8e58c068 100644 --- a/modules/io/include/visp3/io/vpVideoStorageWorker.h +++ b/modules/io/include/visp3/io/vpVideoStorageWorker.h @@ -1,6 +1,6 @@ /* * ViSP, open source Visual Servoing Platform software. - * Copyright (C) 2005 - 2023 by Inria. All rights reserved. + * 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 @@ -28,12 +28,19 @@ * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Description: - * TODO: . + * Video worker to execute code inside runOnce(): + * - pop data from the concurrent queue (thread-safe queue) + * - still images are supported (e.g. JPEG, PNG) + * - see vpVideoWriter class */ /*! \file vpVideoStorageWorker.h - \brief TODO:, see https://stackoverflow.com/a/37146523. + \brief See https://stackoverflow.com/a/37146523 for inspiration + Video worker to execute code inside runOnce(): + - pop data from the concurrent queue (thread-safe queue) + - still images are supported (e.g. JPEG, PNG) + - see vpVideoWriter class */ #ifndef vpVideoStorageWorker_h diff --git a/script/PlotRealsenseData.py b/script/PlotRealsenseData.py index be594b602e..355a40d749 100644 --- a/script/PlotRealsenseData.py +++ b/script/PlotRealsenseData.py @@ -32,18 +32,14 @@ def load_data(iter, input_color_filenames, input_infrared_filenames, input_depth with np.load(depth_filename) as data: height = data['height'][0] width = data['width'][0] - # print(f"Depth height={height} ; widht={width}") depth_data_raw = data['data'].reshape((height, width)) max_uint16 = 65536 hist, bin_edges = np.histogram(depth_data_raw, bins=max_uint16, range=(0, max_uint16-1)) - # print(f"hist={len(hist)} ; bin_edges={bin_edges}") # https://stackoverflow.com/a/30460089 dx = bin_edges[1] - bin_edges[0] cumsum = np.cumsum(hist)*dx depth = np.zeros(depth_data_raw.shape, dtype=np.uint8) - # print(f"cumsum={cumsum.shape}") - # print(f"depth_data={depth_data.shape}") for i in range(depth_data_raw.shape[0]): for j in range(depth_data_raw.shape[1]): try: @@ -70,14 +66,16 @@ def main(): formatter_class=argparse.ArgumentDefaultsHelpFormatter) parser.add_argument('-i', '--input', type=str, default="", help='Input data folder.') - parser.add_argument('--color-pattern', type=str, default="color_image_*.jpg", + parser.add_argument('--color-pattern', type=str, default="color_image_*.png", help='Filename pattern for the color images.') - parser.add_argument('--infrared-pattern', type=str, default="infrared_image_*.jpg", + parser.add_argument('--infrared-pattern', type=str, default="infrared_image_*.png", help='Filename pattern for the infrared images.') parser.add_argument('--depth-pattern', type=str, default="depth_image_*.npz", help='Filename pattern for the depth images.') parser.add_argument('--pcl-pattern', type=str, default="pointcloud_*.npz", help='Filename pattern for the pointcloud data.') + parser.add_argument('--pcl-channel', type=int, default=2, + help='Pointcloud channel (0, 1 or 2) to be displayed.') args = parser.parse_args() input_folder = Path(args.input) @@ -85,11 +83,13 @@ def main(): infrared_pattern = Path(input_folder) / args.infrared_pattern depth_pattern = Path(input_folder) / args.depth_pattern pcl_pattern = Path(input_folder) / args.pcl_pattern + pcl_channel = args.pcl_channel print(f"Input folder: {input_folder}") print(f"Color filename pattern: {color_pattern}") print(f"Infrared filename pattern: {infrared_pattern}") print(f"Depth filename pattern: {depth_pattern}") print(f"Pointcloud filename pattern: {pcl_pattern}") + print(f"Pointcloud channel to be displayed: {pcl_channel}") if not input_folder.is_dir(): print(f"Invalid folder: {input_folder}") @@ -123,7 +123,7 @@ def main(): # https://matplotlib.org/stable/gallery/mplot3d/mixed_subplots.html # https://matplotlib.org/stable/gallery/mplot3d/subplot3d.html fig = plt.figure() - fig.suptitle('RGB + IR + Depth + PCL') + fig.suptitle('RGB + IR + Depth + PCL', fontsize=30) ax00 = fig.add_subplot(2, 2, 1) ax01 = fig.add_subplot(2, 2, 2) ax10 = fig.add_subplot(2, 2, 3) @@ -134,22 +134,20 @@ def main(): im3 = ax11.scatter(pcl_vec[0][::10,::10,0], pcl_vec[0][::10,::10,1], pcl_vec[0][::10,::10,2]) def init(): - # print(f"init") - im0.set_array(color_vec[0]) im1.set_array(ir_vec[0]) im2.set_array(depth_vec[0]) subsample = pcl_vec[0][::10,::10,:2] # https://stackoverflow.com/a/9416663 im3.set_offsets(subsample.reshape(-1,2)) - im3.set_array(pcl_vec[0][::10,::10,2].flatten()) + im3.set_array(pcl_vec[0][::10,::10,pcl_channel].flatten()) # https://stackoverflow.com/a/57259405 def update_func(frame): - # print(f"update_func={frame} ; color_vec={len(color_vec)}") - if frame >= len(color_vec): - color_data, ir_data, depth_data, pcl_data = load_data(frame, input_color_filenames, input_infrared_filenames, input_depth_filenames, input_pcl_filenames) + color_data, ir_data, depth_data, pcl_data = load_data(frame, input_color_filenames, + input_infrared_filenames, input_depth_filenames, + input_pcl_filenames) color_vec.append(color_data) ir_vec.append(ir_data) depth_vec.append(depth_data) @@ -158,9 +156,9 @@ def update_func(frame): im0.set_array(color_vec[frame]) im1.set_array(ir_vec[frame]) im2.set_array(depth_vec[frame]) - subsample = pcl_vec[frame][::10,::10,:2] + subsample = pcl_vec[frame][::10,::10,:pcl_channel] im3.set_offsets(subsample.reshape(-1,2)) - im3.set_array(pcl_vec[frame][::10,::10,2].flatten()) + im3.set_array(pcl_vec[frame][::10,::10,pcl_channel].flatten()) # https://matplotlib.org/stable/api/animation_api.html anim = animation.FuncAnimation( diff --git a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp index 8191a7ffe8..78eeccccdc 100644 --- a/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp +++ b/tutorial/tracking/model-based/generic/tutorial-mb-generic-tracker-read.cpp @@ -92,7 +92,11 @@ int main(int argc, char *argv[]) std::cout << "Color mode? " << (channel > 1) << std::endl; visp::cnpy::NpyArray arr_camera_name = npz_data["camera_name"]; - const std::string camera_name = std::string(arr_camera_name.data()); + // For null-terminated character handling, see: + // https://stackoverflow.com/a/8247804 + // https://stackoverflow.com/a/45491652 + std::vector vec_arr_camera_name = arr_camera_name.as_vec(); + const std::string camera_name = std::string(vec_arr_camera_name.begin(), vec_arr_camera_name.end()); std::cout << "Camera name: " << camera_name << std::endl; visp::cnpy::NpyArray arr_px = npz_data["cam_px"];