Skip to content

Commit

Permalink
Add PLYwriter::writeBinary ostream overload (PointCloudLibrary#5975)
Browse files Browse the repository at this point in the history 
* 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 <[email protected]>

* Modified PLYWriter::writeBinary to use std::ostream instead of std::ofstream

* Changed pclWriter::writeBinary ostream variable name to os

---------

Co-authored-by: Markus Vieth <[email protected]>
  • Loading branch information
@kaipomauli @mvieth
kaipomauli and mvieth authored Mar 5, 2024
1 parent d39bfc3 commit aa05e54
Show file tree
Hide file tree
Showing 2 changed files with 148 additions and 97 deletions.
14 changes: 13 additions & 1 deletion io/include/pcl/io/ply_io.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
231 changes: 135 additions & 96 deletions io/src/ply_io.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 ())
Expand All @@ -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;
Expand All @@ -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<float>(i, cloud.fields[xfield].offset);
if (std::isfinite(value))
const float& value = cloud.at<float> (i, cloud.fields[xfield].offset);
if (std::isfinite (value))
{
rangegrid[i] = valid_points;
++valid_points;
Expand All @@ -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
Expand All @@ -1281,87 +1293,111 @@ 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<const char*> (&ucount), sizeof (unsigned int));
os.write (reinterpret_cast<const char*> (&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;
}

for (int c = 0; c < count; ++c)
{
switch (cloud.fields[d].datatype)
{
case pcl::PCLPointField::INT8:
{
fpout.write (&cloud.at<char>(i, cloud.fields[d].offset + (total + c) * sizeof (char)), sizeof (char));
break;
}
case pcl::PCLPointField::UINT8:
{
fpout.write (reinterpret_cast<const char*> (&cloud.at<unsigned char>(i, cloud.fields[d].offset + (total + c) * sizeof (unsigned char))), sizeof (unsigned char));
break;
}
case pcl::PCLPointField::INT16:
{
fpout.write (reinterpret_cast<const char*> (&cloud.at<short>(i, cloud.fields[d].offset + (total + c) * sizeof (short))), sizeof (short));
break;
}
case pcl::PCLPointField::UINT16:
{
fpout.write (reinterpret_cast<const char*> (&cloud.at<unsigned short>(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<char> (i, cloud.fields[d].offset + (total + c) * sizeof (char)),
sizeof (char));
break;
}
case pcl::PCLPointField::UINT8:
{
os.write (
reinterpret_cast<const char*> (&cloud.at<unsigned char> (
i, cloud.fields[d].offset + (total + c) * sizeof (unsigned char))),
sizeof (unsigned char));
break;
}
case pcl::PCLPointField::INT16:
{
os.write (reinterpret_cast<const char*> (&cloud.at<short> (
i, cloud.fields[d].offset + (total + c) * sizeof (short))),
sizeof (short));
break;
}
case pcl::PCLPointField::UINT16:
{
os.write (
reinterpret_cast<const char*> (&cloud.at<unsigned short> (
i, cloud.fields[d].offset + (total + c) * sizeof (unsigned short))),
sizeof (unsigned short));
break;
}
case pcl::PCLPointField::INT32:
{
os.write (reinterpret_cast<const char*> (&cloud.at<int> (
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<const char*> (&cloud.at<int>(i, cloud.fields[d].offset + (total + c) * sizeof (int))), sizeof (int));
break;
os.write (
reinterpret_cast<const char*> (&cloud.at<unsigned int> (
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<const char*> (&cloud.at<unsigned int>(i, cloud.fields[d].offset + (total + c) * sizeof (unsigned int))), sizeof (unsigned int));
}
else
{
const auto& color = cloud.at<pcl::RGB>(i, cloud.fields[d].offset + (total + c) * sizeof (pcl::RGB));
fpout.write (reinterpret_cast<const char*> (&color.r), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&color.g), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&color.b), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&color.a), sizeof (unsigned char));
}
break;
const auto& color = cloud.at<pcl::RGB> (
i, cloud.fields[d].offset + (total + c) * sizeof (pcl::RGB));
os.write (reinterpret_cast<const char*> (&color.r), sizeof (unsigned char));
os.write (reinterpret_cast<const char*> (&color.g), sizeof (unsigned char));
os.write (reinterpret_cast<const char*> (&color.b), sizeof (unsigned char));
os.write (reinterpret_cast<const char*> (&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<const char*> (&cloud.at<float>(i, cloud.fields[d].offset + (total + c) * sizeof (float))), sizeof (float));
}
else
{
const auto& color = cloud.at<pcl::RGB>(i, cloud.fields[d].offset + (total + c) * sizeof (pcl::RGB));
fpout.write (reinterpret_cast<const char*> (&color.r), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&color.g), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&color.b), sizeof (unsigned char));
}
break;
os.write (reinterpret_cast<const char*> (&cloud.at<float> (
i, cloud.fields[d].offset + (total + c) * sizeof (float))),
sizeof (float));
}
case pcl::PCLPointField::FLOAT64:
else
{
fpout.write (reinterpret_cast<const char*> (&cloud.at<double>(i, cloud.fields[d].offset + (total + c) * sizeof (double))), sizeof (double));
break;
const auto& color = cloud.at<pcl::RGB> (
i, cloud.fields[d].offset + (total + c) * sizeof (pcl::RGB));
os.write (reinterpret_cast<const char*> (&color.r), sizeof (unsigned char));
os.write (reinterpret_cast<const char*> (&color.g), sizeof (unsigned char));
os.write (reinterpret_cast<const char*> (&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<const char*> (&cloud.at<double> (
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;
}
}
}
Expand All @@ -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<const char*> (&t), sizeof (float));
os.write (reinterpret_cast<const char*> (&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<const char*> (&R (i, j)),sizeof (float));
}
{
os.write (reinterpret_cast<const char*> (&R (i, j)), sizeof (float));
}

/////////////////////////////////////////////////////
// Append those properties directly. //
Expand All @@ -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<const char*> (&zerof), sizeof (float));
os.write (reinterpret_cast<const char*> (&zerof), sizeof (float));

// width and height
int width = cloud.width;
fpout.write (reinterpret_cast<const char*> (&width), sizeof (int));
os.write (reinterpret_cast<const char*> (&width), sizeof (int));

int height = cloud.height;
fpout.write (reinterpret_cast<const char*> (&height), sizeof (int));
os.write (reinterpret_cast<const char*> (&height), sizeof (int));

for (int i = 0; i < 2; ++i)
fpout.write (reinterpret_cast<const char*> (&zerof), sizeof (float));
os.write (reinterpret_cast<const char*> (&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<const char*> (&listlen), sizeof (pcl::io::ply::uint8));
fpout.write (reinterpret_cast<const char*> (&rangegrid[i]), sizeof (pcl::io::ply::int32));
os.write (reinterpret_cast<const char*> (&listlen),
sizeof (pcl::io::ply::uint8));
os.write (reinterpret_cast<const char*> (&rangegrid[i]),
sizeof (pcl::io::ply::int32));
}
else
{
listlen = 0;
fpout.write (reinterpret_cast<const char*> (&listlen), sizeof (pcl::io::ply::uint8));
os.write (reinterpret_cast<const char*> (&listlen),
sizeof (pcl::io::ply::uint8));
}
}
}

// Close file
fpout.close ();

return (0);
}

Expand Down

0 comments on commit aa05e54

Please sign in to comment.