Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix a bug in CentroidPoint #2875

Merged
merged 4 commits into from
Feb 22, 2019
Merged

Fix a bug in CentroidPoint #2875

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
1b52f8b
Move CentroidPoint implementation into hpp file
taketwo Feb 22, 2019
5798145
Reformat CentroidPoint documentation as a table
taketwo Feb 22, 2019
8d1ec42
Fix a bug in CentroidPoint implementation
taketwo Feb 22, 2019
aeb7ce2
Add unit tests for CentroidPoint that check the fixed bug
taketwo Feb 22, 2019
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
60 changes: 13 additions & 47 deletions common/include/pcl/common/centroid.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -970,30 +970,14 @@ namespace pcl
* taking into account the meaning of the data inside fields. Currently the
* following fields are supported:
*
* - XYZ (\c x, \c y, \c z)
*
* Separate average for each field.
*
* - Normal (\c normal_x, \c normal_y, \c normal_z)
*
* Separate average for each field, and the resulting vector is normalized.
*
* - Curvature (\c curvature)
*
* Average.
*
* - RGB/RGBA (\c rgb or \c rgba)
*
* Separate average for R, G, B, and alpha channels.
*
* - Intensity (\c intensity)
*
* Average.
*
* - Label (\c label)
*
* Majority vote. If several labels have the same largest support then the
* smaller label wins.
* Data | Point fields | Algorithm
* --------- | ------------------------------------- | -------------------------------------------------------------------------------------------
* XYZ | \c x, \c y, \c z | Average (separate for each field)
* Normal | \c normal_x, \c normal_y, \c normal_z | Average (separate for each field), resulting vector is normalized
* Curvature | \c curvature | Average
* Color | \c rgb or \c rgba | Average (separate for R, G, B, and alpha channels)
* Intensity | \c intensity | Average
* Label | \c label | Majority vote; if several labels have the same largest support then the smaller label wins
*
* The template parameter defines the type of points that may be accumulated
* with this class. This may be an arbitrary PCL point type, and centroid
Expand Down Expand Up @@ -1038,22 +1022,14 @@ namespace pcl

public:

CentroidPoint ()
: num_points_ (0)
{
}
CentroidPoint () = default;

/** Add a new point to the centroid computation.
*
* In this function only the accumulators and point counter are updated,
* actual centroid computation does not happen until get() is called. */
void
add (const PointT& point)
{
// Invoke add point on each accumulator
boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
++num_points_;
}
add (const PointT& point);

/** Retrieve the current centroid.
*
Expand All @@ -1065,20 +1041,10 @@ namespace pcl
* If the number of accumulated points is zero, then the point will be
* left untouched. */
template <typename PointOutT> void
get (PointOutT& point) const
{
if (num_points_ != 0)
{
// Filter accumulators so that only those that are compatible with
// both PointT and requested point type remain
typename pcl::detail::Accumulators<PointT, PointOutT>::type ca (accumulators_);
// Invoke get point on each accumulator in filtered list
boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
}
}
get (PointOutT& point) const;

/** Get the total number of points that were added. */
size_t
inline size_t
getSize () const
{
return (num_points_);
Expand All @@ -1088,7 +1054,7 @@ namespace pcl

private:

size_t num_points_;
size_t num_points_ = 0;
typename pcl::detail::Accumulators<PointT>::type accumulators_;

};
Expand Down
32 changes: 16 additions & 16 deletions common/include/pcl/common/impl/accumulators.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -44,6 +44,7 @@
#include <boost/fusion/include/mpl.hpp>
#include <boost/fusion/include/for_each.hpp>
#include <boost/fusion/include/as_vector.hpp>
#include <boost/fusion/include/filter_if.hpp>

#include <pcl/point_types.h>

Expand Down Expand Up @@ -221,19 +222,23 @@ namespace pcl

};

/* This is a meta-function that may be used to create a Fusion vector of
* those accumulator types that are compatible with given point type(s). */

/* Meta-function that checks if an accumulator is compatible with given
* point type(s). */
template <typename Point1T, typename Point2T = Point1T>
struct Accumulators
{
struct IsAccumulatorCompatible {

// Check if a given accumulator type is compatible with a given point type
template <typename AccumulatorT, typename PointT>
struct IsCompatible : boost::mpl::apply<typename AccumulatorT::IsCompatible, PointT> { };
template <typename AccumulatorT>
struct apply : boost::mpl::and_<
boost::mpl::apply<typename AccumulatorT::IsCompatible, Point1T>,
boost::mpl::apply<typename AccumulatorT::IsCompatible, Point2T>
> {};
};

// A Fusion vector with accumulator types that are compatible with given
// point types
/* Meta-function that creates a Fusion vector of accumulator types that are
* compatible with a given point type. */
template <typename PointT>
struct Accumulators
{
typedef
typename boost::fusion::result_of::as_vector<
typename boost::mpl::filter_view<
Expand All @@ -245,18 +250,14 @@ namespace pcl
, AccumulatorIntensity
, AccumulatorLabel
>
, boost::mpl::and_<
IsCompatible<boost::mpl::_1, Point1T>
, IsCompatible<boost::mpl::_1, Point2T>
>
, IsAccumulatorCompatible<PointT>
>
>::type
type;
};

/* Fusion function object to invoke point addition on every accumulator in
* a fusion sequence. */

template <typename PointT>
struct AddPoint
{
Expand All @@ -275,7 +276,6 @@ namespace pcl

/* Fusion function object to invoke get point on every accumulator in a
* fusion sequence. */

template <typename PointT>
struct GetPoint
{
Expand Down
24 changes: 22 additions & 2 deletions common/include/pcl/common/impl/centroid.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -858,7 +858,28 @@ pcl::computeNDCentroid (const pcl::PointCloud<PointT> &cloud,
return (pcl::computeNDCentroid (cloud, indices.indices, centroid));
}

/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointT> void
pcl::CentroidPoint<PointT>::add (const PointT& point)
{
// Invoke add point on each accumulator
boost::fusion::for_each (accumulators_, detail::AddPoint<PointT> (point));
++num_points_;
}

template <typename PointT>
template <typename PointOutT> void
pcl::CentroidPoint<PointT>::get (PointOutT& point) const
{
if (num_points_ != 0)
{
// Filter accumulators so that only those that are compatible with
// both PointT and requested point type remain
auto ca = boost::fusion::filter_if<detail::IsAccumulatorCompatible<PointT, PointOutT>> (accumulators_);
// Invoke get point on each accumulator in filtered list
boost::fusion::for_each (ca, detail::GetPoint<PointOutT> (point, num_points_));
}
}

template <typename PointInT, typename PointOutT> size_t
pcl::computeCentroid (const pcl::PointCloud<PointInT>& cloud,
PointOutT& centroid)
Expand All @@ -877,7 +898,6 @@ pcl::computeCentroid (const pcl::PointCloud<PointInT>& cloud,
return (cp.getSize ());
}

/////////////////////////////////////////////////////////////////////////////////////////////
template <typename PointInT, typename PointOutT> size_t
pcl::computeCentroid (const pcl::PointCloud<PointInT>& cloud,
const std::vector<int>& indices,
Expand Down
38 changes: 32 additions & 6 deletions test/common/test_centroid.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -813,6 +813,7 @@ TEST (PCL, CentroidPoint)
centroid.get (c);
EXPECT_XYZ_EQ (PointXYZ (100, 100, 100), c);
}

// Single point
{
CentroidPoint<PointXYZ> centroid;
Expand All @@ -821,6 +822,7 @@ TEST (PCL, CentroidPoint)
centroid.get (c);
EXPECT_XYZ_EQ (p1, c);
}

// Multiple points
{
CentroidPoint<PointXYZ> centroid;
Expand All @@ -834,12 +836,36 @@ TEST (PCL, CentroidPoint)

// Retrieve centroid into a different point type
{
CentroidPoint<PointXYZ> centroid;
PointNormal p1; p1.getVector3fMap () << 1, 2, 3; p1.getNormalVector3fMap() << 0, 1, 0;
CentroidPoint<PointNormal> centroid;
centroid.add (p1);
PointXYZRGB c; c.rgba = 0x00FFFFFF;
centroid.get (c);
EXPECT_XYZ_EQ (p1, c);
EXPECT_EQ (0x00FFFFFF, c.rgba);
// Retrieve into a point type that is a "superset" of the original
{
PointXYZRGBNormal c; c.rgba = 0x00FFFFFF;
centroid.get (c);
EXPECT_XYZ_EQ (p1, c);
EXPECT_NORMAL_EQ (p1, c);
EXPECT_EQ (0x00FFFFFF, c.rgba); // unchanged
}
// Retrieve into a point type that is a "subset" of the original
{
Normal c;
centroid.get (c);
EXPECT_NORMAL_EQ (p1, c);
}
// Retrieve into a point type that partially "overlaps" with the original
{
PointXYZRGB c; c.rgba = 0x00FFFFFF;
centroid.get (c);
EXPECT_XYZ_EQ (p1, c);
EXPECT_EQ (0x00FFFFFF, c.rgba); // unchanged
}
// Retrieve into a point type that does not "overlap" with the original
{
RGB c; c.rgba = 0x00FFFFFF;
centroid.get (c);
EXPECT_EQ (0x00FFFFFF, c.rgba); // unchanged
}
}

// Centroid with XYZ and RGB
Expand All @@ -857,7 +883,7 @@ TEST (PCL, CentroidPoint)
EXPECT_EQ (0xFF111111, c.rgba);
}

// Centroid with normal and curavture
// Centroid with normal and curvature
{
Normal np1; np1.getNormalVector4fMap () << 1, 0, 0, 0; np1.curvature = 0.2;
Normal np2; np2.getNormalVector4fMap () << -1, 0, 0, 0; np2.curvature = 0.1;
Expand Down