Skip to content

Commit

Permalink
Use NPZ format instead of raw binary file with example/device/framegr…
Browse files Browse the repository at this point in the history 
…abber/readRealSenseData.cpp and example/device/framegrabber/saveRealSenseData.cpp files. Save in PNG by default.

Try to improve saving time:
./saveRealSenseData -s -c
Acquisition time, mean=33.5689 ms ; median=31.1479 ms ; std=12.8222 ms
FPS, mean=29.7895 fps ; median=32.1048 fps

./saveRealSenseData -s -c -i
Acquisition time, mean=35.7134 ms ; median=35.0071 ms ; std=13.9597 ms
FPS, mean=28.0007 fps ; median=28.5657 fps

./saveRealSenseData -s -c -i -d
Acquisition time, mean=34.4988 ms ; median=32.8796 ms ; std=12.283 ms
FPS, mean=28.9865 fps ; median=30.414 fps

./saveRealSenseData -s -c -i -d -p
Acquisition time, mean=123.71 ms ; median=121.119 ms ; std=9.79244 ms
FPS, mean=8.08343 fps ; median=8.25637 fps

Better, saving pointcloud still takes lot of time (3.7 mo instead of 620 ko for depth)?
  • Loading branch information
@s-trinh
s-trinh committed May 10, 2024
1 parent e4afdf1 commit 24771aa
Show file tree
Hide file tree
Showing 2 changed files with 467 additions and 158 deletions.
213 changes: 149 additions & 64 deletions example/device/framegrabber/readRealSenseData.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <iostream>
Expand Down Expand Up @@ -68,7 +68,7 @@
#endif
#endif

#define GETOPTARGS "ci:bodh"
#define GETOPTARGS "ci:e:jbzodh"

namespace
{
Expand All @@ -82,8 +82,11 @@ void usage(const char *name, const char *badparam)
<< "SYNOPSIS " << std::endl
<< " " << name
<< " [-i <directory>]"
<< " [-e <filename pattern (e.g. %04d)>]"
<< " [-c]"
<< " [-j]"
<< " [-b]"
<< " [-z]"
<< " [-o]"
<< " [-d]"
<< " [--help,-h]"
Expand All @@ -92,14 +95,23 @@ void usage(const char *name, const char *badparam)
<< " -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
<< " Point cloud stream is saved in binary format." << std::endl
<< " Depth and Pointcloud streams are saved in binary format." << std::endl
<< std::endl
<< " -z" << std::endl
<< " Pointcloud stream is saved in NPZ format." << 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
Expand All @@ -113,8 +125,8 @@ 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 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;
Expand All @@ -125,11 +137,20 @@ bool getOptions(int argc, const char *argv[], std::string &input_directory, bool
case 'i':
input_directory = optarg;
break;
case 'e':
pattern = optarg;
break;
case 'c':
click = true;
break;
case 'j':
read_jpeg = true;
break;
case 'b':
pointcloud_binary_format = true;
force_binary_format = true;
break;
case 'z':
read_npz = true;
break;
case 'o':
save_video = true;
Expand Down Expand Up @@ -161,26 +182,29 @@ bool getOptions(int argc, const char *argv[], std::string &input_directory, bool
return true;
}

bool readData(int cpt, const std::string &input_directory, vpImage<vpRGBa> &I_color, vpImage<uint16_t> &I_depth_raw,
bool pointcloud_binary_format
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_%04d.jpg";
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_%04d.bin";
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_%04d" << (pointcloud_binary_format ? ".bin" : ".pcd");
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;

Expand All @@ -196,79 +220,137 @@ bool readData(int cpt, const std::string &input_directory, vpImage<vpRGBa> &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 (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;
}
}
}
}
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);
}
}

// Read pointcloud
if (vpIoTools::checkFilename(filename_pointcloud)) {
#if defined(VISP_HAVE_PCL)
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;
// 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;
}
}
}
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.");
}

uint32_t height = 0, width = 0;
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;
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);

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 {
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;
}
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"));
throw(vpIoException(vpIoException::ioError, "Cannot read pcd file without PCL io module"));
#endif
}
}
#endif
}

return true;
}
} // Namespace
} // Namespace

int main(int argc, const char *argv[])
{
std::string input_directory = "";
std::string pattern = "%04d";
bool click = false;
bool pointcloud_binary_format = 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, click, pointcloud_binary_format, save_video, color_depth)) {
if (!getOptions(argc, argv, input_directory, pattern, click, force_binary_format, read_jpeg, read_npz,
save_video, color_depth)) {
return EXIT_FAILURE;
}

Expand Down Expand Up @@ -296,8 +378,8 @@ int main(int argc, const char *argv[])
if (save_video) {
std::string output_directory = vpTime::getDateTime("%Y-%m-%d_%H.%M.%S");
vpIoTools::makeDirectory(output_directory);
writer.setFileName(output_directory + "/%04d.png");
}
writer.setFileName(output_directory + "/" + pattern + ".png");
}

int cpt_frame = 0;
bool quit = false;
Expand All @@ -307,15 +389,18 @@ int main(int argc, const char *argv[])
#if defined(VISP_HAVE_PCL) && defined(VISP_HAVE_PCL_COMMON)
{
std::lock_guard<std::mutex> lock(mutex);
quit = !readData(cpt_frame, input_directory, I_color, I_depth_raw, pointcloud_binary_format, pointcloud);
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, I_color, I_depth_raw, pointcloud_binary_format);
quit = !readData(cpt_frame, input_directory, pattern, I_color, I_depth_raw, force_binary_format, read_jpeg,
read_npz);
#endif

vpImageConvert::createDepthHistogram(I_depth_raw, I_depth);
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;
Expand Down
Loading

0 comments on commit 24771aa

Please sign in to comment.