Fitting Plane to Point Cloud – Hands on

In my last post i described how to get the equations for fitting a plane to a given pointcloud. Now lets put them into some sample code. I will use the same notation as in the previous post.

When i first started with the optimization, i thought about distinguishing the different cases by Txx != 0. I already worked out the math for the case Txx == 0 etc. And then i started testing with real world data. Random points which are approximatly on a plane. The sad part is: Even with a pointcloud where Txx should be approximatly zero it wasnt close enough to make a valid decision. (And yes i know that comparing a float with 0 is not the way you do it) So sometimes the algorithm ended in the Txx != 0 case, setting c = 1 where c should actually be 0, but because of the unequally distributed values, it wasnt…

So i have to add some modifications in the rule-set.

Dont make decisions based on fixed number-comparision. Better use relative comparison.

In this case: dont check for Txx == 0 (or close to zero), but compare Txx, Tyy and Tzz. Since Txx describes (in a way) the variance of the x values, it gives you a hint how distributed the data is along the x axis. If you think about it, the bigger the variance in a direction, the smaller the value in the plane-normal will be. On the other hand, the lower the distribution (compared to the other dimensions), the higher the according value in the plane-normal.

In the equations we need to set one parameter to 1, and the rest follows. Now you dont want that parameter to one that should be close to zero, because with all the floating-point-precision you might end up totally off the chart. Better to have the parameter set to 1, which probably will be the biggest anyway.

Therefore the algorithm now takes the biggest of the 3 (Txx,Tyy,Tzz) and eliminates the according parameter first. Then the parameter for the smallest of the 3 T’s is set to 1 first.

One more thing: in an attempt to keep the numbers as low as possible (specially for the probably quite big sums in Txx etc.) i decided to normalize the points first. It turns out all the calculations for a,b and c don’t change. Its just the value of d that moves the resulting plane back to the original position.

But enough foreplay, lets see some code.

Sample Code

Again its using the eigen framework, this time only for the datatypes.

Step 1: Calculate Txx,Tyy…

As said before, substract the mean-value first to keep the values during the calculation low.

double Txx, Txy, Txz, Tyy, Tyz, Tzz;
Txx= Txy= Txz= Tyy = Tyz = Tzz= 0;
auto mean= calcMean(somePoints);

for(auto point : somePoints) {
  double x_0= point.x() - mean.x();
  double y_0= point.y() - mean.y();
  double z_0= point.z() - mean.z();
  Txx += x_0*x_0;
  Txy += x_0*y_0;
  Txz += x_0*z_0;
  Tyy += y_0*y_0;
  Tyz += y_0*z_0;
  Tzz += z_0*z_0;

No magic happening here. As you can see, there is no need to substract the mean-parts from the T’s anymore (since they are all 0 anyway).

Step 2: Determining which parameter to go first with
//Sab_c = Tab*Tcc - Tca*Tcb

float absX= ABS(Txx);
float absY= ABS(Tyy);
float absZ= ABS(Tzz);
//The direction with the maximal deviation is considered last (possible 0 in the normal)
if(absX >= absY && absX >= absZ) { 
 //x = a is last  
 // calculate b,c with respect to x

 a= (-b*Txy - c*Txz)/Txx;
} else if(absY >= absX && absY >= absZ) {
 //y = b is last
 //calculate a,c with respect to y

 b= (-a*Txy - c*Tyz)/Tyy;
} else { //if(absZ >= absX && absZ >= absY) {
 //z = c is last
 // calculate a,b with respect to z
 c= (-a*Txz - b*Tyz)/Tzz;

as you can see, i left the most important part as a comment here. This part is nearly the same for all three cases so lets focus on x:

Step 3: calculating the first two Parameters

I show that step for x only, but i am sure you can figure it out for y and z yourself. Otherwise leave me a comment.

So we now know that x has the most variance, which may lead to a being very small. Now we choose b or c to be set to 0 according to Tyy and Tzz. The variable with the smallest Variance wins.

For the equations i stated the notation for Sab. i have to extend that now, since we not only look at x:

Sab_c= Tab*Tcc - Tca*Tcb
Syz_x= Tyz*Txx - Txy*Txz

So Syz from the equations is Syz_x now. For the x-case we only need the Sab_x’s.

Lets calculate them in code:

double Syy_x = Tyy*Txx - Txy*Txy;
double Syz_x = Tyz*Txx - Txy*Txz;
double Szz_x = Tzz*Txx - Txz*Txz;

Yes, no surprises here.

Now we choose the bigger of the remaining two Tyy and Tzz. if absY is bigger than absZ, then z has the smallest variance and c is set first.

Otherwise its the other way round.

Consider the first case.

We take the bigger of the bigger of Syy_x and Syz_x to calculate b according to c (which is set to 1 by default). The two versions are mathematically the same, but we want to avoid floating-point-problems whereever possible. And dividing by bigger numbers is better.

It should basically never happen, but if Syy and Syz are both close to 0 AND Szz isnt, then we set c to 0.

if(!isConsideredZero(Syy_x) || !isConsideredZero(Syz_x) {
  if(ABS(Syy_x) > ABS(Syz_x)) {
    b= -c*Syz_x/Syy_x;
  } else {
    b= -c*Szz_x/Syz_x;
} else if(!isConsideredZero(Szz_x)) {
 c= 0;

The second case looks kinda the same, just with changed indices:

if(!isConsideredZero(Szz_x) || !isConsideredZero(Syz_x) {
  if(ABS(Szz_x) > ABS(Syz_x)) {
    c= -b*Syz_x/Szz_x;
  } else {
    c= -b*Syy_x/Syz_x;
} else if(!isConsideredZero(Syy_x)) {
 b= 0;
Step 4: calculating d

finally we normalize the resulting normal vector and calculate d with it:

outNormal.x()= a;
outNormal.y()= b;
outNormal.z()= c;

//finally calc D
outD= -(outNormal.transpose()*mean).x();

And thats it. Now you have all the code you need.

Feel free to leave a comment!

Leave a Reply

Your email address will not be published. Required fields are marked *

This site uses Akismet to reduce spam. Learn how your comment data is processed.