Skip to content

Commit

Permalink
Fix: issue 959 loading PointXYZRGBL
Browse files Browse the repository at this point in the history 
rgba is uint32_t while rgb is float



git-svn-id: svn+ssh://svn.pointclouds.org/pcl/trunk@8687 a9d63959-f2ad-4865-b262-bf0e56cfafb6
  • Loading branch information
Nizar Sallem committed Feb 13, 2013
1 parent cf5d9c4 commit dbed646
Show file tree
Hide file tree
Showing 2 changed files with 25 additions and 49 deletions.
2 changes: 1 addition & 1 deletion io/include/pcl/io/ply_io.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -415,7 +415,7 @@ namespace pcl
* param[in] new_name property new name
*/
void
amendFloatProperty (const std::string& old_name, const std::string& new_name);
amendProperty (const std::string& old_name, const std::string& new_name, uint8_t datatype = 0);

/** Append an unsigned int property to the cloud fields.
* param[in] name property name
Expand Down
72 changes: 24 additions & 48 deletions io/src/ply_io.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -112,14 +112,16 @@ pcl::PLYReader::appendFloatProperty (const std::string& name, const size_t& size
}

void
pcl::PLYReader::amendFloatProperty (const std::string& old_name, const std::string& new_name)
pcl::PLYReader::amendProperty (const std::string& old_name, const std::string& new_name, uint8_t new_datatype)
{
std::vector< ::sensor_msgs::PointField>::reverse_iterator finder = cloud_->fields.rbegin ();
for (; finder != cloud_->fields.rend (); ++finder)
if (finder->name == old_name)
break;
assert (finder != cloud_->fields.rend ());
finder->name = new_name;
if (new_datatype > 0 && new_datatype != finder->datatype)
finder->datatype = new_datatype;
}

void
Expand Down Expand Up @@ -220,7 +222,7 @@ namespace pcl
}
else if (property_name == "alpha")
{
amendFloatProperty ("rgb", "rgba");
amendProperty ("rgb", "rgba", sensor_msgs::PointField::UINT32);
return boost::bind (&pcl::PLYReader::vertexAlphaCallback, this, _1);
}
else if (property_name == "intensity")
Expand Down Expand Up @@ -329,26 +331,26 @@ pcl::PLYReader::vertexColorCallback (const std::string& color_name, pcl::io::ply
int32_t rgb = r << 16 | g << 8 | b;
memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + rgb_offset_before_],
&rgb,
sizeof (int32_t));
vertex_offset_before_ += static_cast<int> (sizeof (int32_t));
sizeof (pcl::io::ply::float32));
vertex_offset_before_ += static_cast<int> (sizeof (pcl::io::ply::float32));
}
}

void
pcl::PLYReader::vertexAlphaCallback (pcl::io::ply::uint8 alpha)
{
int32_t a (alpha);
static uint32_t a, rgba;
a = uint32_t (alpha);
// get anscient rgb value and store it in rgba
int32_t rgba;
memcpy (&rgba,
&cloud_->data[vertex_count_ * cloud_->point_step + vertex_offset_before_],
memcpy (&rgba,
&cloud_->data[vertex_count_ * cloud_->point_step + rgb_offset_before_],
sizeof (pcl::io::ply::float32));
// shift left and append alpha
rgba = rgba << 8 | a;
// append alpha
rgba = rgba | a << 24;
// put rgba back
memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + rgb_offset_before_],
&rgba,
sizeof (int32_t));
memcpy (&cloud_->data[vertex_count_ * cloud_->point_step + rgb_offset_before_],
&rgba,
sizeof (uint32_t));
}

void
Expand Down Expand Up @@ -784,7 +786,7 @@ pcl::PLYWriter::writeContentWithCameraASCII (int nr_points,
}
case sensor_msgs::PointField::UINT32:
{
if (cloud.fields[d].name.find ("rgb") == std::string::npos)
if (cloud.fields[d].name.find ("rgba") == std::string::npos)
{
unsigned int value;
memcpy (&value, &cloud.data[i * point_size + cloud.fields[d].offset + c * sizeof (unsigned int)], sizeof (unsigned int));
Expand All @@ -797,12 +799,8 @@ pcl::PLYWriter::writeContentWithCameraASCII (int nr_points,
int r = color.r;
int g = color.g;
int b = color.b;
fs << r << " " << g << " " << b;
if (cloud.fields[d].name.find ("rgba") != std::string::npos)
{
int a = color.a;
fs << " " << a;
}
int a = color.a;
fs << r << " " << g << " " << b << " " << a;
}
break;
}
Expand All @@ -822,11 +820,6 @@ pcl::PLYWriter::writeContentWithCameraASCII (int nr_points,
int g = color.g;
int b = color.b;
fs << r << " " << g << " " << b;
if (cloud.fields[d].name.find ("rgba") != std::string::npos)
{
int a = color.a;
fs << " " << a;
}
}
break;
}
Expand Down Expand Up @@ -932,7 +925,7 @@ pcl::PLYWriter::writeContentWithRangeGridASCII (int nr_points,
}
case sensor_msgs::PointField::UINT32:
{
if (cloud.fields[d].name.find ("rgb") == std::string::npos)
if (cloud.fields[d].name.find ("rgba") == std::string::npos)
{
unsigned int value;
memcpy (&value, &cloud.data[i * point_size + cloud.fields[d].offset + c * sizeof (unsigned int)], sizeof (unsigned int));
Expand All @@ -945,12 +938,8 @@ pcl::PLYWriter::writeContentWithRangeGridASCII (int nr_points,
int r = color.r;
int g = color.g;
int b = color.b;
line << r << " " << g << " " << b;
if (cloud.fields[d].name.find ("rgba") != std::string::npos)
{
int a = color.a;
line << " " << a;
}
int a = color.a;
line << r << " " << g << " " << b << " " << a;
}
break;
}
Expand All @@ -976,11 +965,6 @@ pcl::PLYWriter::writeContentWithRangeGridASCII (int nr_points,
int g = color.g;
int b = color.b;
line << r << " " << g << " " << b;
if (cloud.fields[d].name.find ("rgba") != std::string::npos)
{
int a = color.a;
line << " " << a;
}
}
break;
}
Expand Down Expand Up @@ -1164,7 +1148,7 @@ pcl::PLYWriter::writeBinary (const std::string &file_name,
}
case sensor_msgs::PointField::UINT32:
{
if (cloud.fields[d].name.find ("rgb") == std::string::npos)
if (cloud.fields[d].name.find ("rgba") == std::string::npos)
{
unsigned int value;
memcpy (&value, &cloud.data[i * point_size + cloud.fields[d].offset + (total + c) * sizeof (unsigned int)], sizeof (unsigned int));
Expand All @@ -1177,14 +1161,11 @@ pcl::PLYWriter::writeBinary (const std::string &file_name,
unsigned char r = color.r;
unsigned char g = color.g;
unsigned char b = color.b;
unsigned char a = color.a;
fpout.write (reinterpret_cast<const char*> (&r), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&g), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&b), sizeof (unsigned char));
if (cloud.fields[d].name.find ("rgba") != std::string::npos)
{
unsigned char a = color.a;
fpout.write (reinterpret_cast<const char*> (&a), sizeof (unsigned char));
}
fpout.write (reinterpret_cast<const char*> (&a), sizeof (unsigned char));
}
break;
}
Expand All @@ -1206,11 +1187,6 @@ pcl::PLYWriter::writeBinary (const std::string &file_name,
fpout.write (reinterpret_cast<const char*> (&r), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&g), sizeof (unsigned char));
fpout.write (reinterpret_cast<const char*> (&b), sizeof (unsigned char));
if (cloud.fields[d].name.find ("rgba") != std::string::npos)
{
unsigned char a = color.a;
fpout.write (reinterpret_cast<const char*> (&a), sizeof (unsigned char));
}
}
break;
}
Expand Down

0 comments on commit dbed646

Please sign in to comment.