clearpath_tools: clearpath_tools: bestfit.cpp Source File

00001 #include <stdio.h>
00002 #include <stdlib.h>
00003 #include <string.h>
00004 #include <assert.h>
00005 #include <math.h>
00006 
00007 // Geometric Tools, Inc.
00008 // http://www.geometrictools.com
00009 // Copyright (c) 1998-2006.  All Rights Reserved
00010 //
00011 // The Wild Magic Library (WM3) source code is supplied under the terms of
00012 // the license agreement
00013 //     http://www.geometrictools.com/License/WildMagic3License.pdf
00014 // and may not be copied or disclosed except in accordance with the terms
00015 // of that agreement.
00016 
00073 #include "bestfit.h"
00074 
00075 namespace DoublePrecision
00076 {
00077 
00078 class Vec3
00079 {
00080 public:
00081   Vec3(void) { };
00082   Vec3(double _x,double _y,double _z) { x = _x; y = _y; z = _z; };
00083 
00084 
00085   double dot(const Vec3 &v)
00086   {
00087     return x*v.x + y*v.y + z*v.z; // the dot product
00088   }
00089 
00090   double x;
00091   double y;
00092   double z;
00093 };
00094 
00095 
00096 class Eigen
00097 {
00098 public:
00099 
00100 
00101   void DecrSortEigenStuff(void)
00102   {
00103     Tridiagonal(); //diagonalize the matrix.
00104     QLAlgorithm(); //
00105     DecreasingSort();
00106     GuaranteeRotation();
00107   }
00108 
00109   void Tridiagonal(void)
00110   {
00111     double fM00 = mElement[0][0];
00112     double fM01 = mElement[0][1];
00113     double fM02 = mElement[0][2];
00114     double fM11 = mElement[1][1];
00115     double fM12 = mElement[1][2];
00116     double fM22 = mElement[2][2];
00117 
00118     m_afDiag[0] = fM00;
00119     m_afSubd[2] = 0;
00120     if (fM02 != (double)0.0)
00121     {
00122       double fLength = sqrt(fM01*fM01+fM02*fM02);
00123       double fInvLength = ((double)1.0)/fLength;
00124       fM01 *= fInvLength;
00125       fM02 *= fInvLength;
00126       double fQ = ((double)2.0)*fM01*fM12+fM02*(fM22-fM11);
00127       m_afDiag[1] = fM11+fM02*fQ;
00128       m_afDiag[2] = fM22-fM02*fQ;
00129       m_afSubd[0] = fLength;
00130       m_afSubd[1] = fM12-fM01*fQ;
00131       mElement[0][0] = (double)1.0;
00132       mElement[0][1] = (double)0.0;
00133       mElement[0][2] = (double)0.0;
00134       mElement[1][0] = (double)0.0;
00135       mElement[1][1] = fM01;
00136       mElement[1][2] = fM02;
00137       mElement[2][0] = (double)0.0;
00138       mElement[2][1] = fM02;
00139       mElement[2][2] = -fM01;
00140       m_bIsRotation = false;
00141     }
00142     else
00143     {
00144       m_afDiag[1] = fM11;
00145       m_afDiag[2] = fM22;
00146       m_afSubd[0] = fM01;
00147       m_afSubd[1] = fM12;
00148       mElement[0][0] = (double)1.0;
00149       mElement[0][1] = (double)0.0;
00150       mElement[0][2] = (double)0.0;
00151       mElement[1][0] = (double)0.0;
00152       mElement[1][1] = (double)1.0;
00153       mElement[1][2] = (double)0.0;
00154       mElement[2][0] = (double)0.0;
00155       mElement[2][1] = (double)0.0;
00156       mElement[2][2] = (double)1.0;
00157       m_bIsRotation = true;
00158     }
00159   }
00160 
00161   bool QLAlgorithm(void)
00162   {
00163     const int iMaxIter = 32;
00164 
00165     for (int i0 = 0; i0 <3; i0++)
00166     {
00167       int i1;
00168       for (i1 = 0; i1 < iMaxIter; i1++)
00169       {
00170         int i2;
00171         for (i2 = i0; i2 <= (3-2); i2++)
00172         {
00173           double fTmp = fabs(m_afDiag[i2]) + fabs(m_afDiag[i2+1]);
00174           if ( fabs(m_afSubd[i2]) + fTmp == fTmp )
00175             break;
00176         }
00177         if (i2 == i0)
00178         {
00179           break;
00180         }
00181 
00182         double fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((double)2.0) * m_afSubd[i0]);
00183         double fR = sqrt(fG*fG+(double)1.0);
00184         if (fG < (double)0.0)
00185         {
00186           fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
00187         }
00188         else
00189         {
00190           fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
00191         }
00192         double fSin = (double)1.0, fCos = (double)1.0, fP = (double)0.0;
00193         for (int i3 = i2-1; i3 >= i0; i3--)
00194         {
00195           double fF = fSin*m_afSubd[i3];
00196           double fB = fCos*m_afSubd[i3];
00197           if (fabs(fF) >= fabs(fG))
00198           {
00199             fCos = fG/fF;
00200             fR = sqrt(fCos*fCos+(double)1.0);
00201             m_afSubd[i3+1] = fF*fR;
00202             fSin = ((double)1.0)/fR;
00203             fCos *= fSin;
00204           }
00205           else
00206           {
00207             fSin = fF/fG;
00208             fR = sqrt(fSin*fSin+(double)1.0);
00209             m_afSubd[i3+1] = fG*fR;
00210             fCos = ((double)1.0)/fR;
00211             fSin *= fCos;
00212           }
00213           fG = m_afDiag[i3+1]-fP;
00214           fR = (m_afDiag[i3]-fG)*fSin+((double)2.0)*fB*fCos;
00215           fP = fSin*fR;
00216           m_afDiag[i3+1] = fG+fP;
00217           fG = fCos*fR-fB;
00218           for (int i4 = 0; i4 < 3; i4++)
00219           {
00220             fF = mElement[i4][i3+1];
00221             mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
00222             mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
00223           }
00224         }
00225         m_afDiag[i0] -= fP;
00226         m_afSubd[i0] = fG;
00227         m_afSubd[i2] = (double)0.0;
00228       }
00229       if (i1 == iMaxIter)
00230       {
00231         return false;
00232       }
00233     }
00234     return true;
00235   }
00236 
00237   void DecreasingSort(void)
00238   {
00239     //sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
00240     for (int i0 = 0, i1; i0 <= 3-2; i0++)
00241     {
00242       // locate maximum eigenvalue
00243       i1 = i0;
00244       double fMax = m_afDiag[i1];
00245       int i2;
00246       for (i2 = i0+1; i2 < 3; i2++)
00247       {
00248         if (m_afDiag[i2] > fMax)
00249         {
00250           i1 = i2;
00251           fMax = m_afDiag[i1];
00252         }
00253       }
00254 
00255       if (i1 != i0)
00256       {
00257         // swap eigenvalues
00258         m_afDiag[i1] = m_afDiag[i0];
00259         m_afDiag[i0] = fMax;
00260         // swap eigenvectors
00261         for (i2 = 0; i2 < 3; i2++)
00262         {
00263           double fTmp = mElement[i2][i0];
00264           mElement[i2][i0] = mElement[i2][i1];
00265           mElement[i2][i1] = fTmp;
00266           m_bIsRotation = !m_bIsRotation;
00267         }
00268       }
00269     }
00270   }
00271 
00272 
00273   void GuaranteeRotation(void)
00274   {
00275     if (!m_bIsRotation)
00276     {
00277       // change sign on the first column
00278       for (int iRow = 0; iRow <3; iRow++)
00279       {
00280         mElement[iRow][0] = -mElement[iRow][0];
00281       }
00282     }
00283   }
00284 
00285   double mElement[3][3];
00286   double m_afDiag[3];
00287   double m_afSubd[3];
00288   bool m_bIsRotation;
00289 };
00290 
00291 }; // end of double precision namespace
00292 
00293 
00294 bool getBestFitPlane(unsigned int vcount,
00295                      const double *points,
00296                      unsigned int vstride,
00297                      const double *weights,
00298                      unsigned int wstride,
00299                      double *plane)
00300 {
00301   bool ret = false;
00302 
00303   DoublePrecision::Vec3 kOrigin(0,0,0);
00304 
00305   double wtotal = 0;
00306 
00307   if ( 1 )
00308   {
00309     const char *source  = (const char *) points;
00310     const char *wsource = (const char *) weights;
00311 
00312     for (unsigned int i=0; i<vcount; i++)
00313     {
00314 
00315       const double *p = (const double *) source;
00316 
00317       double w = 1;
00318 
00319       if ( wsource )
00320       {
00321         const double *ws = (const double *) wsource;
00322         w = *ws; //
00323         wsource+=wstride;
00324       }
00325 
00326       kOrigin.x+=p[0]*w;
00327       kOrigin.y+=p[1]*w;
00328       kOrigin.z+=p[2]*w;
00329 
00330       wtotal+=w;
00331 
00332       source+=vstride;
00333     }
00334   }
00335 
00336   double recip = 1.0f / wtotal; // reciprocol of total weighting
00337 
00338   kOrigin.x*=recip;
00339   kOrigin.y*=recip;
00340   kOrigin.z*=recip;
00341 
00342 
00343   double fSumXX=0;
00344   double fSumXY=0;
00345   double fSumXZ=0;
00346 
00347   double fSumYY=0;
00348   double fSumYZ=0;
00349   double fSumZZ=0;
00350 
00351 
00352   if ( 1 )
00353   {
00354     const char *source  = (const char *) points;
00355     const char *wsource = (const char *) weights;
00356 
00357     for (unsigned int i=0; i<vcount; i++)
00358     {
00359 
00360       const double *p = (const double *) source;
00361 
00362       double w = 1;
00363 
00364       if ( wsource )
00365       {
00366         const double *ws = (const double *) wsource;
00367         w = *ws; //
00368         wsource+=wstride;
00369       }
00370 
00371       DoublePrecision::Vec3 kDiff;
00372 
00373       kDiff.x = w*(p[0] - kOrigin.x); // apply vertex weighting!
00374       kDiff.y = w*(p[1] - kOrigin.y);
00375       kDiff.z = w*(p[2] - kOrigin.z);
00376 
00377       fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences.
00378       fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences.
00379       fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences.
00380 
00381       fSumYY+= kDiff.y * kDiff.y;
00382       fSumYZ+= kDiff.y * kDiff.z;
00383       fSumZZ+= kDiff.z * kDiff.z;
00384 
00385 
00386       source+=vstride;
00387     }
00388   }
00389 
00390   fSumXX *= recip;
00391   fSumXY *= recip;
00392   fSumXZ *= recip;
00393   fSumYY *= recip;
00394   fSumYZ *= recip;
00395   fSumZZ *= recip;
00396 
00397   // setup the eigensolver
00398   DoublePrecision::Eigen kES;
00399 
00400   kES.mElement[0][0] = fSumXX;
00401   kES.mElement[0][1] = fSumXY;
00402   kES.mElement[0][2] = fSumXZ;
00403 
00404   kES.mElement[1][0] = fSumXY;
00405   kES.mElement[1][1] = fSumYY;
00406   kES.mElement[1][2] = fSumYZ;
00407 
00408   kES.mElement[2][0] = fSumXZ;
00409   kES.mElement[2][1] = fSumYZ;
00410   kES.mElement[2][2] = fSumZZ;
00411 
00412   // compute eigenstuff, smallest eigenvalue is in last position
00413   kES.DecrSortEigenStuff();
00414 
00415   DoublePrecision::Vec3 kNormal;
00416 
00417   kNormal.x = kES.mElement[0][2];
00418   kNormal.y = kES.mElement[1][2];
00419   kNormal.z = kES.mElement[2][2];
00420 
00421   // the minimum energy
00422   plane[0] = kNormal.x;
00423   plane[1] = kNormal.y;
00424   plane[2] = kNormal.z;
00425 
00426   plane[3] = 0 - kNormal.dot(kOrigin);
00427 
00428   return ret;
00429 }
00430 
00431 
00432 
00433 namespace SinglePrecision
00434 {
00435 
00436 class Vec3
00437 {
00438 public:
00439   Vec3(void) { };
00440   Vec3(float _x,float _y,float _z) { x = _x; y = _y; z = _z; };
00441 
00442 
00443   float dot(const Vec3 &v)
00444   {
00445     return x*v.x + y*v.y + z*v.z; // the dot product
00446   }
00447 
00448   float x;
00449   float y;
00450   float z;
00451 };
00452 
00453 
00454 class Eigen
00455 {
00456 public:
00457 
00458 
00459   void DecrSortEigenStuff(void)
00460   {
00461     Tridiagonal(); //diagonalize the matrix.
00462     QLAlgorithm(); //
00463     DecreasingSort();
00464     GuaranteeRotation();
00465   }
00466 
00467   void Tridiagonal(void)
00468   {
00469     float fM00 = mElement[0][0];
00470     float fM01 = mElement[0][1];
00471     float fM02 = mElement[0][2];
00472     float fM11 = mElement[1][1];
00473     float fM12 = mElement[1][2];
00474     float fM22 = mElement[2][2];
00475 
00476     m_afDiag[0] = fM00;
00477     m_afSubd[2] = 0;
00478     if (fM02 != (float)0.0)
00479     {
00480       float fLength = sqrt(fM01*fM01+fM02*fM02);
00481       float fInvLength = ((float)1.0)/fLength;
00482       fM01 *= fInvLength;
00483       fM02 *= fInvLength;
00484       float fQ = ((float)2.0)*fM01*fM12+fM02*(fM22-fM11);
00485       m_afDiag[1] = fM11+fM02*fQ;
00486       m_afDiag[2] = fM22-fM02*fQ;
00487       m_afSubd[0] = fLength;
00488       m_afSubd[1] = fM12-fM01*fQ;
00489       mElement[0][0] = (float)1.0;
00490       mElement[0][1] = (float)0.0;
00491       mElement[0][2] = (float)0.0;
00492       mElement[1][0] = (float)0.0;
00493       mElement[1][1] = fM01;
00494       mElement[1][2] = fM02;
00495       mElement[2][0] = (float)0.0;
00496       mElement[2][1] = fM02;
00497       mElement[2][2] = -fM01;
00498       m_bIsRotation = false;
00499     }
00500     else
00501     {
00502       m_afDiag[1] = fM11;
00503       m_afDiag[2] = fM22;
00504       m_afSubd[0] = fM01;
00505       m_afSubd[1] = fM12;
00506       mElement[0][0] = (float)1.0;
00507       mElement[0][1] = (float)0.0;
00508       mElement[0][2] = (float)0.0;
00509       mElement[1][0] = (float)0.0;
00510       mElement[1][1] = (float)1.0;
00511       mElement[1][2] = (float)0.0;
00512       mElement[2][0] = (float)0.0;
00513       mElement[2][1] = (float)0.0;
00514       mElement[2][2] = (float)1.0;
00515       m_bIsRotation = true;
00516     }
00517   }
00518 
00519   bool QLAlgorithm(void)
00520   {
00521     const int iMaxIter = 32;
00522 
00523     for (int i0 = 0; i0 <3; i0++)
00524     {
00525       int i1;
00526       for (i1 = 0; i1 < iMaxIter; i1++)
00527       {
00528         int i2;
00529         for (i2 = i0; i2 <= (3-2); i2++)
00530         {
00531           float fTmp = fabs(m_afDiag[i2]) + fabs(m_afDiag[i2+1]);
00532           if ( fabs(m_afSubd[i2]) + fTmp == fTmp )
00533             break;
00534         }
00535         if (i2 == i0)
00536         {
00537           break;
00538         }
00539 
00540         float fG = (m_afDiag[i0+1] - m_afDiag[i0])/(((float)2.0) * m_afSubd[i0]);
00541         float fR = sqrt(fG*fG+(float)1.0);
00542         if (fG < (float)0.0)
00543         {
00544           fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG-fR);
00545         }
00546         else
00547         {
00548           fG = m_afDiag[i2]-m_afDiag[i0]+m_afSubd[i0]/(fG+fR);
00549         }
00550         float fSin = (float)1.0, fCos = (float)1.0, fP = (float)0.0;
00551         for (int i3 = i2-1; i3 >= i0; i3--)
00552         {
00553           float fF = fSin*m_afSubd[i3];
00554           float fB = fCos*m_afSubd[i3];
00555           if (fabs(fF) >= fabs(fG))
00556           {
00557             fCos = fG/fF;
00558             fR = sqrt(fCos*fCos+(float)1.0);
00559             m_afSubd[i3+1] = fF*fR;
00560             fSin = ((float)1.0)/fR;
00561             fCos *= fSin;
00562           }
00563           else
00564           {
00565             fSin = fF/fG;
00566             fR = sqrt(fSin*fSin+(float)1.0);
00567             m_afSubd[i3+1] = fG*fR;
00568             fCos = ((float)1.0)/fR;
00569             fSin *= fCos;
00570           }
00571           fG = m_afDiag[i3+1]-fP;
00572           fR = (m_afDiag[i3]-fG)*fSin+((float)2.0)*fB*fCos;
00573           fP = fSin*fR;
00574           m_afDiag[i3+1] = fG+fP;
00575           fG = fCos*fR-fB;
00576           for (int i4 = 0; i4 < 3; i4++)
00577           {
00578             fF = mElement[i4][i3+1];
00579             mElement[i4][i3+1] = fSin*mElement[i4][i3]+fCos*fF;
00580             mElement[i4][i3] = fCos*mElement[i4][i3]-fSin*fF;
00581           }
00582         }
00583         m_afDiag[i0] -= fP;
00584         m_afSubd[i0] = fG;
00585         m_afSubd[i2] = (float)0.0;
00586       }
00587       if (i1 == iMaxIter)
00588       {
00589         return false;
00590       }
00591     }
00592     return true;
00593   }
00594 
00595   void DecreasingSort(void)
00596   {
00597     //sort eigenvalues in decreasing order, e[0] >= ... >= e[iSize-1]
00598     for (int i0 = 0, i1; i0 <= 3-2; i0++)
00599     {
00600       // locate maximum eigenvalue
00601       i1 = i0;
00602       float fMax = m_afDiag[i1];
00603       int i2;
00604       for (i2 = i0+1; i2 < 3; i2++)
00605       {
00606         if (m_afDiag[i2] > fMax)
00607         {
00608           i1 = i2;
00609           fMax = m_afDiag[i1];
00610         }
00611       }
00612 
00613       if (i1 != i0)
00614       {
00615         // swap eigenvalues
00616         m_afDiag[i1] = m_afDiag[i0];
00617         m_afDiag[i0] = fMax;
00618         // swap eigenvectors
00619         for (i2 = 0; i2 < 3; i2++)
00620         {
00621           float fTmp = mElement[i2][i0];
00622           mElement[i2][i0] = mElement[i2][i1];
00623           mElement[i2][i1] = fTmp;
00624           m_bIsRotation = !m_bIsRotation;
00625         }
00626       }
00627     }
00628   }
00629 
00630 
00631   void GuaranteeRotation(void)
00632   {
00633     if (!m_bIsRotation)
00634     {
00635       // change sign on the first column
00636       for (int iRow = 0; iRow <3; iRow++)
00637       {
00638         mElement[iRow][0] = -mElement[iRow][0];
00639       }
00640     }
00641   }
00642 
00643   float mElement[3][3];
00644   float m_afDiag[3];
00645   float m_afSubd[3];
00646   bool m_bIsRotation;
00647 };
00648 
00649 }; // end of float precision namespace
00650 
00651 
00652 bool getBestFitPlane(unsigned int vcount,
00653                      const float *points,
00654                      unsigned int vstride,
00655                      const float *weights,
00656                      unsigned int wstride,
00657                      float *plane)
00658 {
00659   bool ret = false;
00660 
00661   SinglePrecision::Vec3 kOrigin(0,0,0);
00662 
00663   float wtotal = 0;
00664 
00665   if ( 1 )
00666   {
00667     const char *source  = (const char *) points;
00668     const char *wsource = (const char *) weights;
00669 
00670     for (unsigned int i=0; i<vcount; i++)
00671     {
00672 
00673       const float *p = (const float *) source;
00674 
00675       float w = 1;
00676 
00677       if ( wsource )
00678       {
00679         const float *ws = (const float *) wsource;
00680         w = *ws; //
00681         wsource+=wstride;
00682       }
00683 
00684       kOrigin.x+=p[0]*w;
00685       kOrigin.y+=p[1]*w;
00686       kOrigin.z+=p[2]*w;
00687 
00688       wtotal+=w;
00689 
00690       source+=vstride;
00691     }
00692   }
00693 
00694   float recip = 1.0f / wtotal; // reciprocol of total weighting
00695 
00696   kOrigin.x*=recip;
00697   kOrigin.y*=recip;
00698   kOrigin.z*=recip;
00699 
00700 
00701   float fSumXX=0;
00702   float fSumXY=0;
00703   float fSumXZ=0;
00704 
00705   float fSumYY=0;
00706   float fSumYZ=0;
00707   float fSumZZ=0;
00708 
00709 
00710   if ( 1 )
00711   {
00712     const char *source  = (const char *) points;
00713     const char *wsource = (const char *) weights;
00714 
00715     for (unsigned int i=0; i<vcount; i++)
00716     {
00717 
00718       const float *p = (const float *) source;
00719 
00720       float w = 1;
00721 
00722       if ( wsource )
00723       {
00724         const float *ws = (const float *) wsource;
00725         w = *ws; //
00726         wsource+=wstride;
00727       }
00728 
00729       SinglePrecision::Vec3 kDiff;
00730 
00731       kDiff.x = w*(p[0] - kOrigin.x); // apply vertex weighting!
00732       kDiff.y = w*(p[1] - kOrigin.y);
00733       kDiff.z = w*(p[2] - kOrigin.z);
00734 
00735       fSumXX+= kDiff.x * kDiff.x; // sume of the squares of the differences.
00736       fSumXY+= kDiff.x * kDiff.y; // sume of the squares of the differences.
00737       fSumXZ+= kDiff.x * kDiff.z; // sume of the squares of the differences.
00738 
00739       fSumYY+= kDiff.y * kDiff.y;
00740       fSumYZ+= kDiff.y * kDiff.z;
00741       fSumZZ+= kDiff.z * kDiff.z;
00742 
00743 
00744       source+=vstride;
00745     }
00746   }
00747 
00748   fSumXX *= recip;
00749   fSumXY *= recip;
00750   fSumXZ *= recip;
00751   fSumYY *= recip;
00752   fSumYZ *= recip;
00753   fSumZZ *= recip;
00754 
00755   // setup the eigensolver
00756   SinglePrecision::Eigen kES;
00757 
00758   kES.mElement[0][0] = fSumXX;
00759   kES.mElement[0][1] = fSumXY;
00760   kES.mElement[0][2] = fSumXZ;
00761 
00762   kES.mElement[1][0] = fSumXY;
00763   kES.mElement[1][1] = fSumYY;
00764   kES.mElement[1][2] = fSumYZ;
00765 
00766   kES.mElement[2][0] = fSumXZ;
00767   kES.mElement[2][1] = fSumYZ;
00768   kES.mElement[2][2] = fSumZZ;
00769 
00770   // compute eigenstuff, smallest eigenvalue is in last position
00771   kES.DecrSortEigenStuff();
00772 
00773   SinglePrecision::Vec3 kNormal;
00774 
00775   kNormal.x = kES.mElement[0][2];
00776   kNormal.y = kES.mElement[1][2];
00777   kNormal.z = kES.mElement[2][2];
00778 
00779   // the minimum energy
00780   plane[0] = kNormal.x;
00781   plane[1] = kNormal.y;
00782   plane[2] = kNormal.z;
00783 
00784   plane[3] = 0 - kNormal.dot(kOrigin);
00785 
00786   return ret;
00787 }
00788 
00789 
00790 
00791 
00792 #define CREATE_MAIN 0
00793 
00794 #if CREATE_MAIN
00795 
00796 #include "bestfit.h"
00797 #include <stdio.h>
00798 
00799 void main(int argc,const char **argv)
00800 {
00801         float points[9] = { 0,0,0,   1,1,1, 0,0,1 };
00802         float plane[4];
00803         getBestFitPlane(3,points,sizeof(float)*3,0,0,plane);
00804         printf("Plane: %f %f %f %f\r\n", plane[0], plane[1], plane[2] );
00805 }
00806 #endif