最小二乘拟合平面

最小二乘拟合平面

#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