[OpenCV] Plane Fitting using OpenCV

출처 : http://mariotapilouw.blogspot.com/2011/05/plane-fitting-using-opencv.html

OpenCV를 이용한 Plane Fitting 방법을 소개한 글을 찾아서 기록차원에서 일부를 발췌하였다. 아래는 기록용으로 발췌하여 기록한 것이므로 참고는 위 링크의 원문을 참고하기 바란다.

As what I've promised, I'm going to write about plane fitting using OpenCV. It's quite straight away and quite similar to my previous post, the difference is only about the physical representation of the variables in the equations. This time it's only a plane fitting, so it's a linear least square fitting. Later I would like to explore non-linear fitting as well and share it here when I have made it.

Let's say that we have a set of data that represents a plane in 3D coordinate X-Y-Z and modeled as Axi + Byi + C = zi. where i is the enumerator of the data (0~n).

Using the same approach, we can use matrix to represent the simultaneous equation and solve it.I will directly show you how.
1. prepare the matrices for the data.
CvMat *res = cvCreateMat(3, 1, CV_32FC1); CvMat *matX = cvCreateMat(10000, 3, CV_32FC1); CvMat *matZ = cvCreateMat(10000, 1, CV_32FC1);

2. input the data into the matrices.
for(int row=0;row<100;row++) { for(int col=0;col<100;col++) { int idx = row * col; double val = cvmGet(filteredMat, row, col); cvmSet(matX, idx, 0, col*pixScale); cvmSet(matX, idx, 1, row*pixScale); cvmSet(matX, idx, 2, 1); cvmSet(matZ, idx, 0, val); } }

3. solve the equation
cvSolve(matX, matZ, res, CV_SVD); A = cvmGet(res, 0, 0); B = cvmGet(res, 1, 0); C = cvmGet(res, 2, 0);

4. calculate the distance between the plane and the real data.
double sqrtc = sqrt(pow(A, 2) + pow(B, 2) + pow(C, 2)); float min = 100; float max = -100; // plane generation, Z = Ax + By + C // distance calculation d = Ax + By - z + C / sqrt(A^2 + B^2 + C^2) // pointlist generation for(int row=0;row<filteredMat->rows;row++) { for(int col=0;col<filteredMat->cols;col++) { double Zval = cvmGet(filteredMat, row, col); double val = col*pixScale*A + row*pixScale*B - Zval + C; double distance = val / sqrtc; if(min > distance) min = distance; if(max < distance) max = distance; cvmSet(planeMat, row, col, distance); } }