39 #ifndef PCL_FEATURES_IMPL_FLARE_H_
40 #define PCL_FEATURES_IMPL_FLARE_H_
42 #include <pcl/features/flare.h>
43 #include <pcl/common/geometry.h>
46 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename SignedDistanceT>
bool
51 PCL_ERROR (
"[pcl::%s::initCompute] Init failed.\n", getClassName ().c_str ());
55 if (tangent_radius_ == 0.0f)
57 PCL_ERROR (
"[pcl::%s::initCompute] tangent_radius_ not set.\n", getClassName ().c_str ());
62 if (!sampled_surface_)
64 fake_sampled_surface_ =
true;
65 sampled_surface_ = surface_;
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");
77 if (sampled_surface_->isOrganized () && surface_->isOrganized () && input_->isOrganized ())
83 if (sampled_tree_->getInputCloud () != sampled_surface_)
84 sampled_tree_->setInputCloud (sampled_surface_);
90 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename SignedDistanceT>
bool
97 fake_surface_ =
false;
100 if (fake_sampled_surface_)
102 sampled_surface_.reset ();
103 fake_sampled_surface_ =
false;
109 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename SignedDistanceT> SignedDistanceT
111 Eigen::Matrix3f &lrf)
113 Eigen::Vector3f x_axis, y_axis;
114 Eigen::Vector3f fitted_normal;
119 std::vector<int> neighbours_indices;
120 std::vector<float> neighbours_distances;
121 int n_neighbours = this->searchForNeighbors (index, search_parameter_, neighbours_indices, neighbours_distances);
123 if (n_neighbours < min_neighbors_for_normal_axis_)
129 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
130 return (std::numeric_limits<SignedDistanceT>::max ());
134 fitted_normal = (*normals_)[index].getNormalVector3fMap ();
138 float plane_curvature;
139 normal_estimation_.computePointNormal (*surface_, neighbours_indices, fitted_normal (0), fitted_normal (1), fitted_normal (2), plane_curvature);
142 if (!pcl::flipNormalTowardsNormalsMean<PointNT> (*normals_, neighbours_indices, fitted_normal))
146 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
147 return (std::numeric_limits<SignedDistanceT>::max ());
152 lrf.row (2).matrix () = fitted_normal;
157 n_neighbours = sampled_tree_->radiusSearch ((*input_)[index], tangent_radius_, neighbours_indices, neighbours_distances);
159 if (n_neighbours < min_neighbors_for_tangent_axis_)
163 y_axis = fitted_normal.cross (x_axis);
165 lrf.row (0).matrix () = x_axis;
166 lrf.row (1).matrix () = y_axis;
168 return (std::numeric_limits<SignedDistanceT>::max ());
173 SignedDistanceT shape_score;
174 SignedDistanceT best_shape_score = -std::numeric_limits<SignedDistanceT>::max ();
175 int best_shape_index = -1;
177 Eigen::Vector3f best_margin_point;
179 const float radius2 = tangent_radius_ * tangent_radius_;
180 const float margin_distance2 = margin_thresh_ * margin_thresh_ * radius2;
184 for (
int curr_neigh = 0; curr_neigh < n_neighbours; ++curr_neigh)
186 const int& curr_neigh_idx = neighbours_indices[curr_neigh];
187 const float& neigh_distance_sqr = neighbours_distances[curr_neigh];
189 if (neigh_distance_sqr <= margin_distance2)
196 shape_score = fitted_normal.dot ((*sampled_surface_)[curr_neigh_idx].getVector3fMap ());
198 if (shape_score > best_shape_score)
200 best_shape_index = curr_neigh_idx;
201 best_shape_score = shape_score;
205 if (best_shape_index == -1)
208 y_axis = fitted_normal.cross (x_axis);
210 lrf.row (0).matrix () = x_axis;
211 lrf.row (1).matrix () = y_axis;
213 return (std::numeric_limits<SignedDistanceT>::max ());
219 y_axis = fitted_normal.cross (x_axis);
221 lrf.row (0).matrix () = x_axis;
222 lrf.row (1).matrix () = y_axis;
225 best_shape_score -= fitted_normal.dot (feature_point);
226 return (best_shape_score);
230 template<
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT,
typename SignedDistanceT>
void
234 if (this->getKSearch () != 0)
237 "[pcl::%s::computeFeature] Error! Search method set to k-neighborhood. Call setKSearch (0) and setRadiusSearch (radius) to use this class.\n",
238 getClassName ().c_str ());
242 signed_distances_from_highest_points_.resize (indices_->size ());
244 for (
size_t point_idx = 0; point_idx < indices_->size (); ++point_idx)
246 Eigen::Matrix3f currentLrf;
247 PointOutT &rf = output[point_idx];
249 signed_distances_from_highest_points_[point_idx] = computePointLRF ((*indices_)[point_idx], currentLrf);
250 if (signed_distances_from_highest_points_[point_idx] == std::numeric_limits<SignedDistanceT>::max ())
255 rf.getXAxisVector3fMap () = currentLrf.row (0);
256 rf.getYAxisVector3fMap () = currentLrf.row (1);
257 rf.getZAxisVector3fMap () = currentLrf.row (2);
261 #define PCL_INSTANTIATE_FLARELocalReferenceFrameEstimation(T,NT,OutT,SdT) template class PCL_EXPORTS pcl::FLARELocalReferenceFrameEstimation<T,NT,OutT,SdT>;
263 #endif // PCL_FEATURES_IMPL_FLARE_H_
Eigen::Vector3f randomOrthogonalAxis(Eigen::Vector3f const &axis)
Define a random unit vector orthogonal to axis.
search::KdTree is a wrapper class which inherits the pcl::KdTree class for performing search function...
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...
virtual bool initCompute()
This method should get called before starting the actual computation.
virtual bool deinitCompute()
This method should get called after the actual computation is ended.
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...
virtual void computeFeature(PointCloudOut &output)
Abstract feature estimation method.
const Eigen::Map< const Eigen::Vector3f > Vector3fMapConst
PointCloud represents the base class in PCL for storing collections of 3D points. ...
OrganizedNeighbor is a class for optimized nearest neigbhor search in organized point clouds...
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...
bool is_dense
True if no points are invalid (e.g., have NaN or Inf values in any of their floating point fields)...