Sample Consensus

Posted 2020-11-22 larry-xia

The following models are supported:

• SACMODEL_PLANE - used to determine plane models. The four coefficients of the plane are its Hessian Normal form: [normal_x normal_y normal_z d]
• SACMODEL_LINE - used to determine line models. The six coefficients of the line are given by a point on the line and the direction of the line as: [point_on_line.x point_on_line.y point_on_line.z line_direction.x line_direction.y line_direction.z]
• SACMODEL_CIRCLE2D - used to determine 2D circles in a plane. The circle‘s three coefficients are given by its center and radius as: [center.x center.y radius]
• SACMODEL_CIRCLE3D - used to determine 3D circles in a plane. The circle‘s seven coefficients are given by its center, radius and normal as: [center.x, center.y, center.z, radius, normal.x, normal.y, normal.z]
• SACMODEL_SPHERE - used to determine sphere models. The four coefficients of the sphere are given by its 3D center and radius as: [center.x center.y center.z radius]
• SACMODEL_CYLINDER - used to determine cylinder models. The seven coefficients of the cylinder are given by a point on its axis, the axis direction, and a radius, as: [point_on_axis.x point_on_axis.y point_on_axis.z axis_direction.x axis_direction.y axis_direction.z radius]
• SACMODEL_CONE - used to determine cone models. The seven coefficients of the cone are given by a point of its apex, the axis direction and the opening angle, as: [apex.x, apex.y, apex.z, axis_direction.x, axis_direction.y, axis_direction.z, opening_angle]
• SACMODEL_TORUS - not implemented yet
• SACMODEL_PARALLEL_LINE - a model for determining a line parallel with a given axis, within a maximum specified angular deviation. The line coefficients are similar to SACMODEL_LINE .
• SACMODEL_PERPENDICULAR_PLANE - a model for determining a plane perpendicular to an user-specified axis, within a maximum specified angular deviation. The plane coefficients are similar to SACMODEL_PLANE .
• SACMODEL_PARALLEL_LINES - not implemented yet
• SACMODEL_NORMAL_PLANE - a model for determining plane models using an additional constraint: the surface normals at each inlier point has to be parallel to the surface normal of the output plane, within a maximum specified angular deviation. The plane coefficients are similar to SACMODEL_PLANE .
• SACMODEL_PARALLEL_PLANE - a model for determining a plane parallel to an user-specified axis, within a maximum specified angular deviation. SACMODEL_PLANE .
• SACMODEL_NORMAL_PARALLEL_PLANE defines a model for 3D plane segmentation using additional surface normal constraints. The plane must lie parallel to a user-specified axis. SACMODEL_NORMAL_PARALLEL_PLANE therefore is equivalent to SACMODEL_NORMAL_PLANE + SACMODEL_PARALLEL_PLANE. The plane coefficients are similar to SACMODEL_PLANE .

The following list describes the robust sample consensus estimators implemented:

• SAC_RANSAC - RANdom SAmple Consensus
• SAC_LMEDS - Least Median of Squares
• SAC_MSAC - M-Estimator SAmple Consensus
• SAC_RRANSAC - Randomized RANSAC
• SAC_RMSAC - Randomized MSAC
• SAC_MLESAC - Maximum LikeLihood Estimation SAmple Consensus
• SAC_PROSAC - PROgressive SAmple Consensus

#include <iostream>
#include <thread>

#include <pcl/console/parse.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/sample_consensus/sac_model_sphere.h>
#include <pcl/visualization/pcl_visualizer.h>

using namespace std::chrono_literals;

int
main(int argc, char** argv)
{
    // initialize PointClouds
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
    pcl::io::loadPCDFile("D:\pcd\sphere.pcd", *cloud);
    pcl::SampleConsensusModelSphere<pcl::PointXYZ>::Ptr model_s(new pcl::SampleConsensusModelSphere<pcl::PointXYZ>(cloud));
    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_s);
    ransac.setDistanceThreshold(2);
    ransac.computeModel();
    std::vector<int> inliers;
    ransac.getInliers(inliers);
    Eigen::VectorXf coff;
    ransac.getModelCoefficients(coff);
    //cout << coff << endl;
    
    // copies all inliers of the model computed to another PointCloud
    pcl::copyPointCloud(*cloud, inliers, *final);

    // creates the visualization object and adds either our original cloud or all of the inliers
    // depending on the command line arguments specified.
    
    pcl::visualization::PCLVisualizer::Ptr viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
    viewer->setBackgroundColor(0, 0, 0);
    viewer->addPointCloud<pcl::PointXYZ>(final, "sample cloud");
    pcl::PointXYZ center(coff(0), coff(1), coff(2));
    double R = coff(3);
    cout << "center " << center.x << " " << center.y << " " << center.z << endl;
    cout << "R" << R << endl;
    //viewer->addSphere(center, coff(2),"mySphere");
    viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 2, "sample cloud");
    viewer->addCoordinateSystem (1.0, "global");
    viewer->initCameraParameters();
    

    while (!viewer->wasStopped())
    {
        viewer->spinOnce(100);
        std::this_thread::sleep_for(100ms);
    }
    return 0;
}

View Code