自适应阈值的ransac平面拟合

于 2017-06-25 16:07:18 发布
阅读量5.6k 收藏 43
点赞数 7
分类专栏： PCL
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/hanshuobest/article/details/73718440
版权

原来一直用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中的平面拟合做了一个比较，如果没有设定阈值，程序将自动计算出一个距离阈值。

MagiChos
关注
  • 7
    点赞
  • 踩
  • 43
    收藏
    觉得还不错? 一键收藏
  • 7
    评论
专栏目录
评论 7
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值