Plane Fitting using OpenCV

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);
 }
}

Here is an example of this plane fitting, this is the surface before fitting, this is measurement result of a flat surface using white light interferometry:

and after fitting the result is:

The color represents the height, so we can see that after fitting the color of the surface is almost the same only a small variation of the surface. The sample should be flat and smooth, however it's not smooth anymore due to wear. However, the real scale is nanometer, the variation that we see in the result is around 100 nm.