opencv3 最小二乘拟合平面

最新推荐文章于 2024-07-19 00:00:00 发布

weixin_33938733

最新推荐文章于 2024-07-19 00:00:00 发布

阅读量1.7k

点赞数

CC 4.0 BY-SA版权

文章标签：人工智能数据结构与算法 python

原文链接：https://my.oschina.net/u/1046919/blog/1612923

本文介绍了一种使用最小二乘法进行三维点云数据拟合平面的方法。该算法首先计算点云数据的质心，并基于质心调整点云坐标，然后利用奇异值分解求解最佳拟合平面的参数。

2019独角兽企业重金招聘Python工程师标准>>>

/*
 * 
 * 
 *  最小二乘拟合平面，平面方程：Ax+By+Cz=D
 *  A = plane.at<float>(0,0)
 *  B = plane.at<float>(1,0)
 *  C = plane.at<float>(2,0)
 *  D = plane.at<float>(3,0)
 * 
 * */
void fitPlane(const cv::Mat &points, cv::Mat& plane){
    int rows = points.rows;
    int cols = points.cols;

    cv::Mat centroid = cv::Mat::zeros(1,cols,CV_32FC1);
    for(int i=0;i<cols;i++){
        for(int j=0;j<rows;j++){
            centroid.at<float>(0,i) += points.at<float>(j,i);
        }
        centroid.at<float>(0,i)/=rows;

    }

    cv::Mat points2 = cv::Mat::ones(rows,cols,CV_32FC1);
    for(int i=0;i<rows;i++){
        for(int j=0;j<cols;j++){
            points2.at<float>(i,j) = points.at<float>(i,j) - centroid.at<float>(0,j) ;
        }
    }
    cv::Mat A,W,U,V;
    cv::gemm(points2,points,1,NULL,0,A,CV_GEMM_A_T);
    SVD::compute(A,W,U,V);


    plane = cv::Mat::zeros(cols+1,1,CV_32FC1);
    for (int c = 0; c<cols; c++){
        plane.at<float>(c,0) = V.at<float>(cols-1,c);
        plane.at<float>(cols,0) += plane.at<float>(c,0)*centroid.at<float>(0,c);
    }

}

转载于:https://my.oschina.net/u/1046919/blog/1612923