FLARELocalReferenceFrameEstimation class added

common/include/pcl/common/geometry.h

@@ -43,6 +43,7 @@
 #endif
 
 #include <Eigen/Core>
+#include <pcl/console/print.h>
 
 /**
   * \file common/geometry.h
@@ -101,6 +102,62 @@ namespace pcl
       float lambda = plane_normal.dot(po);
       projected = point - (lambda * plane_normal);
     }
+
+
+    /** \brief Given a plane defined by plane_origin and plane_normal, find the unit vector pointing from plane_origin to the projection of point on the plane.
+      * 
+      * \param[in] point Point projected on the plane
+      * \param[in] plane_origin The plane origin
+      * \param[in] plane_normal The plane normal 
+      * \return unit vector pointing from plane_origin to the projection of point on the plane.
+      * \ingroup geometry
+      */
+    inline Eigen::Vector3f
+    projectedAsUnitVector (Eigen::Vector3f const &point,
+                           Eigen::Vector3f const &plane_origin,
+                           Eigen::Vector3f const &plane_normal)
+    {
+      Eigen::Vector3f projection;
+      project (point, plane_origin, plane_normal, projection);
+      Eigen::Vector3f projected_as_unit_vector = projection - plane_origin;
+      projected_as_unit_vector.normalize ();
+      return projected_as_unit_vector;
+    }
+
+
+    /** \brief Define a random unit vector orthogonal to axis.
+      * 
+      * \param[in] axis Axis
+      * \return random unit vector orthogonal to axis
+      * \ingroup geometry
+      */
+    inline Eigen::Vector3f
+    randomOrthogonalAxis (Eigen::Vector3f const &axis)
+    {
+      Eigen::Vector3f rand_ortho_axis;
+      rand_ortho_axis.setRandom();
+      if (std::abs (axis.z ()) > 1E-8f)
+      {
+        rand_ortho_axis.z () = -(axis.x () * rand_ortho_axis.x () + axis.y () * rand_ortho_axis.y ()) / axis.z ();
+      }
+      else if (std::abs (axis.y ()) > 1E-8f)
+      {
+        rand_ortho_axis.y () = -(axis.x () * rand_ortho_axis.x () + axis.z () * rand_ortho_axis.z ()) / axis.y ();
+      }
+      else if (std::abs (axis.x ()) > 1E-8f)
+      {
+        rand_ortho_axis.x () = -(axis.y () * rand_ortho_axis.y () + axis.z () * rand_ortho_axis.z ()) / axis.x ();
+      }
+      else
+      {
+        PCL_WARN ("[pcl::randomOrthogonalAxis] provided axis has norm < 1E-8f");
+      }
+
+      rand_ortho_axis.normalize ();
+      return rand_ortho_axis;
+    }
+
+
   }
 }
 

features/CMakeLists.txt

@@ -15,6 +15,7 @@ if(build)
         "include/pcl/${SUBSYS_NAME}/boost.h"
         "include/pcl/${SUBSYS_NAME}/eigen.h"
         "include/pcl/${SUBSYS_NAME}/board.h"
+        "include/pcl/${SUBSYS_NAME}/flare.h"
         "include/pcl/${SUBSYS_NAME}/brisk_2d.h"
         "include/pcl/${SUBSYS_NAME}/cppf.h"
         "include/pcl/${SUBSYS_NAME}/cvfh.h"
@@ -66,6 +67,7 @@ if(build)
 
     set(impl_incs
         "include/pcl/${SUBSYS_NAME}/impl/board.hpp"
+        "include/pcl/${SUBSYS_NAME}/impl/flare.hpp"
         "include/pcl/${SUBSYS_NAME}/impl/brisk_2d.hpp"
         "include/pcl/${SUBSYS_NAME}/impl/cppf.hpp"
         "include/pcl/${SUBSYS_NAME}/impl/cvfh.hpp"
@@ -114,6 +116,7 @@ if(build)
 
     set(srcs
         src/board.cpp
+        src/flare.cpp
         src/brisk_2d.cpp
         src/boundary.cpp
         src/cppf.cpp

features/include/pcl/features/flare.h

@@ -0,0 +1,293 @@
+/*
+* Software License Agreement (BSD License)
+*
+*  Point Cloud Library (PCL) - www.pointclouds.org
+*  Copyright (c) 2016-, Open Perception, Inc.
+*
+*  All rights reserved.
+*
+*  Redistribution and use in source and binary forms, with or without
+*  modification, are permitted provided that the following conditions
+*  are met:
+*
+*   * Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   * Redistributions in binary form must reproduce the above
+*     copyright notice, this list of conditions and the following
+*     disclaimer in the documentation and/or other materials provided
+*     with the distribution.
+*   * Neither the name of the copyright holder(s) nor the names of its
+*     contributors may be used to endorse or promote products derived
+*     from this software without specific prior written permission.
+*
+*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+*  POSSIBILITY OF SUCH DAMAGE.
+*
+*
+*/
+
+#ifndef PCL_FLARE_H_
+#define PCL_FLARE_H_
+
+#include <pcl/point_types.h>
+#include <pcl/features/feature.h>
+#include <pcl/features/normal_3d.h>
+
+
+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<typename PointInT, typename PointNT, typename PointOutT = ReferenceFrame, typename SignedDistanceT = float>
+  class FLARELocalReferenceFrameEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT>
+  {
+    protected:
+      using Feature<PointInT, PointOutT>::feature_name_;
+      using Feature<PointInT, PointOutT>::input_;
+      using Feature<PointInT, PointOutT>::indices_;
+      using Feature<PointInT, PointOutT>::surface_;
+      using Feature<PointInT, PointOutT>::tree_;
+      using Feature<PointInT, PointOutT>::search_parameter_;
+      using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_;
+      using Feature<PointInT, PointOutT>::fake_surface_;
+      using Feature<PointInT, PointOutT>::getClassName;
+
+      using typename Feature<PointInT, PointOutT>::PointCloudIn;
+      using typename Feature<PointInT, PointOutT>::PointCloudOut;
+
+      using typename Feature<PointInT, PointOutT>::PointCloudInConstPtr;
+
+      using typename Feature<PointInT, PointOutT>::KdTreePtr;
+
+      typedef typename pcl::PointCloud<SignedDistanceT> PointCloudSignedDistance;
+      typedef typename PointCloudSignedDistance::Ptr PointCloudSignedDistancePtr;
+
+      typedef boost::shared_ptr<FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> > Ptr;
+      typedef boost::shared_ptr<const FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> > ConstPtr;
+
+    public:
+      /** \brief Constructor. */
+      FLARELocalReferenceFrameEstimation () :
+        tangent_radius_ (0.0f),
+        margin_thresh_ (0.85f),
+        min_neighbors_for_normal_axis_ (6),
+        min_neighbors_for_tangent_axis_ (6),
+        sampled_surface_ (), 
+        sampled_tree_ (),
+        fake_sampled_surface_ (false)
+      {
+        feature_name_ = "FLARELocalReferenceFrameEstimation";
+      }
+
+      //Getters/Setters
+
+      /** \brief Set the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point.
+        *
+        * \param[in] radius The search radius for x axis.
+        */
+      inline void
+      setTangentRadius (float radius)
+      {
+        tangent_radius_ = radius;
+      }
+
+      /** \brief Get the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point.
+        *
+        * \return The search radius for x axis.
+        */
+      inline float
+      getTangentRadius () const
+      {
+        return (tangent_radius_);
+      }
+
+      /** \brief Set the percentage of the search tangent radius after which a point is considered part of the support.
+        *
+        * \param[in] margin_thresh the percentage of the search tangent radius after which a point is considered part of the support.
+        */
+      inline void
+      setMarginThresh (float margin_thresh)
+      {
+        margin_thresh_ = margin_thresh;
+      }
+
+      /** \brief Get the percentage of the search tangent radius after which a point is considered part of the support.
+        *
+        * \return The percentage of the search tangent radius after which a point is considered part of the support.
+        */
+      inline float
+      getMarginThresh () const
+      {
+        return (margin_thresh_);
+      }
+
+
+      /** \brief Set min number of neighbours required for the computation of Z axis.
+        *
+        * \param[in] min_neighbors_for_normal_axis min number of neighbours required for the computation of Z axis.
+        */
+      inline void
+      setMinNeighboursForNormalAxis (int min_neighbors_for_normal_axis)
+      {
+        min_neighbors_for_normal_axis_ = min_neighbors_for_normal_axis;
+      }
+
+      /** \brief Get min number of neighbours required for the computation of Z axis.
+        *
+        * \return min number of neighbours required for the computation of Z axis.
+        */
+      inline int
+      getMinNeighboursForNormalAxis () const
+      {
+        return (min_neighbors_for_normal_axis_);
+      }
+
+
+      /** \brief Set min number of neighbours required for the computation of X axis.
+        *
+        * \param[in] min_neighbors_for_tangent_axis min number of neighbours required for the computation of X axis.
+        */
+      inline void
+      setMinNeighboursForTangentAxis (int min_neighbors_for_tangent_axis)
+      {
+        min_neighbors_for_tangent_axis_ = min_neighbors_for_tangent_axis;
+      }
+
+      /** \brief Get min number of neighbours required for the computation of X axis.
+        *
+        * \return min number of neighbours required for the computation of X axis.
+        */
+      inline int
+      getMinNeighboursForTangentAxis () const
+      {
+        return (min_neighbors_for_tangent_axis_);
+      }
+
+
+      /** \brief Provide a pointer to the dataset used for the estimation of X axis.
+        * As the estimation of x axis is negligibly affected by surface downsampling,
+        * this method lets to consider a downsampled version of surface_ in the estimation of x axis.  
+        * This is optional, if this is not set, it will only use the data in the
+        * surface_ cloud to estimate the x axis. 
+        * \param[in] cloud a pointer to a PointCloud 
+        */
+      inline void 
+      setSearchSampledSurface(const PointCloudInConstPtr &cloud)
+      {
+        sampled_surface_ = cloud;
+        fake_sampled_surface_ = false;
+      }
+
+      /** \brief Get a pointer to the sampled_surface_ cloud dataset. */
+      inline const PointCloudInConstPtr& 
+      getSearchSampledSurface() const
+      {
+        return (sampled_surface_);
+      }
+
+      /** \brief Provide a pointer to the search object linked to sampled_surface.
+        * \param[in] tree a pointer to the spatial search object linked to sampled_surface.
+        */
+      inline void 
+      setSearchMethodForSampledSurface (const KdTreePtr &tree) { sampled_tree_ = tree; }
+
+      /** \brief Get a pointer to the search method used for the extimation of x axis. */
+      inline const KdTreePtr&  
+      getSearchMethodForSampledSurface () const
+      {
+        return (sampled_tree_);
+      }
+
+      /** \brief Get the signed distances of the highest points from the fitted planes. */
+      inline const std::vector<SignedDistanceT> & 
+      getSignedDistancesFromHighestPoints () const 
+      {
+        return (signed_distances_from_highest_points_);
+      }
+
+    protected:
+      /** \brief This method should get called before starting the actual computation. */
+      virtual bool
+      initCompute ();
+
+      /** \brief This method should get called after the actual computation is ended. */
+      virtual bool
+      deinitCompute ();
+
+      /** \brief Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with normals
+        * \param[in] index the index of the point in input_
+        * \param[out] lrf the resultant local reference frame
+        * \return signed distance of the highest point from the fitted plane. Max if the lrf is not computable.
+        */
+      SignedDistanceT
+      computePointLRF (const int index, Eigen::Matrix3f &lrf);
+
+      /** \brief Abstract feature estimation method.
+      * \param[out] output the resultant features
+      */
+      virtual void
+      computeFeature (PointCloudOut &output);
+
+
+    private:
+      /** \brief Radius used to find tangent axis. */
+      float tangent_radius_;
+
+      /** \brief Threshold that define if a support point is near the margins. */
+      float margin_thresh_; 
+
+      /** \brief Min number of neighbours required for the computation of Z axis. Otherwise, feature point normal is used. */
+      int min_neighbors_for_normal_axis_;
+
+      /** \brief Min number of neighbours required for the computation of X axis. Otherwise, a random X axis is set */
+      int min_neighbors_for_tangent_axis_;
+
+      /** \brief An input point cloud describing the surface that is to be used
+        * for nearest neighbor searches for the estimation of X axis.
+        */
+      PointCloudInConstPtr sampled_surface_;
+
+      /** \brief A pointer to the spatial search object used for the estimation of X axis. */
+      KdTreePtr sampled_tree_;
+
+      /** \brief Class for normal estimation. */
+      NormalEstimation<PointInT, PointNT> normal_estimation_;
+
+      /** \brief Signed distances of the highest points from the fitted planes.*/
+      std::vector<SignedDistanceT> 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 <pcl/features/impl/flare.hpp>
+#endif
+
+#endif  //#ifndef PCL_FLARE_H_