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/Fitting>
 
#include "Eigen/Eigen"
 
class GlsPoint
{
public:
    enum {Dim = 3};
    typedef float Scalar;
    typedef Eigen::Matrix<Scalar, 3, 1> VectorType;
    typedef Eigen::Matrix<Scalar, 3, 3> MatrixType;
 
    PONCA_MULTIARCH inline GlsPoint(Eigen::Map< const VectorType > pos, Eigen::Map< const VectorType > normal)
        : pos_   (pos),
          normal_(normal)
    {}
 
    PONCA_MULTIARCH inline const Eigen::Map< const VectorType >& pos()    const { return pos_; }
    PONCA_MULTIARCH inline const Eigen::Map< const VectorType >& normal() const { return normal_; }
 
private:
    Eigen::Map< const VectorType > pos_, normal_;
};
 
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_;
 
        typedef typename Feature<PointInT, PointOutT>::PointCloudOut PointCloudOut;
        typedef typename Feature<PointInT, PointOutT>::PointCloudConstPtr PointCloudConstPtr;
 
    public:
        GlsCurvature()
        {
            feature_name_ = "GlsCurvature";
        }
 
        void computeCurvature(const pcl::PointCloud<PointInT> &cloud, int p_idx, const std::vector<int> &indices, float &curvature);
 
    protected:
        void
        computeFeature (PointCloudOut &output);
 
    private:
        void
        computeFeatureEigen (pcl::PointCloud<Eigen::MatrixXf> &) {}
    };
}

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 <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)
{
    typedef GlsPoint::Scalar Scalar;
    typedef Ponca::DistWeightFunc<GlsPoint, Ponca::SmoothWeightKernel<Scalar> > WeightFunc;
    //typedef Ponca::Basket<GlsPoint, WeightFunc, Ponca::MeanPlaneFit, Ponca::NormalCovarianceCurvature> Fit;
    typedef Ponca::Basket<GlsPoint, WeightFunc, Ponca::CovariancePlaneFit, Ponca::CovariancePlaneSpaceDer, Ponca::NormalDerivativesCurvatureEstimator> Fit;
 
    Fit fit;
    // Set a weighting function instance using the search radius of the tree as scale
    fit.setWeightFunc(WeightFunc(search_radius_));
 
    // Set the evaluation position
    fit.init(cloud.points[p_idx].getVector3fMap());
 
    // 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(size_t idx = 0; idx < indices.size (); ++idx)
    {
        int id = indices[idx];
 
        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"
 
 
typedef struct
{
    double r,g,b;
}Color;
 
// 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));
    }
}