自适应阈值的ransac平面拟合

原来一直用pcl中的ransac平面拟合，现在突然想自己写一个不需要设置阈值的ransac平面拟合，下面把我的代码贴出来，欢迎大神们指正。

#pragma once#include "new3s_PointXYZ.h"#include <vector>class RansacPlaneFit{public:    RansacPlaneFit();    ~RansacPlaneFit();    void setInputCloud(const std::vector<new3s_PointXYZ> &clouds);    // 设置距离阈值    void setThreshValue(const double threshvalue);    // 设置最大迭代次数    void setIteration(const int iters);    // 计算平面系数    Eigen::Matrix<double, 4, 1> computePlaneCoef();    bool getSample(std::vector<int> &samples);    // 三点不共线为条件判断选择点是否可行    bool isGoodSample(const std::vector<int> &indices);    // 计算点到直线的距离    double computeDist(const int index, const Eigen::Matrix4Xd &coefs);    // 计算平面的系数    bool computeModelCoefficients(const std::vector<int> &samples, Eigen::Matrix<double , 4 , 1> &model_coefficient);    // 统计在阈值范围内到平面的点数，并记录索引    int countNum(const Eigen::Matrix4Xd &model_coef , std::vector<int> &indices);    // 计算点云中心    void compute3DCentroid(const std::vector<int> &indices ,  Eigen::Matrix<double, 4, 1> &centroid);    // 计算协方差矩阵    void computeCovarianceMatrix(const std::vector<int> &indices, const Eigen::Matrix<double, 4, 1> &centroid, Eigen::Matrix<double, 3, 3> &covariance_matrix);    // 设置inlier和outlier的比率    void setRatio(const double ratio);    // 获得迭代次数    int getIteration() const;    // 获得距离阈值    double getThreshValue() const;private:    bool isSetThreshValue() const;private:    // 点数    int m_num;    std::vector<new3s_PointXYZ> m_cloud;    double m_threshValue;    // 设置迭代次数    int m_iters;    // 记录程序迭代的次数    int m_iteration;    // 保存采样索引    std::vector<int> m_samples;    // 全局索引    std::vector<int> m_globalindexs;    // inlier和outlier比率    double m_ratio;    // 是否设置阈值标记    bool m_threshFlag;};

实现文件

#include "RansacPlaneFit.h"#include <Eigen/eigen3/Eigen/Eigen>#include <math.h>#include <iostream>RansacPlaneFit::RansacPlaneFit()    : m_threshValue(0.0)    , m_iters(1000)    , m_num(0)    , m_ratio(2.0)    , m_threshFlag(false)    , m_iteration(0){}RansacPlaneFit::~RansacPlaneFit(){    if (!m_cloud.empty())    {        m_cloud.clear();        std::vector<new3s_PointXYZ>().swap(m_cloud);    }}void RansacPlaneFit::setInputCloud(const std::vector<new3s_PointXYZ>& clouds){    if (!clouds.empty())    {        m_cloud = clouds;        m_num= m_cloud.size();        for (int i = 0 ; i < m_num; ++i)        {            m_globalindexs.push_back(i);        }    }}void RansacPlaneFit::setThreshValue(const double threshvalue){    m_threshValue = threshvalue;    m_threshFlag = true;}void RansacPlaneFit::setIteration(const int iters){    m_iters = iters;}bool RansacPlaneFit::getSample(std::vector<int>& samples){    samples.resize(3);    srand(unsigned(time(0)));    int id1 = rand() % m_globalindexs.size();    int id2 = rand() % m_globalindexs.size();    int id3 = rand() % m_globalindexs.size();    samples[0] = id1;    samples[1] = id2;    samples[2] = id3;    if (isGoodSample(samples))    {        return true;    }    return false;}Eigen::Matrix<double, 4, 1> RansacPlaneFit::computePlaneCoef(){    Eigen::Matrix<double , 4 , 1> coef ;    coef.setZero();    // 记录迭代次数    int iter = 0;    // 记录内点数    int max_best_num = 0;    std::vector<int> samples;    // 统计每次迭代到拟合平面点的数量    std::vector<int> best_nums;    std::vector<std::vector<int> > inlier_indices;    inlier_indices.resize(m_iters);    double ratio = 0;    while (iter < m_iters)    {        ++iter;        if (getSample(samples))        {            computeModelCoefficients(samples, coef);            ratio = (countNum(coef, inlier_indices[iter - 1]) / (double)(m_num - inlier_indices[iter - 1].size()));            std::cout << "inlier和outlier的比率：" << ratio << std::endl;            if (ratio > m_ratio)            {                break;            }        }        samples.clear();        std::vector<int>().swap(samples);    }    m_iteration = iter;//  std::sort(inlier_indices.begin(), inlier_indices.end());    best_nums.resize(inlier_indices[iter - 1].size());    std::copy(inlier_indices[iter - 1].begin(), inlier_indices[iter - 1].end(), best_nums.begin());    Eigen::Matrix<double, 4, 1> xyz_centroid;    xyz_centroid.setZero();    compute3DCentroid(best_nums , xyz_centroid);    Eigen::Matrix3d covariance_matrix;    covariance_matrix.setZero();    computeCovarianceMatrix(best_nums, xyz_centroid, covariance_matrix);    EIGEN_ALIGN16 Eigen::Vector3f::Scalar eigen_value;    EIGEN_ALIGN16 Eigen::Vector3f eigen_vector;    //计算协方差矩阵最小特征值的特征向量    Eigen::EigenSolver<Eigen::Matrix3d> CovarianceMatrix(covariance_matrix);    Eigen::Matrix3d D = CovarianceMatrix.pseudoEigenvalueMatrix();    Eigen::Matrix3d V = CovarianceMatrix.pseudoEigenvectors();    std::vector<double> eval;//保存特征值    for (int i = 0; i < 3; ++i)    {        eval.push_back(D(i, i));    }    int k_val = 0;//记录特征值最小的是哪个    double min_val = 100000000;    for (int i = 0; i < 3; ++i)    {        if (eval[i] < min_val)        {            min_val = eval[i];            k_val = i;        }    }    for (int i = 0; i < 3; ++i)    {        eigen_vector[i] = V(i, k_val);    }    coef[0] = eigen_vector[0];    coef[1] = eigen_vector[1];    coef[2] = eigen_vector[2];    coef[3] = 0;    coef[3] = -1 * coef.dot(xyz_centroid);    if (coef[3] != 0)    {        coef[0] /= coef[3];        coef[1] /= coef[3];        coef[2] /= coef[3];        coef[3] = 1.0;    }    return coef;}bool RansacPlaneFit::isGoodSample(const std::vector<int>& indices){    if (indices.size() < 3)    {        return false;    }    Eigen::Vector3d p0p1 = Eigen::Vector3d(m_cloud[indices[1]].get_x() - m_cloud[indices[0]].get_x(), m_cloud[indices[1]].get_y() - m_cloud[indices[0]].get_y(), m_cloud[indices[1]].get_z() - m_cloud[indices[0]].get_z());    Eigen::Vector3d p0p2 = Eigen::Vector3d(m_cloud[indices[2]].get_x() - m_cloud[indices[0]].get_x(), m_cloud[indices[2]].get_y() - m_cloud[indices[2]].get_y(), m_cloud[indices[1]].get_z() - m_cloud[indices[0]].get_z());    if (p0p1(0) * p0p2(1) == p0p2(0) * p0p1(1) && p0p1(1) * p0p2(2) == p0p2(1) * p0p1(2))    {        return false;    }    return true;}double RansacPlaneFit::computeDist(const int index, const Eigen::Matrix4Xd & coefs){    double a = coefs(0, 0);    double b = coefs(1, 0);    double c = coefs(2, 0);    double d = coefs(3, 0);    double x = m_cloud[index].get_x();    double y = m_cloud[index].get_y();    double z = m_cloud[index].get_z();    return fabs((x * a + y * b + z * c + d) / (a * a + b * b + c * c));}bool RansacPlaneFit::computeModelCoefficients(const std::vector<int>& samples, Eigen::Matrix<double, 4, 1>& model_coefficient){    if (samples.size() != 3)    {        std::cout << "采样点数量不足3！\n";        return (false);    }    new3s_PointXYZ p0 = m_cloud[samples[0]];    new3s_PointXYZ p1 = m_cloud[samples[1]];    new3s_PointXYZ p2 = m_cloud[samples[2]];    Eigen::Vector3d p0p1(p1.get_x() - p0.get_x(), p1.get_y() - p0.get_y(), p1.get_z() - p0.get_z());    Eigen::Vector3d p0p2(p2.get_x() - p0.get_x(), p2.get_y() - p0.get_y(), p2.get_z() - p0.get_z());    // 利用向量的叉积求法向量    model_coefficient.resize(4);    model_coefficient[0] = p0p1[1] * p0p2[2] - p0p2[1] * p0p1[2];    model_coefficient[1] = p0p2[0] * p0p1[2] - p0p1[0] * p0p2[2];    model_coefficient[2] = p0p1[0] * p0p2[1] - p0p2[0] * p0p1[1];    model_coefficient[3] = 0;    model_coefficient.normalize(); //模型系数单位化    // ... + d = 0    model_coefficient[3] = -(model_coefficient[0] * p0.get_x() + model_coefficient[1] * p0.get_y() + model_coefficient[2] * p0.get_z());    return (true);}int RansacPlaneFit::countNum(const Eigen::Matrix4Xd &model_coef, std::vector<int> &indices){    double d = 0;    if (isSetThreshValue() != true) //如果没有设置阈值，自适应产生距离阈值    {        double adatp_threshVal = 0;        std::vector<double> vec_d;        double aved = 0;        for (unsigned int i = 0; i < m_num; ++i)        {            d = computeDist(m_globalindexs[i], model_coef);            aved += d;            vec_d.push_back(d);        }        aved /= m_num;        for (size_t i = 0; i < m_num; ++i)        {            adatp_threshVal += (vec_d[i] - aved) * (vec_d[i] - aved);        }        adatp_threshVal /= m_num;        adatp_threshVal = std::sqrt(adatp_threshVal);        m_threshValue = adatp_threshVal;    }    for (size_t i = 0; i < m_num; ++i)    {        d = computeDist(m_globalindexs[i], model_coef);        if (d < m_threshValue)        {            indices.push_back(i);        }    }    return indices.size();}void RansacPlaneFit::compute3DCentroid(const std::vector<int>& indices, Eigen::Matrix<double, 4, 1>& centroid){    if (indices.empty())    {        std::cout << "索引文件为空!\n";        return;    }    centroid.setZero();    int num = indices.size();    for (int i = 0 ; i < num ; ++i)    {        centroid[0] += m_cloud[indices[i]].get_x();        centroid[1] += m_cloud[indices[i]].get_y();        centroid[2] += m_cloud[indices[i]].get_z();    }    centroid /= static_cast<double>(indices.size());    centroid[3] = 1;}void RansacPlaneFit::computeCovarianceMatrix(const std::vector<int>& indices, const Eigen::Matrix<double, 4, 1>& centroid, Eigen::Matrix<double, 3, 3>& covariance_matrix){    if (indices.empty())    {        std::cout << "索引为空！\n";        return ;    }    covariance_matrix.setZero();    size_t num = indices.size();    for (size_t i = 0; i < num; i++)    {        Eigen::Matrix<double, 4, 1> pt;        pt[0] = m_cloud[indices[i]].get_x() - centroid[0];        pt[1] = m_cloud[indices[i]].get_y() - centroid[1];        pt[2] = m_cloud[indices[i]].get_z() - centroid[2];        covariance_matrix(1, 1) += pt.y() * pt.y();        covariance_matrix(1, 2) += pt.y() * pt.z();        covariance_matrix(2, 2) += pt.z() * pt.z();        pt *= pt.x();        covariance_matrix(0, 0) += pt.x();        covariance_matrix(0, 1) += pt.y();        covariance_matrix(0, 2) += pt.z();    }    covariance_matrix(1, 0) = covariance_matrix(0, 1);    covariance_matrix(2, 0) = covariance_matrix(0, 2);    covariance_matrix(2, 1) = covariance_matrix(1, 2);}void RansacPlaneFit::setRatio(const double ratio){    m_ratio = ratio;}int RansacPlaneFit::getIteration() const{    return m_iteration;}double RansacPlaneFit::getThreshValue() const{    return m_threshValue;}bool RansacPlaneFit::isSetThreshValue() const{    return m_threshFlag;}main文件代码如下：#include <pcl/sample_consensus/ransac.h>#include <pcl/sample_consensus/sac_model_plane.h>#include <pcl/io/pcd_io.h>#include <pcl/point_types.h>#include <pcl/point_cloud.h>#include <math.h>#include <vector>#include "new3s_PointXYZ.h"#include <pcl/common/common.h>#include <pcl/common/centroid.h>#include <opencv.hpp>#include "RansacPlaneFit.h"d test_pcl(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud, const std::vector<int> &indices);void test_opencv(cv::Mat &mat);void test_myself(const std::vector<new3s_PointXYZ> &clouds, const std::vector<int> &indices);void main(){    std::vector<new3s_PointXYZ> myself_cloud;    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);    cloud->width = 10000;    cloud->height = 1;    cloud->is_dense = FALSE;    cloud->resize(cloud->width * cloud->height);    // 生成平面x2 + y2 + z2 = 1的点云数据    size_t num = cloud->size();    for (size_t i = 0 ; i < num ; ++i)    {        double x = rand() / (RAND_MAX + 1.0);        double y = rand() / (RAND_MAX + 1.0);        double z = 1 - (x + y);        double z_outlier = rand() / (RAND_MAX + 1.0);        cloud->points[i].x = x;        cloud->points[i].y = y;        // 生成局外点        if (i % 5 == 0)        {            cloud->points[i].z = z_outlier ;            myself_cloud.push_back(new3s_PointXYZ(x , y , z_outlier));        }        else        {            cloud->points[i].z = 1 - (cloud->points[i].x + cloud->points[i].y);            myself_cloud.push_back(new3s_PointXYZ(x, y , z));        }       }//  pcl::io::savePCDFile("1.pcd", *cloud);    // 保存局内点索引    std::vector<int> inliers;    // 采样一致性模型对象    pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_p(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));    pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_p);    ransac.setDistanceThreshold(0.01);    ransac.computeModel();    ransac.getInliers(inliers);    std::cout << "局内点：" << inliers.size() << std::endl;    pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);    final->resize(inliers.size());    pcl::copyPointCloud(*cloud, inliers, *final);    Eigen::VectorXf coef = Eigen::VectorXf::Zero(4 , 1);    ransac.getModelCoefficients(coef);    std::cout << "拟合系数：\n";    std::cout << "a = " << coef[0] << " b = " << coef[1] << "   c = " <<coef[2]<< " d = " << coef[3] << "\n";    RansacPlaneFit ransacfit;    ransacfit.setInputCloud(myself_cloud);    Eigen::Matrix<double, 4, 1> myself_coef = ransacfit.computePlaneCoef();    std::cout << "迭代次数：" << ransacfit.getIteration() << std::endl;    std::cout << "自适应距离阈值：" << ransacfit.getThreshValue() << std::endl;    std::cout << "myself 拟合系数:" << std::endl;    std::cout << myself_coef << std::endl;}

和pcl中的平面拟合做了一个比较，如果没有设定阈值，程序将自动计算出一个距离阈值。