Skip to content

Commit

Permalink
Merge pull request #1239 from v4hn/point-normal-might-fail
Browse files Browse the repository at this point in the history 
set normal-cloud to non-dense when normal computation fails
  • Loading branch information
@taketwo
taketwo committed May 21, 2015
2 parents 085e13a + 032f321 commit b60e20d
Show file tree
Hide file tree
Showing 2 changed files with 16 additions and 16 deletions.
12 changes: 4 additions & 8 deletions features/include/pcl/features/impl/normal_3d.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,17 +59,15 @@ pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &outpu
// 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)
if (this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
!computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], 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;
}

computePointNormal (*surface_, nn_indices,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);

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

Expand All @@ -81,17 +79,15 @@ pcl::NormalEstimation<PointInT, PointOutT>::computeFeature (PointCloudOut &outpu
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)
this->searchForNeighbors ((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0 ||
!computePointNormal (*surface_, nn_indices, output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], 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;
}

computePointNormal (*surface_, nn_indices,
output.points[idx].normal[0], output.points[idx].normal[1], output.points[idx].normal[2], output.points[idx].curvature);

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

Expand Down
20 changes: 12 additions & 8 deletions features/include/pcl/features/normal_3d.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -56,7 +56,7 @@ namespace pcl
* \f]
* \ingroup features
*/
template <typename PointT> inline void
template <typename PointT> inline bool
computePointNormal (const pcl::PointCloud<PointT> &cloud,
Eigen::Vector4f &plane_parameters, float &curvature)
{
Expand All @@ -70,11 +70,12 @@ namespace pcl
{
plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
curvature = std::numeric_limits<float>::quiet_NaN ();
return;
return false;
}

// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
return true;
}

/** \brief Compute the Least-Squares plane fit for a given set of points, using their indices,
Expand All @@ -88,7 +89,7 @@ namespace pcl
* \f]
* \ingroup features
*/
template <typename PointT> inline void
template <typename PointT> inline bool
computePointNormal (const pcl::PointCloud<PointT> &cloud, const std::vector<int> &indices,
Eigen::Vector4f &plane_parameters, float &curvature)
{
Expand All @@ -101,10 +102,11 @@ namespace pcl
{
plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
curvature = std::numeric_limits<float>::quiet_NaN ();
return;
return false;
}
// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix, xyz_centroid, plane_parameters, curvature);
return true;
}

/** \brief Flip (in place) the estimated normal of a point towards a given viewpoint
Expand Down Expand Up @@ -236,7 +238,7 @@ namespace pcl
* \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
* \f]
*/
inline void
inline bool
computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
Eigen::Vector4f &plane_parameters, float &curvature)
{
Expand All @@ -245,11 +247,12 @@ namespace pcl
{
plane_parameters.setConstant (std::numeric_limits<float>::quiet_NaN ());
curvature = std::numeric_limits<float>::quiet_NaN ();
return;
return false;
}

// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix_, xyz_centroid_, plane_parameters, curvature);
return true;
}

/** \brief Compute the Least-Squares plane fit for a given set of points, using their indices,
Expand All @@ -264,19 +267,20 @@ namespace pcl
* \lambda_0 / (\lambda_0 + \lambda_1 + \lambda_2)
* \f]
*/
inline void
inline bool
computePointNormal (const pcl::PointCloud<PointInT> &cloud, const std::vector<int> &indices,
float &nx, float &ny, float &nz, float &curvature)
{
if (indices.size () < 3 ||
computeMeanAndCovarianceMatrix (cloud, indices, covariance_matrix_, xyz_centroid_) == 0)
{
nx = ny = nz = curvature = std::numeric_limits<float>::quiet_NaN ();
return;
return false;
}

// Get the plane normal and surface curvature
solvePlaneParameters (covariance_matrix_, nx, ny, nz, curvature);
return true;
}

/** \brief Provide a pointer to the input dataset
Expand Down

0 comments on commit b60e20d

Please sign in to comment.