43 #include <pcl/features/boundary.h>
44 #include <pcl/common/point_tests.h>
50 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
53 const std::vector<int> &indices,
54 const Eigen::Vector4f &u,
const Eigen::Vector4f &v,
55 const float angle_threshold)
57 return (isBoundaryPoint (cloud, cloud[q_idx], indices, u, v, angle_threshold));
61 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
bool
65 const Eigen::Vector4f &u,
const Eigen::Vector4f &v,
66 const float angle_threshold)
68 if (indices.size () < 3)
71 if (!std::isfinite (q_point.x) || !std::isfinite (q_point.y) || !std::isfinite (q_point.z))
75 std::vector<float> angles (indices.size ());
76 float max_dif = FLT_MIN, dif;
79 for (
const auto &index : indices)
81 if (!std::isfinite (cloud[index].x) ||
82 !std::isfinite (cloud[index].y) ||
83 !std::isfinite (cloud[index].z))
86 Eigen::Vector4f delta = cloud[index].getVector4fMap () - q_point.getVector4fMap ();
87 if (delta == Eigen::Vector4f::Zero())
90 angles[cp++] = std::atan2 (v.dot (delta), u.dot (delta));
96 std::sort (angles.begin (), angles.end ());
99 for (std::size_t i = 0; i < angles.size () - 1; ++i)
101 dif = angles[i + 1] - angles[i];
106 dif = 2 *
static_cast<float> (
M_PI) - angles[angles.size () - 1] + angles[0];
111 return (max_dif > angle_threshold);
115 template <
typename Po
intInT,
typename Po
intNT,
typename Po
intOutT>
void
121 std::vector<float> nn_dists (k_);
123 Eigen::Vector4f u = Eigen::Vector4f::Zero (), v = Eigen::Vector4f::Zero ();
125 output.is_dense =
true;
127 if (input_->is_dense)
130 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
132 if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
134 output[idx].boundary_point = std::numeric_limits<std::uint8_t>::quiet_NaN ();
135 output.is_dense =
false;
142 getCoordinateSystemOnPlane ((*normals_)[(*indices_)[idx]], u, v);
145 output[idx].boundary_point = isBoundaryPoint (*surface_, (*input_)[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
151 for (std::size_t idx = 0; idx < indices_->size (); ++idx)
153 if (!
isFinite ((*input_)[(*indices_)[idx]]) ||
154 this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
156 output[idx].boundary_point = std::numeric_limits<std::uint8_t>::quiet_NaN ();
157 output.is_dense =
false;
164 getCoordinateSystemOnPlane ((*normals_)[(*indices_)[idx]], u, v);
167 output[idx].boundary_point = isBoundaryPoint (*surface_, (*input_)[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
172 #define PCL_INSTANTIATE_BoundaryEstimation(PointInT,PointNT,PointOutT) template class PCL_EXPORTS pcl::BoundaryEstimation<PointInT, PointNT, PointOutT>;