最小二乘拟合平面

原创 2016年07月26日 16:07:36

最小二乘拟合平面


#include <iostream>  
#include <windows.h>  
#include "opencv.hpp"  
#include <string>
#include <stdlib.h>
#include <stdio.h>


using namespace std;
using namespace cv;

//Ax+by+cz=D  
void cvFitPlane(const CvMat* points, float* plane){
    // Estimate geometric centroid.  
    int nrows = points->rows;
    int ncols = points->cols;
    int type = points->type;
    CvMat* centroid = cvCreateMat(1, ncols, type);
    cvSet(centroid, cvScalar(0));
    for (int c = 0; c<ncols; c++){
        for (int r = 0; r < nrows; r++)
        {
            centroid->data.fl[c] += points->data.fl[ncols*r + c];
        }
        centroid->data.fl[c] /= nrows;
    }
    // Subtract geometric centroid from each point.  
    CvMat* points2 = cvCreateMat(nrows, ncols, type);
    for (int r = 0; r<nrows; r++)
    for (int c = 0; c<ncols; c++)
        points2->data.fl[ncols*r + c] = points->data.fl[ncols*r + c] - centroid->data.fl[c];
    // Evaluate SVD of covariance matrix.  
    CvMat* A = cvCreateMat(ncols, ncols, type);
    CvMat* W = cvCreateMat(ncols, ncols, type);
    CvMat* V = cvCreateMat(ncols, ncols, type);
    cvGEMM(points2, points, 1, NULL, 0, A, CV_GEMM_A_T);
    cvSVD(A, W, NULL, V, CV_SVD_V_T);
    // Assign plane coefficients by singular vector corresponding to smallest singular value.  
    plane[ncols] = 0;
    for (int c = 0; c<ncols; c++){
        plane[c] = V->data.fl[ncols*(ncols - 1) + c];
        plane[ncols] += plane[c] * centroid->data.fl[c];
    }
    // Release allocated resources.  
    //cvReleaseMat(¢roid);
    cvReleaseMat(&points2);
    cvReleaseMat(&A);
    cvReleaseMat(&W);
    cvReleaseMat(&V);
}



int main()
{

    //source img 8192*14000
    //Mat im = imread("D:\\XXX02.bmp", 0);
    Mat im = imread("D:\\XXX02.bmp", 0);


    double t = (double)getTickCount();

    Mat im_resize;
    resize(im, im_resize, Size(0.1*im.cols, 0.1*im.rows));


    //================================ using least squares to fit the plane==================================//
    vector<int> X_vector;
    vector<int> Y_vector;
    vector<int> Z_vector;
    //初始值为左上角点,
    for (int i = 0; i < im_resize.rows; i++){
        //uchar*rowptr = im.ptr<uchar>(i);
        for (int j = 0; j < im_resize.cols; j++){
            X_vector.push_back(j * 10);
            Y_vector.push_back(i * 10);
            Z_vector.push_back(im_resize.at<uchar>(i, j));
            //cout << rowptr[j] << endl;
        }
    }

    /*//测试用
    double x_point[] = { 1, 2, 1, 4, 2, 6, 7, 3, 9 };
    double y_point[] = { 1, 1, 3, 4, 5, 2, 7, 8, 2 };
    double z_point[] = { 91, 102, 103, 104, 105, 106, 107, 108, 109 };

    vector<double> X_vector(x_point, x_point + 9);
    vector<double> Y_vector(y_point, y_point + 9);
    vector<double> Z_vector(z_point, z_point + 9);*/

    CvMat*points_mat = cvCreateMat(X_vector.size(), 3, CV_32FC1);//定义用来存储需要拟合点的矩阵   
    for (int i = 0; i < X_vector.size(); ++i)
    {
        points_mat->data.fl[i * 3 + 0] = X_vector[i];//矩阵的值进行初始化   X的坐标值  
        points_mat->data.fl[i * 3 + 1] = Y_vector[i];//  Y的坐标值  
        points_mat->data.fl[i * 3 + 2] = Z_vector[i]; //<span style = "font-family: Arial, Helvetica, sans-serif;">//  Z的坐标值</span>  

    }
    float plane12[4] = { 0 };//定义用来储存平面参数的数组   
    cvFitPlane(points_mat, plane12);//调用方程   

    //Ax+By+Cz=D
    //其中 A = plane12[0], B = plane12[1], C = plane12[2], D = plane12[3]
    float A = plane12[0], B = plane12[1], C = plane12[2], D = plane12[3];
    cout << "A=" << plane12[0] << ",B=" << plane12[1] << ",C=" << plane12[2] << ",D=" << plane12[3] << endl;

    t = double(getTickCount() - t) * 1000 / getTickFrequency();
    cout << "the least squares time = " << t << " ms" << endl;

    //=============================== END using least squares to fit the plane================================//

    //test the result plane 
    Mat resultImg(im.size(), CV_8U);
    for (int i = 0; i < im.rows; i++){
        //uchar*rowptr = im.ptr<uchar>(i);
        uchar*resultptr = resultImg.ptr<uchar>(i);
        for (int j = 0; j < im.cols; j++){
            //Ax+By+Cz=D, z=(D-Ax-By)/C
            resultptr[j] = (D - A*j - B*i) / C;
        }
    }

    //resultImg -srcimg
    Mat diff = resultImg - im;
    namedWindow("diff img", 0);
    imshow("diff img", diff);

    Mat diffThre;
    threshold(diff, diffThre, 10, 255, THRESH_BINARY);

    namedWindow("diffThre img", 0);
    imshow("diffThre img", diffThre);

    waitKey();

    //Sleep(5000);
    return 0;

}

参考了这个http://blog.csdn.net/zhouyelihua/article/details/46122977

版权声明:本文为博主原创文章,未经博主允许不得转载。

相关文章推荐

最小二乘曲线拟合Fortran源程序

  • 2017年08月17日 16:10
  • 3KB
  • 下载

最小二乘拟合

  • 2015年11月14日 20:05
  • 173KB
  • 下载

最小二乘法平面方程拟合计算, 点云法向量估算

利用最小二乘法求解点云/点集/多点的拟合平面,求解点云的法向量。

最小二乘椭圆拟合matlab

  • 2016年05月22日 14:14
  • 2KB
  • 下载

最小二乘法 拟合平面直线

前言:     最近要实现一个算法,“对一系列点拟合出一条线,且区分出不属于改线的点”。在网上找了许多资料,用数学公式解释原理以及用matlab实现的居多,本文章主要解释用最小二乘法的进行点拟合成线,...

最小二乘拟合

本文对最小二乘拟合直线中出现的问题,当直线垂直时参数无法求出,使用ax+by+c=0或者p=xcos(a)+ysin(a)计算量会增加,针对这种情况,本算做了修正可以判断直线垂直情况。   ...

最小二乘 拟合 matlab

  • 2016年05月10日 20:00
  • 680B
  • 下载
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:最小二乘拟合平面
举报原因:
原因补充:

(最多只允许输入30个字)