1 /* 2 * Software License Agreement (BSD License) 3 * 4 * Point Cloud Library (PCL) - www.pointclouds.org 5 * Copyright (c) 2016-, Open Perception, Inc. 6 * 7 * All rights reserved. 8 * 9 * Redistribution and use in source and binary forms, with or without 10 * modification, are permitted provided that the following conditions 11 * are met: 12 * 13 * * Redistributions of source code must retain the above copyright 14 * notice, this list of conditions and the following disclaimer. 15 * * Redistributions in binary form must reproduce the above 16 * copyright notice, this list of conditions and the following 17 * disclaimer in the documentation and/or other materials provided 18 * with the distribution. 19 * * Neither the name of the copyright holder(s) nor the names of its 20 * contributors may be used to endorse or promote products derived 21 * from this software without specific prior written permission. 22 * 23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 34 * POSSIBILITY OF SUCH DAMAGE. 35 * 36 * 37 */ 38 39 #pragma once 40 41 #include <pcl/point_types.h> 42 #include <pcl/features/feature.h> 43 #include <pcl/features/normal_3d.h> 44 45 46 namespace pcl 47 { 48 49 /** \brief FLARELocalReferenceFrameEstimation implements the Fast LocAl Reference framE algorithm 50 * for local reference frame estimation as described here: 51 * 52 * - A. Petrelli, L. Di Stefano, 53 * "A repeatable and efficient canonical reference for surface matching", 54 * 3DimPVT, 2012 55 * 56 * FLARE algorithm is deployed in ReLOC algorithm proposed in: 57 * 58 * Petrelli A., Di Stefano L., "Pairwise registration by local orientation cues", Computer Graphics Forum, 2015. 59 * 60 * \author Alioscia Petrelli 61 * \ingroup features 62 */ 63 template<typename PointInT, typename PointNT, typename PointOutT = ReferenceFrame, typename SignedDistanceT = float> 64 class FLARELocalReferenceFrameEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT> 65 { 66 protected: 67 using Feature<PointInT, PointOutT>::feature_name_; 68 using Feature<PointInT, PointOutT>::input_; 69 using Feature<PointInT, PointOutT>::indices_; 70 using Feature<PointInT, PointOutT>::surface_; 71 using Feature<PointInT, PointOutT>::tree_; 72 using Feature<PointInT, PointOutT>::search_parameter_; 73 using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_; 74 using Feature<PointInT, PointOutT>::fake_surface_; 75 using Feature<PointInT, PointOutT>::getClassName; 76 77 using typename Feature<PointInT, PointOutT>::PointCloudIn; 78 using typename Feature<PointInT, PointOutT>::PointCloudOut; 79 80 using typename Feature<PointInT, PointOutT>::PointCloudInConstPtr; 81 82 using typename Feature<PointInT, PointOutT>::KdTreePtr; 83 84 using PointCloudSignedDistance = pcl::PointCloud<SignedDistanceT>; 85 using PointCloudSignedDistancePtr = typename PointCloudSignedDistance::Ptr; 86 87 using Ptr = shared_ptr<FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> >; 88 using ConstPtr = shared_ptr<const FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> >; 89 90 public: 91 /** \brief Constructor. */ FLARELocalReferenceFrameEstimation()92 FLARELocalReferenceFrameEstimation () : 93 tangent_radius_ (0.0f), 94 margin_thresh_ (0.85f), 95 min_neighbors_for_normal_axis_ (6), 96 min_neighbors_for_tangent_axis_ (6), 97 sampled_surface_ (), 98 sampled_tree_ (), 99 fake_sampled_surface_ (false) 100 { 101 feature_name_ = "FLARELocalReferenceFrameEstimation"; 102 } 103 104 //Getters/Setters 105 106 /** \brief Set the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point. 107 * 108 * \param[in] radius The search radius for x axis. 109 */ 110 inline void setTangentRadius(float radius)111 setTangentRadius (float radius) 112 { 113 tangent_radius_ = radius; 114 } 115 116 /** \brief Get the maximum distance of the points used to estimate the x_axis of the FLARE Reference Frame for a given point. 117 * 118 * \return The search radius for x axis. 119 */ 120 inline float getTangentRadius()121 getTangentRadius () const 122 { 123 return (tangent_radius_); 124 } 125 126 /** \brief Set the percentage of the search tangent radius after which a point is considered part of the support. 127 * 128 * \param[in] margin_thresh the percentage of the search tangent radius after which a point is considered part of the support. 129 */ 130 inline void setMarginThresh(float margin_thresh)131 setMarginThresh (float margin_thresh) 132 { 133 margin_thresh_ = margin_thresh; 134 } 135 136 /** \brief Get the percentage of the search tangent radius after which a point is considered part of the support. 137 * 138 * \return The percentage of the search tangent radius after which a point is considered part of the support. 139 */ 140 inline float getMarginThresh()141 getMarginThresh () const 142 { 143 return (margin_thresh_); 144 } 145 146 147 /** \brief Set min number of neighbours required for the computation of Z axis. 148 * 149 * \param[in] min_neighbors_for_normal_axis min number of neighbours required for the computation of Z axis. 150 */ 151 inline void setMinNeighboursForNormalAxis(int min_neighbors_for_normal_axis)152 setMinNeighboursForNormalAxis (int min_neighbors_for_normal_axis) 153 { 154 min_neighbors_for_normal_axis_ = min_neighbors_for_normal_axis; 155 } 156 157 /** \brief Get min number of neighbours required for the computation of Z axis. 158 * 159 * \return min number of neighbours required for the computation of Z axis. 160 */ 161 inline int getMinNeighboursForNormalAxis()162 getMinNeighboursForNormalAxis () const 163 { 164 return (min_neighbors_for_normal_axis_); 165 } 166 167 168 /** \brief Set min number of neighbours required for the computation of X axis. 169 * 170 * \param[in] min_neighbors_for_tangent_axis min number of neighbours required for the computation of X axis. 171 */ 172 inline void setMinNeighboursForTangentAxis(int min_neighbors_for_tangent_axis)173 setMinNeighboursForTangentAxis (int min_neighbors_for_tangent_axis) 174 { 175 min_neighbors_for_tangent_axis_ = min_neighbors_for_tangent_axis; 176 } 177 178 /** \brief Get min number of neighbours required for the computation of X axis. 179 * 180 * \return min number of neighbours required for the computation of X axis. 181 */ 182 inline int getMinNeighboursForTangentAxis()183 getMinNeighboursForTangentAxis () const 184 { 185 return (min_neighbors_for_tangent_axis_); 186 } 187 188 189 /** \brief Provide a pointer to the dataset used for the estimation of X axis. 190 * As the estimation of x axis is negligibly affected by surface downsampling, 191 * this method lets to consider a downsampled version of surface_ in the estimation of x axis. 192 * This is optional, if this is not set, it will only use the data in the 193 * surface_ cloud to estimate the x axis. 194 * \param[in] cloud a pointer to a PointCloud 195 */ 196 inline void setSearchSampledSurface(const PointCloudInConstPtr & cloud)197 setSearchSampledSurface(const PointCloudInConstPtr &cloud) 198 { 199 sampled_surface_ = cloud; 200 fake_sampled_surface_ = false; 201 } 202 203 /** \brief Get a pointer to the sampled_surface_ cloud dataset. */ 204 inline const PointCloudInConstPtr& getSearchSampledSurface()205 getSearchSampledSurface() const 206 { 207 return (sampled_surface_); 208 } 209 210 /** \brief Provide a pointer to the search object linked to sampled_surface. 211 * \param[in] tree a pointer to the spatial search object linked to sampled_surface. 212 */ 213 inline void setSearchMethodForSampledSurface(const KdTreePtr & tree)214 setSearchMethodForSampledSurface (const KdTreePtr &tree) { sampled_tree_ = tree; } 215 216 /** \brief Get a pointer to the search method used for the extimation of x axis. */ 217 inline const KdTreePtr& getSearchMethodForSampledSurface()218 getSearchMethodForSampledSurface () const 219 { 220 return (sampled_tree_); 221 } 222 223 /** \brief Get the signed distances of the highest points from the fitted planes. */ 224 inline const std::vector<SignedDistanceT> & getSignedDistancesFromHighestPoints()225 getSignedDistancesFromHighestPoints () const 226 { 227 return (signed_distances_from_highest_points_); 228 } 229 230 protected: 231 /** \brief This method should get called before starting the actual computation. */ 232 bool 233 initCompute () override; 234 235 /** \brief This method should get called after the actual computation is ended. */ 236 bool 237 deinitCompute () override; 238 239 /** \brief Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with normals 240 * \param[in] index the index of the point in input_ 241 * \param[out] lrf the resultant local reference frame 242 * \return signed distance of the highest point from the fitted plane. Max if the lrf is not computable. 243 */ 244 SignedDistanceT 245 computePointLRF (const int index, Eigen::Matrix3f &lrf); 246 247 /** \brief Abstract feature estimation method. 248 * \param[out] output the resultant features 249 */ 250 void 251 computeFeature (PointCloudOut &output) override; 252 253 254 private: 255 /** \brief Radius used to find tangent axis. */ 256 float tangent_radius_; 257 258 /** \brief Threshold that define if a support point is near the margins. */ 259 float margin_thresh_; 260 261 /** \brief Min number of neighbours required for the computation of Z axis. Otherwise, feature point normal is used. */ 262 int min_neighbors_for_normal_axis_; 263 264 /** \brief Min number of neighbours required for the computation of X axis. Otherwise, a random X axis is set */ 265 int min_neighbors_for_tangent_axis_; 266 267 /** \brief An input point cloud describing the surface that is to be used 268 * for nearest neighbor searches for the estimation of X axis. 269 */ 270 PointCloudInConstPtr sampled_surface_; 271 272 /** \brief A pointer to the spatial search object used for the estimation of X axis. */ 273 KdTreePtr sampled_tree_; 274 275 /** \brief Class for normal estimation. */ 276 NormalEstimation<PointInT, PointNT> normal_estimation_; 277 278 /** \brief Signed distances of the highest points from the fitted planes.*/ 279 std::vector<SignedDistanceT> signed_distances_from_highest_points_; 280 281 /** \brief If no sampled_surface_ is given, we use surface_ as the sampled surface. */ 282 bool fake_sampled_surface_; 283 284 }; 285 286 } 287 288 #ifdef PCL_NO_PRECOMPILE 289 #include <pcl/features/impl/flare.hpp> 290 #endif 291