From 5e19c141949ab61fd936a1bacd895e3ed4183a4b Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Fri, 21 Mar 2014 14:53:02 +0100 Subject: [PATCH 1/4] Add CentroidPoint class --- common/CMakeLists.txt | 1 + common/include/pcl/common/centroid.h | 145 +++++++++ .../pcl/common/impl/detail/accumulators.hpp | 296 ++++++++++++++++++ 3 files changed, 442 insertions(+) create mode 100644 common/include/pcl/common/impl/detail/accumulators.hpp diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt index 293f273278a..835bd2716dc 100644 --- a/common/CMakeLists.txt +++ b/common/CMakeLists.txt @@ -133,6 +133,7 @@ if(build) include/pcl/common/impl/random.hpp include/pcl/common/impl/generate.hpp include/pcl/common/impl/projection_matrix.hpp + include/pcl/common/impl/detail/accumulators.hpp ) set(impl_incs diff --git a/common/include/pcl/common/centroid.h b/common/include/pcl/common/centroid.h index 112b956539a..fb30afdf2af 100644 --- a/common/include/pcl/common/centroid.h +++ b/common/include/pcl/common/centroid.h @@ -945,6 +945,151 @@ namespace pcl return (computeNDCentroid (cloud, indices, centroid)); } +} + +#include + +namespace pcl +{ + + /** A generic class that computes the centroid of points fed to it. + * + * Here by "centroid" we denote not just the mean of 3D point coordinates, + * but also mean of values in the other data fields. The general-purpose + * \ref computeNDCentroid() function also implements this sort of + * functionality, however it does it in a "dumb" way, i.e. regardless of the + * semantics of the data inside a field it simply averages the values. In + * certain cases (e.g. for \c x, \c y, \c z, \c intensity fields) this + * behavior is reasonable, however in other cases (e.g. \c rgb, \c rgba, + * \c label fields) this does not lead to meaningful results. + * + * This class is capable of computing the centroid in a "smart" way, i.e. + * 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. + * + * 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 + * computation will happen only for the fields that are present in it and are + * supported. + * + * Current centroid may be retrieved at any time using get(). Note that the + * function is templated on point type, so it is possible to fetch the + * centroid into a point type that differs from the type of points that are + * being accumulated. All the "extra" fields for which the centroid is not + * being calculated will be left untouched. + * + * Example usage: + * + * \code + * // Create and accumulate points + * CentroidPoint centroid; + * centroid.add (pcl::PointXYZ (1, 2, 3); + * centroid.add (pcl::PointXYZ (5, 6, 7); + * // Fetch centroid using `get()` + * pcl::PointXYZ c1; + * centroid.get (c1); + * // The expected result is: c1.x == 3, c1.y == 4, c1.z == 5 + * // It is also okay to use `get()` with a different point type + * pcl::PointXYZRGB c2; + * centroid.get (c2); + * // The expected result is: c2.x == 3, c2.y == 4, c2.z == 5, + * // and c2.rgb is left untouched + * \endcode + * + * \note Assumes that the points being inserted are valid. + * + * \note This class template can be successfully instantiated for *any* + * PCL point type. Of course, each of the field averages is computed only if + * the point type has the corresponding field. + * + * \ingroup common + * \author Sergey Alexandrov */ + template + class CentroidPoint + { + + public: + + CentroidPoint () + : num_points_ (0) + { + } + + /** 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 (point)); + ++num_points_; + } + + /** Retrieve the current centroid. + * + * Computation (division of accumulated values by the number of points + * and normalization where applicable) happens here. The result is not + * cached, so any subsequent call to this function will trigger + * re-computation. + * + * If the number of accumulated points is zero, then the point will be + * left untouched. */ + template 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::type ca (accumulators_); + // Invoke get point on each accumulator in filtered list + boost::fusion::for_each (ca, detail::GetPoint (point, num_points_)); + } + } + + /** Get the total number of points that were added. */ + size_t + getSize () const + { + return (num_points_); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + private: + + size_t num_points_; + typename pcl::detail::Accumulators::type accumulators_; + + }; + } /*@}*/ #include diff --git a/common/include/pcl/common/impl/detail/accumulators.hpp b/common/include/pcl/common/impl/detail/accumulators.hpp new file mode 100644 index 00000000000..1c8128fd292 --- /dev/null +++ b/common/include/pcl/common/impl/detail/accumulators.hpp @@ -0,0 +1,296 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#ifndef PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP +#define PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP + +#include + +#include +#include +#include +#include + +#include + +namespace pcl +{ + + namespace detail + { + + /* Below are several helper accumulator structures that are used by the + * `CentroidPoint` class. Each of them is capable of accumulating + * information from a particular field(s) of a point. The points are + * inserted via `add()` and extracted via `get()` functions. Note that the + * accumulators are not templated on point type, so in principle it is + * possible to insert and extract points of different types. It is the + * responsibility of the user to make sure that points have corresponding + * fields. */ + + struct AccumulatorXYZ + { + + // Requires that point type has x, y, and z fields + typedef pcl::traits::has_xyz IsCompatible; + + // Storage + Eigen::Vector3f xyz; + + AccumulatorXYZ () : xyz (Eigen::Vector3f::Zero ()) { } + + template void + add (const PointT& t) { xyz += t.getVector3fMap (); } + + template void + get (PointT& t, size_t n) const { t.getVector3fMap () = xyz / n; } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + }; + + struct AccumulatorNormal + { + + // Requires that point type has normal_x, normal_y, and normal_z fields + typedef pcl::traits::has_normal IsCompatible; + + // Storage + Eigen::Vector4f normal; + + AccumulatorNormal () : normal (Eigen::Vector4f::Zero ()) { } + + // Requires that the normal of the given point is normalized, otherwise it + // does not make sense to sum it up with the accumulated value. + template void + add (const PointT& t) { normal += t.getNormalVector4fMap (); } + + template void + get (PointT& t, size_t n) const + { + t.getNormalVector4fMap () = normal; + t.getNormalVector4fMap ().normalize (); + } + + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + }; + + struct AccumulatorCurvature + { + + // Requires that point type has curvature field + typedef pcl::traits::has_curvature IsCompatible; + + // Storage + float curvature; + + AccumulatorCurvature () : curvature (0) { } + + template void + add (const PointT& t) { curvature += t.curvature; } + + template void + get (PointT& t, size_t n) const { t.curvature = curvature / n; } + + }; + + struct AccumulatorRGBA + { + + // Requires that point type has rgb or rgba field + typedef pcl::traits::has_color IsCompatible; + + // Storage + float r, g, b, a; + + AccumulatorRGBA () : r (0), g (0), b (0), a (0) { } + + template void + add (const PointT& t) + { + r += static_cast (t.r); + g += static_cast (t.g); + b += static_cast (t.b); + a += static_cast (t.a); + } + + template void + get (PointT& t, size_t n) const + { + t.rgba = static_cast (a / n) << 24 | + static_cast (r / n) << 16 | + static_cast (g / n) << 8 | + static_cast (b / n); + } + + }; + + struct AccumulatorIntensity + { + + // Requires that point type has intensity field + typedef pcl::traits::has_intensity IsCompatible; + + // Storage + float intensity; + + AccumulatorIntensity () : intensity (0) { } + + template void + add (const PointT& t) { intensity += t.intensity; } + + template void + get (PointT& t, size_t n) const { t.intensity = intensity / n; } + + }; + + struct AccumulatorLabel + { + + // Requires that point type has label field + typedef pcl::traits::has_label IsCompatible; + + // Storage + // A better performance may be achieved with a heap structure + std::map labels; + + AccumulatorLabel () { } + + template void + add (const PointT& t) + { + std::map::iterator itr = labels.find (t.label); + if (itr == labels.end ()) + labels.insert (std::make_pair (t.label, 1)); + else + ++itr->second; + } + + template void + get (PointT& t, size_t n) const + { + size_t max = 0; + std::map::const_iterator itr; + for (itr = labels.begin (); itr != labels.end (); ++itr) + if (itr->second > max) + { + max = itr->second; + t.label = itr->first; + } + } + + }; + + /* 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). */ + + template + struct Accumulators + { + + // Check if a given accumulator type is compatible with a given point type + template + struct IsCompatible : boost::mpl::apply { }; + + // A Fusion vector with accumulator types that are compatible with given + // point types + typedef + typename boost::fusion::result_of::as_vector< + typename boost::mpl::filter_view< + boost::mpl::vector< + AccumulatorXYZ + , AccumulatorNormal + , AccumulatorCurvature + , AccumulatorRGBA + , AccumulatorIntensity + , AccumulatorLabel + > + , boost::mpl::and_< + IsCompatible + , IsCompatible + > + > + >::type + type; + }; + + /* Fusion function object to invoke point addition on every accumulator in + * a fusion sequence. */ + + template + struct AddPoint + { + + const PointT& p; + + AddPoint (const PointT& point) : p (point) { } + + template void + operator () (AccumulatorT& accumulator) const + { + accumulator.add (p); + } + + }; + + /* Fusion function object to invoke get point on every accumulator in a + * fusion sequence. */ + + template + struct GetPoint + { + + PointT& p; + size_t n; + + GetPoint (PointT& point, size_t num) : p (point), n (num) { } + + template void + operator () (AccumulatorT& accumulator) const + { + accumulator.get (p, n); + } + + }; + + } + +} + +#endif /* PCL_COMMON_IMPL_DETAIL_ACCUMULATORS_HPP */ + From 7d6124c4707594cb269eb501cd121842ea42d7eb Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Fri, 21 Mar 2014 19:18:19 +0100 Subject: [PATCH 2/4] Add computeCentroid() functions that use CentroidPoint --- common/include/pcl/common/centroid.h | 33 +++++++++++++++++ common/include/pcl/common/impl/centroid.hpp | 39 +++++++++++++++++++++ 2 files changed, 72 insertions(+) diff --git a/common/include/pcl/common/centroid.h b/common/include/pcl/common/centroid.h index fb30afdf2af..1f113739340 100644 --- a/common/include/pcl/common/centroid.h +++ b/common/include/pcl/common/centroid.h @@ -1090,6 +1090,39 @@ namespace pcl }; + /** Compute the centroid of a set of points and return it as a point. + * + * Implementation leverages \ref CentroidPoint class and therefore behaves + * differently from \ref compute3DCentroid() and \ref computeNDCentroid(). + * See \ref CentroidPoint documentation for explanation. + * + * \param[in] cloud input point cloud + * \param[out] centroid output centroid + * + * \return number of valid points used to determine the centroid (will be the + * same as the size of the cloud if it is dense) + * + * \note If return value is \c 0, then the centroid is not changed, thus is + * not valid. + * + * \ingroup common */ + template size_t + computeCentroid (const pcl::PointCloud& cloud, + PointOutT& centroid); + + /** Compute the centroid of a set of points and return it as a point. + * + * \param[in] indices point cloud indices that need to be used + * + * This is an overloaded function provided for convenience. See the + * documentation for computeCentroid(). + * + * \ingroup common */ + template size_t + computeCentroid (const pcl::PointCloud& cloud, + const std::vector& indices, + PointOutT& centroid); + } /*@}*/ #include diff --git a/common/include/pcl/common/impl/centroid.hpp b/common/include/pcl/common/impl/centroid.hpp index 22a04e4844d..d8fe67a5f6c 100644 --- a/common/include/pcl/common/impl/centroid.hpp +++ b/common/include/pcl/common/impl/centroid.hpp @@ -859,5 +859,44 @@ pcl::computeNDCentroid (const pcl::PointCloud &cloud, return (pcl::computeNDCentroid (cloud, indices.indices, centroid)); } +///////////////////////////////////////////////////////////////////////////////////////////// +template size_t +pcl::computeCentroid (const pcl::PointCloud& cloud, + PointOutT& centroid) +{ + pcl::CentroidPoint cp; + + if (cloud.is_dense) + for (size_t i = 0; i < cloud.size (); ++i) + cp.add (cloud[i]); + else + for (size_t i = 0; i < cloud.size (); ++i) + if (pcl::isFinite (cloud[i])) + cp.add (cloud[i]); + + cp.get (centroid); + return (cp.getSize ()); +} + +///////////////////////////////////////////////////////////////////////////////////////////// +template size_t +pcl::computeCentroid (const pcl::PointCloud& cloud, + const std::vector& indices, + PointOutT& centroid) +{ + pcl::CentroidPoint cp; + + if (cloud.is_dense) + for (size_t i = 0; i < indices.size (); ++i) + cp.add (cloud[indices[i]]); + else + for (size_t i = 0; i < indices.size (); ++i) + if (pcl::isFinite (cloud[indices[i]])) + cp.add (cloud[indices[i]]); + + cp.get (centroid); + return (cp.getSize ()); +} + #endif //#ifndef PCL_COMMON_IMPL_CENTROID_H_ From 9b740dc383303f5aecd9f50230d34e4ec2a07f5b Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Fri, 21 Mar 2014 15:37:33 +0100 Subject: [PATCH 3/4] Extract centroid-related tests from test_common.cpp There are quite a number of functions dealing with centroid, mean, and covariance computation. They all are grouped in 'common/centroid.h' header. Therefore it makes sense to group their corresponding test functions in a separate file. Hence creation of 'test_centroid.cpp'. --- test/common/CMakeLists.txt | 1 + test/common/test_centroid.cpp | 776 ++++++++++++++++++++++++++++++++++ test/common/test_common.cpp | 718 ------------------------------- 3 files changed, 777 insertions(+), 718 deletions(-) create mode 100644 test/common/test_centroid.cpp diff --git a/test/common/CMakeLists.txt b/test/common/CMakeLists.txt index d9b6f751555..e21d379ff71 100644 --- a/test/common/CMakeLists.txt +++ b/test/common/CMakeLists.txt @@ -4,6 +4,7 @@ PCL_ADD_TEST(common_test_macros test_macros FILES test_macros.cpp LINK_WITH pcl_ PCL_ADD_TEST(common_vector_average test_vector_average FILES test_vector_average.cpp LINK_WITH pcl_gtest) PCL_ADD_TEST(common_common test_common FILES test_common.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_copy_point test_copy_point FILES test_copy_point.cpp LINK_WITH pcl_gtest pcl_common) +PCL_ADD_TEST(common_centroid test_centroid FILES test_centroid.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_int test_plane_intersection FILES test_plane_intersection.cpp LINK_WITH pcl_gtest pcl_common) PCL_ADD_TEST(common_pca test_pca FILES test_pca.cpp LINK_WITH pcl_gtest pcl_common) #PCL_ADD_TEST(common_spring test_spring FILES test_spring.cpp LINK_WITH pcl_gtest pcl_common) diff --git a/test/common/test_centroid.cpp b/test/common/test_centroid.cpp new file mode 100644 index 00000000000..994e6690eb4 --- /dev/null +++ b/test/common/test_centroid.cpp @@ -0,0 +1,776 @@ +/* + * Software License Agreement (BSD License) + * + * Point Cloud Library (PCL) - www.pointclouds.org + * Copyright (c) 2010-2012, Willow Garage, Inc. + * Copyright (c) 2014-, Open Perception, Inc. + * + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder(s) nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +using namespace pcl; + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, compute3DCentroidFloat) +{ + pcl::PointIndices pindices; + std::vector indices; + PointXYZ point; + PointCloud cloud; + Eigen::Vector4f centroid; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 0); + + cloud.clear (); + indices.clear (); + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + + EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); + + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + indices.push_back (8); // add the NaN + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + + pindices.indices = indices; + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, compute3DCentroidDouble) +{ + pcl::PointIndices pindices; + std::vector indices; + PointXYZ point; + PointCloud cloud; + Eigen::Vector4d centroid; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 0); + + cloud.clear (); + indices.clear (); + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + + EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); + + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + + centroid [0] = -100; + centroid [1] = -200; + centroid [2] = -300; + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + indices.push_back (8); // add the NaN + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); + + pindices.indices = indices; + EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); + + EXPECT_EQ (centroid [0], 0.0); + EXPECT_EQ (centroid [1], 1.0); + EXPECT_EQ (centroid [2], 0.0); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, compute3DCentroidCloudIterator) +{ + pcl::PointIndices pindices; + std::vector indices; + PointXYZ point; + PointCloud cloud; + Eigen::Vector4f centroid_f; + + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + + ConstCloudIterator it (cloud, indices); + + EXPECT_EQ (compute3DCentroid (it, centroid_f), 4); + + EXPECT_EQ (centroid_f[0], 0.0f); + EXPECT_EQ (centroid_f[1], 1.0f); + EXPECT_EQ (centroid_f[2], 0.0f); + + Eigen::Vector4d centroid_d; + it.reset (); + EXPECT_EQ (compute3DCentroid (it, centroid_d), 4); + + EXPECT_EQ (centroid_d[0], 0.0); + EXPECT_EQ (centroid_d[1], 1.0); + EXPECT_EQ (centroid_d[2], 0.0); +} + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, computeCovarianceMatrix) +{ + PointCloud cloud; + PointXYZ point; + std::vector indices; + Eigen::Vector4f centroid; + Eigen::Matrix3f covariance_matrix; + + centroid [0] = 0; + centroid [1] = 0; + centroid [2] = 0; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 0); + + cloud.clear (); + indices.clear (); + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = 0; + centroid [1] = 0; + centroid [2] = 0; + + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 8); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 8); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 8); + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 1; + + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 4); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 4); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 0; + + EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 8); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 8); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 8); + + indices.push_back (8); // add the NaN + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 1; + + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 4); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 4); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, computeCovarianceMatrixNormalized) +{ + PointCloud cloud; + PointXYZ point; + std::vector indices; + Eigen::Vector4f centroid; + Eigen::Matrix3f covariance_matrix; + + centroid [0] = 0; + centroid [1] = 0; + centroid [2] = 0; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 0); + + cloud.clear (); + indices.clear (); + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = 0; + centroid [1] = 0; + centroid [2] = 0; + + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8); + + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 1; + + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4); + + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 0; + + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.push_back (8); // add the NaN + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [1] = 1; + + EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, computeDemeanedCovariance) +{ + PointCloud cloud; + PointXYZ point; + std::vector indices; + Eigen::Matrix3f covariance_matrix; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 0); + + cloud.clear (); + indices.clear (); + + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + + EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.push_back (8); // add the NaN + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + + EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); +} + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, computeMeanAndCovariance) +{ + PointCloud cloud; + PointXYZ point; + std::vector indices; + Eigen::Matrix3f covariance_matrix; + Eigen::Vector4f centroid; + + // test empty cloud which is dense + cloud.is_dense = true; + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0); + + // test empty cloud non_dense + cloud.is_dense = false; + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0); + + // test non-empty cloud non_dense + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + indices.push_back (1); + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 0); + + cloud.clear (); + indices.clear (); + + for (point.x = -1; point.x < 2; point.x += 2) + { + for (point.y = -1; point.y < 2; point.y += 2) + { + for (point.z = -1; point.z < 2; point.z += 2) + { + cloud.push_back (point); + } + } + } + cloud.is_dense = true; + + // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) + + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = -100; + centroid [1] = -101; + centroid [2] = -102; + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8); + + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.resize (4); // only positive y values + indices [0] = 2; + indices [1] = 3; + indices [2] = 6; + indices [3] = 7; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = -100; + centroid [1] = -101; + centroid [2] = -102; + + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 1); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); + cloud.push_back (point); + cloud.is_dense = false; + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = -100; + centroid [1] = -101; + centroid [2] = -102; + + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 0); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 1); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); + + indices.push_back (8); // add the NaN + covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; + centroid [0] = -100; + centroid [1] = -101; + centroid [2] = -102; + + EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4); + EXPECT_EQ (centroid [0], 0); + EXPECT_EQ (centroid [1], 1); + EXPECT_EQ (centroid [2], 0); + EXPECT_EQ (covariance_matrix (0, 0), 1); + EXPECT_EQ (covariance_matrix (0, 1), 0); + EXPECT_EQ (covariance_matrix (0, 2), 0); + EXPECT_EQ (covariance_matrix (1, 0), 0); + EXPECT_EQ (covariance_matrix (1, 1), 0); + EXPECT_EQ (covariance_matrix (1, 2), 0); + EXPECT_EQ (covariance_matrix (2, 0), 0); + EXPECT_EQ (covariance_matrix (2, 1), 0); + EXPECT_EQ (covariance_matrix (2, 2), 1); +} + +int +main (int argc, char** argv) +{ + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} + diff --git a/test/common/test_common.cpp b/test/common/test_common.cpp index d7ad43bb108..a753406935e 100644 --- a/test/common/test_common.cpp +++ b/test/common/test_common.cpp @@ -383,724 +383,6 @@ TEST (PCL, Intersections) //intersection: [ 3.06416e+08 15.2237 3.06416e+08 4.04468e-34 ] } -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, compute3DCentroidFloat) -{ - pcl::PointIndices pindices; - std::vector indices; - PointXYZ point; - PointCloud cloud; - Eigen::Vector4f centroid; - - // test empty cloud which is dense - cloud.is_dense = true; - EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); - - // test empty cloud non_dense - cloud.is_dense = false; - EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - indices.push_back (1); - EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 0); - - cloud.clear (); - indices.clear (); - for (point.x = -1; point.x < 2; point.x += 2) - { - for (point.y = -1; point.y < 2; point.y += 2) - { - for (point.z = -1; point.z < 2; point.z += 2) - { - cloud.push_back (point); - } - } - } - cloud.is_dense = true; - - // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) - centroid [0] = -100; - centroid [1] = -200; - centroid [2] = -300; - - EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); - EXPECT_EQ (centroid [0], 0); - EXPECT_EQ (centroid [1], 0); - EXPECT_EQ (centroid [2], 0); - - centroid [0] = -100; - centroid [1] = -200; - centroid [2] = -300; - indices.resize (4); // only positive y values - indices [0] = 2; - indices [1] = 3; - indices [2] = 6; - indices [3] = 7; - EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); - - EXPECT_EQ (centroid [0], 0.0); - EXPECT_EQ (centroid [1], 1.0); - EXPECT_EQ (centroid [2], 0.0); - - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - cloud.is_dense = false; - - centroid [0] = -100; - centroid [1] = -200; - centroid [2] = -300; - EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); - - EXPECT_EQ (centroid [0], 0); - EXPECT_EQ (centroid [1], 0); - EXPECT_EQ (centroid [2], 0); - - centroid [0] = -100; - centroid [1] = -200; - centroid [2] = -300; - indices [0] = 2; - indices [1] = 3; - indices [2] = 6; - indices [3] = 7; - indices.push_back (8); // add the NaN - EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); - - EXPECT_EQ (centroid [0], 0.0); - EXPECT_EQ (centroid [1], 1.0); - EXPECT_EQ (centroid [2], 0.0); - - pindices.indices = indices; - EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); - - EXPECT_EQ (centroid [0], 0.0); - EXPECT_EQ (centroid [1], 1.0); - EXPECT_EQ (centroid [2], 0.0); -} - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, compute3DCentroidDouble) -{ - pcl::PointIndices pindices; - std::vector indices; - PointXYZ point; - PointCloud cloud; - Eigen::Vector4d centroid; - - // test empty cloud which is dense - cloud.is_dense = true; - EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); - - // test empty cloud non_dense - cloud.is_dense = false; - EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - EXPECT_EQ (compute3DCentroid (cloud, centroid), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - indices.push_back (1); - EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 0); - - cloud.clear (); - indices.clear (); - for (point.x = -1; point.x < 2; point.x += 2) - { - for (point.y = -1; point.y < 2; point.y += 2) - { - for (point.z = -1; point.z < 2; point.z += 2) - { - cloud.push_back (point); - } - } - } - cloud.is_dense = true; - - // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) - centroid [0] = -100; - centroid [1] = -200; - centroid [2] = -300; - - EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); - EXPECT_EQ (centroid [0], 0); - EXPECT_EQ (centroid [1], 0); - EXPECT_EQ (centroid [2], 0); - - centroid [0] = -100; - centroid [1] = -200; - centroid [2] = -300; - indices.resize (4); // only positive y values - indices [0] = 2; - indices [1] = 3; - indices [2] = 6; - indices [3] = 7; - EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); - - EXPECT_EQ (centroid [0], 0.0); - EXPECT_EQ (centroid [1], 1.0); - EXPECT_EQ (centroid [2], 0.0); - - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - cloud.is_dense = false; - - centroid [0] = -100; - centroid [1] = -200; - centroid [2] = -300; - EXPECT_EQ (compute3DCentroid (cloud, centroid), 8); - - EXPECT_EQ (centroid [0], 0); - EXPECT_EQ (centroid [1], 0); - EXPECT_EQ (centroid [2], 0); - - centroid [0] = -100; - centroid [1] = -200; - centroid [2] = -300; - indices [0] = 2; - indices [1] = 3; - indices [2] = 6; - indices [3] = 7; - indices.push_back (8); // add the NaN - EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); - - EXPECT_EQ (centroid [0], 0.0); - EXPECT_EQ (centroid [1], 1.0); - EXPECT_EQ (centroid [2], 0.0); - - pindices.indices = indices; - EXPECT_EQ (compute3DCentroid (cloud, indices, centroid), 4); - - EXPECT_EQ (centroid [0], 0.0); - EXPECT_EQ (centroid [1], 1.0); - EXPECT_EQ (centroid [2], 0.0); -} - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, compute3DCentroidCloudIterator) -{ - pcl::PointIndices pindices; - std::vector indices; - PointXYZ point; - PointCloud cloud; - Eigen::Vector4f centroid_f; - - for (point.x = -1; point.x < 2; point.x += 2) - { - for (point.y = -1; point.y < 2; point.y += 2) - { - for (point.z = -1; point.z < 2; point.z += 2) - { - cloud.push_back (point); - } - } - } - cloud.is_dense = true; - - indices.resize (4); // only positive y values - indices [0] = 2; - indices [1] = 3; - indices [2] = 6; - indices [3] = 7; - - ConstCloudIterator it (cloud, indices); - - EXPECT_EQ (compute3DCentroid (it, centroid_f), 4); - - EXPECT_EQ (centroid_f[0], 0.0f); - EXPECT_EQ (centroid_f[1], 1.0f); - EXPECT_EQ (centroid_f[2], 0.0f); - - Eigen::Vector4d centroid_d; - it.reset (); - EXPECT_EQ (compute3DCentroid (it, centroid_d), 4); - - EXPECT_EQ (centroid_d[0], 0.0); - EXPECT_EQ (centroid_d[1], 1.0); - EXPECT_EQ (centroid_d[2], 0.0); -} - - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, computeCovarianceMatrix) -{ - PointCloud cloud; - PointXYZ point; - std::vector indices; - Eigen::Vector4f centroid; - Eigen::Matrix3f covariance_matrix; - - centroid [0] = 0; - centroid [1] = 0; - centroid [2] = 0; - - // test empty cloud which is dense - cloud.is_dense = true; - EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0); - - // test empty cloud non_dense - cloud.is_dense = false; - EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - indices.push_back (1); - EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 0); - - cloud.clear (); - indices.clear (); - for (point.x = -1; point.x < 2; point.x += 2) - { - for (point.y = -1; point.y < 2; point.y += 2) - { - for (point.z = -1; point.z < 2; point.z += 2) - { - cloud.push_back (point); - } - } - } - cloud.is_dense = true; - - // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) - - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [0] = 0; - centroid [1] = 0; - centroid [2] = 0; - - EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8); - EXPECT_EQ (covariance_matrix (0, 0), 8); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 8); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 8); - - indices.resize (4); // only positive y values - indices [0] = 2; - indices [1] = 3; - indices [2] = 6; - indices [3] = 7; - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [1] = 1; - - EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4); - EXPECT_EQ (covariance_matrix (0, 0), 4); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 0); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 4); - - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - cloud.is_dense = false; - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [1] = 0; - - EXPECT_EQ (computeCovarianceMatrix (cloud, centroid, covariance_matrix), 8); - EXPECT_EQ (covariance_matrix (0, 0), 8); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 8); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 8); - - indices.push_back (8); // add the NaN - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [1] = 1; - - EXPECT_EQ (computeCovarianceMatrix (cloud, indices, centroid, covariance_matrix), 4); - EXPECT_EQ (covariance_matrix (0, 0), 4); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 0); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 4); -} - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, computeCovarianceMatrixNormalized) -{ - PointCloud cloud; - PointXYZ point; - std::vector indices; - Eigen::Vector4f centroid; - Eigen::Matrix3f covariance_matrix; - - centroid [0] = 0; - centroid [1] = 0; - centroid [2] = 0; - - // test empty cloud which is dense - cloud.is_dense = true; - EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0); - - // test empty cloud non_dense - cloud.is_dense = false; - EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - indices.push_back (1); - EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 0); - - cloud.clear (); - indices.clear (); - for (point.x = -1; point.x < 2; point.x += 2) - { - for (point.y = -1; point.y < 2; point.y += 2) - { - for (point.z = -1; point.z < 2; point.z += 2) - { - cloud.push_back (point); - } - } - } - cloud.is_dense = true; - - // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) - - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [0] = 0; - centroid [1] = 0; - centroid [2] = 0; - - EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8); - - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 1); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); - - indices.resize (4); // only positive y values - indices [0] = 2; - indices [1] = 3; - indices [2] = 6; - indices [3] = 7; - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [1] = 1; - - EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4); - - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 0); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); - - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - cloud.is_dense = false; - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [1] = 0; - - EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, centroid, covariance_matrix), 8); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 1); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); - - indices.push_back (8); // add the NaN - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [1] = 1; - - EXPECT_EQ (computeCovarianceMatrixNormalized (cloud, indices, centroid, covariance_matrix), 4); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 0); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); -} - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, computeDemeanedCovariance) -{ - PointCloud cloud; - PointXYZ point; - std::vector indices; - Eigen::Matrix3f covariance_matrix; - - // test empty cloud which is dense - cloud.is_dense = true; - EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0); - - // test empty cloud non_dense - cloud.is_dense = false; - EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - indices.push_back (1); - EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 0); - - cloud.clear (); - indices.clear (); - - for (point.x = -1; point.x < 2; point.x += 2) - { - for (point.y = -1; point.y < 2; point.y += 2) - { - for (point.z = -1; point.z < 2; point.z += 2) - { - cloud.push_back (point); - } - } - } - cloud.is_dense = true; - - // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) - - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - - EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 1); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); - - indices.resize (4); // only positive y values - indices [0] = 2; - indices [1] = 3; - indices [2] = 6; - indices [3] = 7; - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - - EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 1); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); - - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - cloud.is_dense = false; - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - - EXPECT_EQ (computeCovarianceMatrix (cloud, covariance_matrix), 8); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 1); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); - - indices.push_back (8); // add the NaN - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - - EXPECT_EQ (computeCovarianceMatrix (cloud, indices, covariance_matrix), 4); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 1); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); -} - -////////////////////////////////////////////////////////////////////////////////////////////////////////////////// -TEST (PCL, computeMeanAndCovariance) -{ - PointCloud cloud; - PointXYZ point; - std::vector indices; - Eigen::Matrix3f covariance_matrix; - Eigen::Vector4f centroid; - - // test empty cloud which is dense - cloud.is_dense = true; - EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0); - - // test empty cloud non_dense - cloud.is_dense = false; - EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 0); - - // test non-empty cloud non_dense - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - indices.push_back (1); - EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 0); - - cloud.clear (); - indices.clear (); - - for (point.x = -1; point.x < 2; point.x += 2) - { - for (point.y = -1; point.y < 2; point.y += 2) - { - for (point.z = -1; point.z < 2; point.z += 2) - { - cloud.push_back (point); - } - } - } - cloud.is_dense = true; - - // eight points with (0, 0, 0) as centroid and covarmat (1, 0, 0, 0, 1, 0, 0, 0, 1) - - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [0] = -100; - centroid [1] = -101; - centroid [2] = -102; - EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8); - - EXPECT_EQ (centroid [0], 0); - EXPECT_EQ (centroid [1], 0); - EXPECT_EQ (centroid [2], 0); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 1); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); - - indices.resize (4); // only positive y values - indices [0] = 2; - indices [1] = 3; - indices [2] = 6; - indices [3] = 7; - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [0] = -100; - centroid [1] = -101; - centroid [2] = -102; - - EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4); - EXPECT_EQ (centroid [0], 0); - EXPECT_EQ (centroid [1], 1); - EXPECT_EQ (centroid [2], 0); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 0); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); - - point.x = point.y = point.z = std::numeric_limits::quiet_NaN (); - cloud.push_back (point); - cloud.is_dense = false; - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [0] = -100; - centroid [1] = -101; - centroid [2] = -102; - - EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, covariance_matrix, centroid), 8); - EXPECT_EQ (centroid [0], 0); - EXPECT_EQ (centroid [1], 0); - EXPECT_EQ (centroid [2], 0); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 1); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); - - indices.push_back (8); // add the NaN - covariance_matrix << -100, -101, -102, -110, -111, -112, -120, -121, -122; - centroid [0] = -100; - centroid [1] = -101; - centroid [2] = -102; - - EXPECT_EQ (computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix, centroid), 4); - EXPECT_EQ (centroid [0], 0); - EXPECT_EQ (centroid [1], 1); - EXPECT_EQ (centroid [2], 0); - EXPECT_EQ (covariance_matrix (0, 0), 1); - EXPECT_EQ (covariance_matrix (0, 1), 0); - EXPECT_EQ (covariance_matrix (0, 2), 0); - EXPECT_EQ (covariance_matrix (1, 0), 0); - EXPECT_EQ (covariance_matrix (1, 1), 0); - EXPECT_EQ (covariance_matrix (1, 2), 0); - EXPECT_EQ (covariance_matrix (2, 0), 0); - EXPECT_EQ (covariance_matrix (2, 1), 0); - EXPECT_EQ (covariance_matrix (2, 2), 1); -} - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, CopyIfFieldExists) { From b5de9e3b7ceb12549fa3cc2d553130b19e5eddca Mon Sep 17 00:00:00 2001 From: Sergey Alexandrov Date: Fri, 21 Mar 2014 15:46:51 +0100 Subject: [PATCH 4/4] Add unit tests for CentroidPoint and computeCentroid() --- test/common/test_centroid.cpp | 214 ++++++++++++++++++++++++++++++++++ 1 file changed, 214 insertions(+) diff --git a/test/common/test_centroid.cpp b/test/common/test_centroid.cpp index 994e6690eb4..66bc785eaa3 100644 --- a/test/common/test_centroid.cpp +++ b/test/common/test_centroid.cpp @@ -44,6 +44,7 @@ #include #include #include +#include #include @@ -767,6 +768,219 @@ TEST (PCL, computeMeanAndCovariance) EXPECT_EQ (covariance_matrix (2, 2), 1); } +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, CentroidPoint) +{ + PointXYZ p1; p1.getVector3fMap () << 1, 2, 3; + PointXYZ p2; p2.getVector3fMap () << 3, 2, 1; + PointXYZ p3; p3.getVector3fMap () << 5, 5, 5; + + // Zero points (get should have no effect) + { + CentroidPoint centroid; + PointXYZ c (100, 100, 100); + centroid.get (c); + EXPECT_XYZ_EQ (PointXYZ (100, 100, 100), c); + } + // Single point + { + CentroidPoint centroid; + centroid.add (p1); + PointXYZ c; + centroid.get (c); + EXPECT_XYZ_EQ (p1, c); + } + // Multiple points + { + CentroidPoint centroid; + centroid.add (p1); + centroid.add (p2); + centroid.add (p3); + PointXYZ c; + centroid.get (c); + EXPECT_XYZ_EQ (PointXYZ (3, 3, 3), c); + } + + // Retrieve centroid into a different point type + { + CentroidPoint centroid; + centroid.add (p1); + PointXYZRGB c; c.rgba = 0x00FFFFFF; + centroid.get (c); + EXPECT_XYZ_EQ (p1, c); + EXPECT_EQ (0x00FFFFFF, c.rgba); + } + + // Centroid with XYZ and RGB + { + PointXYZRGB cp1; cp1.getVector3fMap () << 4, 2, 4; cp1.rgba = 0xFF330000; + PointXYZRGB cp2; cp2.getVector3fMap () << 2, 4, 2; cp2.rgba = 0xFF003300; + PointXYZRGB cp3; cp3.getVector3fMap () << 3, 3, 3; cp3.rgba = 0xFF000033; + CentroidPoint centroid; + centroid.add (cp1); + centroid.add (cp2); + centroid.add (cp3); + PointXYZRGB c; + centroid.get (c); + EXPECT_XYZ_EQ (PointXYZ (3, 3, 3), c); + EXPECT_EQ (0xFF111111, c.rgba); + } + + // Centroid with normal and curavture + { + Normal np1; np1.getNormalVector4fMap () << 1, 0, 0, 0; np1.curvature = 0.2; + Normal np2; np2.getNormalVector4fMap () << -1, 0, 0, 0; np2.curvature = 0.1; + Normal np3; np3.getNormalVector4fMap () << 0, 1, 0, 0; np3.curvature = 0.9; + CentroidPoint centroid; + centroid.add (np1); + centroid.add (np2); + centroid.add (np3); + Normal c; + centroid.get (c); + EXPECT_NORMAL_EQ (np3, c); + EXPECT_FLOAT_EQ (0.4, c.curvature); + } + + // Centroid with XYZ and intensity + { + PointXYZI ip1; ip1.getVector3fMap () << 1, 2, 3; ip1.intensity = 0.8; + PointXYZI ip2; ip2.getVector3fMap () << 3, 2, 1; ip2.intensity = 0.2; + PointXYZI ip3; ip3.getVector3fMap () << 5, 5, 5; ip3.intensity = 0.2; + CentroidPoint centroid; + centroid.add (ip1); + centroid.add (ip2); + centroid.add (ip3); + PointXYZI c; + centroid.get (c); + EXPECT_XYZ_EQ (PointXYZ (3, 3, 3), c); + EXPECT_FLOAT_EQ (0.4, c.intensity); + } + + // Centroid with label + { + Label lp1; lp1.label = 1; + Label lp2; lp2.label = 1; + Label lp3; lp3.label = 2; + CentroidPoint