Using Ponca to compute surface curvature in PCL

Introduction

This is an example that use Ponca to compute surface curvature with Point Cloud Library (PCL) (recommended version: 1.9) It provides a class implementation to be incorporate in the PCL tree, section "features", and a method to use that feature and visualize data using a colormap.

Usage

To compile this example, you need to enable the compilation of the examples with cmake, and build the examples:

cmake [..] -DPONCA_CONFIGURE_EXAMPLES=ON
make ponca-examples // or make ponca_pclwrapper

Note

Tip if you are using visual studio: if you have a problem with boost or pcl exceptions, go to Project -> Properties -> C/C++ -> Code Generation -> Enable C++ exceptions and turn it to "Yes (/EHsc)"

Then execute the program and you will visualize two color point clouds: on the left, the signed curvature estimated using Ponca, and on the right, the unsigned curvature estimated using PCL.

Surface Curvature on a point cloud using GLS method

PCL wrapper

Here is the PCL implementation of our method.

Datastructures declaration

• GlsPoint defines how point samples are represented,
• pcl::GlsCurvature defines the wrapper that brings Ponca algorithms into the PCL API

Source file: pcl_wrapper.h

/*
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
 
#pragma once
 
#include <pcl/features/feature.h>
#include <Ponca/src/Common/pointTypes.h>
 
using GlsPoint = Ponca::PointPositionNormal<float, 3>;
 
namespace pcl
{
    template <typename PointInT, typename PointOutT>
    class GlsCurvature : public Feature<PointInT, PointOutT>
    {
        using Feature<PointInT, PointOutT>::feature_name_;
        using Feature<PointInT, PointOutT>::getClassName;
        using Feature<PointInT, PointOutT>::indices_;
        using Feature<PointInT, PointOutT>::input_;
        using Feature<PointInT, PointOutT>::surface_;
        using Feature<PointInT, PointOutT>::k_;
        using Feature<PointInT, PointOutT>::search_radius_;
        using Feature<PointInT, PointOutT>::search_parameter_;
 
        using PointCloudOut      = typename Feature<PointInT, PointOutT>::PointCloudOut;
        using PointCloudConstPtr = typename Feature<PointInT, PointOutT>::PointCloudConstPtr;
 
    public:
        GlsCurvature() { feature_name_ = "GlsCurvature"; }
 
        void computeCurvature(const PointCloud<PointInT>& cloud, int p_idx, const std::vector<int>& indices,
                              float& curvature);
 
    protected:
        void computeFeature(PointCloudOut& output) override;
 
    private:
        static void computeFeatureEigen(PointCloud<Eigen::MatrixXf>& output) {}
    };
} // namespace pcl

Datastructures implementation

Source file: pcl_wrapper.hpp

/*
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
 
#pragma once
 
#include "pcl_wrapper.h"
#include <Ponca/Fitting>
#include <pcl/common/point_tests.h> // isFinite
 
template <typename PointInT, typename PointOutT>
void pcl::GlsCurvature<PointInT, 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_);
 
    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].curvature = std::numeric_limits<float>::quiet_NaN();
                output.is_dense              = false;
                continue;
            }
 
            computeCurvature(*surface_, (*indices_)[idx], nn_indices, output.points[idx].curvature);
        }
    }
    else
    {
        // Iterating over the entire index vector
        for (size_t idx = 0; idx < indices_->size(); ++idx)
        {
            if (!pcl::isFinite((*input_)[(*indices_)[idx]]) ||
                this->searchForNeighbors((*indices_)[idx], search_parameter_, nn_indices, nn_dists) == 0)
            {
                output.points[idx].curvature = std::numeric_limits<float>::quiet_NaN();
                output.is_dense              = false;
                continue;
            }
 
            computeCurvature(*surface_, (*indices_)[idx], nn_indices, output.points[idx].curvature);
        }
    }
}
 
template <typename PointInT, typename PointOutT>
void pcl::GlsCurvature<PointInT, PointOutT>::computeCurvature(const pcl::PointCloud<PointInT>& cloud, int p_idx,
                                                              const std::vector<int>& indices, float& curvature)
{
    using Scalar     = GlsPoint::Scalar;
    using WeightFunc = Ponca::DistWeightFunc<GlsPoint, Ponca::SmoothWeightKernel<Scalar>>;
    using FitBasket  = Ponca::Basket<GlsPoint, WeightFunc, Ponca::CovariancePlaneFit>;
    using Fit =
        Ponca::BasketDiff<FitBasket, Ponca::FitScaleSpaceDer, Ponca::CovariancePlaneDer, Ponca::CurvatureEstimatorDer,
                          Ponca::NormalDerivativeWeingartenEstimator, Ponca::WeingartenCurvatureEstimatorDer>;
 
    Fit fit;
    // Set a weighting function instance using the search radius of the tree as scale
    fit.setNeighborFilter({cloud.points[p_idx].getVector3fMap(), float(search_radius_)});
 
    fit.init();
 
    // Iterate over indices and fit the primitive
    // A GlsPoint instance is generated on the fly to bind the positions to the
    // library representation. No copy is done at this step.
    for (int id : indices)
    {
        fit.addNeighbor(GlsPoint(cloud.points[id].getVector3fMap(), cloud.points[id].getNormalVector3fMap()));
    }
 
    // Finalize fitting
    fit.finalize();
 
    // Test if the fitting ended without errors. Set curvature to qNan otherwise.
    if (fit.isStable())
    {
        curvature = fit.kMean();
    }
    else
    {
        curvature = std::numeric_limits<float>::quiet_NaN();
    }
}
 
#define PCL_INSTANTIATE_GlsCurvature(T, OutT) template class PCL_EXPORTS pcl::GlsCurvature<T, OutT>;

Datastructures instanciation

Source file: pcl_wrapper.cpp

/*
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
 
#include <pcl/point_types.h>
#include <pcl/impl/instantiate.hpp>
 
#include "pcl_wrapper.h"
#include "pcl_wrapper.hpp"
 
// Instantiations of specific point types
PCL_INSTANTIATE_PRODUCT(GlsCurvature, ((pcl::PointNormal)(pcl::PointXYZRGBNormal)(pcl::PointXYZINormal))(
                                          (pcl::PointNormal)(pcl::PointXYZRGBNormal)(pcl::PointXYZINormal)));

Main program

Now, a basic method to use the previous class and to visualize the result.

/*
 This Source Code Form is subject to the terms of the Mozilla Public
 License, v. 2.0. If a copy of the MPL was not distributed with this
 file, You can obtain one at http://mozilla.org/MPL/2.0/.
*/
 
#include <boost/thread/thread.hpp>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/features/principal_curvatures.h>
#include <pcl/features/normal_3d.h>
#include <pcl/visualization/pcl_plotter.h>
#include <pcl/common/time.h>
 
#include "pcl_wrapper.h"
 
struct Color
{
    double r, g, b;
};
 
// Colormap function
Color getColor(float value, float min_value, float max_value)
{
    Color c = {1.0, 1.0, 1.0};
    double dv;
    // Unknown values in our kernel
    if (value == 0.)
    {
        return c;
    }
    // Threshold
    if (value < min_value)
    {
        value = min_value;
    }
    if (value > max_value)
    {
        value = max_value;
    }
    // Interval
    dv = max_value - min_value;
    // Seismic color map like
    if (value < (min_value + 0.5 * dv))
    {
        c.r = 2 * (value - min_value) / dv;
        c.g = 2 * (value - min_value) / dv;
        c.b = 1;
    }
    else
    {
        c.b = 2 - 2 * (value - min_value) / dv;
        c.g = 2 - 2 * (value - min_value) / dv;
        c.r = 1;
    }
    return c;
}
 
int main()
{
    pcl::PointCloud<pcl::PointNormal>::Ptr cloud(new pcl::PointCloud<pcl::PointNormal>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_without_normals(new pcl::PointCloud<pcl::PointXYZ>);
 
    // load an object into a point cloud
    if (pcl::io::loadPLYFile<pcl::PointXYZ>("bun_zipper.ply", *cloud_without_normals) == -1)
    {
        PCL_ERROR("bun_zipper.ply \n");
        return (-1);
    }
 
    float radius = 0.005;
 
    // calculate surface normals with a search radius of 0.01
    pcl::NormalEstimation<pcl::PointXYZ, pcl::Normal> ne;
    ne.setInputCloud(cloud_without_normals);
    pcl::search::KdTree<pcl::PointXYZ>::Ptr tree(new pcl::search::KdTree<pcl::PointXYZ>());
    ne.setSearchMethod(tree);
    pcl::PointCloud<pcl::Normal>::Ptr normals(new pcl::PointCloud<pcl::Normal>);
    ne.setRadiusSearch(radius);
    ne.compute(*normals);
 
    // concatenate normals with positions into another pointcloud
    pcl::concatenateFields(*cloud_without_normals, *normals, *cloud);
 
    // compute the gls curvature feature
    pcl::GlsCurvature<pcl::PointNormal, pcl::PointNormal> ne2;
    ne2.setInputCloud(cloud);
    pcl::search::KdTree<pcl::PointNormal>::Ptr tree2(new pcl::search::KdTree<pcl::PointNormal>());
    ne2.setSearchMethod(tree2);
    pcl::PointCloud<pcl::PointNormal>::Ptr output_cloud(new pcl::PointCloud<pcl::PointNormal>);
    ne2.setRadiusSearch(radius);
    {
        pcl::ScopeTime t1("Curvature computation using Ponca");
        ne2.compute(*output_cloud);
    }
 
    // compare timings with PCL curvature estimation
    pcl::PrincipalCurvaturesEstimation<pcl::PointXYZ, pcl::Normal> pce;
    pce.setInputCloud(cloud_without_normals);
    pce.setInputNormals(normals);
    pce.setSearchMethod(tree);
    pce.setRadiusSearch(radius);
    pcl::PointCloud<pcl::PrincipalCurvatures>::Ptr principalCurvatures(new pcl::PointCloud<pcl::PrincipalCurvatures>());
    {
        pcl::ScopeTime t1("Curvature computation using PCL");
        pce.compute(*principalCurvatures);
    }
 
    // Use another point cloud for the visualizer (use a colormap to represent the curvature)
    pcl::PointCloud<pcl::PointXYZRGB>::Ptr cloud_rgb1(new pcl::PointCloud<pcl::PointXYZRGB>), // ponca version
        cloud_rgb2(new pcl::PointCloud<pcl::PointXYZRGB>);                                    // pcl version
    std::vector<double> curvBuffer1(cloud->size()),                                           // ponca version
        curvBuffer2(cloud->size());                                                           // pcl version
    for (size_t i = 0; i < cloud->size(); ++i)
    {
        // \FIXME There is problem with the magnitude of the derivative computed with Ponca.
        float curvature1 = 1.f / 6000.f * output_cloud->points[i].curvature;
        curvBuffer1[i]   = curvature1;
        float curvature2 = 1.f / 0.5f * (principalCurvatures->points[i].pc1 + principalCurvatures->points[i].pc2) / 2.f;
        curvBuffer2[i]   = curvature2;
 
        Color c1 = getColor(curvature1, -0.5f, 0.5f);
        Color c2 = getColor(-curvature2, -0.5f, 0.5f);
 
        pcl::PointXYZRGB point;
 
        point.x = cloud->points[i].x;
        point.y = cloud->points[i].y;
        point.z = cloud->points[i].z;
 
        // convert into int
        point.r = c1.r * 255;
        point.g = c1.g * 255;
        point.b = c1.b * 255;
        cloud_rgb1->points.push_back(point);
 
        point.r = c2.r * 255;
        point.g = c2.g * 255;
        point.b = c2.b * 255;
        cloud_rgb2->points.push_back(point);
    }
    //    // visualize curvature plots
    //    pcl::visualization::PCLPlotter * plotter = new pcl::visualization::PCLPlotter ();
    //    plotter->addHistogramData (curvBuffer2, 100, "PCL");
    //    plotter->addHistogramData (curvBuffer1, 100, "Ponca");
    //    plotter->setShowLegend (true); //show legends
 
    // visualize curvatures
    boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(
        new pcl::visualization::PCLVisualizer("Ponca - PCL Demo"));
    viewer->setBackgroundColor(0.5, 0.5, 0.5);
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb1(cloud_rgb1);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud_rgb1, rgb1, "Ponca estimation");
    pcl::visualization::PointCloudColorHandlerRGBField<pcl::PointXYZRGB> rgb2(cloud_rgb2);
    viewer->addPointCloud<pcl::PointXYZRGB>(cloud_rgb2, rgb2, "PCL estimation");
 
    // make cloud visible directly
    Eigen::Affine3f tr{Eigen::Affine3f::Identity()};
 
    tr.translate(Eigen::Vector3f{0.1, -0.1, 0.4});
    viewer->updatePointCloudPose("Ponca estimation", tr);
 
    tr.translate(Eigen::Vector3f{-0.2, 0.0, 0.0});
    viewer->updatePointCloudPose("PCL estimation", tr);
 
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "Ponca estimation");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 5, "PCL estimation");
    viewer->initCameraParameters();
 
    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        //        plotter->spinOnce (100);
        boost::this_thread::sleep(boost::posix_time::microseconds(100000));
    }
}