From 55f3f80b57b0804b1d8881d3c7521d27ccc6b200 Mon Sep 17 00:00:00 2001 From: Patrick Scheckenbach Date: Sat, 4 Feb 2023 20:57:12 +0100 Subject: [PATCH 1/7] Add writeBinary to ostream for PCDWriter Add method writeBinary to serialize point clouds in memory uncompressed. --- io/include/pcl/io/pcd_io.h | 11 ++++++++++ io/src/pcd_io.cpp | 44 +++++++++++++++++++++++--------------- 2 files changed, 38 insertions(+), 17 deletions(-) diff --git a/io/include/pcl/io/pcd_io.h b/io/include/pcl/io/pcd_io.h index c10c525e111..cc6cb2a8768 100644 --- a/io/include/pcl/io/pcd_io.h +++ b/io/include/pcl/io/pcd_io.h @@ -402,6 +402,17 @@ namespace pcl const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); + /** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY format + * \param[out] os the stream into which to write the data + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + */ + int + writeBinary (std::ostream &os, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); + /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format * \param[in] file_name the output file name * \param[in] cloud the point cloud data message diff --git a/io/src/pcd_io.cpp b/io/src/pcd_io.cpp index 49075bcebb7..6e53ba6947e 100644 --- a/io/src/pcd_io.cpp +++ b/io/src/pcd_io.cpp @@ -1175,7 +1175,7 @@ pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PCLPointClo /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// int -pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, +pcl::PCDWriter::writeBinary (std::ostream &os, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) { if (cloud.data.empty ()) @@ -1188,13 +1188,26 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCl return (-1); } - std::streamoff data_idx = 0; - std::ostringstream oss; - oss.imbue (std::locale::classic ()); + os.imbue (std::locale::classic ()); + os << generateHeaderBinary (cloud, origin, orientation) << "DATA binary\n"; + std::copy (cloud.data.cbegin(), cloud.data.cend(), std::ostream_iterator (os)); + os.flush (); - oss << generateHeaderBinary (cloud, origin, orientation) << "DATA binary\n"; - oss.flush(); - data_idx = static_cast (oss.tellp ()); + return (os ? 0 : -1); +} + +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +int +pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) +{ + std::ostringstream oss; + int status = writeBinary (oss, cloud, origin, orientation); + if (status) + { + return status; + } + std::string ostr = oss.str (); #ifdef _WIN32 HANDLE h_native_file = CreateFile (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); @@ -1240,12 +1253,12 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCl #endif // Prepare the map #ifdef _WIN32 - HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + cloud.data.size ()), NULL); - char *map = static_cast(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + cloud.data.size ())); + HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, ostr.size (), NULL); + char *map = static_cast(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, ostr.size())); CloseHandle (fm); #else - char *map = static_cast (mmap (nullptr, static_cast (data_idx + cloud.data.size ()), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0)); + char *map = static_cast (mmap (nullptr, ostr.size(), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0)); if (map == reinterpret_cast (-1)) // MAP_FAILED { io::raw_close (fd); @@ -1255,23 +1268,20 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCl } #endif - // copy header - memcpy (&map[0], oss.str().c_str (), static_cast (data_idx)); - - // Copy the data - memcpy (&map[0] + data_idx, cloud.data.data(), cloud.data.size ()); + // copy header + data + memcpy (&map[0], ostr.data (), ostr.size ()); #ifndef _WIN32 // If the user set the synchronization flag on, call msync if (map_synchronization_) - msync (map, static_cast (data_idx + cloud.data.size ()), MS_SYNC); + msync (map, ostr.size (), MS_SYNC); #endif // Unmap the pages of memory #ifdef _WIN32 UnmapViewOfFile (map); #else - if (::munmap (map, static_cast (data_idx + cloud.data.size ())) == -1) + if (::munmap (map, ostr.size()) == -1) { io::raw_close (fd); resetLockingPermissions (file_name, file_lock); From 926625bcd2e118edd9b2c217bbcebc255fc57c63 Mon Sep 17 00:00:00 2001 From: Patrick Scheckenbach <124401292+ptrckschcknbch@users.noreply.github.com> Date: Tue, 7 Feb 2023 19:53:24 +0100 Subject: [PATCH 2/7] Revert "Add writeBinary to ostream for PCDWriter" This reverts commit 55f3f80b57b0804b1d8881d3c7521d27ccc6b200. --- io/include/pcl/io/pcd_io.h | 11 ---------- io/src/pcd_io.cpp | 44 +++++++++++++++----------------------- 2 files changed, 17 insertions(+), 38 deletions(-) diff --git a/io/include/pcl/io/pcd_io.h b/io/include/pcl/io/pcd_io.h index cc6cb2a8768..c10c525e111 100644 --- a/io/include/pcl/io/pcd_io.h +++ b/io/include/pcl/io/pcd_io.h @@ -402,17 +402,6 @@ namespace pcl const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); - /** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY format - * \param[out] os the stream into which to write the data - * \param[in] cloud the point cloud data message - * \param[in] origin the sensor acquisition origin - * \param[in] orientation the sensor acquisition orientation - */ - int - writeBinary (std::ostream &os, const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), - const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); - /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format * \param[in] file_name the output file name * \param[in] cloud the point cloud data message diff --git a/io/src/pcd_io.cpp b/io/src/pcd_io.cpp index 6e53ba6947e..49075bcebb7 100644 --- a/io/src/pcd_io.cpp +++ b/io/src/pcd_io.cpp @@ -1175,7 +1175,7 @@ pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PCLPointClo /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// int -pcl::PCDWriter::writeBinary (std::ostream &os, const pcl::PCLPointCloud2 &cloud, +pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) { if (cloud.data.empty ()) @@ -1188,26 +1188,13 @@ pcl::PCDWriter::writeBinary (std::ostream &os, const pcl::PCLPointCloud2 &cloud, return (-1); } - os.imbue (std::locale::classic ()); - os << generateHeaderBinary (cloud, origin, orientation) << "DATA binary\n"; - std::copy (cloud.data.cbegin(), cloud.data.cend(), std::ostream_iterator (os)); - os.flush (); - - return (os ? 0 : -1); -} - -/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -int -pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) -{ + std::streamoff data_idx = 0; std::ostringstream oss; - int status = writeBinary (oss, cloud, origin, orientation); - if (status) - { - return status; - } - std::string ostr = oss.str (); + oss.imbue (std::locale::classic ()); + + oss << generateHeaderBinary (cloud, origin, orientation) << "DATA binary\n"; + oss.flush(); + data_idx = static_cast (oss.tellp ()); #ifdef _WIN32 HANDLE h_native_file = CreateFile (file_name.c_str (), GENERIC_READ | GENERIC_WRITE, 0, NULL, CREATE_ALWAYS, FILE_ATTRIBUTE_NORMAL, NULL); @@ -1253,12 +1240,12 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCl #endif // Prepare the map #ifdef _WIN32 - HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, ostr.size (), NULL); - char *map = static_cast(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, ostr.size())); + HANDLE fm = CreateFileMapping (h_native_file, NULL, PAGE_READWRITE, 0, (DWORD) (data_idx + cloud.data.size ()), NULL); + char *map = static_cast(MapViewOfFile (fm, FILE_MAP_READ | FILE_MAP_WRITE, 0, 0, data_idx + cloud.data.size ())); CloseHandle (fm); #else - char *map = static_cast (mmap (nullptr, ostr.size(), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0)); + char *map = static_cast (mmap (nullptr, static_cast (data_idx + cloud.data.size ()), PROT_READ | PROT_WRITE, MAP_SHARED, fd, 0)); if (map == reinterpret_cast (-1)) // MAP_FAILED { io::raw_close (fd); @@ -1268,20 +1255,23 @@ pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCl } #endif - // copy header + data - memcpy (&map[0], ostr.data (), ostr.size ()); + // copy header + memcpy (&map[0], oss.str().c_str (), static_cast (data_idx)); + + // Copy the data + memcpy (&map[0] + data_idx, cloud.data.data(), cloud.data.size ()); #ifndef _WIN32 // If the user set the synchronization flag on, call msync if (map_synchronization_) - msync (map, ostr.size (), MS_SYNC); + msync (map, static_cast (data_idx + cloud.data.size ()), MS_SYNC); #endif // Unmap the pages of memory #ifdef _WIN32 UnmapViewOfFile (map); #else - if (::munmap (map, ostr.size()) == -1) + if (::munmap (map, static_cast (data_idx + cloud.data.size ())) == -1) { io::raw_close (fd); resetLockingPermissions (file_name, file_lock); From 8dcdf67939f5a20e15f1f73d0e80cd431da88f87 Mon Sep 17 00:00:00 2001 From: Patrick Scheckenbach <124401292+ptrckschcknbch@users.noreply.github.com> Date: Tue, 7 Feb 2023 23:03:27 +0100 Subject: [PATCH 3/7] Add independent writeBinary to ostream function Add method writeBinary to serialize point clouds in memory uncompressed. --- io/include/pcl/io/pcd_io.h | 11 +++++++++++ io/src/pcd_io.cpp | 23 +++++++++++++++++++++++ 2 files changed, 34 insertions(+) diff --git a/io/include/pcl/io/pcd_io.h b/io/include/pcl/io/pcd_io.h index c10c525e111..cc6cb2a8768 100644 --- a/io/include/pcl/io/pcd_io.h +++ b/io/include/pcl/io/pcd_io.h @@ -402,6 +402,17 @@ namespace pcl const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); + /** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY format + * \param[out] os the stream into which to write the data + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor acquisition origin + * \param[in] orientation the sensor acquisition orientation + */ + int + writeBinary (std::ostream &os, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin = Eigen::Vector4f::Zero (), + const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity ()); + /** \brief Save point cloud data to a PCD file containing n-D points, in BINARY_COMPRESSED format * \param[in] file_name the output file name * \param[in] cloud the point cloud data message diff --git a/io/src/pcd_io.cpp b/io/src/pcd_io.cpp index 49075bcebb7..869f28f3d77 100644 --- a/io/src/pcd_io.cpp +++ b/io/src/pcd_io.cpp @@ -1173,6 +1173,29 @@ pcl::PCDWriter::writeASCII (const std::string &file_name, const pcl::PCLPointClo return (0); } +/////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +int +pcl::PCDWriter::writeBinary (std::ostream &os, const pcl::PCLPointCloud2 &cloud, + const Eigen::Vector4f &origin, const Eigen::Quaternionf &orientation) +{ + if (cloud.data.empty ()) + { + PCL_WARN ("[pcl::PCDWriter::writeBinary] Input point cloud has no data!\n"); + } + if (cloud.fields.empty()) + { + PCL_ERROR ("[pcl::PCDWriter::writeBinary] Input point cloud has no field data!\n"); + return (-1); + } + + os.imbue (std::locale::classic ()); + os << generateHeaderBinary (cloud, origin, orientation) << "DATA binary\n"; + std::copy (cloud.data.cbegin(), cloud.data.cend(), std::ostream_iterator (os)); + os.flush (); + + return (os ? 0 : -1); +} + /////////////////////////////////////////////////////////////////////////////////////////////////////////////////////// int pcl::PCDWriter::writeBinary (const std::string &file_name, const pcl::PCLPointCloud2 &cloud, From 5db3966784e7b0fad8a7cd3d507836d84688610f Mon Sep 17 00:00:00 2001 From: Patrick Scheckenbach <124401292+ptrckschcknbch@users.noreply.github.com> Date: Tue, 7 Feb 2023 23:04:43 +0100 Subject: [PATCH 4/7] Add unit test for writeBinary to ostream --- test/io/test_io.cpp | 68 +++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 68 insertions(+) diff --git a/test/io/test_io.cpp b/test/io/test_io.cpp index a534c29e677..97716de54aa 100644 --- a/test/io/test_io.cpp +++ b/test/io/test_io.cpp @@ -1379,6 +1379,74 @@ TEST (PCL, LZFExtended) remove ("test_pcl_io_compressed.pcd"); } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, WriteBinaryToOStream) +{ + PointCloud cloud; + cloud.width = 640; + cloud.height = 480; + cloud.resize (cloud.width * cloud.height); + cloud.is_dense = true; + + srand (static_cast (time (nullptr))); + const auto nr_p = cloud.size (); + // Randomly create a new point cloud + for (std::size_t i = 0; i < nr_p; ++i) + { + cloud[i].x = static_cast (1024 * rand () / (RAND_MAX + 1.0)); + cloud[i].y = static_cast (1024 * rand () / (RAND_MAX + 1.0)); + cloud[i].z = static_cast (1024 * rand () / (RAND_MAX + 1.0)); + cloud[i].normal_x = static_cast (1024 * rand () / (RAND_MAX + 1.0)); + cloud[i].normal_y = static_cast (1024 * rand () / (RAND_MAX + 1.0)); + cloud[i].normal_z = static_cast (1024 * rand () / (RAND_MAX + 1.0)); + cloud[i].rgb = static_cast (1024 * rand () / (RAND_MAX + 1.0)); + } + + pcl::PCLPointCloud2 blob; + pcl::toPCLPointCloud2 (cloud, blob); + + std::ostringstream oss; + PCDWriter writer; + int res = writer.writeBinary (oss, blob); + EXPECT_EQ (res, 0); + std::string pcd_str = oss.str (); + + Eigen::Vector4f origin; + Eigen::Quaternionf orientation; + int pcd_version = -1; + int data_type = -1; + unsigned int data_idx = 0; + std::istringstream iss (pcd_str, std::ios::binary); + PCDReader reader; + pcl::PCLPointCloud2 blob2; + res = reader.readHeader (iss, blob2, origin, orientation, pcd_version, data_type, data_idx); + EXPECT_EQ (res, 0); + EXPECT_EQ (blob2.width, blob.width); + EXPECT_EQ (blob2.height, blob.height); + EXPECT_EQ (data_type, 1); // since it was written by writeBinary (), it should be uncompressed. + + const auto *data = reinterpret_cast (pcd_str.data ()); + res = reader.readBodyBinary (data, blob2, pcd_version, data_type == 2, data_idx); + PointCloud cloud2; + pcl::fromPCLPointCloud2 (blob2, cloud2); + EXPECT_EQ (res, 0); + EXPECT_EQ (cloud2.width, blob.width); + EXPECT_EQ (cloud2.height, blob.height); + EXPECT_EQ (cloud2.is_dense, cloud.is_dense); + EXPECT_EQ (cloud2.size (), cloud.size ()); + + for (std::size_t i = 0; i < cloud2.size (); ++i) + { + EXPECT_EQ (cloud2[i].x, cloud[i].x); + EXPECT_EQ (cloud2[i].y, cloud[i].y); + EXPECT_EQ (cloud2[i].z, cloud[i].z); + EXPECT_EQ (cloud2[i].normal_x, cloud[i].normal_x); + EXPECT_EQ (cloud2[i].normal_y, cloud[i].normal_y); + EXPECT_EQ (cloud2[i].normal_z, cloud[i].normal_z); + EXPECT_EQ (cloud2[i].rgb, cloud[i].rgb); + } +} + ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, LZFInMem) { From b522e74139184e957bdf1acfe5ca746b14471ebb Mon Sep 17 00:00:00 2001 From: Patrick Scheckenbach <124401292+ptrckschcknbch@users.noreply.github.com> Date: Wed, 8 Feb 2023 21:02:24 +0100 Subject: [PATCH 5/7] Fix register macros to match struct definitions Fix the sequence order in POINT_CLOUD_REGISTER macros to match structs --- common/include/pcl/impl/point_types.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/common/include/pcl/impl/point_types.hpp b/common/include/pcl/impl/point_types.hpp index a3aee983201..28bcc121a24 100644 --- a/common/include/pcl/impl/point_types.hpp +++ b/common/include/pcl/impl/point_types.hpp @@ -1894,10 +1894,10 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBNormal, (float, x, x) (float, y, y) (float, z, z) - (float, rgb, rgb) (float, normal_x, normal_x) (float, normal_y, normal_y) (float, normal_z, normal_z) + (float, rgb, rgb) (float, curvature, curvature) ) POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBNormal, pcl::_PointXYZRGBNormal) @@ -1905,20 +1905,20 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZINormal, (float, x, x) (float, y, y) (float, z, z) - (float, intensity, intensity) (float, normal_x, normal_x) (float, normal_y, normal_y) (float, normal_z, normal_z) + (float, intensity, intensity) (float, curvature, curvature) ) POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZLNormal, (float, x, x) (float, y, y) (float, z, z) - (std::uint32_t, label, label) (float, normal_x, normal_x) (float, normal_y, normal_y) (float, normal_z, normal_z) + (std::uint32_t, label, label) (float, curvature, curvature) ) POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointWithRange, From f6227c0637abf5d3180ebc9dca20d56f5141d74e Mon Sep 17 00:00:00 2001 From: Patrick Scheckenbach <124401292+ptrckschcknbch@users.noreply.github.com> Date: Fri, 10 Feb 2023 14:47:49 +0100 Subject: [PATCH 6/7] Revert "Fix register macros to match struct definitions" This reverts commit b522e74139184e957bdf1acfe5ca746b14471ebb. --- common/include/pcl/impl/point_types.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/common/include/pcl/impl/point_types.hpp b/common/include/pcl/impl/point_types.hpp index 28bcc121a24..a3aee983201 100644 --- a/common/include/pcl/impl/point_types.hpp +++ b/common/include/pcl/impl/point_types.hpp @@ -1894,10 +1894,10 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::_PointXYZRGBNormal, (float, x, x) (float, y, y) (float, z, z) + (float, rgb, rgb) (float, normal_x, normal_x) (float, normal_y, normal_y) (float, normal_z, normal_z) - (float, rgb, rgb) (float, curvature, curvature) ) POINT_CLOUD_REGISTER_POINT_WRAPPER(pcl::PointXYZRGBNormal, pcl::_PointXYZRGBNormal) @@ -1905,20 +1905,20 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZINormal, (float, x, x) (float, y, y) (float, z, z) + (float, intensity, intensity) (float, normal_x, normal_x) (float, normal_y, normal_y) (float, normal_z, normal_z) - (float, intensity, intensity) (float, curvature, curvature) ) POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointXYZLNormal, (float, x, x) (float, y, y) (float, z, z) + (std::uint32_t, label, label) (float, normal_x, normal_x) (float, normal_y, normal_y) (float, normal_z, normal_z) - (std::uint32_t, label, label) (float, curvature, curvature) ) POINT_CLOUD_REGISTER_POINT_STRUCT (pcl::PointWithRange, From 234e9683669504ca9613dae2cb8617284b407314 Mon Sep 17 00:00:00 2001 From: Patrick Scheckenbach <124401292+ptrckschcknbch@users.noreply.github.com> Date: Fri, 10 Feb 2023 16:15:38 +0100 Subject: [PATCH 7/7] Sort fields in generateHeaderBinary Sort cloud.fields by their offset value to generate correct fake fields --- io/src/pcd_io.cpp | 30 ++++++++++++++++++------------ 1 file changed, 18 insertions(+), 12 deletions(-) diff --git a/io/src/pcd_io.cpp b/io/src/pcd_io.cpp index 869f28f3d77..04eedaa2abb 100644 --- a/io/src/pcd_io.cpp +++ b/io/src/pcd_io.cpp @@ -930,9 +930,15 @@ pcl::PCDWriter::generateHeaderBinary (const pcl::PCLPointCloud2 &cloud, "\nVERSION 0.7" "\nFIELDS"; + auto fields = cloud.fields; + std::sort(fields.begin(), fields.end(), [](const auto& field_a, const auto& field_b) + { + return field_a.offset < field_b.offset; + }); + // Compute the total size of the fields unsigned int fsize = 0; - for (const auto &field : cloud.fields) + for (const auto &field : fields) fsize += field.count * getFieldSize (field.datatype); // The size of the fields cannot be larger than point_step @@ -945,20 +951,20 @@ pcl::PCDWriter::generateHeaderBinary (const pcl::PCLPointCloud2 &cloud, std::stringstream field_names, field_types, field_sizes, field_counts; // Check if the size of the fields is smaller than the size of the point step std::size_t toffset = 0; - for (std::size_t i = 0; i < cloud.fields.size (); ++i) + for (std::size_t i = 0; i < fields.size (); ++i) { // If field offsets do not match, then we need to create fake fields - if (toffset != cloud.fields[i].offset) + if (toffset != fields[i].offset) { // If we're at the last "valid" field int fake_offset = (i == 0) ? // Use the current_field offset - (cloud.fields[i].offset) + (fields[i].offset) : // Else, do cur_field.offset - prev_field.offset + sizeof (prev_field) - (cloud.fields[i].offset - - (cloud.fields[i-1].offset + - cloud.fields[i-1].count * getFieldSize (cloud.fields[i-1].datatype))); + (fields[i].offset - + (fields[i-1].offset + + fields[i-1].count * getFieldSize (fields[i-1].datatype))); toffset += fake_offset; @@ -969,11 +975,11 @@ pcl::PCDWriter::generateHeaderBinary (const pcl::PCLPointCloud2 &cloud, } // Add the regular dimension - toffset += cloud.fields[i].count * getFieldSize (cloud.fields[i].datatype); - field_names << " " << cloud.fields[i].name; - field_sizes << " " << pcl::getFieldSize (cloud.fields[i].datatype); - field_types << " " << pcl::getFieldType (cloud.fields[i].datatype); - int count = std::abs (static_cast (cloud.fields[i].count)); + toffset += fields[i].count * getFieldSize (fields[i].datatype); + field_names << " " << fields[i].name; + field_sizes << " " << pcl::getFieldSize (fields[i].datatype); + field_types << " " << pcl::getFieldType (fields[i].datatype); + int count = std::abs (static_cast (fields[i].count)); if (count == 0) count = 1; // check for 0 counts (coming from older converter code) field_counts << " " << count; }