From fca69e358339eb12e047c7f878c8d25f28273838 Mon Sep 17 00:00:00 2001 From: aliosciapetrelli Date: Wed, 30 Mar 2016 16:22:52 +0200 Subject: [PATCH 01/11] Add FLARELocalReferenceFrameEstimation class - Add FLARELocalReferenceFrameEstimation class - Add unit test for FLARELocalReferenceFrameEstimation class - Add LRF_utils.h - Add LRF_utils.cpp - Add directedOrthogonalAxis, projectPointOnPlane in lrf_utils - getAngleBetweenUnitVectors, randomOrthogonalAxis, normalDisambiguation and planeFitting methods of BOARDLocalReferenceFrameEstimation class moved to lrf_utils.hpp --- features/CMakeLists.txt | 6 + features/include/pcl/features/board.h | 76 ----- features/include/pcl/features/flare.h | 300 +++++++++++++++++ features/include/pcl/features/impl/board.hpp | 160 +-------- features/include/pcl/features/impl/flare.hpp | 318 ++++++++++++++++++ .../include/pcl/features/impl/lrf_utils.hpp | 86 +++++ features/include/pcl/features/lrf_utils.h | 142 ++++++++ features/src/flare.cpp | 49 +++ features/src/lrf_utils.cpp | 165 +++++++++ test/features/CMakeLists.txt | 5 +- test/features/test_flare_estimation.cpp | 203 +++++++++++ 11 files changed, 1283 insertions(+), 227 deletions(-) create mode 100644 features/include/pcl/features/flare.h create mode 100644 features/include/pcl/features/impl/flare.hpp create mode 100644 features/include/pcl/features/impl/lrf_utils.hpp create mode 100644 features/include/pcl/features/lrf_utils.h create mode 100644 features/src/flare.cpp create mode 100644 features/src/lrf_utils.cpp create mode 100644 test/features/test_flare_estimation.cpp diff --git a/features/CMakeLists.txt b/features/CMakeLists.txt index ad5d60bbd7e..c0a69963409 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" @@ -31,6 +32,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/intensity_gradient.h" "include/pcl/${SUBSYS_NAME}/intensity_spin.h" "include/pcl/${SUBSYS_NAME}/linear_least_squares_normal.h" + "include/pcl/${SUBSYS_NAME}/lrf_utils.h" "include/pcl/${SUBSYS_NAME}/moment_invariants.h" "include/pcl/${SUBSYS_NAME}/moment_of_inertia_estimation.h" "include/pcl/${SUBSYS_NAME}/multiscale_feature_persistence.h" @@ -66,6 +68,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" @@ -81,6 +84,7 @@ if(build) "include/pcl/${SUBSYS_NAME}/impl/intensity_gradient.hpp" "include/pcl/${SUBSYS_NAME}/impl/intensity_spin.hpp" "include/pcl/${SUBSYS_NAME}/impl/linear_least_squares_normal.hpp" + "include/pcl/${SUBSYS_NAME}/impl/lrf_utils.hpp" "include/pcl/${SUBSYS_NAME}/impl/moment_invariants.hpp" "include/pcl/${SUBSYS_NAME}/impl/moment_of_inertia_estimation.hpp" "include/pcl/${SUBSYS_NAME}/impl/multiscale_feature_persistence.hpp" @@ -114,6 +118,7 @@ if(build) set(srcs src/board.cpp + src/flare.cpp src/brisk_2d.cpp src/boundary.cpp src/cppf.cpp @@ -127,6 +132,7 @@ if(build) src/intensity_gradient.cpp src/intensity_spin.cpp src/linear_least_squares_normal.cpp + src/lrf_utils.cpp src/moment_invariants.cpp src/moment_of_inertia_estimation.cpp src/multiscale_feature_persistence.cpp diff --git a/features/include/pcl/features/board.h b/features/include/pcl/features/board.h index a9e1b4961ca..b55060f6b7d 100644 --- a/features/include/pcl/features/board.h +++ b/features/include/pcl/features/board.h @@ -258,82 +258,6 @@ namespace pcl virtual void computeFeature (PointCloudOut &output); - /** \brief Given an axis (with origin axis_origin), return the orthogonal axis directed to point. - * - * \note axis must be normalized. - * - * \param[in] axis the axis - * \param[in] axis_origin the axis_origin - * \param[in] point the point towards which the resulting axis is directed - * \param[out] directed_ortho_axis the directed orthogonal axis calculated - */ - void - directedOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f const &axis_origin, - Eigen::Vector3f const &point, Eigen::Vector3f &directed_ortho_axis); - - /** \brief return the angle (in radians) that rotate v1 to v2 with respect to axis . - * - * \param[in] v1 the first vector - * \param[in] v2 the second vector - * \param[in] axis the rotation axis. Axis must be normalized and orthogonal to plane defined by v1 and v2. - * \return angle - */ - float - getAngleBetweenUnitVectors (Eigen::Vector3f const &v1, Eigen::Vector3f const &v2, Eigen::Vector3f const &axis); - - /** \brief Disambiguates a normal direction using adjacent normals - * - * \param[in] normals_cloud a cloud of normals used for the calculation - * \param[in] normal_indices the indices of the normals in the cloud that should to be used for the calculation - * \param[in,out] normal the normal to disambiguate, the calculation is performed in place - */ - void - normalDisambiguation (pcl::PointCloud const &normals_cloud, std::vector const &normal_indices, - Eigen::Vector3f &normal); - - /** \brief Compute Least Square Plane Fitting in a set of 3D points - * - * \param[in] points Matrix(nPoints,3) of 3D points coordinates - * \param[out] center centroid of the distribution of points that belongs to the fitted plane - * \param[out] norm normal to the fitted plane - */ - void - planeFitting (Eigen::Matrix const &points, Eigen::Vector3f ¢er, - Eigen::Vector3f &norm); - - /** \brief Given a plane (origin and normal) and a point, return the projection of x on plane - * - * Equivalent to vtkPlane::ProjectPoint() - * - * \param[in] point the point to project - * \param[in] origin_point a point belonging to the plane - * \param[in] plane_normal normal of the plane - * \param[out] projected_point the projection of the point on the plane - */ - void - projectPointOnPlane (Eigen::Vector3f const &point, Eigen::Vector3f const &origin_point, - Eigen::Vector3f const &plane_normal, Eigen::Vector3f &projected_point); - - /** \brief Given an axis, return a random orthogonal axis - * - * \param[in] axis input axis - * \param[out] rand_ortho_axis an axis orthogonal to the input axis and whose direction is random - */ - void - randomOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f &rand_ortho_axis); - - /** \brief Check if val1 and val2 are equals. - * - * \param[in] val1 first number to check. - * \param[in] val2 second number to check. - * \param[in] zero_float_eps epsilon - * \return true if val1 is equal to val2, false otherwise. - */ - inline bool - areEquals (float val1, float val2, float zero_float_eps = 1E-8f) const - { - return (std::abs (val1 - val2) < zero_float_eps); - } private: /** \brief Radius used to find tangent axis. */ diff --git a/features/include/pcl/features/flare.h b/features/include/pcl/features/flare.h new file mode 100644 index 00000000000..c21d21f644d --- /dev/null +++ b/features/include/pcl/features/flare.h @@ -0,0 +1,300 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2010-2012, Willow Garage, Inc. +* Copyright (c) 2012-, 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; + + typedef typename Feature::PointCloudIn PointCloudIn; + typedef typename Feature::PointCloudOut PointCloudOut; + + typedef typename pcl::PointCloud PointCloudSignedDistance; + typedef typename PointCloudSignedDistance::Ptr PointCloudSignedDistancePtr; + + typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; + + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + + typedef typename pcl::search::Search::Ptr KdTreePtr; + + + + 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"; + } + + /** \brief Empty destructor */ + virtual ~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 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 KdTreePtr + getSearchMethodForSampledSurface () const + { + return (sampled_tree_); + } + + /** \brief Get the signed distances of the highest points from the fitted planes. */ + inline std::vector & + getSignedDistancesFromHighestPoints () + { + 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/board.hpp b/features/include/pcl/features/impl/board.hpp index bb3e4687799..c482edfd6fb 100644 --- a/features/include/pcl/features/impl/board.hpp +++ b/features/include/pcl/features/impl/board.hpp @@ -43,154 +43,10 @@ #include #include #include +#include -////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::BOARDLocalReferenceFrameEstimation::directedOrthogonalAxis ( - Eigen::Vector3f const &axis, - Eigen::Vector3f const &axis_origin, - Eigen::Vector3f const &point, - Eigen::Vector3f &directed_ortho_axis) -{ - Eigen::Vector3f projection; - projectPointOnPlane (point, axis_origin, axis, projection); - directed_ortho_axis = projection - axis_origin; - directed_ortho_axis.normalize (); - // check if the computed x axis is orthogonal to the normal - //assert(areEquals((float)(directed_ortho_axis.dot(axis)), 0.0f, 1E-3f)); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::BOARDLocalReferenceFrameEstimation::projectPointOnPlane ( - Eigen::Vector3f const &point, - Eigen::Vector3f const &origin_point, - Eigen::Vector3f const &plane_normal, - Eigen::Vector3f &projected_point) -{ - float t; - Eigen::Vector3f xo; - - xo = point - origin_point; - t = plane_normal.dot (xo); - - projected_point = point - (t * plane_normal); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -template float -pcl::BOARDLocalReferenceFrameEstimation::getAngleBetweenUnitVectors ( - Eigen::Vector3f const &v1, - Eigen::Vector3f const &v2, - Eigen::Vector3f const &axis) -{ - Eigen::Vector3f angle_orientation; - angle_orientation = v1.cross (v2); - float angle_radians = acosf (std::max (-1.0f, std::min (1.0f, v1.dot (v2)))); - - angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast (M_PI) - angle_radians) : angle_radians; - - return (angle_radians); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::BOARDLocalReferenceFrameEstimation::randomOrthogonalAxis ( - Eigen::Vector3f const &axis, - Eigen::Vector3f &rand_ortho_axis) -{ - if (!areEquals (axis.z (), 0.0f)) - { - rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z (); - } - else if (!areEquals (axis.y (), 0.0f)) - { - rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y (); - } - else if (!areEquals (axis.x (), 0.0f)) - { - rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x (); - } - - rand_ortho_axis.normalize (); - - // check if the computed x axis is orthogonal to the normal - //assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f)); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::BOARDLocalReferenceFrameEstimation::planeFitting ( - Eigen::Matrix const &points, - Eigen::Vector3f ¢er, - Eigen::Vector3f &norm) -{ - // ----------------------------------------------------- - // Plane Fitting using Singular Value Decomposition (SVD) - // ----------------------------------------------------- - - int n_points = static_cast (points.rows ()); - if (n_points == 0) - { - return; - } - - //find the center by averaging the points positions - center.setZero (); - - for (int i = 0; i < n_points; ++i) - { - center += points.row (i); - } - - center /= static_cast (n_points); - - //copy points - average (center) - Eigen::Matrix A (n_points, 3); //PointData - for (int i = 0; i < n_points; ++i) - { - A (i, 0) = points (i, 0) - center.x (); - A (i, 1) = points (i, 1) - center.y (); - A (i, 2) = points (i, 2) - center.z (); - } - - Eigen::JacobiSVD svd (A, Eigen::ComputeFullV); - norm = svd.matrixV ().col (2); -} - -////////////////////////////////////////////////////////////////////////////////////////////// -template void -pcl::BOARDLocalReferenceFrameEstimation::normalDisambiguation ( - pcl::PointCloud const &normal_cloud, - std::vector const &normal_indices, - Eigen::Vector3f &normal) -{ - Eigen::Vector3f normal_mean; - normal_mean.setZero (); - - for (size_t i = 0; i < normal_indices.size (); ++i) - { - const PointNT& curPt = normal_cloud[normal_indices[i]]; - - normal_mean += curPt.getNormalVector3fMap (); - } - - normal_mean.normalize (); - - if (normal.dot (normal_mean) < 0) - { - normal = -normal; - } -} ////////////////////////////////////////////////////////////////////////////////////////////// template float @@ -231,7 +87,13 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo planeFitting (neigh_points_mat, centroid, fitted_normal); //disambiguate Z axis with normal mean - normalDisambiguation (*normals_, neighbours_indices, fitted_normal); + if (!normalDisambiguation (*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; @@ -303,8 +165,7 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo { //find angle with respect to random axis previously calculated Eigen::Vector3f indicating_normal_vect; - directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), - surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect); + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect); float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal); int check_margin_array_idx = std::min (static_cast (floor (angle / max_boundary_angle)), check_margin_array_size_ - 1); @@ -432,8 +293,7 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo } //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis - directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), - surface_->at (min_normal_index).getVector3fMap (), x_axis); + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), surface_->at (min_normal_index).getVector3fMap (), x_axis); y_axis = fitted_normal.cross (x_axis); lrf.row (0).matrix () = x_axis; diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp new file mode 100644 index 00000000000..c2dd5e9e5c4 --- /dev/null +++ b/features/include/pcl/features/impl/flare.hpp @@ -0,0 +1,318 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2010-2011, Willow Garage, Inc. +* Copyright (c) 2012-, 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 +#include +#include + + +////////////////////////////////////////////////////////////////////////////////////////////// +template bool + pcl::FLARELocalReferenceFrameEstimation::initCompute () +{ + if (!FeatureFromNormals::initCompute ()) + { + PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); + return (false); + } + + if(!sampled_surface_ && sampled_tree_) + { + PCL_WARN ("[pcl::%s::initCompute] sampled_surface_ is not set even if sampled_tree_ is already set. sampled_tree_ will be rebuilt from surface_. Use sampled_surface_.\n", getClassName ().c_str ()); + } + + 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_; + } + + // 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(); + + return (std::numeric_limits::max ()); + } + else + { + ////find centroid for plane fitting + // Eigen::Vector3f centroid = Eigen::Vector3f::Zero(); + // Eigen::Vector3f mean_normal = Eigen::Vector3f::Zero(); + //for(size_t ne = 0; ne < neighbours_indices.size(); ++ne) + //{ + // centroid += (*surface_)[ neighbours_indices[ne] ].getVector3fMap(); + // mean_normal += (*normals_)[ neighbours_indices[ne] ].getNormalVector3fMap(); + //} + // centroid /= (float)neighbours_indices.size(); + // mean_normal.normalize(); + + + ////plane fitting + // EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero(); + // float temp; + // Eigen::Vector3f no_centroid = Eigen::Vector3f::Zero(); + //for(size_t ne = 0; ne < neighbours_indices.size(); ++ne) + //{ + // no_centroid = (*surface_)[ neighbours_indices[ne] ].getVector3fMap() - centroid; + + // covariance_matrix (0,0) += no_centroid.x() * no_centroid.x(); + // covariance_matrix (1,1) += no_centroid.y() * no_centroid.y(); + // covariance_matrix (2,2) = no_centroid.z() * no_centroid.z(); + + // temp = no_centroid.x() * no_centroid.y(); + // covariance_matrix (0,1) += temp; + // covariance_matrix (1,0) += temp; + // temp = no_centroid.x() * no_centroid.z(); + // covariance_matrix (0,2) += temp; + // covariance_matrix (2,0) += temp; + // temp = no_centroid.y() * no_centroid.z(); + // covariance_matrix (1,2) += temp; + // covariance_matrix (2,1) += temp; + // } + + //EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; + //EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; + //pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); + + //fitted_normal.x() = eigen_vector [0]; + //fitted_normal.y() = eigen_vector [1]; + //fitted_normal.z() = eigen_vector [2]; + + 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 (!normalDisambiguation (*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 + randomOrthogonalAxis (fitted_normal, x_axis); + y_axis = fitted_normal.cross (x_axis); + + return (std::numeric_limits::max ()); + } + + //find point with the largest signed distance from tangent plane + + SignedDistanceT shapeScore; + SignedDistanceT bestShapeScore = -std::numeric_limits::max(); + int bestShapeIndex = -1; + + Eigen::Vector3f best_margin_point; + + float radius2 = tangent_radius_ * tangent_radius_; + + 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 + + shapeScore = (*sampled_surface_)[curr_neigh_idx].getVector3fMap()[0] * fitted_normal[0] + + (*sampled_surface_)[curr_neigh_idx].getVector3fMap()[1] * fitted_normal[1] + + (*sampled_surface_)[curr_neigh_idx].getVector3fMap()[2] * fitted_normal[2]; + + if(shapeScore > bestShapeScore) + { + bestShapeIndex = curr_neigh_idx; + bestShapeScore = shapeScore; + } + + } //for each neighbor + + if (bestShapeIndex == -1) + { + + randomOrthogonalAxis (fitted_normal, x_axis); + y_axis = fitted_normal.cross (x_axis); + + return (std::numeric_limits::max ()); + } + + //find orthogonal axis directed to bestShapeIndex point projection on plane with fittedNormal as axis + directedOrthogonalAxis (fitted_normal, feature_point, sampled_surface_->at(bestShapeIndex).getVector3fMap(), x_axis); + + y_axis = fitted_normal.cross (x_axis); + + lrf.row (0).matrix () = x_axis; + lrf.row (1).matrix () = y_axis; + //z axis already set + + bestShapeScore -= (fitted_normal[0]*feature_point[0] + fitted_normal[1]*feature_point[1] + fitted_normal[2]*feature_point[2]); + return (bestShapeScore); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +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; + } + + if(signed_distances_from_highest_points_.size () != indices_->size ()) + 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; + } + + for (int d = 0; d < 3; ++d) + { + rf.x_axis[d] = currentLrf (0, d); + rf.y_axis[d] = currentLrf (1, d); + rf.z_axis[d] = currentLrf (2, d); + } + } +} + + +#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/impl/lrf_utils.hpp b/features/include/pcl/features/impl/lrf_utils.hpp new file mode 100644 index 00000000000..3f737ffa99b --- /dev/null +++ b/features/include/pcl/features/impl/lrf_utils.hpp @@ -0,0 +1,86 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2010-2011, Willow Garage, Inc. +* Copyright (c) 2012-, 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_LRF_UTILS_H_ +#define PCL_FEATURES_IMPL_LRF_UTILS_H_ + + +#include + + + +template bool + pcl::normalDisambiguation ( + pcl::PointCloud const &normal_cloud, + std::vector const &normal_indices, + Eigen::Vector3f &normal) +{ + Eigen::Vector3f normal_mean; + normal_mean.setZero (); + + bool at_least_one_valid_point = false; + for (size_t i = 0; i < normal_indices.size (); ++i) + { + const PointNT& curPt = normal_cloud[normal_indices[i]]; + + if(pcl::isFinite(curPt)) + { + normal_mean += curPt.getNormalVector3fMap (); + at_least_one_valid_point = true; + } + } + + if(!at_least_one_valid_point) + return false; + + normal_mean.normalize (); + + if (normal.dot (normal_mean) < 0) + { + normal = -normal; + } + + return true; +} + + +#define PCL_INSTANTIATE_normalDisambiguation(NT) template PCL_EXPORTS bool pcl::normalDisambiguation( pcl::PointCloud const &normal_cloud, std::vector const &normal_indices, Eigen::Vector3f &normal); + + +#endif diff --git a/features/include/pcl/features/lrf_utils.h b/features/include/pcl/features/lrf_utils.h new file mode 100644 index 00000000000..123dd9d5b09 --- /dev/null +++ b/features/include/pcl/features/lrf_utils.h @@ -0,0 +1,142 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2010-2012, Willow Garage, Inc. +* Copyright (c) 2012-, 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_LRF_UTILS_H_ +#define PCL_LRF_UTILS_H_ + +#include +#include + +namespace pcl +{ + + /** \brief Project point on the plane defined by plane_point and plane_normal. + * + * \param[in] point point to project + * \param[in] indices of normals used to disambiguate + * \param[in] normal to disambiguate. normal is modified by the function. + * \ingroup features + */ + void + projectPointOnPlane ( + Eigen::Vector3f const &point, + Eigen::Vector3f const &plane_point, + Eigen::Vector3f const &plane_normal, + Eigen::Vector3f &projected_point); + + + /** \brief Find the unit vector from axis_origin, directed toward point and orthogonal to axis. + * + * \param[in] axis input axis + * \param[in] axis_origin point belonging to axis + * \param[in] point point input point not belonging to axis + * \param[out] directed_ortho_axis unit vector from axis_origin, directed toward point and orthogonal to axis. + * \ingroup features + */ + void + directedOrthogonalAxis ( + Eigen::Vector3f const &axis, + Eigen::Vector3f const &axis_origin, + Eigen::Vector3f const &point, + Eigen::Vector3f &directed_ortho_axis); + + + /** \brief Find the angle between unit vectors v1 and v2. + * + * \param[in] v1 first unit vector + * \param[in] v1 second unit vector + * \param[in] axis axis orthogonal to v1 and v2. Sign of axis defines the sign of returned angle + * \return angle between unit vectors v1 and v2 + * \ingroup features + */ + float + getAngleBetweenUnitVectors ( + Eigen::Vector3f const &v1, + Eigen::Vector3f const &v2, + Eigen::Vector3f const &axis); + + /** \brief Define a random unit vector orthogonal to axis. + * + * \param[in] axis input axis + * \param[out] rand_ortho_axis random unit vector orthogonal to axis + * \ingroup features + */ + void + randomOrthogonalAxis ( + Eigen::Vector3f const &axis, + Eigen::Vector3f &rand_ortho_axis); + + /** \brief Find the plane that best fits points in the least-square sense. Use SVD decomposition. + * + * \param[in] points set of points to fit + * \param[out] centroid point belonging to the fitted plane and centroid of points + * \param[out] plane_normal normal of the fitted plane + * \ingroup features + */ + void + planeFitting ( + Eigen::Matrix const &points, + Eigen::Vector3f ¢roid, + Eigen::Vector3f &plane_normal); + + + /** \brief Disambiguate normal sign as 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 + * \param[in] normal_cloud input cloud of normals + * \param[in] normal_indices indices of normals used to disambiguate + * \param[in] normal normal to disambiguate. normal is modified by the function. + * \return false if normal_indices does not contain any valid normal. + * \ingroup features + */ + template bool + normalDisambiguation ( + pcl::PointCloud const &normal_cloud, + std::vector const &normal_indices, + Eigen::Vector3f &normal); + + +} + +#ifdef PCL_NO_PRECOMPILE +#include +#endif + +#endif //#ifndef PCL_LRF_UTILS_H_ diff --git a/features/src/flare.cpp b/features/src/flare.cpp new file mode 100644 index 00000000000..9da95ead616 --- /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) 2010-2012, Willow Garage, Inc. +* Copyright (c) 2012-, 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)(int)(short))) +#else +PCL_INSTANTIATE_PRODUCT(FLARELocalReferenceFrameEstimation, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::ReferenceFrame))((float)(int)(short))) +#endif +#endif // PCL_NO_PRECOMPILE diff --git a/features/src/lrf_utils.cpp b/features/src/lrf_utils.cpp new file mode 100644 index 00000000000..3dc6ecfc63c --- /dev/null +++ b/features/src/lrf_utils.cpp @@ -0,0 +1,165 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2010-2012, Willow Garage, Inc. +* Copyright (c) 2012-, 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 + + + +void + pcl::projectPointOnPlane ( + Eigen::Vector3f const &point, + Eigen::Vector3f const &plane_point, + Eigen::Vector3f const &plane_normal, + Eigen::Vector3f &projected_point) +{ + float t; + Eigen::Vector3f xo; + + xo = point - plane_point; + t = plane_normal.dot (xo); + + projected_point = point - (t * plane_normal); +} + + +void + pcl::directedOrthogonalAxis ( + Eigen::Vector3f const &axis, + Eigen::Vector3f const &axis_origin, + Eigen::Vector3f const &point, + Eigen::Vector3f &directed_ortho_axis) +{ + Eigen::Vector3f projection; + projectPointOnPlane (point, axis_origin, axis, projection); + directed_ortho_axis = projection - axis_origin; + + directed_ortho_axis.normalize (); +} + + +float + pcl::getAngleBetweenUnitVectors ( + Eigen::Vector3f const &v1, + Eigen::Vector3f const &v2, + Eigen::Vector3f const &axis) +{ + Eigen::Vector3f angle_orientation; + angle_orientation = v1.cross (v2); + float angle_radians = acosf (std::max (-1.0f, std::min (1.0f, v1.dot (v2)))); + + angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast (M_PI) - angle_radians) : angle_radians; + + return (angle_radians); +} + + +void + pcl::randomOrthogonalAxis ( + Eigen::Vector3f const &axis, + Eigen::Vector3f &rand_ortho_axis) +{ + if (std::abs (axis.z ()) > 1E-8f) + { + rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + 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.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + 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.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x (); + } + + rand_ortho_axis.normalize (); +} + + +void + pcl::planeFitting ( + Eigen::Matrix const &points, + Eigen::Vector3f ¢roid, + Eigen::Vector3f &plane_normal) +{ + + int n_points = static_cast (points.rows ()); + if (n_points == 0) + { + return; + } + + //find the center by averaging the points positions + centroid.setZero (); + + for (int i = 0; i < n_points; ++i) + { + centroid += points.row (i); + } + + centroid /= static_cast (n_points); + + //copy points - average (center) + Eigen::Matrix A (n_points, 3); + for (int i = 0; i < n_points; ++i) + { + A (i, 0) = points (i, 0) - centroid.x (); + A (i, 1) = points (i, 1) - centroid.y (); + A (i, 2) = points (i, 2) - centroid.z (); + } + + Eigen::JacobiSVD svd (A, Eigen::ComputeFullV); + plane_normal = svd.matrixV ().col (2); +} + + +#ifndef PCL_NO_PRECOMPILE +#include +#include +// Instantiations of specific point types +#ifdef PCL_ONLY_CORE_POINT_TYPES +PCL_INSTANTIATE(normalDisambiguation, (pcl::Normal)); +#else +PCL_INSTANTIATE(normalDisambiguation, PCL_NORMAL_POINT_TYPES); +#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..3236c5fe0d6 --- /dev/null +++ b/test/features/test_flare_estimation.cpp @@ -0,0 +1,203 @@ +/* +* Software License Agreement (BSD License) +* +* Point Cloud Library (PCL) - www.pointclouds.org +* Copyright (c) 2010-2012, Willow Garage, 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 + + +using namespace pcl; +using namespace pcl::test; +using namespace pcl::io; +using namespace std; + +typedef search::KdTree::Ptr KdTreePtr; +typedef PointCloud::Ptr PointCloudPtr; + +PointCloudPtr cloud; +vector indices; +KdTreePtr tree; + +//sampled surface for the computation of tangent X axis +PointCloudPtr sampled_cloud; +KdTreePtr sampled_tree; + + + + + +////////////////////////////////////////////////////////////////////////////////////////////////////////////////// +TEST (PCL, FLARELocalReferenceFrameEstimation) +{ + PointCloud::Ptr normals (new PointCloud ()); + PointCloud bunny_LRF; + + float meshRes = 0.005f; + + boost::shared_ptr > indicesptr (new vector (indices)); + + // Compute normals + NormalEstimation ne; + + ne.setRadiusSearch (2.0f*meshRes); + ne.setViewPoint (1, 1, 10); + ne.setInputCloud (cloud); + ne.setSearchMethod (tree); + ne.setIndices (indicesptr); + + ne.compute (*normals); + + // Compute FLARE LRF + FLARELocalReferenceFrameEstimation lrf_estimator; + + lrf_estimator.setRadiusSearch (5 * meshRes); + lrf_estimator.setTangentRadius (20 * meshRes); + + lrf_estimator.setInputCloud (cloud ); + lrf_estimator.setSearchSurface( cloud ); + lrf_estimator.setInputNormals (normals); + lrf_estimator.setSearchMethod (tree); + lrf_estimator.setIndices (indicesptr); + lrf_estimator.setSearchMethodForSampledSurface (sampled_tree); + lrf_estimator.setSearchSampledSurface(sampled_cloud); + + lrf_estimator.compute (bunny_LRF); + + // TESTS + EXPECT_EQ (indices.size (), bunny_LRF.size ()); + + 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 PointCloud() ); + + if (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); + } + + indices.resize (cloud->points.size ()); + for (size_t i = 0; i < indices.size (); ++i) + indices[i] = static_cast (i); + + tree.reset (new search::KdTree (false)); + tree->setInputCloud (cloud); + + //create and set sampled point cloud for computation of X axis + const float samplingPerc = 0.2f; + const float samplingIncr = 1.0f/samplingPerc; + + sampled_cloud.reset( new PointCloud() ); + + vector indices_sampled; + for(float sa = 0.0f; sa < (float)cloud->points.size (); sa += samplingIncr) + indices_sampled.push_back(static_cast (sa) ); + copyPointCloud(*cloud, indices_sampled, *sampled_cloud); + + sampled_tree.reset (new search::KdTree (false)); + sampled_tree->setInputCloud(sampled_cloud); + + + //start tests + testing::InitGoogleTest (&argc, argv); + return (RUN_ALL_TESTS ()); +} +/* ]--- */ + From 9f482de54a2c2fc9983cd364fa876675609f435e Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Mon, 17 Jul 2017 23:11:08 +0200 Subject: [PATCH 02/11] Refinements from review --- features/CMakeLists.txt | 9 +- features/include/pcl/features/board.h | 76 ++++ features/include/pcl/features/flare.h | 399 +++++++++++-------- features/include/pcl/features/impl/board.hpp | 160 +++++++- features/include/pcl/features/impl/flare.hpp | 122 +++--- features/src/flare.cpp | 62 ++- test/features/test_flare_estimation.cpp | 194 +++++---- 7 files changed, 664 insertions(+), 358 deletions(-) diff --git a/features/CMakeLists.txt b/features/CMakeLists.txt index c0a69963409..27fa44ec07c 100644 --- a/features/CMakeLists.txt +++ b/features/CMakeLists.txt @@ -15,7 +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}/flare.h" "include/pcl/${SUBSYS_NAME}/brisk_2d.h" "include/pcl/${SUBSYS_NAME}/cppf.h" "include/pcl/${SUBSYS_NAME}/cvfh.h" @@ -32,7 +32,6 @@ if(build) "include/pcl/${SUBSYS_NAME}/intensity_gradient.h" "include/pcl/${SUBSYS_NAME}/intensity_spin.h" "include/pcl/${SUBSYS_NAME}/linear_least_squares_normal.h" - "include/pcl/${SUBSYS_NAME}/lrf_utils.h" "include/pcl/${SUBSYS_NAME}/moment_invariants.h" "include/pcl/${SUBSYS_NAME}/moment_of_inertia_estimation.h" "include/pcl/${SUBSYS_NAME}/multiscale_feature_persistence.h" @@ -68,7 +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/flare.hpp" "include/pcl/${SUBSYS_NAME}/impl/brisk_2d.hpp" "include/pcl/${SUBSYS_NAME}/impl/cppf.hpp" "include/pcl/${SUBSYS_NAME}/impl/cvfh.hpp" @@ -84,7 +83,6 @@ if(build) "include/pcl/${SUBSYS_NAME}/impl/intensity_gradient.hpp" "include/pcl/${SUBSYS_NAME}/impl/intensity_spin.hpp" "include/pcl/${SUBSYS_NAME}/impl/linear_least_squares_normal.hpp" - "include/pcl/${SUBSYS_NAME}/impl/lrf_utils.hpp" "include/pcl/${SUBSYS_NAME}/impl/moment_invariants.hpp" "include/pcl/${SUBSYS_NAME}/impl/moment_of_inertia_estimation.hpp" "include/pcl/${SUBSYS_NAME}/impl/multiscale_feature_persistence.hpp" @@ -118,7 +116,7 @@ if(build) set(srcs src/board.cpp - src/flare.cpp + src/flare.cpp src/brisk_2d.cpp src/boundary.cpp src/cppf.cpp @@ -132,7 +130,6 @@ if(build) src/intensity_gradient.cpp src/intensity_spin.cpp src/linear_least_squares_normal.cpp - src/lrf_utils.cpp src/moment_invariants.cpp src/moment_of_inertia_estimation.cpp src/multiscale_feature_persistence.cpp diff --git a/features/include/pcl/features/board.h b/features/include/pcl/features/board.h index b55060f6b7d..a9e1b4961ca 100644 --- a/features/include/pcl/features/board.h +++ b/features/include/pcl/features/board.h @@ -258,6 +258,82 @@ namespace pcl virtual void computeFeature (PointCloudOut &output); + /** \brief Given an axis (with origin axis_origin), return the orthogonal axis directed to point. + * + * \note axis must be normalized. + * + * \param[in] axis the axis + * \param[in] axis_origin the axis_origin + * \param[in] point the point towards which the resulting axis is directed + * \param[out] directed_ortho_axis the directed orthogonal axis calculated + */ + void + directedOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f const &axis_origin, + Eigen::Vector3f const &point, Eigen::Vector3f &directed_ortho_axis); + + /** \brief return the angle (in radians) that rotate v1 to v2 with respect to axis . + * + * \param[in] v1 the first vector + * \param[in] v2 the second vector + * \param[in] axis the rotation axis. Axis must be normalized and orthogonal to plane defined by v1 and v2. + * \return angle + */ + float + getAngleBetweenUnitVectors (Eigen::Vector3f const &v1, Eigen::Vector3f const &v2, Eigen::Vector3f const &axis); + + /** \brief Disambiguates a normal direction using adjacent normals + * + * \param[in] normals_cloud a cloud of normals used for the calculation + * \param[in] normal_indices the indices of the normals in the cloud that should to be used for the calculation + * \param[in,out] normal the normal to disambiguate, the calculation is performed in place + */ + void + normalDisambiguation (pcl::PointCloud const &normals_cloud, std::vector const &normal_indices, + Eigen::Vector3f &normal); + + /** \brief Compute Least Square Plane Fitting in a set of 3D points + * + * \param[in] points Matrix(nPoints,3) of 3D points coordinates + * \param[out] center centroid of the distribution of points that belongs to the fitted plane + * \param[out] norm normal to the fitted plane + */ + void + planeFitting (Eigen::Matrix const &points, Eigen::Vector3f ¢er, + Eigen::Vector3f &norm); + + /** \brief Given a plane (origin and normal) and a point, return the projection of x on plane + * + * Equivalent to vtkPlane::ProjectPoint() + * + * \param[in] point the point to project + * \param[in] origin_point a point belonging to the plane + * \param[in] plane_normal normal of the plane + * \param[out] projected_point the projection of the point on the plane + */ + void + projectPointOnPlane (Eigen::Vector3f const &point, Eigen::Vector3f const &origin_point, + Eigen::Vector3f const &plane_normal, Eigen::Vector3f &projected_point); + + /** \brief Given an axis, return a random orthogonal axis + * + * \param[in] axis input axis + * \param[out] rand_ortho_axis an axis orthogonal to the input axis and whose direction is random + */ + void + randomOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f &rand_ortho_axis); + + /** \brief Check if val1 and val2 are equals. + * + * \param[in] val1 first number to check. + * \param[in] val2 second number to check. + * \param[in] zero_float_eps epsilon + * \return true if val1 is equal to val2, false otherwise. + */ + inline bool + areEquals (float val1, float val2, float zero_float_eps = 1E-8f) const + { + return (std::abs (val1 - val2) < zero_float_eps); + } private: /** \brief Radius used to find tangent axis. */ diff --git a/features/include/pcl/features/flare.h b/features/include/pcl/features/flare.h index c21d21f644d..f8d41bb2ae5 100644 --- a/features/include/pcl/features/flare.h +++ b/features/include/pcl/features/flare.h @@ -2,8 +2,7 @@ * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org -* Copyright (c) 2010-2012, Willow Garage, Inc. -* Copyright (c) 2012-, Open Perception, Inc. +* Copyright (c) 2016-, Open Perception, Inc. * * All rights reserved. * @@ -47,250 +46,304 @@ namespace pcl { + + /** \brief Project point on the plane defined by plane_point and plane_normal. + * + * \param[in] point point to project + * \param[in] indices of normals used to disambiguate + * \param[in] normal to disambiguate. normal is modified by the function. + * \ingroup features + */ + void + projectPointOnPlane ( Eigen::Vector3f const &point, + Eigen::Vector3f const &plane_point, + Eigen::Vector3f const &plane_normal, + Eigen::Vector3f &projected_point); + + + /** \brief Find the unit vector from axis_origin, directed toward point and orthogonal to axis. + * + * \param[in] axis input axis + * \param[in] axis_origin point belonging to axis + * \param[in] point point input point not belonging to axis + * \param[out] directed_ortho_axis unit vector from axis_origin, directed toward point and orthogonal to axis. + * \ingroup features + */ + void + directedOrthogonalAxis ( Eigen::Vector3f const &axis, + Eigen::Vector3f const &axis_origin, + Eigen::Vector3f const &point, + Eigen::Vector3f &directed_ortho_axis); + + + /** \brief Define a random unit vector orthogonal to axis. + * + * \param[in] axis input axis + * \param[out] rand_ortho_axis random unit vector orthogonal to axis + * \ingroup features + */ + void + randomOrthogonalAxis ( Eigen::Vector3f const &axis, + Eigen::Vector3f &rand_ortho_axis); + + + /** \brief Disambiguate normal sign as 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 + * \param[in] normal_cloud input cloud of normals + * \param[in] normal_indices indices of normals used to disambiguate + * \param[in] normal normal to disambiguate. normal is modified by the function. + * \return false if normal_indices does not contain any valid normal. + * \ingroup features + */ + template bool + normalDisambiguation ( pcl::PointCloud const &normal_cloud, + std::vector const &normal_indices, + Eigen::Vector3f &normal); + + /** \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 - */ + * 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; - - typedef typename Feature::PointCloudIn PointCloudIn; - typedef typename Feature::PointCloudOut PointCloudOut; + 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; - typedef typename pcl::PointCloud PointCloudSignedDistance; - typedef typename PointCloudSignedDistance::Ptr PointCloudSignedDistancePtr; + typedef typename Feature::PointCloudIn PointCloudIn; + typedef typename Feature::PointCloudOut PointCloudOut; - typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; + typedef typename pcl::PointCloud PointCloudSignedDistance; + typedef typename PointCloudSignedDistance::Ptr PointCloudSignedDistancePtr; - typedef boost::shared_ptr > Ptr; - typedef boost::shared_ptr > ConstPtr; + typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; - typedef typename pcl::search::Search::Ptr KdTreePtr; + typedef boost::shared_ptr > Ptr; + typedef boost::shared_ptr > ConstPtr; + typedef typename pcl::search::Search::Ptr KdTreePtr; - - public: - - /** \brief Constructor. */ - FLARELocalReferenceFrameEstimation () : + 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"; - } - - /** \brief Empty destructor */ - virtual ~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. + 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"; + } + + /** \brief Empty destructor */ + virtual + ~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; - } + 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. + /** \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_); - } + 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. + /** \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; - } + 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. + /** \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_); - } + inline float + getMarginThresh () const + { + return (margin_thresh_); + } - /** \brief Set min number of neighbours required for the computation of Z axis. + /** \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; - } + 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. + /** \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_); - } + inline int + getMinNeighboursForNormalAxis () const + { + return (min_neighbors_for_normal_axis_); + } - /** \brief Set min number of neighbours required for the computation of X 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; - } + 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. + /** \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_); - } + inline int + getMinNeighboursForTangentAxis () const + { + return (min_neighbors_for_tangent_axis_); + } - /** \brief Provide a pointer to the dataset used for the estimation of X 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 PointCloudInConstPtr - getSearchSampledSurface() const - { - return (sampled_surface_); - } - - /** \brief Provide a pointer to the search object linked to sampled_surface. + 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 KdTreePtr - getSearchMethodForSampledSurface () const - { - return (sampled_tree_); - } - - /** \brief Get the signed distances of the highest points from the fitted planes. */ - inline std::vector & - getSignedDistancesFromHighestPoints () - { - return (signed_distances_from_highest_points_); - } - - protected: - /** \brief This method should get called before starting the actual computation. */ - virtual bool + 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 + /** \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 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 + /** \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_; + 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 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 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 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 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 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 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 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_; + /** \brief If no sampled_surface_ is given, we use surface_ as the sampled surface. */ + bool fake_sampled_surface_; }; - } #ifdef PCL_NO_PRECOMPILE diff --git a/features/include/pcl/features/impl/board.hpp b/features/include/pcl/features/impl/board.hpp index c482edfd6fb..bb3e4687799 100644 --- a/features/include/pcl/features/impl/board.hpp +++ b/features/include/pcl/features/impl/board.hpp @@ -43,10 +43,154 @@ #include #include #include -#include +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::directedOrthogonalAxis ( + Eigen::Vector3f const &axis, + Eigen::Vector3f const &axis_origin, + Eigen::Vector3f const &point, + Eigen::Vector3f &directed_ortho_axis) +{ + Eigen::Vector3f projection; + projectPointOnPlane (point, axis_origin, axis, projection); + directed_ortho_axis = projection - axis_origin; + directed_ortho_axis.normalize (); + // check if the computed x axis is orthogonal to the normal + //assert(areEquals((float)(directed_ortho_axis.dot(axis)), 0.0f, 1E-3f)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::projectPointOnPlane ( + Eigen::Vector3f const &point, + Eigen::Vector3f const &origin_point, + Eigen::Vector3f const &plane_normal, + Eigen::Vector3f &projected_point) +{ + float t; + Eigen::Vector3f xo; + + xo = point - origin_point; + t = plane_normal.dot (xo); + + projected_point = point - (t * plane_normal); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template float +pcl::BOARDLocalReferenceFrameEstimation::getAngleBetweenUnitVectors ( + Eigen::Vector3f const &v1, + Eigen::Vector3f const &v2, + Eigen::Vector3f const &axis) +{ + Eigen::Vector3f angle_orientation; + angle_orientation = v1.cross (v2); + float angle_radians = acosf (std::max (-1.0f, std::min (1.0f, v1.dot (v2)))); + + angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast (M_PI) - angle_radians) : angle_radians; + + return (angle_radians); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::randomOrthogonalAxis ( + Eigen::Vector3f const &axis, + Eigen::Vector3f &rand_ortho_axis) +{ + if (!areEquals (axis.z (), 0.0f)) + { + rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z (); + } + else if (!areEquals (axis.y (), 0.0f)) + { + rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y (); + } + else if (!areEquals (axis.x (), 0.0f)) + { + rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x (); + } + + rand_ortho_axis.normalize (); + + // check if the computed x axis is orthogonal to the normal + //assert(areEquals(rand_ortho_axis.dot(axis), 0.0f, 1E-6f)); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::planeFitting ( + Eigen::Matrix const &points, + Eigen::Vector3f ¢er, + Eigen::Vector3f &norm) +{ + // ----------------------------------------------------- + // Plane Fitting using Singular Value Decomposition (SVD) + // ----------------------------------------------------- + + int n_points = static_cast (points.rows ()); + if (n_points == 0) + { + return; + } + + //find the center by averaging the points positions + center.setZero (); + + for (int i = 0; i < n_points; ++i) + { + center += points.row (i); + } + + center /= static_cast (n_points); + + //copy points - average (center) + Eigen::Matrix A (n_points, 3); //PointData + for (int i = 0; i < n_points; ++i) + { + A (i, 0) = points (i, 0) - center.x (); + A (i, 1) = points (i, 1) - center.y (); + A (i, 2) = points (i, 2) - center.z (); + } + + Eigen::JacobiSVD svd (A, Eigen::ComputeFullV); + norm = svd.matrixV ().col (2); +} + +////////////////////////////////////////////////////////////////////////////////////////////// +template void +pcl::BOARDLocalReferenceFrameEstimation::normalDisambiguation ( + pcl::PointCloud const &normal_cloud, + std::vector const &normal_indices, + Eigen::Vector3f &normal) +{ + Eigen::Vector3f normal_mean; + normal_mean.setZero (); + + for (size_t i = 0; i < normal_indices.size (); ++i) + { + const PointNT& curPt = normal_cloud[normal_indices[i]]; + + normal_mean += curPt.getNormalVector3fMap (); + } + + normal_mean.normalize (); + + if (normal.dot (normal_mean) < 0) + { + normal = -normal; + } +} ////////////////////////////////////////////////////////////////////////////////////////////// template float @@ -87,13 +231,7 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo planeFitting (neigh_points_mat, centroid, fitted_normal); //disambiguate Z axis with normal mean - if (!normalDisambiguation (*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 ()); - } + normalDisambiguation (*normals_, neighbours_indices, fitted_normal); //setting LRF Z axis lrf.row (2).matrix () = fitted_normal; @@ -165,7 +303,8 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo { //find angle with respect to random axis previously calculated Eigen::Vector3f indicating_normal_vect; - directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect); + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), + surface_->at (curr_neigh_idx).getVector3fMap (), indicating_normal_vect); float angle = getAngleBetweenUnitVectors (x_axis, indicating_normal_vect, fitted_normal); int check_margin_array_idx = std::min (static_cast (floor (angle / max_boundary_angle)), check_margin_array_size_ - 1); @@ -293,7 +432,8 @@ pcl::BOARDLocalReferenceFrameEstimation::computePo } //find orthogonal axis directed to minNormalIndex point projection on plane with fittedNormal as axis - directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), surface_->at (min_normal_index).getVector3fMap (), x_axis); + directedOrthogonalAxis (fitted_normal, input_->at (index).getVector3fMap (), + surface_->at (min_normal_index).getVector3fMap (), x_axis); y_axis = fitted_normal.cross (x_axis); lrf.row (0).matrix () = x_axis; diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp index c2dd5e9e5c4..c9d7a31b9ba 100644 --- a/features/include/pcl/features/impl/flare.hpp +++ b/features/include/pcl/features/impl/flare.hpp @@ -2,8 +2,7 @@ * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org -* Copyright (c) 2010-2011, Willow Garage, Inc. -* Copyright (c) 2012-, Open Perception, Inc. +* Copyright (c) 2016-, Open Perception, Inc. * * All rights reserved. * @@ -44,7 +43,40 @@ #include #include #include -#include + + +template bool +pcl::normalDisambiguation ( pcl::PointCloud const &normal_cloud, + std::vector const &normal_indices, + Eigen::Vector3f &normal) +{ + Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero(); + + bool at_least_one_valid_point = false; + for (size_t i = 0; i < normal_indices.size (); ++i) + { + const PointNT& curPt = normal_cloud[normal_indices[i]]; + + if(pcl::isFinite(curPt)) + { + normal_mean += curPt.getNormalVector3fMap (); + at_least_one_valid_point = true; + } + } + + if(!at_least_one_valid_point) + return false; + + normal_mean.normalize (); + + if (normal.dot (normal_mean) < 0) + { + normal = -normal; + } + + return true; +} + ////////////////////////////////////////////////////////////////////////////////////////////// @@ -57,11 +89,6 @@ template SignedDistanceT - pcl::FLARELocalReferenceFrameEstimation::computePointLRF (const int &index, + pcl::FLARELocalReferenceFrameEstimation::computePointLRF (const int index, Eigen::Matrix3f &lrf) { Eigen::Vector3f x_axis, y_axis; @@ -122,7 +155,7 @@ template neighbours_indices; std::vector neighbours_distances; - int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); + int n_neighbours = searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); if (n_neighbours < min_neighbors_for_normal_axis_) { @@ -136,54 +169,9 @@ template::max ()); } else { - ////find centroid for plane fitting - // Eigen::Vector3f centroid = Eigen::Vector3f::Zero(); - // Eigen::Vector3f mean_normal = Eigen::Vector3f::Zero(); - //for(size_t ne = 0; ne < neighbours_indices.size(); ++ne) - //{ - // centroid += (*surface_)[ neighbours_indices[ne] ].getVector3fMap(); - // mean_normal += (*normals_)[ neighbours_indices[ne] ].getNormalVector3fMap(); - //} - // centroid /= (float)neighbours_indices.size(); - // mean_normal.normalize(); - - - ////plane fitting - // EIGEN_ALIGN16 Eigen::Matrix3f covariance_matrix = Eigen::Matrix3f::Zero(); - // float temp; - // Eigen::Vector3f no_centroid = Eigen::Vector3f::Zero(); - //for(size_t ne = 0; ne < neighbours_indices.size(); ++ne) - //{ - // no_centroid = (*surface_)[ neighbours_indices[ne] ].getVector3fMap() - centroid; - - // covariance_matrix (0,0) += no_centroid.x() * no_centroid.x(); - // covariance_matrix (1,1) += no_centroid.y() * no_centroid.y(); - // covariance_matrix (2,2) = no_centroid.z() * no_centroid.z(); - - // temp = no_centroid.x() * no_centroid.y(); - // covariance_matrix (0,1) += temp; - // covariance_matrix (1,0) += temp; - // temp = no_centroid.x() * no_centroid.z(); - // covariance_matrix (0,2) += temp; - // covariance_matrix (2,0) += temp; - // temp = no_centroid.y() * no_centroid.z(); - // covariance_matrix (1,2) += temp; - // covariance_matrix (2,1) += temp; - // } - - //EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value; - //EIGEN_ALIGN16 Eigen::Vector3f eigen_vector; - //pcl::eigen33 (covariance_matrix, eigen_value, eigen_vector); - - //fitted_normal.x() = eigen_vector [0]; - //fitted_normal.y() = eigen_vector [1]; - //fitted_normal.z() = eigen_vector [2]; - float plane_curvature; normal_estimation_.computePointNormal(*surface_, neighbours_indices, fitted_normal(0), fitted_normal(1), fitted_normal(2), plane_curvature); @@ -212,6 +200,9 @@ template::max ()); } @@ -242,9 +233,7 @@ template bestShapeScore) { @@ -256,10 +245,12 @@ template::max ()); } @@ -272,7 +263,7 @@ template::computeFeature (PointCloudOut &output) { //check whether used with search radius or search k-neighbors - if (this->getKSearch () != 0) + if (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", @@ -303,12 +294,9 @@ template + +void +pcl::projectPointOnPlane ( Eigen::Vector3f const &point, + Eigen::Vector3f const &plane_point, + Eigen::Vector3f const &plane_normal, + Eigen::Vector3f &projected_point) +{ + const Eigen::Vector3f xo = point - plane_point; + const float t = plane_normal.dot (xo); + + projected_point = point - (t * plane_normal); +} + + +void +pcl::directedOrthogonalAxis ( Eigen::Vector3f const &axis, + Eigen::Vector3f const &axis_origin, + Eigen::Vector3f const &point, + Eigen::Vector3f &directed_ortho_axis) +{ + Eigen::Vector3f projection; + projectPointOnPlane (point, axis_origin, axis, projection); + directed_ortho_axis = projection - axis_origin; + + directed_ortho_axis.normalize (); +} + + +void +pcl::randomOrthogonalAxis ( Eigen::Vector3f const &axis, + Eigen::Vector3f &rand_ortho_axis) +{ + if (std::abs (axis.z ()) > 1E-8f) + { + rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + 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.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + 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.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + 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 (); +} + + #ifndef PCL_NO_PRECOMPILE #include #include diff --git a/test/features/test_flare_estimation.cpp b/test/features/test_flare_estimation.cpp index 3236c5fe0d6..df5c50c11a1 100644 --- a/test/features/test_flare_estimation.cpp +++ b/test/features/test_flare_estimation.cpp @@ -2,7 +2,7 @@ * Software License Agreement (BSD License) * * Point Cloud Library (PCL) - www.pointclouds.org -* Copyright (c) 2010-2012, Willow Garage, Inc. +* Copyright (c) 2016-, Open Perception, Inc. * * All rights reserved. * @@ -43,21 +43,16 @@ #include #include #include -#include +#include #include #include -using namespace pcl; -using namespace pcl::test; -using namespace pcl::io; -using namespace std; - -typedef search::KdTree::Ptr KdTreePtr; -typedef PointCloud::Ptr PointCloudPtr; +typedef pcl::search::KdTree::Ptr KdTreePtr; +typedef pcl::PointCloud::Ptr PointCloudPtr; PointCloudPtr cloud; -vector indices; +std::vector indices; KdTreePtr tree; //sampled surface for the computation of tangent X axis @@ -71,87 +66,87 @@ KdTreePtr sampled_tree; ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, FLARELocalReferenceFrameEstimation) { - PointCloud::Ptr normals (new PointCloud ()); - PointCloud bunny_LRF; - - float meshRes = 0.005f; - - boost::shared_ptr > indicesptr (new vector (indices)); - - // Compute normals - NormalEstimation ne; - - ne.setRadiusSearch (2.0f*meshRes); - ne.setViewPoint (1, 1, 10); - ne.setInputCloud (cloud); - ne.setSearchMethod (tree); - ne.setIndices (indicesptr); - - ne.compute (*normals); - - // Compute FLARE LRF - FLARELocalReferenceFrameEstimation lrf_estimator; - - lrf_estimator.setRadiusSearch (5 * meshRes); - lrf_estimator.setTangentRadius (20 * meshRes); - - lrf_estimator.setInputCloud (cloud ); - lrf_estimator.setSearchSurface( cloud ); - lrf_estimator.setInputNormals (normals); - lrf_estimator.setSearchMethod (tree); - lrf_estimator.setIndices (indicesptr); - lrf_estimator.setSearchMethodForSampledSurface (sampled_tree); - lrf_estimator.setSearchSampledSurface(sampled_cloud); - - lrf_estimator.compute (bunny_LRF); - - // TESTS - EXPECT_EQ (indices.size (), bunny_LRF.size ()); - - 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); + pcl::PointCloud::Ptr normals (new pcl::PointCloud ()); + pcl::PointCloud bunny_LRF; + + const float meshRes = 0.005f; + + boost::shared_ptr > indicesptr (new std::vector (indices)); + + // Compute normals + pcl::NormalEstimation ne; + + ne.setRadiusSearch (2.0f*meshRes); + ne.setViewPoint (1, 1, 10); + ne.setInputCloud (cloud); + ne.setSearchMethod (tree); + ne.setIndices (indicesptr); + + ne.compute (*normals); + + // Compute FLARE LRF + pcl::FLARELocalReferenceFrameEstimation lrf_estimator; + + lrf_estimator.setRadiusSearch (5 * meshRes); + lrf_estimator.setTangentRadius (20 * meshRes); + + lrf_estimator.setInputCloud (cloud ); + lrf_estimator.setSearchSurface( cloud ); + lrf_estimator.setInputNormals (normals); + lrf_estimator.setSearchMethod (tree); + lrf_estimator.setIndices (indicesptr); + lrf_estimator.setSearchMethodForSampledSurface (sampled_tree); + lrf_estimator.setSearchSampledSurface(sampled_cloud); + + lrf_estimator.compute (bunny_LRF); + + // TESTS + EXPECT_EQ (indices.size (), bunny_LRF.size ()); + + 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); } @@ -165,9 +160,9 @@ main (int argc, char** argv) return (-1); } - cloud.reset( new PointCloud() ); + cloud.reset( new pcl::PointCloud() ); - if (loadPCDFile (argv[1], *cloud) < 0) + 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); @@ -177,21 +172,21 @@ main (int argc, char** argv) for (size_t i = 0; i < indices.size (); ++i) indices[i] = static_cast (i); - tree.reset (new search::KdTree (false)); + tree.reset (new pcl::search::KdTree (false)); tree->setInputCloud (cloud); //create and set sampled point cloud for computation of X axis - const float samplingPerc = 0.2f; + const float samplingPerc = 0.2f; const float samplingIncr = 1.0f/samplingPerc; - sampled_cloud.reset( new PointCloud() ); + sampled_cloud.reset( new pcl::PointCloud() ); - vector indices_sampled; + std::vector indices_sampled; for(float sa = 0.0f; sa < (float)cloud->points.size (); sa += samplingIncr) - indices_sampled.push_back(static_cast (sa) ); + indices_sampled.push_back(static_cast (sa) ); copyPointCloud(*cloud, indices_sampled, *sampled_cloud); - sampled_tree.reset (new search::KdTree (false)); + sampled_tree.reset (new pcl::search::KdTree (false)); sampled_tree->setInputCloud(sampled_cloud); @@ -200,4 +195,3 @@ main (int argc, char** argv) return (RUN_ALL_TESTS ()); } /* ]--- */ - From 77c2cb32e23bebae97553241cdf13884cea26a85 Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Tue, 18 Jul 2017 22:24:37 +0200 Subject: [PATCH 03/11] Solve clang compile error clang compiler requires "this->" before getKSearch and searchForNeighbors base class methods of FLARELocalReferenceFrameEstimation. clang reports the error: "must qualify identifier to find this declaration in dependent base class," while Visual studio and gcc let it pass. --- features/include/pcl/features/impl/flare.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp index c9d7a31b9ba..aef2bf69a61 100644 --- a/features/include/pcl/features/impl/flare.hpp +++ b/features/include/pcl/features/impl/flare.hpp @@ -155,7 +155,7 @@ template neighbours_indices; std::vector neighbours_distances; - int n_neighbours = searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); + int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); if (n_neighbours < min_neighbors_for_normal_axis_) { @@ -272,7 +272,7 @@ template::computeFeature (PointCloudOut &output) { //check whether used with search radius or search k-neighbors - if (getKSearch () != 0) + 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", From 58186d3848dceef25be1f2689ad7df97ac1be2cb Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Sun, 30 Jul 2017 15:23:33 +0200 Subject: [PATCH 04/11] Move LRF utilities to common module -Remove LRFutils.* -Rename NormalDisambiguation to flipNormalTowardsNormalsMean and move to normal_3d.h -Remove projectPointOnPlane and use project instead -Move randomOrthogonalAxis and directedOrthogonalAxis to geometry.h --- common/include/pcl/common/geometry.h | 61 +++++++ features/include/pcl/features/flare.h | 58 +----- features/include/pcl/features/impl/flare.hpp | 45 +---- .../include/pcl/features/impl/lrf_utils.hpp | 86 --------- features/include/pcl/features/lrf_utils.h | 142 --------------- features/include/pcl/features/normal_3d.h | 43 +++++ features/src/flare.cpp | 58 ------ features/src/lrf_utils.cpp | 165 ------------------ 8 files changed, 110 insertions(+), 548 deletions(-) delete mode 100644 features/include/pcl/features/impl/lrf_utils.hpp delete mode 100644 features/include/pcl/features/lrf_utils.h delete mode 100644 features/src/lrf_utils.cpp diff --git a/common/include/pcl/common/geometry.h b/common/include/pcl/common/geometry.h index 2c110e7d9a7..1b8ddc382f7 100644 --- a/common/include/pcl/common/geometry.h +++ b/common/include/pcl/common/geometry.h @@ -101,6 +101,67 @@ namespace pcl float lambda = plane_normal.dot(po); projected = point - (lambda * plane_normal); } + + + /** \brief Find the unit vector from axis_origin, directed toward point and orthogonal to axis. + * + * \param[in] axis input axis + * \param[in] axis_origin point belonging to axis + * \param[in] point point input point not belonging to axis + * \param[out] directed_ortho_axis unit vector from axis_origin, directed toward point and orthogonal to axis. + * \ingroup features + */ + inline void + directedOrthogonalAxis ( Eigen::Vector3f const &axis, + Eigen::Vector3f const &axis_origin, + Eigen::Vector3f const &point, + Eigen::Vector3f &directed_ortho_axis) + { + Eigen::Vector3f projection; + project (point, axis_origin, axis, projection); + directed_ortho_axis = projection - axis_origin; + + directed_ortho_axis.normalize (); + } + + + /** \brief Define a random unit vector orthogonal to axis. + * + * \param[in] axis input axis + * \param[out] rand_ortho_axis random unit vector orthogonal to axis + * \ingroup features + */ + inline void + randomOrthogonalAxis ( Eigen::Vector3f const &axis, + Eigen::Vector3f &rand_ortho_axis) + { + if (std::abs (axis.z ()) > 1E-8f) + { + rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + 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.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + 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.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; + 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 (); + } + + } } diff --git a/features/include/pcl/features/flare.h b/features/include/pcl/features/flare.h index f8d41bb2ae5..24720d12e18 100644 --- a/features/include/pcl/features/flare.h +++ b/features/include/pcl/features/flare.h @@ -41,68 +41,12 @@ #include #include -#include +#include namespace pcl { - /** \brief Project point on the plane defined by plane_point and plane_normal. - * - * \param[in] point point to project - * \param[in] indices of normals used to disambiguate - * \param[in] normal to disambiguate. normal is modified by the function. - * \ingroup features - */ - void - projectPointOnPlane ( Eigen::Vector3f const &point, - Eigen::Vector3f const &plane_point, - Eigen::Vector3f const &plane_normal, - Eigen::Vector3f &projected_point); - - - /** \brief Find the unit vector from axis_origin, directed toward point and orthogonal to axis. - * - * \param[in] axis input axis - * \param[in] axis_origin point belonging to axis - * \param[in] point point input point not belonging to axis - * \param[out] directed_ortho_axis unit vector from axis_origin, directed toward point and orthogonal to axis. - * \ingroup features - */ - void - directedOrthogonalAxis ( Eigen::Vector3f const &axis, - Eigen::Vector3f const &axis_origin, - Eigen::Vector3f const &point, - Eigen::Vector3f &directed_ortho_axis); - - - /** \brief Define a random unit vector orthogonal to axis. - * - * \param[in] axis input axis - * \param[out] rand_ortho_axis random unit vector orthogonal to axis - * \ingroup features - */ - void - randomOrthogonalAxis ( Eigen::Vector3f const &axis, - Eigen::Vector3f &rand_ortho_axis); - - - /** \brief Disambiguate normal sign as 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 - * \param[in] normal_cloud input cloud of normals - * \param[in] normal_indices indices of normals used to disambiguate - * \param[in] normal normal to disambiguate. normal is modified by the function. - * \return false if normal_indices does not contain any valid normal. - * \ingroup features - */ - template bool - normalDisambiguation ( pcl::PointCloud const &normal_cloud, - std::vector const &normal_indices, - Eigen::Vector3f &normal); - - /** \brief FLARELocalReferenceFrameEstimation implements the Fast LocAl Reference framE algorithm * for local reference frame estimation as described here: * diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp index aef2bf69a61..2674c5e2c6f 100644 --- a/features/include/pcl/features/impl/flare.hpp +++ b/features/include/pcl/features/impl/flare.hpp @@ -41,42 +41,7 @@ #include -#include -#include - - -template bool -pcl::normalDisambiguation ( pcl::PointCloud const &normal_cloud, - std::vector const &normal_indices, - Eigen::Vector3f &normal) -{ - Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero(); - - bool at_least_one_valid_point = false; - for (size_t i = 0; i < normal_indices.size (); ++i) - { - const PointNT& curPt = normal_cloud[normal_indices[i]]; - - if(pcl::isFinite(curPt)) - { - normal_mean += curPt.getNormalVector3fMap (); - at_least_one_valid_point = true; - } - } - - if(!at_least_one_valid_point) - return false; - - normal_mean.normalize (); - - if (normal.dot (normal_mean) < 0) - { - normal = -normal; - } - - return true; -} - +#include ////////////////////////////////////////////////////////////////////////////////////////////// @@ -176,7 +141,7 @@ template (*normals_, neighbours_indices, fitted_normal) ) + if (!pcl::flipNormalTowardsNormalsMean (*normals_, neighbours_indices, fitted_normal) ) { //all normals in the neighbourood are invalid //setting lrf to NaN @@ -197,7 +162,7 @@ templateat(bestShapeIndex).getVector3fMap(), x_axis); + pcl::geometry::directedOrthogonalAxis (fitted_normal, feature_point, sampled_surface_->at(bestShapeIndex).getVector3fMap(), x_axis); y_axis = fitted_normal.cross (x_axis); diff --git a/features/include/pcl/features/impl/lrf_utils.hpp b/features/include/pcl/features/impl/lrf_utils.hpp deleted file mode 100644 index 3f737ffa99b..00000000000 --- a/features/include/pcl/features/impl/lrf_utils.hpp +++ /dev/null @@ -1,86 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Point Cloud Library (PCL) - www.pointclouds.org -* Copyright (c) 2010-2011, Willow Garage, Inc. -* Copyright (c) 2012-, 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_LRF_UTILS_H_ -#define PCL_FEATURES_IMPL_LRF_UTILS_H_ - - -#include - - - -template bool - pcl::normalDisambiguation ( - pcl::PointCloud const &normal_cloud, - std::vector const &normal_indices, - Eigen::Vector3f &normal) -{ - Eigen::Vector3f normal_mean; - normal_mean.setZero (); - - bool at_least_one_valid_point = false; - for (size_t i = 0; i < normal_indices.size (); ++i) - { - const PointNT& curPt = normal_cloud[normal_indices[i]]; - - if(pcl::isFinite(curPt)) - { - normal_mean += curPt.getNormalVector3fMap (); - at_least_one_valid_point = true; - } - } - - if(!at_least_one_valid_point) - return false; - - normal_mean.normalize (); - - if (normal.dot (normal_mean) < 0) - { - normal = -normal; - } - - return true; -} - - -#define PCL_INSTANTIATE_normalDisambiguation(NT) template PCL_EXPORTS bool pcl::normalDisambiguation( pcl::PointCloud const &normal_cloud, std::vector const &normal_indices, Eigen::Vector3f &normal); - - -#endif diff --git a/features/include/pcl/features/lrf_utils.h b/features/include/pcl/features/lrf_utils.h deleted file mode 100644 index 123dd9d5b09..00000000000 --- a/features/include/pcl/features/lrf_utils.h +++ /dev/null @@ -1,142 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Point Cloud Library (PCL) - www.pointclouds.org -* Copyright (c) 2010-2012, Willow Garage, Inc. -* Copyright (c) 2012-, 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_LRF_UTILS_H_ -#define PCL_LRF_UTILS_H_ - -#include -#include - -namespace pcl -{ - - /** \brief Project point on the plane defined by plane_point and plane_normal. - * - * \param[in] point point to project - * \param[in] indices of normals used to disambiguate - * \param[in] normal to disambiguate. normal is modified by the function. - * \ingroup features - */ - void - projectPointOnPlane ( - Eigen::Vector3f const &point, - Eigen::Vector3f const &plane_point, - Eigen::Vector3f const &plane_normal, - Eigen::Vector3f &projected_point); - - - /** \brief Find the unit vector from axis_origin, directed toward point and orthogonal to axis. - * - * \param[in] axis input axis - * \param[in] axis_origin point belonging to axis - * \param[in] point point input point not belonging to axis - * \param[out] directed_ortho_axis unit vector from axis_origin, directed toward point and orthogonal to axis. - * \ingroup features - */ - void - directedOrthogonalAxis ( - Eigen::Vector3f const &axis, - Eigen::Vector3f const &axis_origin, - Eigen::Vector3f const &point, - Eigen::Vector3f &directed_ortho_axis); - - - /** \brief Find the angle between unit vectors v1 and v2. - * - * \param[in] v1 first unit vector - * \param[in] v1 second unit vector - * \param[in] axis axis orthogonal to v1 and v2. Sign of axis defines the sign of returned angle - * \return angle between unit vectors v1 and v2 - * \ingroup features - */ - float - getAngleBetweenUnitVectors ( - Eigen::Vector3f const &v1, - Eigen::Vector3f const &v2, - Eigen::Vector3f const &axis); - - /** \brief Define a random unit vector orthogonal to axis. - * - * \param[in] axis input axis - * \param[out] rand_ortho_axis random unit vector orthogonal to axis - * \ingroup features - */ - void - randomOrthogonalAxis ( - Eigen::Vector3f const &axis, - Eigen::Vector3f &rand_ortho_axis); - - /** \brief Find the plane that best fits points in the least-square sense. Use SVD decomposition. - * - * \param[in] points set of points to fit - * \param[out] centroid point belonging to the fitted plane and centroid of points - * \param[out] plane_normal normal of the fitted plane - * \ingroup features - */ - void - planeFitting ( - Eigen::Matrix const &points, - Eigen::Vector3f ¢roid, - Eigen::Vector3f &plane_normal); - - - /** \brief Disambiguate normal sign as 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 - * \param[in] normal_cloud input cloud of normals - * \param[in] normal_indices indices of normals used to disambiguate - * \param[in] normal normal to disambiguate. normal is modified by the function. - * \return false if normal_indices does not contain any valid normal. - * \ingroup features - */ - template bool - normalDisambiguation ( - pcl::PointCloud const &normal_cloud, - std::vector const &normal_indices, - Eigen::Vector3f &normal); - - -} - -#ifdef PCL_NO_PRECOMPILE -#include -#endif - -#endif //#ifndef PCL_LRF_UTILS_H_ diff --git a/features/include/pcl/features/normal_3d.h b/features/include/pcl/features/normal_3d.h index 82f538d4b1c..811c57df6dd 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 + * \param[in] normal_cloud input 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(); + + bool at_least_one_valid_point = false; + for (size_t i = 0; i < normal_indices.size (); ++i) + { + const PointNT& curPt = normal_cloud[normal_indices[i]]; + + if(pcl::isFinite(curPt)) + { + normal_mean += curPt.getNormalVector3fMap (); + at_least_one_valid_point = true; + } + } + + if(!at_least_one_valid_point) + 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 index a4ad5bfc351..74917edc351 100644 --- a/features/src/flare.cpp +++ b/features/src/flare.cpp @@ -37,64 +37,6 @@ #include -void -pcl::projectPointOnPlane ( Eigen::Vector3f const &point, - Eigen::Vector3f const &plane_point, - Eigen::Vector3f const &plane_normal, - Eigen::Vector3f &projected_point) -{ - const Eigen::Vector3f xo = point - plane_point; - const float t = plane_normal.dot (xo); - - projected_point = point - (t * plane_normal); -} - - -void -pcl::directedOrthogonalAxis ( Eigen::Vector3f const &axis, - Eigen::Vector3f const &axis_origin, - Eigen::Vector3f const &point, - Eigen::Vector3f &directed_ortho_axis) -{ - Eigen::Vector3f projection; - projectPointOnPlane (point, axis_origin, axis, projection); - directed_ortho_axis = projection - axis_origin; - - directed_ortho_axis.normalize (); -} - - -void -pcl::randomOrthogonalAxis ( Eigen::Vector3f const &axis, - Eigen::Vector3f &rand_ortho_axis) -{ - if (std::abs (axis.z ()) > 1E-8f) - { - rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - 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.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - 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.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - 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 (); -} - - #ifndef PCL_NO_PRECOMPILE #include #include diff --git a/features/src/lrf_utils.cpp b/features/src/lrf_utils.cpp deleted file mode 100644 index 3dc6ecfc63c..00000000000 --- a/features/src/lrf_utils.cpp +++ /dev/null @@ -1,165 +0,0 @@ -/* -* Software License Agreement (BSD License) -* -* Point Cloud Library (PCL) - www.pointclouds.org -* Copyright (c) 2010-2012, Willow Garage, Inc. -* Copyright (c) 2012-, 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 - - - -void - pcl::projectPointOnPlane ( - Eigen::Vector3f const &point, - Eigen::Vector3f const &plane_point, - Eigen::Vector3f const &plane_normal, - Eigen::Vector3f &projected_point) -{ - float t; - Eigen::Vector3f xo; - - xo = point - plane_point; - t = plane_normal.dot (xo); - - projected_point = point - (t * plane_normal); -} - - -void - pcl::directedOrthogonalAxis ( - Eigen::Vector3f const &axis, - Eigen::Vector3f const &axis_origin, - Eigen::Vector3f const &point, - Eigen::Vector3f &directed_ortho_axis) -{ - Eigen::Vector3f projection; - projectPointOnPlane (point, axis_origin, axis, projection); - directed_ortho_axis = projection - axis_origin; - - directed_ortho_axis.normalize (); -} - - -float - pcl::getAngleBetweenUnitVectors ( - Eigen::Vector3f const &v1, - Eigen::Vector3f const &v2, - Eigen::Vector3f const &axis) -{ - Eigen::Vector3f angle_orientation; - angle_orientation = v1.cross (v2); - float angle_radians = acosf (std::max (-1.0f, std::min (1.0f, v1.dot (v2)))); - - angle_radians = angle_orientation.dot (axis) < 0.f ? (2 * static_cast (M_PI) - angle_radians) : angle_radians; - - return (angle_radians); -} - - -void - pcl::randomOrthogonalAxis ( - Eigen::Vector3f const &axis, - Eigen::Vector3f &rand_ortho_axis) -{ - if (std::abs (axis.z ()) > 1E-8f) - { - rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - 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.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - 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.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x (); - } - - rand_ortho_axis.normalize (); -} - - -void - pcl::planeFitting ( - Eigen::Matrix const &points, - Eigen::Vector3f ¢roid, - Eigen::Vector3f &plane_normal) -{ - - int n_points = static_cast (points.rows ()); - if (n_points == 0) - { - return; - } - - //find the center by averaging the points positions - centroid.setZero (); - - for (int i = 0; i < n_points; ++i) - { - centroid += points.row (i); - } - - centroid /= static_cast (n_points); - - //copy points - average (center) - Eigen::Matrix A (n_points, 3); - for (int i = 0; i < n_points; ++i) - { - A (i, 0) = points (i, 0) - centroid.x (); - A (i, 1) = points (i, 1) - centroid.y (); - A (i, 2) = points (i, 2) - centroid.z (); - } - - Eigen::JacobiSVD svd (A, Eigen::ComputeFullV); - plane_normal = svd.matrixV ().col (2); -} - - -#ifndef PCL_NO_PRECOMPILE -#include -#include -// Instantiations of specific point types -#ifdef PCL_ONLY_CORE_POINT_TYPES -PCL_INSTANTIATE(normalDisambiguation, (pcl::Normal)); -#else -PCL_INSTANTIATE(normalDisambiguation, PCL_NORMAL_POINT_TYPES); -#endif -#endif // PCL_NO_PRECOMPILE - From 0a5b932b9c1454112725eee1f7533b54083f243c Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Sun, 30 Jul 2017 17:15:06 +0200 Subject: [PATCH 05/11] Solve PCL_WARN error in Visual Studio --- common/include/pcl/common/geometry.h | 1 + 1 file changed, 1 insertion(+) diff --git a/common/include/pcl/common/geometry.h b/common/include/pcl/common/geometry.h index 1b8ddc382f7..5dfac96575a 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 From 8aee5af0e9f8e3459fba7b0ec1bceea1142fc95d Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Sat, 5 Aug 2017 13:20:07 +0200 Subject: [PATCH 06/11] Rename directedOrthogonalAxis to projectedAsUnitVector --- common/include/pcl/common/geometry.h | 39 +++++++++----------- features/include/pcl/features/flare.h | 10 ++--- features/include/pcl/features/impl/flare.hpp | 39 ++++++++++---------- features/include/pcl/features/normal_3d.h | 20 +++++----- features/src/flare.cpp | 4 +- 5 files changed, 53 insertions(+), 59 deletions(-) diff --git a/common/include/pcl/common/geometry.h b/common/include/pcl/common/geometry.h index 5dfac96575a..14dedb60d9f 100644 --- a/common/include/pcl/common/geometry.h +++ b/common/include/pcl/common/geometry.h @@ -104,54 +104,49 @@ namespace pcl } - /** \brief Find the unit vector from axis_origin, directed toward point and orthogonal to axis. + /** \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] axis input axis - * \param[in] axis_origin point belonging to axis - * \param[in] point point input point not belonging to axis - * \param[out] directed_ortho_axis unit vector from axis_origin, directed toward point and orthogonal to axis. - * \ingroup features + * \param[in] point Point projected on the plane + * \param[in] plane_origin The plane origin + * \param[in] plane_normal The plane normal + * \param[out] projectedAsUnitVector Unit vector pointing from plane_origin to the projection of point on the plane. + * \ingroup geometry */ inline void - directedOrthogonalAxis ( Eigen::Vector3f const &axis, - Eigen::Vector3f const &axis_origin, - Eigen::Vector3f const &point, - Eigen::Vector3f &directed_ortho_axis) + projectedAsUnitVector (Eigen::Vector3f const &point, + Eigen::Vector3f const &plane_origin, + Eigen::Vector3f const &plane_normal, + Eigen::Vector3f &projectedAsUnitVector) { Eigen::Vector3f projection; - project (point, axis_origin, axis, projection); - directed_ortho_axis = projection - axis_origin; + project (point, plane_origin, plane_normal, projection); + projectedAsUnitVector = projection - plane_origin; - directed_ortho_axis.normalize (); + projectedAsUnitVector.normalize (); } /** \brief Define a random unit vector orthogonal to axis. * - * \param[in] axis input axis - * \param[out] rand_ortho_axis random unit vector orthogonal to axis - * \ingroup features + * \param[in] axis Axis + * \param[out] rand_ortho_axis Random unit vector orthogonal to axis + * \ingroup geometry */ inline void randomOrthogonalAxis ( Eigen::Vector3f const &axis, Eigen::Vector3f &rand_ortho_axis) { + rand_ortho_axis.setRandom(); if (std::abs (axis.z ()) > 1E-8f) { - rand_ortho_axis.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; 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.x () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; 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.y () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; - rand_ortho_axis.z () = (static_cast (rand ()) / static_cast (RAND_MAX)) * 2.0f - 1.0f; rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x (); } else diff --git a/features/include/pcl/features/flare.h b/features/include/pcl/features/flare.h index 24720d12e18..a4870f32d69 100644 --- a/features/include/pcl/features/flare.h +++ b/features/include/pcl/features/flare.h @@ -93,11 +93,11 @@ namespace pcl 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) + min_neighbors_for_normal_axis_ (6), + min_neighbors_for_tangent_axis_ (6), + sampled_surface_ (), + sampled_tree_ (), + fake_sampled_surface_ (false) { feature_name_ = "FLARELocalReferenceFrameEstimation"; } diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp index 2674c5e2c6f..e22cb8d07bc 100644 --- a/features/include/pcl/features/impl/flare.hpp +++ b/features/include/pcl/features/impl/flare.hpp @@ -54,7 +54,7 @@ template (*normals_, neighbours_indices, fitted_normal) ) + if (!pcl::flipNormalTowardsNormalsMean (*normals_, neighbours_indices, fitted_normal)) { //all normals in the neighbourood are invalid //setting lrf to NaN @@ -157,7 +157,7 @@ templateradiusSearch( (*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances); + n_neighbours = sampled_tree_->radiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances); if (n_neighbours < min_neighbors_for_tangent_axis_) { @@ -173,9 +173,9 @@ template::max(); - int bestShapeIndex = -1; + SignedDistanceT shape_score; + SignedDistanceT best_shape_score = -std::numeric_limits::max (); + int best_shape_index = -1; Eigen::Vector3f best_margin_point; @@ -198,17 +198,17 @@ template bestShapeScore) + if (shape_score > best_shape_score) { - bestShapeIndex = curr_neigh_idx; - bestShapeScore = shapeScore; + best_shape_index = curr_neigh_idx; + best_shape_score = shape_score; } } //for each neighbor - if (bestShapeIndex == -1) + if (best_shape_index == -1) { pcl::geometry::randomOrthogonalAxis (fitted_normal, x_axis); y_axis = fitted_normal.cross (x_axis); @@ -219,8 +219,8 @@ template::max ()); } - //find orthogonal axis directed to bestShapeIndex point projection on plane with fittedNormal as axis - pcl::geometry::directedOrthogonalAxis (fitted_normal, feature_point, sampled_surface_->at(bestShapeIndex).getVector3fMap(), x_axis); + //find orthogonal axis directed to best_shape_index point projection on plane with fittedNormal as axis + pcl::geometry::projectedAsUnitVector (sampled_surface_->at (best_shape_index).getVector3fMap (), feature_point, fitted_normal, x_axis); y_axis = fitted_normal.cross (x_axis); @@ -228,8 +228,8 @@ templatesize ()) - signed_distances_from_highest_points_.resize (indices_->size ()); + signed_distances_from_highest_points_.resize (indices_->size ()); for (size_t point_idx = 0; point_idx < indices_->size (); ++point_idx) { @@ -254,7 +253,7 @@ template::max () ) + if (signed_distances_from_highest_points_[point_idx] == std::numeric_limits::max ()) { output.is_dense = false; } diff --git a/features/include/pcl/features/normal_3d.h b/features/include/pcl/features/normal_3d.h index 811c57df6dd..1f1b1176e03 100644 --- a/features/include/pcl/features/normal_3d.h +++ b/features/include/pcl/features/normal_3d.h @@ -191,9 +191,11 @@ namespace pcl * 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 - * \param[in] normal_cloud input 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. + * + * 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 */ @@ -202,21 +204,19 @@ namespace pcl std::vector const &normal_indices, Eigen::Vector3f &normal) { - Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero(); + Eigen::Vector3f normal_mean = Eigen::Vector3f::Zero (); - bool at_least_one_valid_point = false; for (size_t i = 0; i < normal_indices.size (); ++i) { - const PointNT& curPt = normal_cloud[normal_indices[i]]; + const PointNT& cur_pt = normal_cloud[normal_indices[i]]; - if(pcl::isFinite(curPt)) + if (pcl::isFinite(cur_pt)) { - normal_mean += curPt.getNormalVector3fMap (); - at_least_one_valid_point = true; + normal_mean += cur_pt.getNormalVector3fMap (); } } - if(!at_least_one_valid_point) + if(normal_mean.isZero()) return false; normal_mean.normalize (); diff --git a/features/src/flare.cpp b/features/src/flare.cpp index 74917edc351..5d35b762f5b 100644 --- a/features/src/flare.cpp +++ b/features/src/flare.cpp @@ -42,8 +42,8 @@ #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)(int)(short))) +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)(int)(short))) +PCL_INSTANTIATE_PRODUCT(FLARELocalReferenceFrameEstimation, (PCL_XYZ_POINT_TYPES)(PCL_NORMAL_POINT_TYPES)((pcl::ReferenceFrame))((float))) #endif #endif // PCL_NO_PRECOMPILE From f754e45a0f9c6c4d1c2e22e504c75da9574ad12f Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Mon, 7 Aug 2017 21:51:45 +0200 Subject: [PATCH 07/11] Format Flare unit test --- common/include/pcl/common/geometry.h | 15 +++-- test/features/test_flare_estimation.cpp | 79 +++++++++++-------------- 2 files changed, 43 insertions(+), 51 deletions(-) diff --git a/common/include/pcl/common/geometry.h b/common/include/pcl/common/geometry.h index 14dedb60d9f..6ecbd9f0036 100644 --- a/common/include/pcl/common/geometry.h +++ b/common/include/pcl/common/geometry.h @@ -107,22 +107,21 @@ namespace pcl /** \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_origin The plane origin * \param[in] plane_normal The plane normal - * \param[out] projectedAsUnitVector Unit vector pointing from plane_origin to the projection of point on the plane. + * \param[out] projected_as_unit_vector Unit vector pointing from plane_origin to the projection of point on the plane. * \ingroup geometry */ inline void projectedAsUnitVector (Eigen::Vector3f const &point, Eigen::Vector3f const &plane_origin, Eigen::Vector3f const &plane_normal, - Eigen::Vector3f &projectedAsUnitVector) + Eigen::Vector3f &projected_as_unit_vector) { Eigen::Vector3f projection; project (point, plane_origin, plane_normal, projection); - projectedAsUnitVector = projection - plane_origin; - - projectedAsUnitVector.normalize (); + projected_as_unit_vector = projection - plane_origin; + projected_as_unit_vector.normalize (); } @@ -133,8 +132,8 @@ namespace pcl * \ingroup geometry */ inline void - randomOrthogonalAxis ( Eigen::Vector3f const &axis, - Eigen::Vector3f &rand_ortho_axis) + randomOrthogonalAxis (Eigen::Vector3f const &axis, + Eigen::Vector3f &rand_ortho_axis) { rand_ortho_axis.setRandom(); if (std::abs (axis.z ()) > 1E-8f) diff --git a/test/features/test_flare_estimation.cpp b/test/features/test_flare_estimation.cpp index df5c50c11a1..5eb86a1145d 100644 --- a/test/features/test_flare_estimation.cpp +++ b/test/features/test_flare_estimation.cpp @@ -37,9 +37,6 @@ * */ - - - #include #include #include @@ -47,7 +44,6 @@ #include #include - typedef pcl::search::KdTree::Ptr KdTreePtr; typedef pcl::PointCloud::Ptr PointCloudPtr; @@ -61,42 +57,40 @@ KdTreePtr sampled_tree; - - ////////////////////////////////////////////////////////////////////////////////////////////////////////////////// TEST (PCL, FLARELocalReferenceFrameEstimation) { pcl::PointCloud::Ptr normals (new pcl::PointCloud ()); pcl::PointCloud bunny_LRF; - const float meshRes = 0.005f; + const float mesh_res = 0.005f; - boost::shared_ptr > indicesptr (new std::vector (indices)); + boost::shared_ptr > indices_ptr (new std::vector (indices)); // Compute normals pcl::NormalEstimation ne; - ne.setRadiusSearch (2.0f*meshRes); + ne.setRadiusSearch (2.0f*mesh_res); ne.setViewPoint (1, 1, 10); ne.setInputCloud (cloud); ne.setSearchMethod (tree); - ne.setIndices (indicesptr); + ne.setIndices (indices_ptr); ne.compute (*normals); // Compute FLARE LRF pcl::FLARELocalReferenceFrameEstimation lrf_estimator; - lrf_estimator.setRadiusSearch (5 * meshRes); - lrf_estimator.setTangentRadius (20 * meshRes); + lrf_estimator.setRadiusSearch (5 * mesh_res); + lrf_estimator.setTangentRadius (20 * mesh_res); - lrf_estimator.setInputCloud (cloud ); - lrf_estimator.setSearchSurface( cloud ); + lrf_estimator.setInputCloud (cloud); + lrf_estimator.setSearchSurface (cloud); lrf_estimator.setInputNormals (normals); lrf_estimator.setSearchMethod (tree); - lrf_estimator.setIndices (indicesptr); + lrf_estimator.setIndices (indices_ptr); lrf_estimator.setSearchMethodForSampledSurface (sampled_tree); - lrf_estimator.setSearchSampledSurface(sampled_cloud); + lrf_estimator.setSearchSampledSurface (sampled_cloud); lrf_estimator.compute (bunny_LRF); @@ -107,21 +101,21 @@ TEST (PCL, FLARELocalReferenceFrameEstimation) // 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) ; + 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); + 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); + 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); + 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 @@ -143,10 +137,10 @@ TEST (PCL, FLARELocalReferenceFrameEstimation) 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); + 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); } @@ -160,9 +154,9 @@ main (int argc, char** argv) return (-1); } - cloud.reset( new pcl::PointCloud() ); + cloud.reset (new pcl::PointCloud ()); - if (pcl::io::loadPCDFile (argv[1], *cloud) < 0) + 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); @@ -176,19 +170,18 @@ main (int argc, char** argv) tree->setInputCloud (cloud); //create and set sampled point cloud for computation of X axis - const float samplingPerc = 0.2f; - const float samplingIncr = 1.0f/samplingPerc; + const float sampling_perc = 0.2f; + const float sampling_incr = 1.0f / sampling_perc; - sampled_cloud.reset( new pcl::PointCloud() ); + sampled_cloud.reset (new pcl::PointCloud ()); - std::vector indices_sampled; - for(float sa = 0.0f; sa < (float)cloud->points.size (); sa += samplingIncr) - indices_sampled.push_back(static_cast (sa) ); - copyPointCloud(*cloud, indices_sampled, *sampled_cloud); + 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); - + sampled_tree->setInputCloud (sampled_cloud); //start tests testing::InitGoogleTest (&argc, argv); From 74e29fc9c46041bbb4cdcf98d6287334b8fde29d Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Thu, 2 Nov 2017 18:35:07 +0100 Subject: [PATCH 08/11] Changes in geometry.h and flare.h --- common/include/pcl/common/geometry.h | 19 ++++++++++--------- features/include/pcl/features/flare.h | 16 ++++++---------- features/include/pcl/features/impl/flare.hpp | 10 +++++----- features/include/pcl/features/normal_3d.h | 4 ++-- test/features/test_flare_estimation.cpp | 4 ---- 5 files changed, 23 insertions(+), 30 deletions(-) diff --git a/common/include/pcl/common/geometry.h b/common/include/pcl/common/geometry.h index 6ecbd9f0036..dd19237f0e1 100644 --- a/common/include/pcl/common/geometry.h +++ b/common/include/pcl/common/geometry.h @@ -109,32 +109,32 @@ namespace pcl * \param[in] point Point projected on the plane * \param[in] plane_origin The plane origin * \param[in] plane_normal The plane normal - * \param[out] projected_as_unit_vector Unit vector pointing from plane_origin to the projection of point on the plane. + * \return unit vector pointing from plane_origin to the projection of point on the plane. * \ingroup geometry */ - inline void + inline Eigen::Vector3f projectedAsUnitVector (Eigen::Vector3f const &point, Eigen::Vector3f const &plane_origin, - Eigen::Vector3f const &plane_normal, - Eigen::Vector3f &projected_as_unit_vector) + Eigen::Vector3f const &plane_normal) { Eigen::Vector3f projection; project (point, plane_origin, plane_normal, projection); - projected_as_unit_vector = projection - plane_origin; + 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 - * \param[out] rand_ortho_axis Random unit vector orthogonal to axis + * \return random unit vector orthogonal to axis * \ingroup geometry */ - inline void - randomOrthogonalAxis (Eigen::Vector3f const &axis, - Eigen::Vector3f &rand_ortho_axis) + inline Eigen::Vector3f + randomOrthogonalAxis (Eigen::Vector3f const &axis) { + Eigen::Vector3f rand_ortho_axis; rand_ortho_axis.setRandom(); if (std::abs (axis.z ()) > 1E-8f) { @@ -154,6 +154,7 @@ namespace pcl } rand_ortho_axis.normalize (); + return rand_ortho_axis; } diff --git a/features/include/pcl/features/flare.h b/features/include/pcl/features/flare.h index a4870f32d69..3a03bcbcf5d 100644 --- a/features/include/pcl/features/flare.h +++ b/features/include/pcl/features/flare.h @@ -75,19 +75,19 @@ namespace pcl using Feature::fake_surface_; using Feature::getClassName; - typedef typename Feature::PointCloudIn PointCloudIn; - typedef typename Feature::PointCloudOut PointCloudOut; + using Feature::PointCloudIn; + using Feature::PointCloudOut; + + using Feature::PointCloudInConstPtr; + + using Feature::KdTreePtr; typedef typename pcl::PointCloud PointCloudSignedDistance; typedef typename PointCloudSignedDistance::Ptr PointCloudSignedDistancePtr; - typedef typename PointCloudIn::ConstPtr PointCloudInConstPtr; - typedef boost::shared_ptr > Ptr; typedef boost::shared_ptr > ConstPtr; - typedef typename pcl::search::Search::Ptr KdTreePtr; - public: /** \brief Constructor. */ FLARELocalReferenceFrameEstimation () : @@ -102,10 +102,6 @@ namespace pcl feature_name_ = "FLARELocalReferenceFrameEstimation"; } - /** \brief Empty destructor */ - virtual - ~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. diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp index e22cb8d07bc..60e932e6cd6 100644 --- a/features/include/pcl/features/impl/flare.hpp +++ b/features/include/pcl/features/impl/flare.hpp @@ -162,7 +162,7 @@ templateat (best_shape_index).getVector3fMap (), feature_point, fitted_normal, x_axis); + x_axis = pcl::geometry::projectedAsUnitVector (sampled_surface_->at (best_shape_index).getVector3fMap (), feature_point, fitted_normal); y_axis = fitted_normal.cross (x_axis); diff --git a/features/include/pcl/features/normal_3d.h b/features/include/pcl/features/normal_3d.h index 1f1b1176e03..52d4668099c 100644 --- a/features/include/pcl/features/normal_3d.h +++ b/features/include/pcl/features/normal_3d.h @@ -210,13 +210,13 @@ namespace pcl { const PointNT& cur_pt = normal_cloud[normal_indices[i]]; - if (pcl::isFinite(cur_pt)) + if (pcl::isFinite (cur_pt)) { normal_mean += cur_pt.getNormalVector3fMap (); } } - if(normal_mean.isZero()) + if(normal_mean.isZero ()) return false; normal_mean.normalize (); diff --git a/test/features/test_flare_estimation.cpp b/test/features/test_flare_estimation.cpp index 5eb86a1145d..d4a127dacb6 100644 --- a/test/features/test_flare_estimation.cpp +++ b/test/features/test_flare_estimation.cpp @@ -65,8 +65,6 @@ TEST (PCL, FLARELocalReferenceFrameEstimation) const float mesh_res = 0.005f; - boost::shared_ptr > indices_ptr (new std::vector (indices)); - // Compute normals pcl::NormalEstimation ne; @@ -74,7 +72,6 @@ TEST (PCL, FLARELocalReferenceFrameEstimation) ne.setViewPoint (1, 1, 10); ne.setInputCloud (cloud); ne.setSearchMethod (tree); - ne.setIndices (indices_ptr); ne.compute (*normals); @@ -88,7 +85,6 @@ TEST (PCL, FLARELocalReferenceFrameEstimation) lrf_estimator.setSearchSurface (cloud); lrf_estimator.setInputNormals (normals); lrf_estimator.setSearchMethod (tree); - lrf_estimator.setIndices (indices_ptr); lrf_estimator.setSearchMethodForSampledSurface (sampled_tree); lrf_estimator.setSearchSampledSurface (sampled_cloud); From f5ad3c3cb6921454f599e9fadd3f57ff27636af3 Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Fri, 3 Nov 2017 15:19:58 +0100 Subject: [PATCH 09/11] Add typename --- features/include/pcl/features/flare.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/features/include/pcl/features/flare.h b/features/include/pcl/features/flare.h index 3a03bcbcf5d..45948b5fe7d 100644 --- a/features/include/pcl/features/flare.h +++ b/features/include/pcl/features/flare.h @@ -75,12 +75,12 @@ namespace pcl using Feature::fake_surface_; using Feature::getClassName; - using Feature::PointCloudIn; - using Feature::PointCloudOut; + using typename Feature::PointCloudIn; + using typename Feature::PointCloudOut; - using Feature::PointCloudInConstPtr; + using typename Feature::PointCloudInConstPtr; - using Feature::KdTreePtr; + using typename Feature::KdTreePtr; typedef typename pcl::PointCloud PointCloudSignedDistance; typedef typename PointCloudSignedDistance::Ptr PointCloudSignedDistancePtr; From 2dc827bc7c4e998dc787724968b7179b8334c00c Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Sat, 4 Nov 2017 17:33:09 +0100 Subject: [PATCH 10/11] Remove indices. --- test/features/test_flare_estimation.cpp | 7 ------- 1 file changed, 7 deletions(-) diff --git a/test/features/test_flare_estimation.cpp b/test/features/test_flare_estimation.cpp index d4a127dacb6..0cfdcfeb4a8 100644 --- a/test/features/test_flare_estimation.cpp +++ b/test/features/test_flare_estimation.cpp @@ -48,7 +48,6 @@ typedef pcl::search::KdTree::Ptr KdTreePtr; typedef pcl::PointCloud::Ptr PointCloudPtr; PointCloudPtr cloud; -std::vector indices; KdTreePtr tree; //sampled surface for the computation of tangent X axis @@ -91,8 +90,6 @@ TEST (PCL, FLARELocalReferenceFrameEstimation) lrf_estimator.compute (bunny_LRF); // TESTS - EXPECT_EQ (indices.size (), bunny_LRF.size ()); - EXPECT_TRUE (bunny_LRF.is_dense); // Expected Results @@ -158,10 +155,6 @@ main (int argc, char** argv) return (-1); } - indices.resize (cloud->points.size ()); - for (size_t i = 0; i < indices.size (); ++i) - indices[i] = static_cast (i); - tree.reset (new pcl::search::KdTree (false)); tree->setInputCloud (cloud); From 937e9b5238da16a2e01118ce72adff49875e2d3c Mon Sep 17 00:00:00 2001 From: Alioscia Petrelli Date: Sun, 5 Nov 2017 13:14:16 +0100 Subject: [PATCH 11/11] Remove spaces --- features/include/pcl/features/impl/flare.hpp | 19 ++++++------------- features/include/pcl/features/normal_3d.h | 2 +- 2 files changed, 7 insertions(+), 14 deletions(-) diff --git a/features/include/pcl/features/impl/flare.hpp b/features/include/pcl/features/impl/flare.hpp index 60e932e6cd6..6b14652b9ed 100644 --- a/features/include/pcl/features/impl/flare.hpp +++ b/features/include/pcl/features/impl/flare.hpp @@ -39,11 +39,9 @@ #ifndef PCL_FEATURES_IMPL_FLARE_H_ #define PCL_FEATURES_IMPL_FLARE_H_ - #include #include - ////////////////////////////////////////////////////////////////////////////////////////////// template bool pcl::FLARELocalReferenceFrameEstimation::initCompute () @@ -133,12 +131,12 @@ template (*normals_, neighbours_indices, fitted_normal)) @@ -153,7 +151,6 @@ templategetKSearch () != 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()); + 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; } @@ -264,7 +258,6 @@ template; #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 52d4668099c..8512d3951da 100644 --- a/features/include/pcl/features/normal_3d.h +++ b/features/include/pcl/features/normal_3d.h @@ -216,7 +216,7 @@ namespace pcl } } - if(normal_mean.isZero ()) + if (normal_mean.isZero ()) return false; normal_mean.normalize ();