From 0fdfc242426efed339c153bc24828acfa9c7bb7a Mon Sep 17 00:00:00 2001 From: aliosciapetrelli Date: Wed, 30 Mar 2016 16:22:52 +0200 Subject: [PATCH] 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 | 4 + test/features/test_flare_estimation.cpp | 203 +++++++++++ 11 files changed, 1283 insertions(+), 226 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 ca60f9dee02..ad28fe7750b 100644 --- a/test/features/CMakeLists.txt +++ b/test/features/CMakeLists.txt @@ -66,6 +66,10 @@ PCL_ADD_TEST(feature_board_estimation test_board_estimation 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 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 ()); +} +/* ]--- */ +