plane_fitting_ransac.hpp

Go to the documentation of this file.

#pragma once
 
#include <pcl/ModelCoefficients.h>
#include <pcl/sample_consensus/method_types.h>
#include <pcl/sample_consensus/model_types.h>
#include <pcl/segmentation/sac_segmentation.h>
 
#include "plane_fitting_ransac.h"
 
 
// This header exposes more includes, but also the function template.
 
namespace visionx::tools
{
 
    template <typename PointT, typename IndicesPtrT = pcl::IndicesPtr>
    std::optional<PlaneFittingResult> fitPlaneRansac(
        typename pcl::PointCloud<PointT>::Ptr cloud,
        double distanceThreshold = 1.0,
        const IndicesPtrT& indices = nullptr)
    {
        // Reference: http://pointclouds.org/documentation/tutorials/planar_segmentation.php
 
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
 
        // Create the segmentation object
        pcl::SACSegmentation<PointT> seg;
 
        // Optional
        seg.setOptimizeCoefficients(true);
 
        // Mandatory
        seg.setModelType(pcl::SACMODEL_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
        seg.setDistanceThreshold(distanceThreshold);
 
        seg.setInputCloud(cloud);
        if (indices)
        {
            seg.setIndices(indices);
        }
        seg.segment(*inliers, *coefficients);
 
        if (inliers->indices.empty())
        {
            return std::nullopt;
        }
 
        float a = coefficients->values.at(0);
        float b = coefficients->values.at(1);
        float c = coefficients->values.at(2);
        float d = coefficients->values.at(3);
        Eigen::Hyperplane3f plane(Eigen::Vector3f(a, b, c), d);
 
        // Assure plane normal points updwards.
        if (plane.normal().dot(Eigen::Vector3f::UnitZ()) < 0)
        {
            plane = Eigen::Hyperplane3f(- plane.normal(), - plane.offset());
        }
 
        return PlaneFittingResult { inliers, plane };
    }
 
}