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 #ifndef PCL_FEATURES_IMPL_FLARE_H_ 40 #define PCL_FEATURES_IMPL_FLARE_H_ 41 42 #include <pcl/features/flare.h> 43 #include <pcl/common/geometry.h> 44 45 ////////////////////////////////////////////////////////////////////////////////////////////// 46 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> bool initCompute()47 pcl::FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT, SignedDistanceT>::initCompute () 48 { 49 if (!FeatureFromNormals<PointInT, PointNT, PointOutT>::initCompute ()) 50 { 51 PCL_ERROR ("[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ()); 52 return (false); 53 } 54 55 if (tangent_radius_ == 0.0f) 56 { 57 PCL_ERROR ("[pcl::%s::initCompute] tangent_radius_ not set.\n", getClassName ().c_str ()); 58 return (false); 59 } 60 61 // If no search sampled_surface_ has been defined, use the surface_ dataset as the search sampled_surface_ itself 62 if (!sampled_surface_) 63 { 64 fake_sampled_surface_ = true; 65 sampled_surface_ = surface_; 66 67 if (sampled_tree_) 68 { 69 PCL_WARN ("[pcl::%s::initCompute] sampled_surface_ is not set even if sampled_tree_ is already set.", getClassName ().c_str ()); 70 PCL_WARN ("sampled_tree_ will be rebuilt from surface_. Use sampled_surface_.\n"); 71 } 72 } 73 74 // Check if a space search locator was given for sampled_surface_ 75 if (!sampled_tree_) 76 { 77 if (sampled_surface_->isOrganized () && surface_->isOrganized () && input_->isOrganized ()) 78 sampled_tree_.reset (new pcl::search::OrganizedNeighbor<PointInT> ()); 79 else 80 sampled_tree_.reset (new pcl::search::KdTree<PointInT> (false)); 81 } 82 83 if (sampled_tree_->getInputCloud () != sampled_surface_) // Make sure the tree searches the sampled surface 84 sampled_tree_->setInputCloud (sampled_surface_); 85 86 return (true); 87 } 88 89 ////////////////////////////////////////////////////////////////////////////////////////////// 90 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> bool deinitCompute()91 pcl::FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT, SignedDistanceT>::deinitCompute () 92 { 93 // Reset the surface 94 if (fake_surface_) 95 { 96 surface_.reset (); 97 fake_surface_ = false; 98 } 99 // Reset the sampled surface 100 if (fake_sampled_surface_) 101 { 102 sampled_surface_.reset (); 103 fake_sampled_surface_ = false; 104 } 105 return (true); 106 } 107 108 ////////////////////////////////////////////////////////////////////////////////////////////// 109 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> SignedDistanceT computePointLRF(const int index,Eigen::Matrix3f & lrf)110 pcl::FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT, SignedDistanceT>::computePointLRF (const int index, 111 Eigen::Matrix3f &lrf) 112 { 113 Eigen::Vector3f x_axis, y_axis; 114 Eigen::Vector3f fitted_normal; //z_axis 115 116 //find Z axis 117 118 //extract support points for the computation of Z axis 119 pcl::Indices neighbours_indices; 120 std::vector<float> neighbours_distances; 121 122 const std::size_t n_normal_neighbours = 123 this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances); 124 if (n_normal_neighbours < static_cast<std::size_t>(min_neighbors_for_normal_axis_)) 125 { 126 if (!pcl::isFinite ((*normals_)[index])) 127 { 128 //normal is invalid 129 //setting lrf to NaN 130 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ()); 131 return (std::numeric_limits<SignedDistanceT>::max ()); 132 } 133 134 //set z_axis as the normal of index point 135 fitted_normal = (*normals_)[index].getNormalVector3fMap (); 136 } 137 else 138 { 139 float plane_curvature; 140 normal_estimation_.computePointNormal (*surface_, neighbours_indices, fitted_normal (0), fitted_normal (1), fitted_normal (2), plane_curvature); 141 142 //disambiguate Z axis with normal mean 143 if (!pcl::flipNormalTowardsNormalsMean<PointNT> (*normals_, neighbours_indices, fitted_normal)) 144 { 145 //all normals in the neighbourood are invalid 146 //setting lrf to NaN 147 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ()); 148 return (std::numeric_limits<SignedDistanceT>::max ()); 149 } 150 } 151 152 //setting LRF Z axis 153 lrf.row (2).matrix () = fitted_normal; 154 155 //find X axis 156 157 //extract support points for Rx radius 158 const std::size_t n_tangent_neighbours = 159 sampled_tree_->radiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances); 160 161 if (n_tangent_neighbours < static_cast<std::size_t>(min_neighbors_for_tangent_axis_)) 162 { 163 //set X axis as a random axis 164 x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal); 165 y_axis = fitted_normal.cross (x_axis); 166 167 lrf.row (0).matrix () = x_axis; 168 lrf.row (1).matrix () = y_axis; 169 170 return (std::numeric_limits<SignedDistanceT>::max ()); 171 } 172 173 //find point with the largest signed distance from tangent plane 174 175 SignedDistanceT shape_score; 176 SignedDistanceT best_shape_score = -std::numeric_limits<SignedDistanceT>::max (); 177 int best_shape_index = -1; 178 179 Eigen::Vector3f best_margin_point; 180 181 const float radius2 = tangent_radius_ * tangent_radius_; 182 const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2; 183 184 Vector3fMapConst feature_point = (*input_)[index].getVector3fMap (); 185 186 for (std::size_t curr_neigh = 0; curr_neigh < n_tangent_neighbours; ++curr_neigh) 187 { 188 const int& curr_neigh_idx = neighbours_indices[curr_neigh]; 189 const float& neigh_distance_sqr = neighbours_distances[curr_neigh]; 190 191 if (neigh_distance_sqr <= margin_distance2) 192 { 193 continue; 194 } 195 196 //point curr_neigh_idx is inside the ring between marginThresh and Radius 197 198 shape_score = fitted_normal.dot ((*sampled_surface_)[curr_neigh_idx].getVector3fMap ()); 199 200 if (shape_score > best_shape_score) 201 { 202 best_shape_index = curr_neigh_idx; 203 best_shape_score = shape_score; 204 } 205 } //for each neighbor 206 207 if (best_shape_index == -1) 208 { 209 x_axis = pcl::geometry::randomOrthogonalAxis (fitted_normal); 210 y_axis = fitted_normal.cross (x_axis); 211 212 lrf.row (0).matrix () = x_axis; 213 lrf.row (1).matrix () = y_axis; 214 215 return (std::numeric_limits<SignedDistanceT>::max ()); 216 } 217 218 //find orthogonal axis directed to best_shape_index point projection on plane with fittedNormal as axis 219 x_axis = pcl::geometry::projectedAsUnitVector (sampled_surface_->at (best_shape_index).getVector3fMap (), feature_point, fitted_normal); 220 221 y_axis = fitted_normal.cross (x_axis); 222 223 lrf.row (0).matrix () = x_axis; 224 lrf.row (1).matrix () = y_axis; 225 //z axis already set 226 227 best_shape_score -= fitted_normal.dot (feature_point); 228 return (best_shape_score); 229 } 230 231 ////////////////////////////////////////////////////////////////////////////////////////////// 232 template<typename PointInT, typename PointNT, typename PointOutT, typename SignedDistanceT> void computeFeature(PointCloudOut & output)233 pcl::FLARELocalReferenceFrameEstimation<PointInT, PointNT, PointOutT, SignedDistanceT>::computeFeature (PointCloudOut &output) 234 { 235 //check whether used with search radius or search k-neighbors 236 if (this->getKSearch () != 0) 237 { 238 PCL_ERROR ( 239 "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch (0) and setRadiusSearch (radius) to use this class.\n", 240 getClassName ().c_str ()); 241 return; 242 } 243 244 signed_distances_from_highest_points_.resize (indices_->size ()); 245 246 for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx) 247 { 248 Eigen::Matrix3f currentLrf; 249 PointOutT &rf = output[point_idx]; 250 251 signed_distances_from_highest_points_[point_idx] = computePointLRF ((*indices_)[point_idx], currentLrf); 252 if (signed_distances_from_highest_points_[point_idx] == std::numeric_limits<SignedDistanceT>::max ()) 253 { 254 output.is_dense = false; 255 } 256 257 rf.getXAxisVector3fMap () = currentLrf.row (0); 258 rf.getYAxisVector3fMap () = currentLrf.row (1); 259 rf.getZAxisVector3fMap () = currentLrf.row (2); 260 } 261 } 262 263 #define PCL_INSTANTIATE_FLARELocalReferenceFrameEstimation(T,NT,OutT,SdT) template class PCL_EXPORTS pcl::FLARELocalReferenceFrameEstimation<T,NT,OutT,SdT>; 264 265 #endif // PCL_FEATURES_IMPL_FLARE_H_ 266