Point Cloud Library (PCL)
1.7.0
|
00001 /* 00002 * Software License Agreement (BSD License) 00003 * 00004 * Point Cloud Library (PCL) - www.pointclouds.org 00005 * Copyright (c) 2010-2012, Willow Garage, Inc. 00006 * Copyright (c) 2012-, Open Perception, Inc. 00007 * 00008 * All rights reserved. 00009 * 00010 * Redistribution and use in source and binary forms, with or without 00011 * modification, are permitted provided that the following conditions 00012 * are met: 00013 * 00014 * * Redistributions of source code must retain the above copyright 00015 * notice, this list of conditions and the following disclaimer. 00016 * * Redistributions in binary form must reproduce the above 00017 * copyright notice, this list of conditions and the following 00018 * disclaimer in the documentation and/or other materials provided 00019 * with the distribution. 00020 * * Neither the name of the copyright holder(s) nor the names of its 00021 * contributors may be used to endorse or promote products derived 00022 * from this software without specific prior written permission. 00023 * 00024 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 00025 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 00026 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 00027 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 00028 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 00029 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 00030 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 00031 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 00032 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 00033 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 00034 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00035 * POSSIBILITY OF SUCH DAMAGE. 00036 * 00037 * 00038 */ 00039 00040 #ifndef PCL_BOARD_H_ 00041 #define PCL_BOARD_H_ 00042 00043 #include <pcl/point_types.h> 00044 #include <pcl/features/feature.h> 00045 00046 namespace pcl 00047 { 00048 /** \brief BOARDLocalReferenceFrameEstimation implements the BOrder Aware Repeatable Directions algorithm 00049 * for local reference frame estimation as described here: 00050 * 00051 * - A. Petrelli, L. Di Stefano, 00052 * "On the repeatability of the local reference frame for partial shape matching", 00053 * 13th International Conference on Computer Vision (ICCV), 2011 00054 * 00055 * \author Alioscia Petrelli (original), Tommaso Cavallari (PCL port) 00056 * \ingroup features 00057 */ 00058 template<typename PointInT, typename PointNT, typename PointOutT = ReferenceFrame> 00059 class BOARDLocalReferenceFrameEstimation : public FeatureFromNormals<PointInT, PointNT, PointOutT> 00060 { 00061 public: 00062 typedef boost::shared_ptr<BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> > Ptr; 00063 typedef boost::shared_ptr<const BOARDLocalReferenceFrameEstimation<PointInT, PointNT, PointOutT> > ConstPtr; 00064 00065 /** \brief Constructor. */ 00066 BOARDLocalReferenceFrameEstimation () : 00067 tangent_radius_ (0.0f), 00068 find_holes_ (false), 00069 margin_thresh_ (0.85f), 00070 check_margin_array_size_ (24), 00071 hole_size_prob_thresh_ (0.2f), 00072 steep_thresh_ (0.1f), 00073 check_margin_array_ (), 00074 margin_array_min_angle_ (), 00075 margin_array_max_angle_ (), 00076 margin_array_min_angle_normal_ (), 00077 margin_array_max_angle_normal_ () 00078 { 00079 feature_name_ = "BOARDLocalReferenceFrameEstimation"; 00080 setCheckMarginArraySize (check_margin_array_size_); 00081 } 00082 00083 /** \brief Empty destructor */ 00084 virtual ~BOARDLocalReferenceFrameEstimation () {} 00085 00086 //Getters/Setters 00087 00088 /** \brief Set the maximum distance of the points used to estimate the x_axis and y_axis of the BOARD Reference Frame for a given point. 00089 * 00090 * \param[in] radius The search radius for x and y axes. If not set or set to 0 the parameter given with setRadiusSearch is used. 00091 */ 00092 inline void 00093 setTangentRadius (float radius) 00094 { 00095 tangent_radius_ = radius; 00096 } 00097 00098 /** \brief Get the maximum distance of the points used to estimate the x_axis and y_axis of the BOARD Reference Frame for a given point. 00099 * 00100 * \return The search radius for x and y axes. If set to 0 the parameter given with setRadiusSearch is used. 00101 */ 00102 inline float 00103 getTangentRadius () const 00104 { 00105 return (tangent_radius_); 00106 } 00107 00108 /** \brief Sets whether holes in the margin of the support, for each point, are searched and accounted for in the estimation of the 00109 * Reference Frame or not. 00110 * 00111 * \param[in] find_holes Enable/Disable the search for holes in the support. 00112 */ 00113 inline void 00114 setFindHoles (bool find_holes) 00115 { 00116 find_holes_ = find_holes; 00117 } 00118 00119 /** \brief Gets whether holes in the margin of the support, for each point, are searched and accounted for in the estimation of the 00120 * Reference Frame or not. 00121 * 00122 * \return The search for holes in the support is enabled/disabled. 00123 */ 00124 inline bool 00125 getFindHoles () const 00126 { 00127 return (find_holes_); 00128 } 00129 00130 /** \brief Sets the percentage of the search radius (or tangent radius if set) after which a point is considered part of the support margin. 00131 * 00132 * \param[in] margin_thresh the percentage of the search radius after which a point is considered a margin point. 00133 */ 00134 inline void 00135 setMarginThresh (float margin_thresh) 00136 { 00137 margin_thresh_ = margin_thresh; 00138 } 00139 00140 /** \brief Gets the percentage of the search radius (or tangent radius if set) after which a point is considered part of the support margin. 00141 * 00142 * \return The percentage of the search radius after which a point is considered a margin point. 00143 */ 00144 inline float 00145 getMarginThresh () const 00146 { 00147 return (margin_thresh_); 00148 } 00149 00150 /** \brief Sets the number of slices in which is divided the margin for the search of missing regions. 00151 * 00152 * \param[in] size the number of margin slices. 00153 */ 00154 void 00155 setCheckMarginArraySize (int size) 00156 { 00157 check_margin_array_size_ = size; 00158 00159 check_margin_array_.clear (); 00160 check_margin_array_.resize (check_margin_array_size_); 00161 00162 margin_array_min_angle_.clear (); 00163 margin_array_min_angle_.resize (check_margin_array_size_); 00164 00165 margin_array_max_angle_.clear (); 00166 margin_array_max_angle_.resize (check_margin_array_size_); 00167 00168 margin_array_min_angle_normal_.clear (); 00169 margin_array_min_angle_normal_.resize (check_margin_array_size_); 00170 00171 margin_array_max_angle_normal_.clear (); 00172 margin_array_max_angle_normal_.resize (check_margin_array_size_); 00173 } 00174 00175 /** \brief Gets the number of slices in which is divided the margin for the search of missing regions. 00176 * 00177 * \return the number of margin slices. 00178 */ 00179 inline int 00180 getCheckMarginArraySize () const 00181 { 00182 return (check_margin_array_size_); 00183 } 00184 00185 /** \brief Given the angle width of a hole in the support margin, sets the minimum percentage of a circumference this angle 00186 * must cover to be considered a missing region in the support and hence used for the estimation of the Reference Frame. 00187 * 00188 * \param[in] prob_thresh the minimum percentage of a circumference after which a hole is considered in the calculation 00189 */ 00190 inline void 00191 setHoleSizeProbThresh (float prob_thresh) 00192 { 00193 hole_size_prob_thresh_ = prob_thresh; 00194 } 00195 00196 /** \brief Given the angle width of a hole in the support margin, gets the minimum percentage of a circumference this angle 00197 * must cover to be considered a missing region in the support and hence used for the estimation of the Reference Frame. 00198 * 00199 * \return the minimum percentage of a circumference after which a hole is considered in the calculation 00200 */ 00201 inline float 00202 getHoleSizeProbThresh () const 00203 { 00204 return (hole_size_prob_thresh_); 00205 } 00206 00207 /** \brief Sets the minimum steepness that the normals of the points situated on the borders of the hole, with reference 00208 * to the normal of the best point found by the algorithm, must have in order to be considered in the calculation of the Reference Frame. 00209 * 00210 * \param[in] steep_thresh threshold that defines if a missing region contains a point with the most different normal. 00211 */ 00212 inline void 00213 setSteepThresh (float steep_thresh) 00214 { 00215 steep_thresh_ = steep_thresh; 00216 } 00217 00218 /** \brief Gets the minimum steepness that the normals of the points situated on the borders of the hole, with reference 00219 * to the normal of the best point found by the algorithm, must have in order to be considered in the calculation of the Reference Frame. 00220 * 00221 * \return threshold that defines if a missing region contains a point with the most different normal. 00222 */ 00223 inline float 00224 getSteepThresh () const 00225 { 00226 return (steep_thresh_); 00227 } 00228 00229 protected: 00230 using Feature<PointInT, PointOutT>::feature_name_; 00231 using Feature<PointInT, PointOutT>::getClassName; 00232 using Feature<PointInT, PointOutT>::input_; 00233 using Feature<PointInT, PointOutT>::indices_; 00234 using Feature<PointInT, PointOutT>::surface_; 00235 using Feature<PointInT, PointOutT>::tree_; 00236 using Feature<PointInT, PointOutT>::search_parameter_; 00237 using FeatureFromNormals<PointInT, PointNT, PointOutT>::normals_; 00238 00239 typedef typename Feature<PointInT, PointOutT>::PointCloudIn PointCloudIn; 00240 typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut; 00241 00242 void 00243 resetData () 00244 { 00245 setCheckMarginArraySize (check_margin_array_size_); 00246 } 00247 00248 /** \brief Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with normals 00249 * \param[in] index the index of the point in input_ 00250 * \param[out] lrf the resultant local reference frame 00251 */ 00252 float 00253 computePointLRF (const int &index, Eigen::Matrix3f &lrf); 00254 00255 /** \brief Abstract feature estimation method. 00256 * \param[out] output the resultant features 00257 */ 00258 virtual void 00259 computeFeature (PointCloudOut &output); 00260 00261 /** \brief Given an axis (with origin axis_origin), return the orthogonal axis directed to point. 00262 * 00263 * \note axis must be normalized. 00264 * 00265 * \param[in] axis the axis 00266 * \param[in] axis_origin the axis_origin 00267 * \param[in] point the point towards which the resulting axis is directed 00268 * \param[out] directed_ortho_axis the directed orthogonal axis calculated 00269 */ 00270 void 00271 directedOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f const &axis_origin, 00272 Eigen::Vector3f const &point, Eigen::Vector3f &directed_ortho_axis); 00273 00274 /** \brief return the angle (in radians) that rotate v1 to v2 with respect to axis . 00275 * 00276 * \param[in] v1 the first vector 00277 * \param[in] v2 the second vector 00278 * \param[in] axis the rotation axis. Axis must be normalized and orthogonal to plane defined by v1 and v2. 00279 * \return angle 00280 */ 00281 float 00282 getAngleBetweenUnitVectors (Eigen::Vector3f const &v1, Eigen::Vector3f const &v2, Eigen::Vector3f const &axis); 00283 00284 /** \brief Disambiguates a normal direction using adjacent normals 00285 * 00286 * \param[in] normals_cloud a cloud of normals used for the calculation 00287 * \param[in] normal_indices the indices of the normals in the cloud that should to be used for the calculation 00288 * \param[in,out] normal the normal to disambiguate, the calculation is performed in place 00289 */ 00290 void 00291 normalDisambiguation (pcl::PointCloud<PointNT> const &normals_cloud, std::vector<int> const &normal_indices, 00292 Eigen::Vector3f &normal); 00293 00294 /** \brief Compute Least Square Plane Fitting in a set of 3D points 00295 * 00296 * \param[in] points Matrix(nPoints,3) of 3D points coordinates 00297 * \param[out] center centroid of the distribution of points that belongs to the fitted plane 00298 * \param[out] norm normal to the fitted plane 00299 */ 00300 void 00301 planeFitting (Eigen::Matrix<float, Eigen::Dynamic, 3> const &points, Eigen::Vector3f ¢er, 00302 Eigen::Vector3f &norm); 00303 00304 /** \brief Given a plane (origin and normal) and a point, return the projection of x on plane 00305 * 00306 * Equivalent to vtkPlane::ProjectPoint() 00307 * 00308 * \param[in] point the point to project 00309 * \param[in] origin_point a point belonging to the plane 00310 * \param[in] plane_normal normal of the plane 00311 * \param[out] projected_point the projection of the point on the plane 00312 */ 00313 void 00314 projectPointOnPlane (Eigen::Vector3f const &point, Eigen::Vector3f const &origin_point, 00315 Eigen::Vector3f const &plane_normal, Eigen::Vector3f &projected_point); 00316 00317 /** \brief Given an axis, return a random orthogonal axis 00318 * 00319 * \param[in] axis input axis 00320 * \param[out] rand_ortho_axis an axis orthogonal to the input axis and whose direction is random 00321 */ 00322 void 00323 randomOrthogonalAxis (Eigen::Vector3f const &axis, Eigen::Vector3f &rand_ortho_axis); 00324 00325 /** \brief Check if val1 and val2 are equals. 00326 * 00327 * \param[in] val1 first number to check. 00328 * \param[in] val2 second number to check. 00329 * \param[in] zero_float_eps epsilon 00330 * \return true if val1 is equal to val2, false otherwise. 00331 */ 00332 inline bool 00333 areEquals (float val1, float val2, float zero_float_eps = 1E-8f) const 00334 { 00335 return (std::abs (val1 - val2) < zero_float_eps); 00336 } 00337 00338 private: 00339 /** \brief Radius used to find tangent axis. */ 00340 float tangent_radius_; 00341 00342 /** \brief If true, check if support is complete or has missing regions because it is too near to mesh borders. */ 00343 bool find_holes_; 00344 00345 /** \brief Threshold that define if a support point is near the margins. */ 00346 float margin_thresh_; 00347 00348 /** \brief Number of slices that divide the support in order to determine if a missing region is present. */ 00349 int check_margin_array_size_; 00350 00351 /** \brief Threshold used to determine a missing region */ 00352 float hole_size_prob_thresh_; 00353 00354 /** \brief Threshold that defines if a missing region contains a point with the most different normal. */ 00355 float steep_thresh_; 00356 00357 std::vector<bool> check_margin_array_; 00358 std::vector<float> margin_array_min_angle_; 00359 std::vector<float> margin_array_max_angle_; 00360 std::vector<float> margin_array_min_angle_normal_; 00361 std::vector<float> margin_array_max_angle_normal_; 00362 }; 00363 } 00364 00365 #ifdef PCL_NO_PRECOMPILE 00366 #include <pcl/features/impl/board.hpp> 00367 #endif 00368 00369 #endif //#ifndef PCL_BOARD_H_