template <typename PointInT, typename PointNT, typename PointOutT> void
pcl16::BoundaryEstimation<PointInT, PointNT, PointOutT>::computeFeature (PointCloudOut &output)
{
// Allocate enough space to hold the results
// \note This resize is irrelevant for a radiusSearch ().
std::vector<int> nn_indices (k_);
std::vector<float> nn_dists (k_);
Eigen::Vector4f u = Eigen::Vector4f::Zero (), v = Eigen::Vector4f::Zero ();
output.is_dense = true;
// Save a few cycles by not checking every point for NaN/Inf values if the cloud is set to dense
if (input_->is_dense)
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points[idx].boundary_point = std::numeric_limits<uint8_t>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Obtain a coordinate system on the least-squares plane
//v = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().unitOrthogonal ();
//u = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().cross3 (v);
getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v);
// Estimate whether the point is lying on a boundary surface or not
output.points[idx].boundary_point = isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
}
}
else
{
// Iterating over the entire index vector
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
{
output.points[idx].boundary_point = std::numeric_limits<uint8_t>::quiet_NaN ();
output.is_dense = false;
continue;
}
// Obtain a coordinate system on the least-squares plane
//v = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().unitOrthogonal ();
//u = normals_->points[(*indices_)[idx]].getNormalVector4fMap ().cross3 (v);
getCoordinateSystemOnPlane (normals_->points[(*indices_)[idx]], u, v);
// Estimate whether the point is lying on a boundary surface or not
output.points[idx].boundary_point = isBoundaryPoint (*surface_, input_->points[(*indices_)[idx]], nn_indices, u, v, angle_threshold_);
}
}
}
template <typename PointInT, typename PointNT, typename PointOutT> bool
pcl16::BoundaryEstimation<PointInT, PointNT, PointOutT>::isBoundaryPoint (
const pcl16::PointCloud<PointInT> &cloud, int q_idx,
const std::vector<int> &indices,
const Eigen::Vector4f &u, const Eigen::Vector4f &v,
const float angle_threshold)
{
return (isBoundaryPoint (cloud, cloud.points[q_idx], indices, u, v, angle_threshold));
}
void makeHollow() {
unsigned int x, y, z, xn, yn, zn, radius = THICKNESS * SIZE / (RANGE * 2);
fprintf(stderr, "hollowing out center of bulb\n");
fprintf(stderr, "radius = %d\n", radius);
for (z = 0; z < SIZE; z++) {
for (y = 0; y < SIZE; y++) {
for (x = 0; x < SIZE; x++) {
buffer[z][y][x] = 0;
}
}
}
for (z = 0; z < SIZE; z++) {
fprintf(stderr, "z = %d\n", z);
for (y = 0; y < SIZE; y++) {
for (x = 0; x < SIZE; x++) {
if (isBoundaryPoint(z, y, x)) {
for (zn = max(0, z - radius); zn < min(SIZE, z + radius); zn++) {
for (yn = max(0, y - radius); yn < min(SIZE, y + radius); yn++) {
for (xn = max(0, x - radius); xn < min(SIZE, x + radius); xn++) {
buffer[zn][yn][xn] = 1;
}
}
}
}
}
}
}
for (z = 0; z < SIZE; z++) {
for (y = 0; y < SIZE; y++) {
for (x = 0; x < SIZE; x++) {
if (voxels[z][y][x]) {
voxels[z][y][x] = buffer[z][y][x];
}
}
}
}
}