Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Update the OpenMP implementations of normal and FPFH estimation #2278

Merged
merged 2 commits into from
Apr 18, 2018
Merged

Update the OpenMP implementations of normal and FPFH estimation #2278

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
569e05e
Set normal-cloud to non-dense when normal computation fails
ThorstenHarter Apr 12, 2018
5ca7c1f
Handle non-dense clouds in FPFHEstimationOMP
ThorstenHarter Apr 12, 2018
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 4 additions & 2 deletions features/include/pcl/features/impl/fpfh_omp.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -62,7 +62,8 @@ pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
for (size_t idx = 0; idx < indices_->size (); ++idx)
{
int p_idx = (*indices_)[idx];
if (this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
if (!isFinite ((*input_)[p_idx]) ||
this->searchForNeighbors (p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;

spfh_indices_set.insert (nn_indices.begin (), nn_indices.end ());
Expand Down Expand Up @@ -98,7 +99,8 @@ pcl::FPFHEstimationOMP<PointInT, PointNT, PointOutT>::computeFeature (PointCloud
int p_idx = spfh_indices_vec[i];

// Find the neighborhood around p_idx
if (this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
if (!isFinite ((*input_)[p_idx]) ||
this->searchForNeighbors (*surface_, p_idx, search_parameter_, nn_indices, nn_dists) == 0)
continue;

// Estimate the SPFH signature around p_idx
Expand Down
31 changes: 12 additions & 19 deletions features/include/pcl/features/impl/normal_3d_omp.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,6 @@ pcl::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &ou
std::vector<float> nn_dists (k_);

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)
{
Expand All @@ -63,59 +62,53 @@ pcl::NormalEstimationOMP<PointInT, PointOutT>::computeFeature (PointCloudOut &ou
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
Eigen::Vector4f n;
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
!computePointNormal (*surface_, nn_indices, n, output.points[idx].curvature))
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();

output.is_dense = false;
continue;
}

Eigen::Vector4f n;
pcl::computePointNormal<PointInT> (*surface_, nn_indices,
n,
output.points[idx].curvature);

output.points[idx].normal_x = n[0];
output.points[idx].normal_y = n[1];
output.points[idx].normal_z = n[2];

flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
output.points[idx].normal[0],
output.points[idx].normal[1],
output.points[idx].normal[2]);
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);

}
}
else
{
#ifdef _OPENMP
#pragma omp parallel for shared (output) private (nn_indices, nn_dists) num_threads(threads_)
#endif
// Iterating over the entire index vector
// Iterating over the entire index vector
for (int idx = 0; idx < static_cast<int> (indices_->size ()); ++idx)
{
Eigen::Vector4f n;
if (!isFinite ((*input_)[(*indices_)[idx]]) ||
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
!computePointNormal (*surface_, nn_indices, n, output.points[idx].curvature))
{
output.points[idx].normal[0] = output.points[idx].normal[1] = output.points[idx].normal[2] = output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN ();

output.is_dense = false;
continue;
}

Eigen::Vector4f n;
pcl::computePointNormal<PointInT> (*surface_, nn_indices,
n,
output.points[idx].curvature);

output.points[idx].normal_x = n[0];
output.points[idx].normal_y = n[1];
output.points[idx].normal_z = n[2];

flipNormalTowardsViewpoint (input_->points[(*indices_)[idx]], vpx_, vpy_, vpz_,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2]);

}
}
}
}

#define PCL_INSTANTIATE_NormalEstimationOMP(T,NT) template class PCL_EXPORTS pcl::NormalEstimationOMP<T,NT>;
Expand Down