最小二乘拟合平面

最小二乘拟合平面

#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;

}