39 #ifndef PCL_FEATURES_IMPL_FLARE_H_
40 #define PCL_FEATURES_IMPL_FLARE_H_
42 #include <pcl/features/flare.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 ())
252 output.is_dense =
false;
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_