From aa05e54491a5a0f5ca766823e812fa71fee34191 Mon Sep 17 00:00:00 2001 From: Guillaume Batun <35939648+kaipomauli@users.noreply.github.com> Date: Tue, 5 Mar 2024 05:46:56 -0400 Subject: [PATCH] Add PLYwriter::writeBinary ostream overload (#5975) * Added PLYWriter::writeBinary() ofstream overload modified existing PLYWriter::writeBinary() to open file and then call new overload for data aggregation added PLYWriter::writeBinary() overload that adds data to referenced ofstream buffer * Correct new PLYWriter::writeBinary() header removed unnecessary class qualifiers * Apply suggestions from code review Improved description of new overload Added default zero value for Eigen::Vector4f$ origin Formatting Co-authored-by: Markus Vieth <39675748+mvieth@users.noreply.github.com> * Modified PLYWriter::writeBinary to use std::ostream instead of std::ofstream * Changed pclWriter::writeBinary ostream variable name to os --------- Co-authored-by: Markus Vieth <39675748+mvieth@users.noreply.github.com> --- io/include/pcl/io/ply_io.h | 14 ++- io/src/ply_io.cpp | 231 ++++++++++++++++++++++--------------- 2 files changed, 148 insertions(+), 97 deletions(-) diff --git a/io/include/pcl/io/ply_io.h b/io/include/pcl/io/ply_io.h index 86e540771ae..dbad3424a3a 100644 --- a/io/include/pcl/io/ply_io.h +++ b/io/include/pcl/io/ply_io.h @@ -591,7 +591,19 @@ namespace pcl const Eigen::Quaternionf &orientation = Eigen::Quaternionf::Identity (), int precision = 8, bool use_camera = true); - + /** \brief Save point cloud data to a std::ostream containing n-D points, in BINARY format + * \param[in] os the output buffer + * \param[in] cloud the point cloud data message + * \param[in] origin the sensor data acquisition origin + * (translation) \param[in] orientation the sensor data acquisition origin + * (rotation) \param[in] use_camera if set to true then PLY file will use element + * camera else element range_grid will be used + */ + int + writeBinary(std::ostream& os, const pcl::PCLPointCloud2& cloud, + const Eigen::Vector4f& origin = Eigen::Vector4f::Zero (), const Eigen::Quaternionf& orientation= Eigen::Quaternionf::Identity (), + bool use_camera=true); + /** \brief Save point cloud data to a PLY file containing n-D points, in BINARY format * \param[in] file_name the output file name * \param[in] cloud the point cloud data message diff --git a/io/src/ply_io.cpp b/io/src/ply_io.cpp index 6008c014527..47b8de6adba 100644 --- a/io/src/ply_io.cpp +++ b/io/src/ply_io.cpp @@ -1196,10 +1196,10 @@ pcl::PLYWriter::writeContentWithRangeGridASCII (int nr_points, //////////////////////////////////////////////////////////////////////////////////////// int -pcl::PLYWriter::writeBinary (const std::string &file_name, - const pcl::PCLPointCloud2 &cloud, - const Eigen::Vector4f &origin, - const Eigen::Quaternionf &orientation, +pcl::PLYWriter::writeBinary (const std::string& file_name, + const pcl::PCLPointCloud2& cloud, + const Eigen::Vector4f& origin, + const Eigen::Quaternionf& orientation, bool use_camera) { if (cloud.data.empty ()) @@ -1209,14 +1209,36 @@ pcl::PLYWriter::writeBinary (const std::string &file_name, } std::ofstream fs; - fs.open (file_name.c_str ()); // Open file + fs.open (file_name.c_str (), + std::ios::out | std::ios::binary | + std::ios::trunc); // Open file in binary mode and erase current contents + // if any if (!fs) { - PCL_ERROR ("[pcl::PLYWriter::writeBinary] Error during opening (%s)!\n", file_name.c_str ()); + PCL_ERROR ("[pcl::PLYWriter::writeBinary] Error during opening (%s)!\n", + file_name.c_str ()); return (-1); } - unsigned int nr_points = cloud.width * cloud.height; + if (!(writeBinary (fs, cloud, origin, orientation, use_camera) == 0)) + { + fs.close(); + return -1; + } + fs.close(); + return 0; +} + +int +pcl::PLYWriter::writeBinary (std::ostream& os, + const pcl::PCLPointCloud2& cloud, + const Eigen::Vector4f& origin, + const Eigen::Quaternionf& orientation, + bool use_camera) +{ + os.imbue(std::locale::classic()); + + unsigned int nr_points = cloud.width * cloud.height; // Compute the range_grid, if necessary, and then write out the PLY header bool doRangeGrid = !use_camera && cloud.height > 1; @@ -1233,17 +1255,17 @@ pcl::PLYWriter::writeBinary (const std::string &file_name, // If no x-coordinate field exists, then assume all points are valid if (xfield < 0) { - for (unsigned int i=0; i < nr_points; ++i) + for (unsigned int i = 0; i < nr_points; ++i) rangegrid[i] = i; valid_points = nr_points; } // Otherwise, look at their x-coordinates to determine if points are valid else { - for (std::size_t i=0; i < nr_points; ++i) + for (std::size_t i = 0; i < nr_points; ++i) { - const float& value = cloud.at(i, cloud.fields[xfield].offset); - if (std::isfinite(value)) + const float& value = cloud.at (i, cloud.fields[xfield].offset); + if (std::isfinite (value)) { rangegrid[i] = valid_points; ++valid_points; @@ -1252,21 +1274,11 @@ pcl::PLYWriter::writeBinary (const std::string &file_name, rangegrid[i] = -1; } } - fs << generateHeader (cloud, origin, orientation, true, use_camera, valid_points); + os << generateHeader (cloud, origin, orientation, true, use_camera, valid_points); } else { - fs << generateHeader (cloud, origin, orientation, true, use_camera, nr_points); - } - - // Close the file - fs.close (); - // Open file in binary appendable - std::ofstream fpout (file_name.c_str (), std::ios::app | std::ios::binary); - if (!fpout) - { - PCL_ERROR ("[pcl::PLYWriter::writeBinary] Error during reopening (%s)!\n", file_name.c_str ()); - return (-1); + os << generateHeader (cloud, origin, orientation, true, use_camera, nr_points); } // Iterate through the points @@ -1281,16 +1293,17 @@ pcl::PLYWriter::writeBinary (const std::string &file_name, { int count = cloud.fields[d].count; if (count == 0) - count = 1; //workaround + count = 1; // workaround if (count > 1) { static unsigned int ucount (count); - fpout.write (reinterpret_cast (&ucount), sizeof (unsigned int)); + os.write (reinterpret_cast (&ucount), sizeof (unsigned int)); } // Ignore invalid padded dimensions that are inherited from binary data if (cloud.fields[d].name == "_") { - total += cloud.fields[d].count; // jump over this many elements in the string token + total += + cloud.fields[d].count; // jump over this many elements in the string token continue; } @@ -1298,70 +1311,93 @@ pcl::PLYWriter::writeBinary (const std::string &file_name, { switch (cloud.fields[d].datatype) { - case pcl::PCLPointField::INT8: - { - fpout.write (&cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (char)), sizeof (char)); - break; - } - case pcl::PCLPointField::UINT8: - { - fpout.write (reinterpret_cast (&cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (unsigned char))), sizeof (unsigned char)); - break; - } - case pcl::PCLPointField::INT16: - { - fpout.write (reinterpret_cast (&cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (short))), sizeof (short)); - break; - } - case pcl::PCLPointField::UINT16: - { - fpout.write (reinterpret_cast (&cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (unsigned short))), sizeof (unsigned short)); - break; - } - case pcl::PCLPointField::INT32: + case pcl::PCLPointField::INT8: + { + os.write ( + &cloud.at (i, cloud.fields[d].offset + (total + c) * sizeof (char)), + sizeof (char)); + break; + } + case pcl::PCLPointField::UINT8: + { + os.write ( + reinterpret_cast (&cloud.at ( + i, cloud.fields[d].offset + (total + c) * sizeof (unsigned char))), + sizeof (unsigned char)); + break; + } + case pcl::PCLPointField::INT16: + { + os.write (reinterpret_cast (&cloud.at ( + i, cloud.fields[d].offset + (total + c) * sizeof (short))), + sizeof (short)); + break; + } + case pcl::PCLPointField::UINT16: + { + os.write ( + reinterpret_cast (&cloud.at ( + i, cloud.fields[d].offset + (total + c) * sizeof (unsigned short))), + sizeof (unsigned short)); + break; + } + case pcl::PCLPointField::INT32: + { + os.write (reinterpret_cast (&cloud.at ( + i, cloud.fields[d].offset + (total + c) * sizeof (int))), + sizeof (int)); + break; + } + case pcl::PCLPointField::UINT32: + { + if (cloud.fields[d].name.find ("rgba") == std::string::npos) { - fpout.write (reinterpret_cast (&cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (int))), sizeof (int)); - break; + os.write ( + reinterpret_cast (&cloud.at ( + i, cloud.fields[d].offset + (total + c) * sizeof (unsigned int))), + sizeof (unsigned int)); } - case pcl::PCLPointField::UINT32: + else { - if (cloud.fields[d].name.find ("rgba") == std::string::npos) - { - fpout.write (reinterpret_cast (&cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (unsigned int))), sizeof (unsigned int)); - } - else - { - const auto& color = cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (pcl::RGB)); - fpout.write (reinterpret_cast (&color.r), sizeof (unsigned char)); - fpout.write (reinterpret_cast (&color.g), sizeof (unsigned char)); - fpout.write (reinterpret_cast (&color.b), sizeof (unsigned char)); - fpout.write (reinterpret_cast (&color.a), sizeof (unsigned char)); - } - break; + const auto& color = cloud.at ( + i, cloud.fields[d].offset + (total + c) * sizeof (pcl::RGB)); + os.write (reinterpret_cast (&color.r), sizeof (unsigned char)); + os.write (reinterpret_cast (&color.g), sizeof (unsigned char)); + os.write (reinterpret_cast (&color.b), sizeof (unsigned char)); + os.write (reinterpret_cast (&color.a), sizeof (unsigned char)); } - case pcl::PCLPointField::FLOAT32: + break; + } + case pcl::PCLPointField::FLOAT32: + { + if (cloud.fields[d].name.find ("rgb") == std::string::npos) { - if (cloud.fields[d].name.find ("rgb") == std::string::npos) - { - fpout.write (reinterpret_cast (&cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (float))), sizeof (float)); - } - else - { - const auto& color = cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (pcl::RGB)); - fpout.write (reinterpret_cast (&color.r), sizeof (unsigned char)); - fpout.write (reinterpret_cast (&color.g), sizeof (unsigned char)); - fpout.write (reinterpret_cast (&color.b), sizeof (unsigned char)); - } - break; + os.write (reinterpret_cast (&cloud.at ( + i, cloud.fields[d].offset + (total + c) * sizeof (float))), + sizeof (float)); } - case pcl::PCLPointField::FLOAT64: + else { - fpout.write (reinterpret_cast (&cloud.at(i, cloud.fields[d].offset + (total + c) * sizeof (double))), sizeof (double)); - break; + const auto& color = cloud.at ( + i, cloud.fields[d].offset + (total + c) * sizeof (pcl::RGB)); + os.write (reinterpret_cast (&color.r), sizeof (unsigned char)); + os.write (reinterpret_cast (&color.g), sizeof (unsigned char)); + os.write (reinterpret_cast (&color.b), sizeof (unsigned char)); } - default: - PCL_WARN ("[pcl::PLYWriter::writeBinary] Incorrect field data type specified (%d)!\n", cloud.fields[d].datatype); - break; + break; + } + case pcl::PCLPointField::FLOAT64: + { + os.write (reinterpret_cast (&cloud.at ( + i, cloud.fields[d].offset + (total + c) * sizeof (double))), + sizeof (double)); + break; + } + default: + PCL_WARN ("[pcl::PLYWriter::writeBinary] Incorrect field data type specified " + "(%d)!\n", + cloud.fields[d].datatype); + break; } } } @@ -1374,17 +1410,17 @@ pcl::PLYWriter::writeBinary (const std::string &file_name, for (int i = 0; i < 3; ++i) { if (origin[3] != 0) - t = origin[i]/origin[3]; + t = origin[i] / origin[3]; else t = origin[i]; - fpout.write (reinterpret_cast (&t), sizeof (float)); + os.write (reinterpret_cast (&t), sizeof (float)); } Eigen::Matrix3f R = orientation.toRotationMatrix (); for (int i = 0; i < 3; ++i) for (int j = 0; j < 3; ++j) - { - fpout.write (reinterpret_cast (&R (i, j)),sizeof (float)); - } + { + os.write (reinterpret_cast (&R (i, j)), sizeof (float)); + } ///////////////////////////////////////////////////// // Append those properties directly. // @@ -1402,41 +1438,44 @@ pcl::PLYWriter::writeBinary (const std::string &file_name, const float zerof = 0; for (int i = 0; i < 5; ++i) - fpout.write (reinterpret_cast (&zerof), sizeof (float)); + os.write (reinterpret_cast (&zerof), sizeof (float)); // width and height int width = cloud.width; - fpout.write (reinterpret_cast (&width), sizeof (int)); + os.write (reinterpret_cast (&width), sizeof (int)); int height = cloud.height; - fpout.write (reinterpret_cast (&height), sizeof (int)); + os.write (reinterpret_cast (&height), sizeof (int)); for (int i = 0; i < 2; ++i) - fpout.write (reinterpret_cast (&zerof), sizeof (float)); + os.write (reinterpret_cast (&zerof), sizeof (float)); } else if (doRangeGrid) { // Write out range_grid - for (std::size_t i=0; i < nr_points; ++i) + for (std::size_t i = 0; i < nr_points; ++i) { pcl::io::ply::uint8 listlen; if (rangegrid[i] >= 0) { listlen = 1; - fpout.write (reinterpret_cast (&listlen), sizeof (pcl::io::ply::uint8)); - fpout.write (reinterpret_cast (&rangegrid[i]), sizeof (pcl::io::ply::int32)); + os.write (reinterpret_cast (&listlen), + sizeof (pcl::io::ply::uint8)); + os.write (reinterpret_cast (&rangegrid[i]), + sizeof (pcl::io::ply::int32)); } else { listlen = 0; - fpout.write (reinterpret_cast (&listlen), sizeof (pcl::io::ply::uint8)); + os.write (reinterpret_cast (&listlen), + sizeof (pcl::io::ply::uint8)); } } } // Close file - fpout.close (); + return (0); }