184 Eigen::Matrix3f &
lrf)
201 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
203 return (std::numeric_limits<float>::max ());
213 Eigen::Vector3f x_axis, y_axis;
216 Eigen::Vector3f centroid;
229 if (tangent_radius_ != 0.0f && search_parameter_ != tangent_radius_)
243 const float radius2 = tangent_radius_ * tangent_radius_;
252 lrf.row (0).matrix () = x_axis;
254 check_margin_array_.assign(check_margin_array_size_,
false);
255 margin_array_min_angle_.assign(check_margin_array_size_, std::numeric_limits<float>::max ());
256 margin_array_max_angle_.assign(check_margin_array_size_, -std::numeric_limits<float>::max ());
257 margin_array_min_angle_normal_.assign(check_margin_array_size_, -1.0);
258 margin_array_max_angle_normal_.assign(check_margin_array_size_, -1.0);
289 directedOrthogonalAxis (
fitted_normal, input_->at (index).getVector3fMap (),
335 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
336 return (std::numeric_limits<float>::max ());
339 directedOrthogonalAxis (
fitted_normal, input_->at (index).getVector3fMap (),
343 lrf.row (0).matrix () = x_axis;
344 lrf.row (1).matrix () = y_axis;
359 lrf.row (0).matrix () = x_axis;
360 lrf.row (1).matrix () = y_axis;
369 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
370 return (std::numeric_limits<float>::max ());
373 directedOrthogonalAxis (
fitted_normal, input_->at (index).getVector3fMap (),
377 lrf.row (0).matrix () = x_axis;
378 lrf.row (1).matrix () = y_axis;
403 lrf.row (0).matrix () = x_axis;
404 lrf.row (1).matrix () = y_axis;
413 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
414 return (std::numeric_limits<float>::max ());
418 directedOrthogonalAxis (
fitted_normal, input_->at (index).getVector3fMap (),
422 lrf.row (0).matrix () = x_axis;
423 lrf.row (1).matrix () = y_axis;
440 const auto result = std::find_if(array.cbegin (), array.cend (),
441 [](
const auto& x) ->
bool { return x;});
442 return std::distance(array.cbegin (),
result);
452 if (!check_margin_array_[
ch])
457 while (!check_margin_array_[
hole_end % check_margin_array_size_])
467 % check_margin_array_size_;
520 if (
hole_end >= check_margin_array_size_)
548 lrf.setConstant (std::numeric_limits<float>::quiet_NaN ());
549 return (std::numeric_limits<float>::max ());
553 directedOrthogonalAxis (
fitted_normal, input_->at (index).getVector3fMap (),
560 lrf.row (0).matrix () = x_axis;
561 lrf.row (1).matrix () = y_axis;