39 #ifndef PCL_FEATURES_IMPL_FLARE_H_
40 #define PCL_FEATURES_IMPL_FLARE_H_
42 #include <pcl/features/flare.h>
44 #include <pcl/search/kdtree.h>
45 #include <pcl/search/organized.h>
48 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename SignedDistanceT>
bool
53 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
57 if (tangent_radius_ == 0.0f)
59 PCL_ERROR (
"[pcl::%s::initCompute] tangent_radius_ not set.\n", getClassName ().c_str ());
64 if (!sampled_surface_)
66 fake_sampled_surface_ =
true;
67 sampled_surface_ = surface_;
71 PCL_WARN (
"[pcl::%s::initCompute] sampled_surface_ is not set even if sampled_tree_ is already set.", getClassName ().c_str ());
72 PCL_WARN (
"sampled_tree_ will be rebuilt from surface_. Use sampled_surface_.\n");
79 if (sampled_surface_->isOrganized () && surface_->isOrganized () && input_->isOrganized ())
85 if (sampled_tree_->getInputCloud () != sampled_surface_)
86 sampled_tree_->setInputCloud (sampled_surface_);
92 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename SignedDistanceT>
bool
99 fake_surface_ =
false;
102 if (fake_sampled_surface_)
104 sampled_surface_.reset ();
105 fake_sampled_surface_ =
false;
111 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename SignedDistanceT> SignedDistanceT
113 Eigen::Matrix3f &lrf)
115 Eigen::Vector3f x_axis, y_axis;
116 Eigen::Vector3f fitted_normal;
122 std::vector<float> neighbours_distances;
124 const std::size_t n_normal_neighbours =
125 this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
126 if (n_normal_neighbours <
static_cast<std::size_t
>(min_neighbors_for_normal_axis_))
132 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
133 return (std::numeric_limits<SignedDistanceT>::max ());
137 fitted_normal = (*normals_)[index].getNormalVector3fMap ();
141 float plane_curvature;
142 normal_estimation_.computePointNormal (*surface_, neighbours_indices, fitted_normal (0), fitted_normal (1), fitted_normal (2), plane_curvature);
145 if (!pcl::flipNormalTowardsNormalsMean<PointNT> (*normals_, neighbours_indices, fitted_normal))
149 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
150 return (std::numeric_limits<SignedDistanceT>::max ());
155 lrf.row (2).matrix () = fitted_normal;
160 const std::size_t n_tangent_neighbours =
161 sampled_tree_->radiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances);
163 if (n_tangent_neighbours <
static_cast<std::size_t
>(min_neighbors_for_tangent_axis_))
167 y_axis = fitted_normal.cross (x_axis);
169 lrf.row (0).matrix () = x_axis;
170 lrf.row (1).matrix () = y_axis;
172 return (std::numeric_limits<SignedDistanceT>::max ());
177 SignedDistanceT shape_score;
178 SignedDistanceT best_shape_score = -std::numeric_limits<SignedDistanceT>::max ();
179 int best_shape_index = -1;
181 Eigen::Vector3f best_margin_point;
183 const float radius2 = tangent_radius_ * tangent_radius_;
184 const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
188 for (std::size_t curr_neigh = 0; curr_neigh < n_tangent_neighbours; ++curr_neigh)
190 const int& curr_neigh_idx = neighbours_indices[curr_neigh];
191 const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
193 if (neigh_distance_sqr <= margin_distance2)
200 shape_score = fitted_normal.dot ((*sampled_surface_)[curr_neigh_idx].getVector3fMap ());
202 if (shape_score > best_shape_score)
204 best_shape_index = curr_neigh_idx;
205 best_shape_score = shape_score;
209 if (best_shape_index == -1)
212 y_axis = fitted_normal.cross (x_axis);
214 lrf.row (0).matrix () = x_axis;
215 lrf.row (1).matrix () = y_axis;
217 return (std::numeric_limits<SignedDistanceT>::max ());
223 y_axis = fitted_normal.cross (x_axis);
225 lrf.row (0).matrix () = x_axis;
226 lrf.row (1).matrix () = y_axis;
229 best_shape_score -= fitted_normal.dot (feature_point);
230 return (best_shape_score);
234 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename SignedDistanceT>
void
238 if (this->getKSearch () != 0)
241 "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch (0) and setRadiusSearch (radius) to use this class.\n",
242 getClassName ().c_str ());
246 signed_distances_from_highest_points_.resize (indices_->size ());
248 for (std::size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
250 Eigen::Matrix3f currentLrf;
251 PointOutT &rf = output[point_idx];
253 signed_distances_from_highest_points_[point_idx] = computePointLRF ((*indices_)[point_idx], currentLrf);
254 if (signed_distances_from_highest_points_[point_idx] == std::numeric_limits<SignedDistanceT>::max ())
256 output.is_dense =
false;
259 rf.getXAxisVector3fMap () = currentLrf.row (0);
260 rf.getYAxisVector3fMap () = currentLrf.row (1);
261 rf.getZAxisVector3fMap () = currentLrf.row (2);
265 #define PCL_INSTANTIATE_FLARELocalReferenceFrameEstimation(T,NT,OutT,SdT) template class PCL_EXPORTS pcl::FLARELocalReferenceFrameEstimation<T,NT,OutT,SdT>;
bool deinitCompute() override
This method should get called after the actual computation is ended.
void computeFeature(PointCloudOut &output) override
Abstract feature estimation method.
bool initCompute() override
This method should get called before starting the actual computation.
SignedDistanceT computePointLRF(const int index, Eigen::Matrix3f &lrf)
Estimate the LRF descriptor for a given point based on its spatial neighborhood of 3D points with nor...
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
OrganizedNeighbor is a class for optimized nearest neighbor search in organized projectable point clo...
Defines some geometrical functions and utility functions.
Eigen::Vector3f projectedAsUnitVector(Eigen::Vector3f const &point, Eigen::Vector3f const &plane_origin, Eigen::Vector3f const &plane_normal)
Given a plane defined by plane_origin and plane_normal, find the unit vector pointing from plane_orig...
Eigen::Vector3f randomOrthogonalAxis(Eigen::Vector3f const &axis)
Define a random unit vector orthogonal to axis.
bool isFinite(const PointT &pt)
Tests if the 3D components of a point are all finite param[in] pt point to be tested return true if f...
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
IndicesAllocator<> Indices
Type used for indices in PCL.