GenericMathTemplateLibrary: GaussPointsFit.h Source File

Go to the documentation of this file.
00001 // GMTL is (C) Copyright 2001-2010 by Allen Bierbaum
00002 // Distributed under the GNU Lesser General Public License 2.1 with an
00003 // addendum covering inlined code. (See accompanying files LICENSE and
00004 // LICENSE.addendum or http://www.gnu.org/copyleft/lesser.txt)
00005 
00006 // Based on code from:
00007 // Magic Software, Inc.
00008 // http://www.magic-software.com
00009 //
00010 #ifndef _GMTL_GAUSSPOINTSFIT_H
00011 #define _GMTL_GAUSSPOINTSFIT_H
00012 
00013 // Fit points with a Gaussian distribution.  The center is the mean of the
00014 // points, the axes are the eigenvectors of the covariance matrix, and the
00015 // extents are the eigenvalues of the covariance matrix and are returned in
00016 // increasing order.  The last two functions allow selection of valid
00017 // vertices from a pool.  The return value is 'true' if and only if at least
00018 // one vertex was valid.
00019 
00020 #include <gmtl/Vec3.h>
00021 #include <gmtl/Point3.h>
00022 #include <gmtl/Numerics/Eigen.h>
00023 
00024 namespace gmtl
00025 {
00026 
00027 /*
00028 void MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint,
00029     MgcVector2& rkCenter, MgcVector2 akAxis[2], MgcReal afExtent[2]);
00030 */
00031 
00032 void GaussPointsFit (int iQuantity, const Point3* akPoint,
00033     Point3& rkCenter, Vec3 akAxis[3], float afExtent[3]);
00034 
00035 /*
00036 bool MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint,
00037     const bool* abValid, MgcVector2& rkCenter, MgcVector2 akAxis[2],
00038     MgcReal afExtent[2]);
00039 */
00040 
00041 bool GaussPointsFit (int iQuantity, const Vec3* akPoint,
00042     const bool* abValid, Vec3& rkCenter, Vec3 akAxis[3],
00043     float afExtent[3]);
00044    
00045 
00046 // --- Implementations ---- //
00047 void GaussPointsFit (int iQuantity, const Point3* akPoint,
00048     Point3& rkCenter, Vec3 akAxis[3], float afExtent[3])
00049 {
00050     // compute mean of points
00051     rkCenter = akPoint[0];
00052     unsigned i;
00053     for (i = 1; i < iQuantity; i++)
00054         rkCenter += akPoint[i];
00055     float fInvQuantity = 1.0f/iQuantity;
00056     rkCenter *= fInvQuantity;
00057 
00058     // compute covariances of points
00059     float fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0;
00060     float fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0;
00061     for (i = 0; i < iQuantity; i++)
00062     {
00063         Vec3 kDiff = akPoint[i] - rkCenter;
00064         fSumXX += kDiff[Xelt]*kDiff[Xelt];
00065         fSumXY += kDiff[Xelt]*kDiff[Yelt];
00066         fSumXZ += kDiff[Xelt]*kDiff[Zelt];
00067         fSumYY += kDiff[Yelt]*kDiff[Yelt];
00068         fSumYZ += kDiff[Yelt]*kDiff[Zelt];
00069         fSumZZ += kDiff[Zelt]*kDiff[Zelt];
00070     }
00071     fSumXX *= fInvQuantity;
00072     fSumXY *= fInvQuantity;
00073     fSumXZ *= fInvQuantity;
00074     fSumYY *= fInvQuantity;
00075     fSumYZ *= fInvQuantity;
00076     fSumZZ *= fInvQuantity;
00077 
00078     // compute eigenvectors for covariance matrix
00079     gmtl::Eigen kES(3);
00080     kES.Matrix(0,0) = fSumXX;
00081     kES.Matrix(0,1) = fSumXY;
00082     kES.Matrix(0,2) = fSumXZ;
00083     kES.Matrix(1,0) = fSumXY;
00084     kES.Matrix(1,1) = fSumYY;
00085     kES.Matrix(1,2) = fSumYZ;
00086     kES.Matrix(2,0) = fSumXZ;
00087     kES.Matrix(2,1) = fSumYZ;
00088     kES.Matrix(2,2) = fSumZZ;
00089     kES.IncrSortEigenStuff3();
00090 
00091     akAxis[0][Xelt] = kES.GetEigenvector(0,0);
00092     akAxis[0][Yelt] = kES.GetEigenvector(1,0);
00093     akAxis[0][Zelt] = kES.GetEigenvector(2,0);
00094 
00095     akAxis[1][Xelt] = kES.GetEigenvector(0,1);
00096     akAxis[1][Yelt] = kES.GetEigenvector(1,1);
00097     akAxis[1][Zelt] = kES.GetEigenvector(2,1);
00098 
00099     akAxis[2][Xelt] = kES.GetEigenvector(0,2);
00100     akAxis[2][Yelt] = kES.GetEigenvector(1,2);
00101     akAxis[2][Zelt] = kES.GetEigenvector(2,2);
00102 
00103     afExtent[0] = kES.GetEigenvalue(0);
00104     afExtent[1] = kES.GetEigenvalue(1);
00105     afExtent[2] = kES.GetEigenvalue(2);
00106 }
00107 
00108 
00109 //
00110 bool GaussPointsFit (int iQuantity, const Vec3* akPoint,
00111     const bool* abValid, Vec3& rkCenter, Vec3 akAxis[3],
00112     float afExtent[3])
00113 {
00114     // compute mean of points
00115     rkCenter = ZeroVec3;
00116     int i, iValidQuantity = 0;
00117     for (i = 0; i < iQuantity; i++)
00118     {
00119         if ( abValid[i] )
00120         {
00121             rkCenter += akPoint[i];
00122             iValidQuantity++;
00123         }
00124     }
00125     if ( iValidQuantity == 0 )
00126         return false;
00127 
00128     float fInvQuantity = 1.0/iValidQuantity;
00129     rkCenter *= fInvQuantity;
00130 
00131     // compute covariances of points
00132     float fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0;
00133     float fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0;
00134     for (i = 0; i < iQuantity; i++)
00135     {
00136         if ( abValid[i] )
00137         {
00138             Vec3 kDiff = akPoint[i] - rkCenter;
00139             fSumXX += kDiff[Xelt]*kDiff[Xelt];
00140             fSumXY += kDiff[Xelt]*kDiff[Yelt];
00141             fSumXZ += kDiff[Xelt]*kDiff[Zelt];
00142             fSumYY += kDiff[Yelt]*kDiff[Yelt];
00143             fSumYZ += kDiff[Yelt]*kDiff[Zelt];
00144             fSumZZ += kDiff[Zelt]*kDiff[Zelt];
00145         }
00146     }
00147     fSumXX *= fInvQuantity;
00148     fSumXY *= fInvQuantity;
00149     fSumXZ *= fInvQuantity;
00150     fSumYY *= fInvQuantity;
00151     fSumYZ *= fInvQuantity;
00152     fSumZZ *= fInvQuantity;
00153 
00154     // compute eigenvectors for covariance matrix
00155     Eigen kES(3);
00156     kES.Matrix(0,0) = fSumXX;
00157     kES.Matrix(0,1) = fSumXY;
00158     kES.Matrix(0,2) = fSumXZ;
00159     kES.Matrix(1,0) = fSumXY;
00160     kES.Matrix(1,1) = fSumYY;
00161     kES.Matrix(1,2) = fSumYZ;
00162     kES.Matrix(2,0) = fSumXZ;
00163     kES.Matrix(2,1) = fSumYZ;
00164     kES.Matrix(2,2) = fSumZZ;
00165     kES.IncrSortEigenStuff3();
00166 
00167     akAxis[0][Xelt] = kES.GetEigenvector(0,0);
00168     akAxis[0][Yelt] = kES.GetEigenvector(1,0);
00169     akAxis[0][Zelt] = kES.GetEigenvector(2,0);
00170 
00171     akAxis[1][Xelt] = kES.GetEigenvector(0,1);
00172     akAxis[1][Yelt] = kES.GetEigenvector(1,1);
00173     akAxis[1][Zelt] = kES.GetEigenvector(2,1);
00174 
00175     akAxis[2][Xelt] = kES.GetEigenvector(0,2);
00176     akAxis[2][Yelt] = kES.GetEigenvector(1,2);
00177     akAxis[2][Zelt] = kES.GetEigenvector(2,2);
00178 
00179     afExtent[0] = kES.GetEigenvalue(0);
00180     afExtent[1] = kES.GetEigenvalue(1);
00181     afExtent[2] = kES.GetEigenvalue(2);
00182 
00183     return true;
00184 }
00185 
00186 };
00187 
00188 /*
00189 void MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint,
00190     MgcVector2& rkCenter, MgcVector2 akAxis[2], float afExtent[2])
00191 {
00192     // compute mean of points
00193     rkCenter = akPoint[0];
00194     int i;
00195     for (i = 1; i < iQuantity; i++)
00196         rkCenter += akPoint[i];
00197     float fInvQuantity = 1.0/iQuantity;
00198     rkCenter *= fInvQuantity;
00199 
00200     // compute covariances of points
00201     float fSumXX = 0.0, fSumXY = 0.0, fSumYY = 0.0;
00202     for (i = 0; i < iQuantity; i++)
00203     {
00204         MgcVector2 kDiff = akPoint[i] - rkCenter;
00205         fSumXX += kDiff.x*kDiff.x;
00206         fSumXY += kDiff.x*kDiff.y;
00207         fSumYY += kDiff.y*kDiff.y;
00208     }
00209     fSumXX *= fInvQuantity;
00210     fSumXY *= fInvQuantity;
00211     fSumYY *= fInvQuantity;
00212 
00213     // solve eigensystem of covariance matrix
00214     MgcEigen kES(2);
00215     kES.Matrix(0,0) = fSumXX;
00216     kES.Matrix(0,1) = fSumXY;
00217     kES.Matrix(1,0) = fSumXY;
00218     kES.Matrix(1,1) = fSumYY;
00219     kES.IncrSortEigenStuff2();
00220 
00221     akAxis[0].x = kES.GetEigenvector(0,0);
00222     akAxis[0].y = kES.GetEigenvector(1,0);
00223     akAxis[1].x = kES.GetEigenvector(0,1);
00224     akAxis[1].y = kES.GetEigenvector(1,1);
00225 
00226     afExtent[0] = kES.GetEigenvalue(0);
00227     afExtent[1] = kES.GetEigenvalue(1);
00228 }
00229 */
00230 
00231 
00232 /*
00233 bool MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint,
00234     const bool* abValid, MgcVector2& rkCenter, MgcVector2 akAxis[2],
00235     float afExtent[2])
00236 {
00237     // compute mean of points
00238     rkCenter = MgcVector2::ZERO;
00239     int i, iValidQuantity = 0;
00240     for (i = 0; i < iQuantity; i++)
00241     {
00242         if ( abValid[i] )
00243         {
00244             rkCenter += akPoint[i];
00245             iValidQuantity++;
00246         }
00247     }
00248     if ( iValidQuantity == 0 )
00249         return false;
00250 
00251     float fInvQuantity = 1.0/iValidQuantity;
00252     rkCenter *= fInvQuantity;
00253 
00254     // compute covariances of points
00255     float fSumXX = 0.0, fSumXY = 0.0, fSumYY = 0.0;
00256     for (i = 0; i < iQuantity; i++)
00257     {
00258         if ( abValid[i] )
00259         {
00260             MgcVector2 kDiff = akPoint[i] - rkCenter;
00261             fSumXX += kDiff.x*kDiff.x;
00262             fSumXY += kDiff.x*kDiff.y;
00263             fSumYY += kDiff.y*kDiff.y;
00264         }
00265     }
00266     fSumXX *= fInvQuantity;
00267     fSumXY *= fInvQuantity;
00268     fSumYY *= fInvQuantity;
00269 
00270     // solve eigensystem of covariance matrix
00271     MgcEigen kES(2);
00272     kES.Matrix(0,0) = fSumXX;
00273     kES.Matrix(0,1) = fSumXY;
00274     kES.Matrix(1,0) = fSumXY;
00275     kES.Matrix(1,1) = fSumYY;
00276     kES.IncrSortEigenStuff2();
00277 
00278     akAxis[0].x = kES.GetEigenvector(0,0);
00279     akAxis[0].y = kES.GetEigenvector(1,0);
00280     akAxis[1].x = kES.GetEigenvector(0,1);
00281     akAxis[1].y = kES.GetEigenvector(1,1);
00282 
00283     afExtent[0] = kES.GetEigenvalue(0);
00284     afExtent[1] = kES.GetEigenvalue(1);
00285 
00286     return true;
00287 }
00288 */
00289 
00290 #endif