Point cloud extraction depending on normal direction

Question

I would like to extract only the points with normals pointing upward or close to pointing upward using the PCL (Point Cloud Library). There is a sample code for estimating surface normals, but no how to extract it. Has someone already done that?

#include <pcl/point_types.h>
#include <pcl/features/normal_3d.h>

{
  pcl::PointCloud<pcl::PointXYZ>::Ptr cloud (new pcl::PointCloud<pcl::PointXYZ>);

  // read, pass in or create a point cloud ...

  // Create the normal estimation class, and pass the input dataset to it
  pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
  ne.setInputCloud (cloud);

  // Create an empty kdtree representation, and pass it to the normal estimation object.
  // Its content will be filled inside the object, based on the given input dataset (as no other search surface is given).
  pcl::search::KdTree<pcl::PointXYZ>::Ptr tree (new pcl::search::KdTree<pcl::PointXYZ> ());
  ne.setSearchMethod (tree);

  // Output datasets
  pcl::PointCloud<pcl::Normal>::Ptr cloud_normals (new pcl::PointCloud<pcl::Normal>);

  // Use all neighbors in a sphere of radius 3cm
  ne.setRadiusSearch (0.03);

  // Compute the features
  ne.compute (*cloud_normals);

  // cloud_normals->size () should have the same size as the input cloud->size ()*
}

Mark Loyman Mark Loyman · Accepted Answer · 2021-03-25T19:31:38

There's no specialized class that does it, but it's rather straightforward to implement. You go over all normal points, and if the normal is "close enough" to the direction you want (which is determined with a dot product - assuming both vectors are normalized), you save the index in pcl::PointIndices.

pcl::ExtractIndices can then be used to extract sub-clouds from both cloud and cloud_normals.

pcl::PointIndicesPtr FilterByNormal(pcl::PointCloud<pcl::Normal>::ConstPtr cloud_normals, const Eigen::Vector3f requiredDirection, float maxAngleDeg)
{
    float thresh = std::cos(maxAngleDeg * M_PI / 180.0);
    
    pcl::PointIndicesPtr indices = pcl::PointIndicesPtr(new pcl::PointIndices);
    for (auto i = 0; i < cloud_normals->size(); ++i)
    {
        const auto& normal = cloud_normals->points[i].getNormalVector3fMap();
        if (normal.dot(requiredDirection) >= thresh)
        {
            indices->indices.push_back(i);
        }
    }

    return indices;
}

Alternatively, you could construct your desired output cloud in the loop without an intermediate pcl::PointIndices.

Point cloud extraction depending on normal direction

1 Answers