Skip to content

Commit

Permalink
Allow type conversion in fromPCLPointCloud2
Browse files Browse the repository at this point in the history
  • Loading branch information
@mvieth
mvieth committed May 30, 2024
1 parent a541c5c commit 9cdf0ec
Show file tree
Hide file tree
Showing 2 changed files with 145 additions and 1 deletion.
70 changes: 69 additions & 1 deletion common/include/pcl/conversions.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,7 +104,7 @@ namespace pcl
}
}
// Disable thrown exception per #595: http://dev.pointclouds.org/issues/595
PCL_WARN ("Failed to find match for field '%s'.\n", pcl::traits::name<PointT, Tag>::value);
PCL_WARN ("Failed to find exact match for field '%s'.\n", pcl::traits::name<PointT, Tag>::value);
//throw pcl::InvalidConversionException (ss.str ());
}

Expand All @@ -118,6 +118,71 @@ namespace pcl
return (a.serialized_offset < b.serialized_offset);
}

// Helps converting PCLPointCloud2 to templated point cloud. Casts fields if datatype is different.
template<typename PointT>
struct FieldCaster
{
FieldCaster (const std::vector<pcl::PCLPointField>& fields, const pcl::PCLPointCloud2& msg, const std::uint8_t* msg_data, std::uint8_t* cloud_data)
: fields_ (fields), msg_(msg), msg_data_(msg_data), cloud_data_(cloud_data)
{}

template<typename Tag> void
operator () ()
{
// first check whether any field matches exactly. Then there is nothing to do because the contents are memcpy-ed elsewhere
for (const auto& field : fields_) {
if (FieldMatches<PointT, Tag>()(field)) {
return;
}
}
for (const auto& field : fields_)
{
// The following check is similar to FieldMatches, but it tests for different datatypes
if ((field.name == pcl::traits::name<PointT, Tag>::value) &&
(field.datatype != pcl::traits::datatype<PointT, Tag>::value) &&
((field.count == pcl::traits::datatype<PointT, Tag>::size) ||
(field.count == 0 && pcl::traits::datatype<PointT, Tag>::size == 1))) {
#define PCL_CAST_POINT_FIELD(TYPE) case ::pcl::traits::asEnum_v<TYPE>: \
PCL_WARN("Will try to cast field '%s' (original type is " #TYPE "). You may loose precision during casting. Make sure that this is acceptable or choose a different point type.\n", pcl::traits::name<PointT, Tag>::value); \
for (std::size_t row = 0; row < msg_.height; ++row) { \
const std::uint8_t* row_data = msg_data_ + row * msg_.row_step; \
for (std::size_t col = 0; col < msg_.width; ++col) { \
const std::uint8_t* msg_data = row_data + col * msg_.point_step; \
for(std::uint32_t i=0; i<pcl::traits::datatype<PointT, Tag>::size; ++i) { \
*(reinterpret_cast<typename pcl::traits::datatype<PointT, Tag>::decomposed::type*>(cloud_data + pcl::traits::offset<PointT, Tag>::value) + i) = *(reinterpret_cast<const TYPE*>(msg_data + field.offset) + i); \
} \
cloud_data += sizeof (PointT); \
} \
} \
break;
// end of PCL_CAST_POINT_FIELD definition

std::uint8_t* cloud_data = cloud_data_;
switch(field.datatype) {
PCL_CAST_POINT_FIELD(bool)
PCL_CAST_POINT_FIELD(std::int8_t)
PCL_CAST_POINT_FIELD(std::uint8_t)
PCL_CAST_POINT_FIELD(std::int16_t)
PCL_CAST_POINT_FIELD(std::uint16_t)
PCL_CAST_POINT_FIELD(std::int32_t)
PCL_CAST_POINT_FIELD(std::uint32_t)
PCL_CAST_POINT_FIELD(std::int64_t)
PCL_CAST_POINT_FIELD(std::uint64_t)
PCL_CAST_POINT_FIELD(float)
PCL_CAST_POINT_FIELD(double)
default: std::cout << "Unknown datatype: " << field.datatype << std::endl;
}
return;
}
#undef PCL_CAST_POINT_FIELD
}
}

const std::vector<pcl::PCLPointField>& fields_;
const pcl::PCLPointCloud2& msg_;
const std::uint8_t* msg_data_;
std::uint8_t* cloud_data_;
};
} //namespace detail

template<typename PointT> void
Expand Down Expand Up @@ -223,6 +288,9 @@ namespace pcl
}
}
}
// if any fields in msg and cloud have different datatypes but the same name, we cast them:
detail::FieldCaster<PointT> caster (msg.fields, msg, msg_data, reinterpret_cast<std::uint8_t*>(cloud.data()));
for_each_type< typename traits::fieldList<PointT>::type > (caster);
}

/** \brief Convert a PCLPointCloud2 binary data blob into a pcl::PointCloud<T> object using a field_map.
Expand Down
76 changes: 76 additions & 0 deletions test/common/test_io.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -296,6 +296,82 @@ TEST (toPCLPointCloud2NoPadding, PointXYZI)
}
}

TEST(PCL, fromPCLPointCloud2CastingXYZI)
{
// test fromPCLPointCloud2, but in PCLPointCloud2 the fields have different types than in PointXYZI
pcl::PCLPointCloud2 msg;
msg.height = 2;
msg.width = 2;
msg.fields.resize(4);
msg.fields[0].name = "x";
msg.fields[0].offset = 0;
msg.fields[0].datatype = pcl::PCLPointField::FLOAT64;
msg.fields[0].count = 1;
msg.fields[1].name = "y";
msg.fields[1].offset = 8;
msg.fields[1].datatype = pcl::PCLPointField::FLOAT64;
msg.fields[1].count = 1;
msg.fields[2].name = "z";
msg.fields[2].offset = 16;
msg.fields[2].datatype = pcl::PCLPointField::FLOAT64;
msg.fields[2].count = 1;
msg.fields[3].name = "intensity";
msg.fields[3].offset = 24;
msg.fields[3].datatype = pcl::PCLPointField::UINT16;
msg.fields[3].count = 1;
msg.point_step = 32;
msg.row_step = 32*msg.width;
msg.data.resize(32*msg.width*msg.height);
for(std::size_t i=0; i<msg.width*msg.height; ++i) {
msg.at<double>(i, 0) = 1.0*i;
msg.at<double>(i, 8) = -1.6*i;
msg.at<double>(i, 16) = -3.141*i;
msg.at<std::uint16_t>(i, 24) = 123*i;
}
pcl::PointCloud<pcl::PointXYZI> cloud;
pcl::fromPCLPointCloud2(msg, cloud);
for(std::size_t i=0; i<msg.width*msg.height; ++i) {
EXPECT_EQ(cloud[i].x, 1.0f*i);
EXPECT_EQ(cloud[i].y, -1.6f*i);
EXPECT_EQ(cloud[i].z, -3.141f*i);
EXPECT_EQ(cloud[i].intensity, 123.0f*i);
}
}

TEST(PCL, fromPCLPointCloud2CastingSHOT352)
{
// test whether casting also works if point type contains arrays
pcl::PCLPointCloud2 msg;
msg.height = 2;
msg.width = 2;
msg.fields.resize(2);
msg.fields[0].name = "shot";
msg.fields[0].offset = 0;
msg.fields[0].datatype = pcl::PCLPointField::INT64;
msg.fields[0].count = 352;
msg.fields[1].name = "rf";
msg.fields[1].offset = 352*8;
msg.fields[1].datatype = pcl::PCLPointField::FLOAT64;
msg.fields[1].count = 9;
msg.point_step = (352*8+9*8);
msg.row_step = msg.point_step*msg.width;
msg.data.resize(msg.point_step*msg.width*msg.height);
for(std::size_t i=0; i<msg.width*msg.height; ++i) {
for(std::size_t j=0; j<352; ++j)
msg.at<std::int64_t>(i, 8*j) = 1000*i+j;
for(std::size_t j=0; j<9; ++j)
msg.at<double>(i, 352*8+8*j) = (10*i+j)/10.0;
}
pcl::PointCloud<pcl::SHOT352> cloud;
pcl::fromPCLPointCloud2(msg, cloud);
for(std::size_t i=0; i<msg.width*msg.height; ++i) {
for(std::size_t j=0; j<352; ++j)
EXPECT_EQ(cloud[i].descriptor[j], 1000*i+j);
for(std::size_t j=0; j<9; ++j)
EXPECT_EQ(cloud[i].rf[j], (10*i+j)/10.0f);
}
}

/* ---[ */
int
main (int argc, char** argv)
Expand Down

0 comments on commit 9cdf0ec

Please sign in to comment.