From 4c67d8108baa66dafbaaa611fabbe22b093ca279 Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Sun, 5 Nov 2017 15:30:29 +0100 Subject: [PATCH] FLARELocalReferenceFrameEstimation class added (#1571) Add implementation of the Fast LocAl Reference framE (FLARE) algorithm --- common/include/pcl/common/geometry.h | 57 ++++ features/CMakeLists.txt | 3 + features/include/pcl/features/flare.h | 293 +++++++++++++++++++ features/include/pcl/features/impl/flare.hpp | 263 +++++++++++++++++ features/include/pcl/features/normal_3d.h | 43 +++ features/src/flare.cpp | 49 ++++ test/features/CMakeLists.txt | 5 +- test/features/test_flare_estimation.cpp | 179 +++++++++++ 8 files changed, 891 insertions(+), 1 deletion(-) create mode 100644 features/include/pcl/features/flare.h create mode 100644 features/include/pcl/features/impl/flare.hpp create mode 100644 features/src/flare.cpp create mode 100644 test/features/test_flare_estimation.cpp diff --git a/common/include/pcl/common/geometry.h b/common/include/pcl/common/geometry.h index 2c110e7d9a7..dd19237f0e1 100644 --- a/common/include/pcl/common/geometry.h +++ b/common/include/pcl/common/geometry.h @@ -43,6 +43,7 @@ #endif #include +#include /** * \file common/geometry.h @@ -101,6 +102,62 @@ namespace pcl float lambda = plane_normal.dot(po); projected = point - (lambda * plane_normal); } + + + /** \brief Given a plane defined by plane_origin and plane_normal, find the unit vector pointing from plane_origin to the projection of point on the plane. + * + * \param[in] point Point projected on the plane + * \param[in] plane_origin The plane origin + * \param[in] plane_normal The plane normal + * \return unit vector pointing from plane_origin to the projection of point on the plane. + * \ingroup geometry + */ + inline Eigen::Vector3f + projectedAsUnitVector (Eigen::Vector3f const &point, + Eigen::Vector3f const &plane_origin, + Eigen::Vector3f const &plane_normal) + { + Eigen::Vector3f projection; + project (point, plane_origin, plane_normal, projection); + Eigen::Vector3f projected_as_unit_vector = projection - plane_origin; + projected_as_unit_vector.normalize (); + return projected_as_unit_vector; + } + + + /** \brief Define a random unit vector orthogonal to axis. + * + * \param[in] axis Axis + * \return random unit vector orthogonal to axis + * \ingroup geometry + */ + inline Eigen::Vector3f + randomOrthogonalAxis (Eigen::Vector3f const &axis) + { + Eigen::Vector3f rand_ortho_axis; + rand_ortho_axis.setRandom(); + if (std::abs (axis.z ()) > 1E-8f) + { + rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z (); + } + else if (std::abs (axis.y ()) > 1E-8f) + { + rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y (); + } + else if (std::abs (axis.x ()) > 1E-8f) + { + rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x (); + } + else + { + PCL_WARN ("[pcl::randomOrthogonalAxis] provided axis has norm < 1E-8f"); + } + + rand_ortho_axis.normalize (); + return rand_ortho_axis; + } + + } } diff --git a/features/CMakeLists.txt b/features/CMakeLists.txt index ad5d60bbd7e..27fa44ec07c 100644 --- a/features/CMakeLists.txt +++ b/features/CMakeLists.txt @@ -15,6 +15,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/boost.h" "include/pcl/${SUBSYS_NAME}/eigen.h" "include/pcl/${SUBSYS_NAME}/board.h" + "include/pcl/${SUBSYS_NAME}/flare.h" "include/pcl/${SUBSYS_NAME}/brisk_2d.h" "include/pcl/${SUBSYS_NAME}/cppf.h" "include/pcl/${SUBSYS_NAME}/cvfh.h" @@ -66,6 +67,7 @@ if(build) set(impl_incs "include/pcl/${SUBSYS_NAME}/impl/board.hpp" + "include/pcl/${SUBSYS_NAME}/impl/flare.hpp" "include/pcl/${SUBSYS_NAME}/impl/brisk_2d.hpp" "include/pcl/${SUBSYS_NAME}/impl/cppf.hpp" "include/pcl/${SUBSYS_NAME}/impl/cvfh.hpp" @@ -114,6 +116,7 @@ if(build) set(srcs src/board.cpp + src/flare.cpp src/brisk_2d.cpp src/boundary.cpp src/cppf.cpp diff --git a/features/include/pcl/features/flare.h b/features/include/pcl/features/flare.h new file mode 100644 index 00000000000..45948b5fe7d --- /dev/null +++ b/features/include/pcl/features/flare.h @@ -0,0 +1,293 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2016-, 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_FLARE_H_ +#define PCL_FLARE_H_ + +#include +#include +#include + + +namespace pcl +{ + + /** \brief FLARELocalReferenceFrameEstimation implements the Fast LocAl Reference framE algorithm + * for local reference frame estimation as described here: + * + * - A. Petrelli, L. Di Stefano, + * "A repeatable and efficient canonical reference for surface matching", + * 3DimPVT, 2012 + * + * FLARE algorithm is deployed in ReLOC algorithm proposed in: + * + * Petrelli A., Di Stefano L., "Pairwise registration by local orientation cues", Computer Graphics Forum, 2015. + * + * \author Alioscia Petrelli + * \ingroup features + */ + template + class FLARELocalReferenceFrameEstimation : public FeatureFromNormals + { + protected: + using Feature::feature_name_; + using Feature::input_; + using Feature::indices_; + using Feature::surface_; + using Feature::tree_; + using Feature::search_parameter_; + using FeatureFromNormals::normals_; + using Feature::fake_surface_; + using Feature::getClassName; + + using typename Feature::PointCloudIn; + using typename Feature::PointCloudOut; + + using typename Feature::PointCloudInConstPtr; + + using typename Feature::KdTreePtr; + + typedef typename pcl::PointCloud PointCloudSignedDistance; + typedef typename PointCloudSignedDistance::Ptr PointCloudSignedDistancePtr; + + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + + public: + /** \brief Constructor. */ + FLARELocalReferenceFrameEstimation () : + tangent_radius_ (0.0f), + margin_thresh_ (0.85f), + min_neighbors_for_normal_axis_ (6), + min_neighbors_for_tangent_axis_ (6), + sampled_surface_ (), + sampled_tree_ (), + fake_sampled_surface_ (false) + { + feature_name_ = "FLARELocalReferenceFrameEstimation"; + } + + //Getters/Setters + + /** \brief Set the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point. + * + * \param[in] radius The search radius for x axis. + */ + inline void + setTangentRadius (float radius) + { + tangent_radius_ = radius; + } + + /** \brief Get the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point. + * + * \return The search radius for x axis. + */ + inline float + getTangentRadius () const + { + return (tangent_radius_); + } + + /** \brief Set the percentage of the search tangent radius after which a point is considered part of the support. + * + * \param[in] margin_thresh the percentage of the search tangent radius after which a point is considered part of the support. + */ + inline void + setMarginThresh (float margin_thresh) + { + margin_thresh_ = margin_thresh; + } + + /** \brief Get the percentage of the search tangent radius after which a point is considered part of the support. + * + * \return The percentage of the search tangent radius after which a point is considered part of the support. + */ + inline float + getMarginThresh () const + { + return (margin_thresh_); + } + + + /** \brief Set min number of neighbours required for the computation of Z axis. + * + * \param[in] min_neighbors_for_normal_axis min number of neighbours required for the computation of Z axis. + */ + inline void + setMinNeighboursForNormalAxis (int min_neighbors_for_normal_axis) + { + min_neighbors_for_normal_axis_ = min_neighbors_for_normal_axis; + } + + /** \brief Get min number of neighbours required for the computation of Z axis. + * + * \return min number of neighbours required for the computation of Z axis. + */ + inline int + getMinNeighboursForNormalAxis () const + { + return (min_neighbors_for_normal_axis_); + } + + + /** \brief Set min number of neighbours required for the computation of X axis. + * + * \param[in] min_neighbors_for_tangent_axis min number of neighbours required for the computation of X axis. + */ + inline void + setMinNeighboursForTangentAxis (int min_neighbors_for_tangent_axis) + { + min_neighbors_for_tangent_axis_ = min_neighbors_for_tangent_axis; + } + + /** \brief Get min number of neighbours required for the computation of X axis. + * + * \return min number of neighbours required for the computation of X axis. + */ + inline int + getMinNeighboursForTangentAxis () const + { + return (min_neighbors_for_tangent_axis_); + } + + + /** \brief Provide a pointer to the dataset used for the estimation of X axis. + * As the estimation of x axis is negligibly affected by surface downsampling, + * this method lets to consider a downsampled version of surface_ in the estimation of x axis. + * This is optional, if this is not set, it will only use the data in the + * surface_ cloud to estimate the x axis. + * \param[in] cloud a pointer to a PointCloud + */ + inline void + setSearchSampledSurface(const PointCloudInConstPtr &cloud) + { + sampled_surface_ = cloud; + fake_sampled_surface_ = false; + } + + /** \brief Get a pointer to the sampled_surface_ cloud dataset. */ + inline const PointCloudInConstPtr& + getSearchSampledSurface() const + { + return (sampled_surface_); + } + + /** \brief Provide a pointer to the search object linked to sampled_surface. + * \param[in] tree a pointer to the spatial search object linked to sampled_surface. + */ + inline void + setSearchMethodForSampledSurface (const KdTreePtr &tree) { sampled_tree_ = tree; } + + /** \brief Get a pointer to the search method used for the extimation of x axis. */ + inline const KdTreePtr& + getSearchMethodForSampledSurface () const + { + return (sampled_tree_); + } + + /** \brief Get the signed distances of the highest points from the fitted planes. */ + inline const std::vector & + getSignedDistancesFromHighestPoints () const + { + return (signed_distances_from_highest_points_); + } + + protected: + /** \brief This method should get called before starting the actual computation. */ + virtual bool + initCompute (); + + /** \brief This method should get called after the actual computation is ended. */ + virtual bool + deinitCompute (); + + /** \brief Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with normals + * \param[in] index the index of the point in input_ + * \param[out] lrf the resultant local reference frame + * \return signed distance of the highest point from the fitted plane. Max if the lrf is not computable. + */ + SignedDistanceT + computePointLRF (const int index, Eigen::Matrix3f &lrf); + + /** \brief Abstract feature estimation method. + * \param[out] output the resultant features + */ + virtual void + computeFeature (PointCloudOut &output); + + + private: + /** \brief Radius used to find tangent axis. */ + float tangent_radius_; + + /** \brief Threshold that define if a support point is near the margins. */ + float margin_thresh_; + + /** \brief Min number of neighbours required for the computation of Z axis. Otherwise, feature point normal is used. */ + int min_neighbors_for_normal_axis_; + + /** \brief Min number of neighbours required for the computation of X axis. Otherwise, a random X axis is set */ + int min_neighbors_for_tangent_axis_; + + /** \brief An input point cloud describing the surface that is to be used + * for nearest neighbor searches for the estimation of X axis. + */ + PointCloudInConstPtr sampled_surface_; + + /** \brief A pointer to the spatial search object used for the estimation of X axis. */ + KdTreePtr sampled_tree_; + + /** \brief Class for normal estimation. */ + NormalEstimation normal_estimation_; + + /** \brief Signed distances of the highest points from the fitted planes.*/ + std::vector signed_distances_from_highest_points_; + + /** \brief If no sampled_surface_ is given, we use surface_ as the sampled surface. */ + bool fake_sampled_surface_; + + }; + +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif //#ifndef PCL_FLARE_H_ diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp new file mode 100644 index 00000000000..6b14652b9ed --- /dev/null +++ b/features/include/pcl/features/impl/flare.hpp @@ -0,0 +1,263 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2016-, 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_FEATURES_IMPL_FLARE_H_ +#define PCL_FEATURES_IMPL_FLARE_H_ + +#include +#include + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool + pcl::FLARELocalReferenceFrameEstimation::initCompute () +{ + if (!FeatureFromNormals::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + if (tangent_radius_ == 0.0f) + { + PCL_ERROR ("[pcl::%s::initCompute] tangent_radius_ not set.\n", getClassName ().c_str ()); + return (false); + } + + // If no search sampled_surface_ has been defined, use the surface_ dataset as the search sampled_surface_ itself + if (!sampled_surface_) + { + fake_sampled_surface_ = true; + sampled_surface_ = surface_; + + if (sampled_tree_) + { + PCL_WARN ("[pcl::%s::initCompute] sampled_surface_ is not set even if sampled_tree_ is already set.", getClassName ().c_str ()); + PCL_WARN ("sampled_tree_ will be rebuilt from surface_. Use sampled_surface_.\n"); + } + } + + // Check if a space search locator was given for sampled_surface_ + if (!sampled_tree_) + { + if (sampled_surface_->isOrganized () && surface_->isOrganized () && input_->isOrganized ()) + sampled_tree_.reset (new pcl::search::OrganizedNeighbor ()); + else + sampled_tree_.reset (new pcl::search::KdTree (false)); + } + + if (sampled_tree_->getInputCloud () != sampled_surface_) // Make sure the tree searches the sampled surface + sampled_tree_->setInputCloud (sampled_surface_); + + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool + pcl::FLARELocalReferenceFrameEstimation::deinitCompute () +{ + // Reset the surface + if (fake_surface_) + { + surface_.reset (); + fake_surface_ = false; + } + // Reset the sampled surface + if (fake_sampled_surface_) + { + sampled_surface_.reset (); + fake_sampled_surface_ = false; + } + return (true); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template SignedDistanceT + pcl::FLARELocalReferenceFrameEstimation::computePointLRF (const int index, + Eigen::Matrix3f &lrf) +{ + Eigen::Vector3f x_axis, y_axis; + Eigen::Vector3f fitted_normal; //z_axis + + //find Z axis + + //extract support points for the computation of Z axis + std::vector neighbours_indices; + std::vector neighbours_distances; + int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); + + if (n_neighbours < min_neighbors_for_normal_axis_) + { + if (!pcl::isFinite ((*normals_)[index])) + { + //normal is invalid + //setting lrf to NaN + lrf.setConstant (std::numeric_limits::quiet_NaN ()); + return (std::numeric_limits::max ()); + } + + //set z_axis as the normal of index point + fitted_normal = (*normals_)[index].getNormalVector3fMap (); + } + else + { + float plane_curvature; + normal_estimation_.computePointNormal (*surface_, neighbours_indices, fitted_normal (0), fitted_normal (1), fitted_normal (2), plane_curvature); + + //disambiguate Z axis with normal mean + if (!pcl::flipNormalTowardsNormalsMean (*normals_, neighbours_indices, fitted_normal)) + { + //all normals in the neighbourood are invalid + //setting lrf to NaN + lrf.setConstant (std::numeric_limits::quiet_NaN ()); + return (std::numeric_limits::max ()); + } + } + + //setting LRF Z axis + lrf.row (2).matrix () = fitted_normal; + + //find X axis + + //extract support points for Rx radius + n_neighbours = sampled_tree_->radiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances); + + if (n_neighbours < min_neighbors_for_tangent_axis_) + { + //set X axis as a random axis + x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal); + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + + return (std::numeric_limits::max ()); + } + + //find point with the largest signed distance from tangent plane + + SignedDistanceT shape_score; + SignedDistanceT best_shape_score = -std::numeric_limits::max (); + int best_shape_index = -1; + + Eigen::Vector3f best_margin_point; + + const float radius2 = tangent_radius_ * tangent_radius_; + const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2; + + Vector3fMapConst feature_point = (*input_)[index].getVector3fMap (); + + for (int curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh) + { + const int& curr_neigh_idx = neighbours_indices[curr_neigh]; + const float& neigh_distance_sqr = neighbours_distances[curr_neigh]; + + if (neigh_distance_sqr <= margin_distance2) + { + continue; + } + + //point curr_neigh_idx is inside the ring between marginThresh and Radius + + shape_score = fitted_normal.dot ((*sampled_surface_)[curr_neigh_idx].getVector3fMap ()); + + if (shape_score > best_shape_score) + { + best_shape_index = curr_neigh_idx; + best_shape_score = shape_score; + } + } //for each neighbor + + if (best_shape_index == -1) + { + x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal); + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + + return (std::numeric_limits::max ()); + } + + //find orthogonal axis directed to best_shape_index point projection on plane with fittedNormal as axis + x_axis = pcl::geometry::projectedAsUnitVector (sampled_surface_->at (best_shape_index).getVector3fMap (), feature_point, fitted_normal); + + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + //z axis already set + + best_shape_score -= fitted_normal.dot (feature_point); + return (best_shape_score); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void + pcl::FLARELocalReferenceFrameEstimation::computeFeature (PointCloudOut &output) +{ + //check whether used with search radius or search k-neighbors + if (this->getKSearch () != 0) + { + PCL_ERROR ( + "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch (0) and setRadiusSearch (radius) to use this class.\n", + getClassName ().c_str ()); + return; + } + + signed_distances_from_highest_points_.resize (indices_->size ()); + + for (size_t point_idx = 0; point_idx < indices_->size (); ++point_idx) + { + Eigen::Matrix3f currentLrf; + PointOutT &rf = output[point_idx]; + + signed_distances_from_highest_points_[point_idx] = computePointLRF ((*indices_)[point_idx], currentLrf); + if (signed_distances_from_highest_points_[point_idx] == std::numeric_limits::max ()) + { + output.is_dense = false; + } + + rf.getXAxisVector3fMap () = currentLrf.row (0); + rf.getYAxisVector3fMap () = currentLrf.row (1); + rf.getZAxisVector3fMap () = currentLrf.row (2); + } +} + +#define PCL_INSTANTIATE_FLARELocalReferenceFrameEstimation(T,NT,OutT,SdT) template class PCL_EXPORTS pcl::FLARELocalReferenceFrameEstimation; + +#endif // PCL_FEATURES_IMPL_FLARE_H_ diff --git a/features/include/pcl/features/normal_3d.h b/features/include/pcl/features/normal_3d.h index 82f538d4b1c..8512d3951da 100644 --- a/features/include/pcl/features/normal_3d.h +++ b/features/include/pcl/features/normal_3d.h @@ -186,6 +186,49 @@ namespace pcl } } + /** \brief Flip (in place) normal to get the same sign of the mean of the normals specified by normal_indices. + * + * The method is described in: + * A. Petrelli, L. Di Stefano, "A repeatable and efficient canonical reference for surface matching", 3DimPVT, 2012 + * A. Petrelli, L. Di Stefano, "On the repeatability of the local reference frame for partial shape matching", 13th International Conference on Computer Vision (ICCV), 2011 + * + * Normals should be unit vectors. Otherwise the resulting mean would be weighted by the normal norms. + * \param[in] normal_cloud Cloud of normals used to compute the mean + * \param[in] normal_indices Indices of normals used to compute the mean + * \param[in] normal input Normal to flip. Normal is modified by the function. + * \return false if normal_indices does not contain any valid normal. + * \ingroup features + */ + template inline bool + flipNormalTowardsNormalsMean ( pcl::PointCloud const &normal_cloud, + std::vector const &normal_indices, + Eigen::Vector3f &normal) + { + Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero (); + + for (size_t i = 0; i < normal_indices.size (); ++i) + { + const PointNT& cur_pt = normal_cloud[normal_indices[i]]; + + if (pcl::isFinite (cur_pt)) + { + normal_mean += cur_pt.getNormalVector3fMap (); + } + } + + if (normal_mean.isZero ()) + return false; + + normal_mean.normalize (); + + if (normal.dot (normal_mean) < 0) + { + normal = -normal; + } + + return true; + } + /** \brief NormalEstimation estimates local surface properties (surface normals and curvatures)at each * 3D point. If PointOutT is specified as pcl::Normal, the normal is stored in the first 3 components (0-2), * and the curvature is stored in component 3. diff --git a/features/src/flare.cpp b/features/src/flare.cpp new file mode 100644 index 00000000000..5d35b762f5b --- /dev/null +++ b/features/src/flare.cpp @@ -0,0 +1,49 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2016-, 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 + + +#ifndef PCL_NO_PRECOMPILE +#include +#include +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES +PCL_INSTANTIATE_PRODUCT(FLARELocalReferenceFrameEstimation, ((pcl::PointXYZ)(pcl::PointXYZI)(pcl::PointXYZRGB)(pcl::PointXYZRGBA))((pcl::Normal))((pcl::ReferenceFrame))((float))) +#else +PCL_INSTANTIATE_PRODUCT(FLARELocalReferenceFrameEstimation, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::ReferenceFrame))((float))) +#endif +#endif // PCL_NO_PRECOMPILE diff --git a/test/features/CMakeLists.txt b/test/features/CMakeLists.txt index afa18a5f1f0..396b9f90b49 100644 --- a/test/features/CMakeLists.txt +++ b/test/features/CMakeLists.txt @@ -78,6 +78,10 @@ if (build) FILES test_board_estimation.cpp LINK_WITH pcl_gtest pcl_features pcl_io ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd") + PCL_ADD_TEST(feature_flare_estimation test_flare_estimation + FILES test_flare_estimation.cpp + LINK_WITH pcl_gtest pcl_features pcl_io + ARGUMENTS "${PCL_SOURCE_DIR}/test/bun0.pcd") PCL_ADD_TEST(feature_shot_lrf_estimation test_shot_lrf_estimation FILES test_shot_lrf_estimation.cpp LINK_WITH pcl_gtest pcl_features pcl_io @@ -105,4 +109,3 @@ if (build) endif (BUILD_keypoints) endif (BUILD_io) endif (build) - diff --git a/test/features/test_flare_estimation.cpp b/test/features/test_flare_estimation.cpp new file mode 100644 index 00000000000..0cfdcfeb4a8 --- /dev/null +++ b/test/features/test_flare_estimation.cpp @@ -0,0 +1,179 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2016-, 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. +* +* $Id$ +* +*/ + +#include +#include +#include +#include +#include +#include + +typedef pcl::search::KdTree::Ptr KdTreePtr; +typedef pcl::PointCloud::Ptr PointCloudPtr; + +PointCloudPtr cloud; +KdTreePtr tree; + +//sampled surface for the computation of tangent X axis +PointCloudPtr sampled_cloud; +KdTreePtr sampled_tree; + + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, FLARELocalReferenceFrameEstimation) +{ + pcl::PointCloud::Ptr normals (new pcl::PointCloud ()); + pcl::PointCloud bunny_LRF; + + const float mesh_res = 0.005f; + + // Compute normals + pcl::NormalEstimation ne; + + ne.setRadiusSearch (2.0f*mesh_res); + ne.setViewPoint (1, 1, 10); + ne.setInputCloud (cloud); + ne.setSearchMethod (tree); + + ne.compute (*normals); + + // Compute FLARE LRF + pcl::FLARELocalReferenceFrameEstimation lrf_estimator; + + lrf_estimator.setRadiusSearch (5 * mesh_res); + lrf_estimator.setTangentRadius (20 * mesh_res); + + lrf_estimator.setInputCloud (cloud); + lrf_estimator.setSearchSurface (cloud); + lrf_estimator.setInputNormals (normals); + lrf_estimator.setSearchMethod (tree); + lrf_estimator.setSearchMethodForSampledSurface (sampled_tree); + lrf_estimator.setSearchSampledSurface (sampled_cloud); + + lrf_estimator.compute (bunny_LRF); + + // TESTS + EXPECT_TRUE (bunny_LRF.is_dense); + + // Expected Results + float score_15 = -0.0059431493f; + Eigen::Vector3f point_15_x (-0.46138301f, 0.75752199f, -0.46182927f); + Eigen::Vector3f point_15_y (-0.78785944f, -0.11049186f, 0.60586232f); + Eigen::Vector3f point_15_z (0.40792558f, 0.64339107f, 0.64779979f); + float score_45 = 0.018918669f; + Eigen::Vector3f point_45_x (0.63724411f, -0.74846953f, -0.18361199f); + Eigen::Vector3f point_45_y (0.76468521f, 0.58447874f, 0.27136898f); + Eigen::Vector3f point_45_z (-0.095794097f, -0.31333363f, 0.94479918f); + float score_163 = -0.050190225f; + Eigen::Vector3f point_163_x (-0.67064381f, 0.45722002f, 0.58411193f); + Eigen::Vector3f point_163_y (-0.58332449f, -0.81150508f, -0.034525186f); + Eigen::Vector3f point_163_z (0.45822418f, -0.36388087f, 0.81093854f); + float score_253 = -0.025943652f; + Eigen::Vector3f point_253_x (0.88240892f, -0.26585102f, 0.38817233f); + Eigen::Vector3f point_253_y (0.19853911f, 0.95840079f, 0.20506138f); + Eigen::Vector3f point_253_z (-0.42654046f, -0.10388060f, 0.89848322f); + + + //Test Results + for (int d = 0; d < 3; ++d) + { + EXPECT_NEAR (point_15_x[d], bunny_LRF.at (15).x_axis[d], 1E-3); + EXPECT_NEAR (point_15_y[d], bunny_LRF.at (15).y_axis[d], 1E-3); + EXPECT_NEAR (point_15_z[d], bunny_LRF.at (15).z_axis[d], 1E-3); + + EXPECT_NEAR (point_45_x[d], bunny_LRF.at (45).x_axis[d], 1E-3); + EXPECT_NEAR (point_45_y[d], bunny_LRF.at (45).y_axis[d], 1E-3); + EXPECT_NEAR (point_45_z[d], bunny_LRF.at (45).z_axis[d], 1E-3); + + EXPECT_NEAR (point_163_x[d], bunny_LRF.at (163).x_axis[d], 1E-3); + EXPECT_NEAR (point_163_y[d], bunny_LRF.at (163).y_axis[d], 1E-3); + EXPECT_NEAR (point_163_z[d], bunny_LRF.at (163).z_axis[d], 1E-3); + + EXPECT_NEAR (point_253_x[d], bunny_LRF.at (253).x_axis[d], 1E-3); + EXPECT_NEAR (point_253_y[d], bunny_LRF.at (253).y_axis[d], 1E-3); + EXPECT_NEAR (point_253_z[d], bunny_LRF.at (253).z_axis[d], 1E-3); + } + EXPECT_NEAR (score_15, lrf_estimator.getSignedDistancesFromHighestPoints ()[15], 1E-4); + EXPECT_NEAR (score_45, lrf_estimator.getSignedDistancesFromHighestPoints ()[45], 1E-4); + EXPECT_NEAR (score_163, lrf_estimator.getSignedDistancesFromHighestPoints ()[163], 1E-4); + EXPECT_NEAR (score_253, lrf_estimator.getSignedDistancesFromHighestPoints ()[253], 1E-4); +} + + +/* ---[ */ +int +main (int argc, char** argv) +{ + if (argc < 2) + { + std::cerr << "No test file given. Please download `bun0.pcd` and pass its path to the test." << std::endl; + return (-1); + } + + cloud.reset (new pcl::PointCloud ()); + + if (pcl::io::loadPCDFile (argv[1], *cloud) < 0) + { + std::cerr << "Failed to read test file. Please download `bun0.pcd` and pass its path to the test." << std::endl; + return (-1); + } + + tree.reset (new pcl::search::KdTree (false)); + tree->setInputCloud (cloud); + + //create and set sampled point cloud for computation of X axis + const float sampling_perc = 0.2f; + const float sampling_incr = 1.0f / sampling_perc; + + sampled_cloud.reset (new pcl::PointCloud ()); + + std::vector sampled_indices; + for (float sa = 0.0f; sa < (float)cloud->points.size (); sa += sampling_incr) + sampled_indices.push_back (static_cast (sa)); + copyPointCloud (*cloud, sampled_indices, *sampled_cloud); + + sampled_tree.reset (new pcl::search::KdTree (false)); + sampled_tree->setInputCloud (sampled_cloud); + + //start tests + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} +/* ]--- */