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_);
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);
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);