Skip to content

Commit

Permalink
Added documentation and sqeeze related features in PCLPointCloud2
Browse files Browse the repository at this point in the history 
Added concatenate free function in io.h
Added concatenate static function in PointCloud<PointT>
  • Loading branch information
@kunaltyagi
kunaltyagi committed Sep 4, 2019
1 parent 54235f4 commit 0e8fac0
Show file tree
Hide file tree
Showing 5 changed files with 196 additions and 55 deletions.
15 changes: 15 additions & 0 deletions common/include/pcl/PCLPointCloud2.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,22 @@ namespace pcl
using ConstPtr = boost::shared_ptr<const ::pcl::PCLPointCloud2>;

//////////////////////////////////////////////////////////////////////////
/** \brief Removes fields named "_"
* \return true if any fields were removed, false otherwise
*/
bool
squeeze ();

/** \brief Inplace concatenate two pcl::PCLPointCloud2
*
* This command doesn't remove any fields named "_" (aka marked as skip)
* IFF the layout of all the fields in both the clouds is the same. However, if
* the order of skip fields is different, relevant fields are selctively copied.
* It considers it an error
* * if the total number of fields to copy after discounting the skip fields
* is different
* * if the non-skip field names in serial order are named differently
* * if the endian-ness of both clouds is different
* \param[in,out] cloud1 the first input and output point cloud dataset
* \param[in] cloud2 the second input point cloud dataset
* \return true if successful, false if failed (e.g., name/number of fields differs)
Expand Down
28 changes: 27 additions & 1 deletion common/include/pcl/common/io.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -244,13 +244,39 @@ namespace pcl
PCL_EXPORTS int
interpolatePointIndex (int p, int length, InterpolationType type);

/** \brief Concatenate two pcl::PCLPointCloud2.
/** \brief Concatenate two pcl::PointCloud<PointT>
* \param[in] cloud1 the first input point cloud dataset
* \param[in] cloud2 the second input point cloud dataset
* \param[out] cloud_out the resultant output point cloud dataset
* \return true if successful, false if failed
* \ingroup common
*/
template <typename PointT>
PCL_EXPORTS bool
concatenate (const pcl::PointCloud<PointT> &cloud1,
const pcl::PointCloud<PointT> &cloud2,
pcl::PointCloud<PointT> &cloud_out);

/** \brief Concatenate two pcl::PCLPointCloud2
* \param[in] cloud1 the first input point cloud dataset
* \param[in] cloud2 the second input point cloud dataset
* \param[out] cloud_out the resultant output point cloud dataset
* \return true if successful, false if failed
* \ingroup common
*/
PCL_EXPORTS bool
concatenate (const pcl::PCLPointCloud2 &cloud1,
const pcl::PCLPointCloud2 &cloud2,
pcl::PCLPointCloud2 &cloud_out);

/** \brief Concatenate two pcl::PCLPointCloud2
* \param[in] cloud1 the first input point cloud dataset
* \param[in] cloud2 the second input point cloud dataset
* \param[out] cloud_out the resultant output point cloud dataset
* \return true if successful, false if failed (e.g., name/number of fields differs)
* \ingroup common
*/
[[deprecated("use pcl::concatenate() instead")]]
PCL_EXPORTS bool
concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1,
const pcl::PCLPointCloud2 &cloud2,
Expand Down
44 changes: 28 additions & 16 deletions common/include/pcl/point_cloud.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -247,34 +247,46 @@ namespace pcl
inline PointCloud&
operator += (const PointCloud& rhs)
{
// Make the resultant point cloud take the newest stamp
if (rhs.header.stamp > header.stamp)
header.stamp = rhs.header.stamp;

size_t nr_points = points.size ();
points.resize (nr_points + rhs.points.size ());
for (size_t i = nr_points; i < points.size (); ++i)
points[i] = rhs.points[i - nr_points];

width = static_cast<uint32_t>(points.size ());
height = 1;
if (rhs.is_dense && is_dense)
is_dense = true;
else
is_dense = false;
concatenate((*this), rhs);
return (*this);
}

/** \brief Add a point cloud to another cloud.
* \param[in] rhs the cloud to add to the current cloud
* \return the new cloud as a concatenation of the current cloud and the new given cloud
*/
inline const PointCloud
inline PointCloud
operator + (const PointCloud& rhs)
{
return (PointCloud (*this) += rhs);
}

inline static bool
concatenate(pcl::PointCloud<PointT> &cloud1,
const pcl::PointCloud<PointT> &cloud2)
{
// Make the resultant point cloud take the newest stamp
cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp);

size_t nr_points = cloud1.points.size ();
cloud1.points.reserve (nr_points + cloud2.points.size ());
cloud1.points.insert (cloud1.points.end (), cloud2.points.begin (), cloud2.points.end ());

cloud1.width = static_cast<uint32_t>(cloud1.points.size ());
cloud1.height = 1;
cloud1.is_dense = cloud1.is_dense && cloud2.is_dense;
return true;
}

inline static bool
concatenate(const pcl::PointCloud<PointT> &cloud1,
const pcl::PointCloud<PointT> &cloud2,
pcl::PointCloud<PointT> &cloud_out)
{
cloud_out = cloud1;
return concatenate(cloud_out, cloud2);
}

/** \brief Obtain the point given by the (column, row) coordinates. Only works on organized
* datasets (those that have height != 1).
* \param[in] column the column coordinate
Expand Down
144 changes: 108 additions & 36 deletions common/src/PCLPointCloud2.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -46,8 +46,7 @@
#include <pcl/PCLPointCloud2.h>

bool
pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1,
const pcl::PCLPointCloud2 &cloud2)
pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1, const pcl::PCLPointCloud2 &cloud2)
{
if (cloud1.is_bigendian != cloud2.is_bigendian)
{
Expand All @@ -67,61 +66,78 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1,
case 0:
PCL_FALLTHROUGH;
case 2:
cloud1.header.stamp = std::max(cloud1.header.stamp, cloud2.header.stamp);
cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp);
return (true);
default:
break;
}

bool strip = true;
if (cloud1.fields.size () == cloud2.fields.size ())
{
// potentially we can do a simple memcpy
if (std::equal(cloud1.fields.begin (),
cloud1.fields.end (),
cloud2.fields.begin (),
[](const auto& f1, const auto&f2)
{
return f1.name == f2.name;
}))
{
strip = false;
}
}

struct FieldDetails
{
std::size_t idx1, idx2;
std::uint16_t size;
FieldDetails(std::size_t idx1_, std::size_t idx2_, std::uint16_t size_): idx1(idx1_), idx2(idx2_), size(size_)
FieldDetails (std::size_t idx1_, std::size_t idx2_, std::uint16_t size_): idx1 (idx1_), idx2 (idx2_), size (size_)
{}
};
std::vector<FieldDetails> valid_fields;
const auto max_field_size = std::max(cloud1.fields.size (), cloud2.fields.size ());
valid_fields.reserve(max_field_size);
const auto max_field_size = std::max (cloud1.fields.size (), cloud2.fields.size ());
valid_fields.reserve (max_field_size);

std::size_t i = 0, j = 0;
bool strip = false;
while (i < cloud1.fields.size () && j < cloud2.fields.size ())
// refactor to return std::optional<std::vector<FieldDetails>>
if (strip)
{
if (cloud1.fields[i].name == "_")
std::size_t i = 0, j = 0;
while (i < cloud1.fields.size () && j < cloud2.fields.size ())
{
strip = true;
++i;
continue;
}
if (cloud2.fields[j].name == "_")
{
strip = true;
++j;
continue;
}
if (cloud1.fields[i].name == "_")
{
++i;
continue;
}
if (cloud2.fields[j].name == "_")
{
++j;
continue;
}

// We're fine with the special RGB vs RGBA use case
if ((cloud1.fields[i].name == "rgb" && cloud2.fields[j].name == "rgba") ||
(cloud1.fields[i].name == "rgba" && cloud2.fields[j].name == "rgb") ||
(cloud1.fields[i].name == cloud2.fields[j].name))
// We're fine with the special RGB vs RGBA use case
if ((cloud1.fields[i].name == "rgb" && cloud2.fields[j].name == "rgba") ||
(cloud1.fields[i].name == "rgba" && cloud2.fields[j].name == "rgb") ||
(cloud1.fields[i].name == cloud2.fields[j].name))
{
valid_fields.emplace_back(i, j, pcl::getFieldSize (cloud2.fields[j].datatype));
++i;
++j;
continue;
}
PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ());
return (false);
}
if (i != cloud1.fields.size () && j != cloud2.fields.size ())
{
valid_fields.emplace_back(i, j, pcl::getFieldSize (cloud2.fields[j].datatype));
++i;
++j;
continue;
PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
return (false);
}
PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Name of field %d in cloud1, %s, does not match name in cloud2, %s\n", i, cloud1.fields[i].name.c_str (), cloud2.fields[i].name.c_str ());
return (false);
}
if (i != cloud1.fields.size () && j != cloud2.fields.size ())
{
PCL_ERROR ("[pcl::PCLPointCloud2::concatenate] Number of fields in cloud1 (%u) != Number of fields in cloud2 (%u)\n", cloud1.fields.size (), cloud2.fields.size ());
return (false);
}

// It's safe to modify the cloud1 inplace
cloud1.header.stamp = std::max(cloud1.header.stamp, cloud2.header.stamp);
cloud1.header.stamp = std::max (cloud1.header.stamp, cloud2.header.stamp);

cloud1.is_dense = cloud1.is_dense && cloud2.is_dense;
cloud1.height = 1;
Expand Down Expand Up @@ -151,6 +167,62 @@ pcl::PCLPointCloud2::concatenate (pcl::PCLPointCloud2 &cloud1,
return (true);
}

bool
pcl::PCLPointCloud2::squeeze ()
{
const auto size = width * height;
struct offset
{
std::size_t from = 0, to = 0, size = 0;
explicit offset(std::size_t f = 0, std::size_t t = 0, std::size_t s = 0): from(f), to(t), size(s) {}
};
std::vector<offset> memcpy_offsets;
// worst case: every second field needs to be moved
memcpy_offsets.reserve (fields.size ()/2 + 1);

offset running_offset;
bool skipping = false;
for (const auto& field: fields)
{
const auto& size = field.count * pcl::getFieldSize (field.datatype);

if (field.name != "_")
{
skipping = false;
running_offset.size += size;
continue;
}
if (skipping)
{
running_offset.from += size;
continue;
}
skipping = true;
memcpy_offsets.emplace_back(running_offset);
running_offset.from += running_offset.size;
running_offset.to += running_offset.size;
running_offset.size = 0;
}
if (!skipping)
{
memcpy_offsets.emplace_back(running_offset);
}

for (std::size_t cp = 0; cp < size; ++cp)
{
for (const auto& memloc: memcpy_offsets)
{
if (memloc.to == memloc.from)
{
continue;
}
memcpy (reinterpret_cast<char*> (&data[cp * point_step + memloc.to]),
reinterpret_cast<const char*> (&data[cp * point_step + memloc.from]),
memloc.size);
}
}
}

pcl::PCLPointCloud2&
pcl::PCLPointCloud2::operator += (const PCLPointCloud2& rhs)
{
Expand Down
20 changes: 18 additions & 2 deletions common/src/io.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -215,13 +215,29 @@ pcl::concatenateFields (const pcl::PCLPointCloud2 &cloud1,
}

//////////////////////////////////////////////////////////////////////////
template <typename PointT>
bool
pcl::concatenate (const pcl::PointCloud<PointT> &cloud1,
const pcl::PointCloud<PointT> &cloud2,
pcl::PointCloud<PointT> &cloud_out)
{
return pcl::PointCloud<PointT>::concatenate(cloud1, cloud2, cloud_out);
}

bool
pcl::concatenate (const pcl::PCLPointCloud2 &cloud1,
const pcl::PCLPointCloud2 &cloud2,
pcl::PCLPointCloud2 &cloud_out)
{
return pcl::PCLPointCloud2::concatenate(cloud1, cloud2, cloud_out);
}

bool
pcl::concatenatePointCloud (const pcl::PCLPointCloud2 &cloud1,
const pcl::PCLPointCloud2 &cloud2,
pcl::PCLPointCloud2 &cloud_out)
{
// replace the following with
return pcl::PCLPointCloud2::concatenate(cloud1, cloud2, cloud_out);
return pcl::concatenate(cloud1, cloud2, cloud_out);
}

//////////////////////////////////////////////////////////////////////////
Expand Down

0 comments on commit 0e8fac0

Please sign in to comment.