PerpendicularPlaneFitting.cpp

Go to the documentation of this file.

#include "PerpendicularPlaneFitting.h"
 
#include <pcl/segmentation/sac_segmentation.h>
 
// #include <armar6/skills/components/GuardPoseEstimation/tools.h>
#include "tools.h"
 
namespace visionx::tools
{
 
    std::optional<PlaneFittingResult>
    PerpendicularPlaneFitting::fit(pcl::PointCloud<PointT>::ConstPtr input)
    {
        pcl::SACSegmentation<PointT> seg;
        seg.setOptimizeCoefficients(true);
        seg.setModelType(pcl::SACMODEL_PERPENDICULAR_PLANE);
        seg.setMethodType(pcl::SAC_RANSAC);
 
        seg.setDistanceThreshold(distanceThreshold);
        seg.setAxis(normal);
        seg.setEpsAngle(epsAngle);
 
        seg.setMaxIterations(maxIterations);
        seg.setProbability(probability);
 
 
        pcl::PointIndices::Ptr inliers(new pcl::PointIndices);
        pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients);
        // Segment the largest planar component from the remaining cloud
        seg.setInputCloud(input);
        seg.segment(*inliers, *coefficients);
 
        if (inliers->indices.empty())
        {
            return std::nullopt;
        }
 
        /* "The four coefficients of the plane are its Hessian Normal form:
         * [normal_x normal_y normal_z d]"
         *
         * - http://docs.pointclouds.org/trunk/group__sample__consensus.html
         */
        Eigen::Vector3f normalEst(
            coefficients->values[0], coefficients->values[1], coefficients->values[2]);
        Eigen::Hyperplane3f plane(normalEst, coefficients->values[3]);
 
        PlaneFittingResult result;
        result.plane = plane;
        result.inliers = inliers;
 
        return result;
    }
 
} // namespace visionx::tools