基于随机抽样或最小二乘法 c++实现三维点云平面检测

随机抽样

cpp 复制代码

std::vector<int> random(int n, int N){
    std::vector<int> rets;
    for(int i=0; i<N; i++){
        while(true){
            int v = rand() % n;
            if(std::find(rets.begin(), rets.end(), v) == rets.end()){
                rets.push_back(v);
                break;
            }
        }
    }
    return rets;
}
bool Plane(std::vector<cv::Point3f>& cloud, std::vector<cv::Point3f>& cloudPlane){

    int nPlane = xxx;
    float thred = xxx;
    float P = xxx;
    int Iter = xxx;
    int S = Iter;
    int N = xxx;

    int n = cloud.size();
    if(n < nPlane){
        std::cout<<"not enough points"<<std::endl;
        return false;
    }

    int iter = 0;
    std::vector<int> inliersBest;
    while(true){
        iter++;
        std::vector<int> index = random(n, N);

        cv::Mat A = cv::Mat(3, 3, CV_32F);
        cv::Mat b = cv::Mat(3, 1, CV_32F);

        A.at<float>(0, 0) = cloud[index[0]].x;
        A.at<float>(0, 1) = cloud[index[0]].y;
        A.at<float>(0, 2) = cloud[index[0]].z;
        A.at<float>(1, 0) = cloud[index[1]].x;
        A.at<float>(1, 1) = cloud[index[1]].y;
        A.at<float>(1, 2) = cloud[index[1]].z;
        A.at<float>(2, 0) = cloud[index[2]].x;
        A.at<float>(2, 1) = cloud[index[2]].y;
        A.at<float>(2, 2) = cloud[index[2]].z;
        b.at<float>(0, 0) = -1;
        b.at<float>(1, 0) = -1;
        b.at<float>(2, 0) = -1;
        b = A.t() * b;
        A = A.t() * A;

        if(cv::determinant(A) == 0) continue;
        cv::Mat normal = A.inv() * b;
        if(cv::norm(normal) == 0) continue;

        std::vector<int> inliers;
        for(int i=0; i<n; i++){
            float distance = std::abs(normal.at<float>(0, 0)*cloud[i].x + normal.at<float>(1, 0)*cloud[i].y + normal.at<float>(2, 0)*cloud[i].z + 1) / cv::norm(normal);
            if(distance < thred) inliers.push_back(i);
        }
        if(inliers.size() > inliersBest.size()){
            inliersBest = inliers;

            float p = float(inliers.size()) / n;
            S = std::log(1-P)/std::log(1 - std::pow(p, N));
        }

        if(iter > Iter || iter > S) break;
    }

    cv::Mat A = cv::Mat(inliersBest.size(), 3, CV_32F);
    cv::Mat b = cv::Mat(inliersBest.size(), 1, CV_32F);
    for(int i=0; i<inliersBest.size(); i++){
        A.at<float>(i, 0) = cloud[inliersBest[i]].x;
        A.at<float>(i, 1) = cloud[inliersBest[i]].y;
        A.at<float>(i, 2) = cloud[inliersBest[i]].z;
        b.at<float>(i, 0) = -1;
    }
    b = A.t() * b;
    A = A.t() * A;
    if(cv::determinant(A) == 0){
        std::cout<<"zero determinant"<<std::endl;
        return false;
    }

    cv::Mat normal = A.inv() * b;
    if(cv::norm(normal) == 0){
        std::cout<<"zero normal"<<std::endl;
        return false;
    }

    for(int i=0; i<cloud.size(); i++){
        float distance = std::abs(normal.at<float>(0, 0)*cloud[i].x + normal.at<float>(1, 0)*cloud[i].y + normal.at<float>(2, 0)*cloud[i].z + 1) / cv::norm(normal);
        if(distance < thred) cloudPlane.push_back(cloud[i]);
    }

    if(cloudPlane.size() < nPlane){
        std::cout<<"not enough plane points"<<std::endl;
        return false;
    }

    return true;
}

最小二乘法：最小二乘法原理

cpp 复制代码

bool Plane(vector<cv::Point3f> cloud, double& a, double& b, double& c, double& angle)
{
    double A11 = 0, A12 = 0, A13 = 0;
    double A21 = 0, A22 = 0, A23 = 0;
    double A31 = 0, A32 = 0, A33 = 0;
    double B1 = 0, B2 = 0, B3 = 0;
    for (int i = 0; i < cloud.size(); i++)
    {
        A11 += cloud.at(i).x*cloud.at(i).x;
        A12 += cloud.at(i).x*cloud.at(i).y;
        A13 += cloud.at(i).x;
        A21 += cloud.at(i).x*cloud.at(i).y;
        A22 += cloud.at(i).y*cloud.at(i).y;
        A23 += cloud.at(i).y;
        A31 += cloud.at(i).x;
        A32 += cloud.at(i).y;
        B1 += cloud.at(i).x*cloud.at(i).z;
        B2 += cloud.at(i).y*cloud.at(i).z;
        B3 += cloud.at(i).z;
    }
    A33 = cloud.size();

    cv::Mat A(3, 3, CV_32FC1);
    A.at<float>(0, 0) = A11; A.at<float>(0, 1) = A12; A.at<float>(0, 2) = A13;
    A.at<float>(1, 0) = A21; A.at<float>(1, 1) = A22; A.at<float>(1, 2) = A23;
    A.at<float>(2, 0) = A31; A.at<float>(2, 1) = A32; A.at<float>(2, 2) = A33;
    cv::Mat B(3, 1, CV_32FC1);
    B.at<float>(0, 0) = B1; B.at<float>(1, 0) = B2; B.at<float>(2, 0) = B3;
    cv::Mat X;
    X = A.inv()*B;                                                              //X为3*1解向量，分别对应平面方程z=ax+by+c中的abc

    a = X.at<float>(0, 0);
    b = X.at<float>(1, 0);
    c = X.at<float>(2, 0);

    // 计算平面的法向量
    Eigen::Vector3d normal_vector(a, b, c);
    normal_vector.normalize();

    // 计算相对于水平面的角度
    angle = std::acos(normal_vector.dot(Eigen::Vector3d(0, 0, 1))) * 180.0 / M_PI;  // Eigen::Vector3d(0, 0, 1)水平面法向量
    return true;
}