GaussPointsFit.h Source File

00001 /************************************************************** ggt-head beg
00002  *
00003  * GGT: Generic Graphics Toolkit
00004  *
00005  * Original Authors:
00006  *   Allen Bierbaum
00007  *
00008  * -----------------------------------------------------------------
00009  * File:          $RCSfile: GaussPointsFit.h,v $
00010  * Date modified: $Date: 2002/01/31 01:16:22 $
00011  * Version:       $Revision: 1.3 $
00012  * -----------------------------------------------------------------
00013  *
00014  *********************************************************** ggt-head end */
00015 /*************************************************************** ggt-cpr beg
00016 *
00017 * GGT: The Generic Graphics Toolkit
00018 * Copyright (C) 2001,2002 Allen Bierbaum
00019 *
00020 * This library is free software; you can redistribute it and/or
00021 * modify it under the terms of the GNU Lesser General Public
00022 * License as published by the Free Software Foundation; either
00023 * version 2.1 of the License, or (at your option) any later version.
00024 *
00025 * This library is distributed in the hope that it will be useful,
00026 * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
00028 * Lesser General Public License for more details.
00029 *
00030 * You should have received a copy of the GNU Lesser General Public
00031 * License along with this library; if not, write to the Free Software
00032 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033 *
00034  ************************************************************ ggt-cpr end */
00035 // Based on code from:
00036 // Magic Software, Inc.
00037 // http://www.magic-software.com
00038 //
00039 #ifndef _GMTL_GAUSSPOINTSFIT_H
00040 #define _GMTL_GAUSSPOINTSFIT_H
00041 
00042 // Fit points with a Gaussian distribution.  The center is the mean of the
00043 // points, the axes are the eigenvectors of the covariance matrix, and the
00044 // extents are the eigenvalues of the covariance matrix and are returned in
00045 // increasing order.  The last two functions allow selection of valid
00046 // vertices from a pool.  The return value is 'true' if and only if at least
00047 // one vertex was valid.
00048 
00049 #include <gmtl/Vec3.h>
00050 #include <gmtl/Point3.h>
00051 #include <gmtl/Numerics/Eigen.h>
00052 
00053 namespace gmtl
00054 {
00055 
00056 /*
00057 void MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint,
00058     MgcVector2& rkCenter, MgcVector2 akAxis[2], MgcReal afExtent[2]);
00059 */
00060 
00061 void GaussPointsFit (int iQuantity, const Point3* akPoint,
00062     Point3& rkCenter, Vec3 akAxis[3], float afExtent[3]);
00063 
00064 /*
00065 bool MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint,
00066     const bool* abValid, MgcVector2& rkCenter, MgcVector2 akAxis[2],
00067     MgcReal afExtent[2]);
00068 */
00069 
00070 bool GaussPointsFit (int iQuantity, const Vec3* akPoint,
00071     const bool* abValid, Vec3& rkCenter, Vec3 akAxis[3],
00072     float afExtent[3]);
00073    
00074 
00075 // --- Implementations ---- //
00076 void GaussPointsFit (int iQuantity, const Point3* akPoint,
00077     Point3& rkCenter, Vec3 akAxis[3], float afExtent[3])
00078 {
00079     // compute mean of points
00080     rkCenter = akPoint[0];
00081     unsigned i;
00082     for (i = 1; i < iQuantity; i++)
00083         rkCenter += akPoint[i];
00084     float fInvQuantity = 1.0f/iQuantity;
00085     rkCenter *= fInvQuantity;
00086 
00087     // compute covariances of points
00088     float fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0;
00089     float fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0;
00090     for (i = 0; i < iQuantity; i++)
00091     {
00092         Vec3 kDiff = akPoint[i] - rkCenter;
00093         fSumXX += kDiff[Xelt]*kDiff[Xelt];
00094         fSumXY += kDiff[Xelt]*kDiff[Yelt];
00095         fSumXZ += kDiff[Xelt]*kDiff[Zelt];
00096         fSumYY += kDiff[Yelt]*kDiff[Yelt];
00097         fSumYZ += kDiff[Yelt]*kDiff[Zelt];
00098         fSumZZ += kDiff[Zelt]*kDiff[Zelt];
00099     }
00100     fSumXX *= fInvQuantity;
00101     fSumXY *= fInvQuantity;
00102     fSumXZ *= fInvQuantity;
00103     fSumYY *= fInvQuantity;
00104     fSumYZ *= fInvQuantity;
00105     fSumZZ *= fInvQuantity;
00106 
00107     // compute eigenvectors for covariance matrix
00108     gmtl::Eigen kES(3);
00109     kES.Matrix(0,0) = fSumXX;
00110     kES.Matrix(0,1) = fSumXY;
00111     kES.Matrix(0,2) = fSumXZ;
00112     kES.Matrix(1,0) = fSumXY;
00113     kES.Matrix(1,1) = fSumYY;
00114     kES.Matrix(1,2) = fSumYZ;
00115     kES.Matrix(2,0) = fSumXZ;
00116     kES.Matrix(2,1) = fSumYZ;
00117     kES.Matrix(2,2) = fSumZZ;
00118     kES.IncrSortEigenStuff3();
00119 
00120     akAxis[0][Xelt] = kES.GetEigenvector(0,0);
00121     akAxis[0][Yelt] = kES.GetEigenvector(1,0);
00122     akAxis[0][Zelt] = kES.GetEigenvector(2,0);
00123 
00124     akAxis[1][Xelt] = kES.GetEigenvector(0,1);
00125     akAxis[1][Yelt] = kES.GetEigenvector(1,1);
00126     akAxis[1][Zelt] = kES.GetEigenvector(2,1);
00127 
00128     akAxis[2][Xelt] = kES.GetEigenvector(0,2);
00129     akAxis[2][Yelt] = kES.GetEigenvector(1,2);
00130     akAxis[2][Zelt] = kES.GetEigenvector(2,2);
00131 
00132     afExtent[0] = kES.GetEigenvalue(0);
00133     afExtent[1] = kES.GetEigenvalue(1);
00134     afExtent[2] = kES.GetEigenvalue(2);
00135 }
00136 
00137 
00138 //
00139 bool GaussPointsFit (int iQuantity, const Vec3* akPoint,
00140     const bool* abValid, Vec3& rkCenter, Vec3 akAxis[3],
00141     float afExtent[3])
00142 {
00143     // compute mean of points
00144     rkCenter = ZeroVec3;
00145     int i, iValidQuantity = 0;
00146     for (i = 0; i < iQuantity; i++)
00147     {
00148         if ( abValid[i] )
00149         {
00150             rkCenter += akPoint[i];
00151             iValidQuantity++;
00152         }
00153     }
00154     if ( iValidQuantity == 0 )
00155         return false;
00156 
00157     float fInvQuantity = 1.0/iValidQuantity;
00158     rkCenter *= fInvQuantity;
00159 
00160     // compute covariances of points
00161     float fSumXX = 0.0, fSumXY = 0.0, fSumXZ = 0.0;
00162     float fSumYY = 0.0, fSumYZ = 0.0, fSumZZ = 0.0;
00163     for (i = 0; i < iQuantity; i++)
00164     {
00165         if ( abValid[i] )
00166         {
00167             Vec3 kDiff = akPoint[i] - rkCenter;
00168             fSumXX += kDiff[Xelt]*kDiff[Xelt];
00169             fSumXY += kDiff[Xelt]*kDiff[Yelt];
00170             fSumXZ += kDiff[Xelt]*kDiff[Zelt];
00171             fSumYY += kDiff[Yelt]*kDiff[Yelt];
00172             fSumYZ += kDiff[Yelt]*kDiff[Zelt];
00173             fSumZZ += kDiff[Zelt]*kDiff[Zelt];
00174         }
00175     }
00176     fSumXX *= fInvQuantity;
00177     fSumXY *= fInvQuantity;
00178     fSumXZ *= fInvQuantity;
00179     fSumYY *= fInvQuantity;
00180     fSumYZ *= fInvQuantity;
00181     fSumZZ *= fInvQuantity;
00182 
00183     // compute eigenvectors for covariance matrix
00184     Eigen kES(3);
00185     kES.Matrix(0,0) = fSumXX;
00186     kES.Matrix(0,1) = fSumXY;
00187     kES.Matrix(0,2) = fSumXZ;
00188     kES.Matrix(1,0) = fSumXY;
00189     kES.Matrix(1,1) = fSumYY;
00190     kES.Matrix(1,2) = fSumYZ;
00191     kES.Matrix(2,0) = fSumXZ;
00192     kES.Matrix(2,1) = fSumYZ;
00193     kES.Matrix(2,2) = fSumZZ;
00194     kES.IncrSortEigenStuff3();
00195 
00196     akAxis[0][Xelt] = kES.GetEigenvector(0,0);
00197     akAxis[0][Yelt] = kES.GetEigenvector(1,0);
00198     akAxis[0][Zelt] = kES.GetEigenvector(2,0);
00199 
00200     akAxis[1][Xelt] = kES.GetEigenvector(0,1);
00201     akAxis[1][Yelt] = kES.GetEigenvector(1,1);
00202     akAxis[1][Zelt] = kES.GetEigenvector(2,1);
00203 
00204     akAxis[2][Xelt] = kES.GetEigenvector(0,2);
00205     akAxis[2][Yelt] = kES.GetEigenvector(1,2);
00206     akAxis[2][Zelt] = kES.GetEigenvector(2,2);
00207 
00208     afExtent[0] = kES.GetEigenvalue(0);
00209     afExtent[1] = kES.GetEigenvalue(1);
00210     afExtent[2] = kES.GetEigenvalue(2);
00211 
00212     return true;
00213 }
00214 
00215 };
00216 
00217 /*
00218 void MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint,
00219     MgcVector2& rkCenter, MgcVector2 akAxis[2], float afExtent[2])
00220 {
00221     // compute mean of points
00222     rkCenter = akPoint[0];
00223     int i;
00224     for (i = 1; i < iQuantity; i++)
00225         rkCenter += akPoint[i];
00226     float fInvQuantity = 1.0/iQuantity;
00227     rkCenter *= fInvQuantity;
00228 
00229     // compute covariances of points
00230     float fSumXX = 0.0, fSumXY = 0.0, fSumYY = 0.0;
00231     for (i = 0; i < iQuantity; i++)
00232     {
00233         MgcVector2 kDiff = akPoint[i] - rkCenter;
00234         fSumXX += kDiff.x*kDiff.x;
00235         fSumXY += kDiff.x*kDiff.y;
00236         fSumYY += kDiff.y*kDiff.y;
00237     }
00238     fSumXX *= fInvQuantity;
00239     fSumXY *= fInvQuantity;
00240     fSumYY *= fInvQuantity;
00241 
00242     // solve eigensystem of covariance matrix
00243     MgcEigen kES(2);
00244     kES.Matrix(0,0) = fSumXX;
00245     kES.Matrix(0,1) = fSumXY;
00246     kES.Matrix(1,0) = fSumXY;
00247     kES.Matrix(1,1) = fSumYY;
00248     kES.IncrSortEigenStuff2();
00249 
00250     akAxis[0].x = kES.GetEigenvector(0,0);
00251     akAxis[0].y = kES.GetEigenvector(1,0);
00252     akAxis[1].x = kES.GetEigenvector(0,1);
00253     akAxis[1].y = kES.GetEigenvector(1,1);
00254 
00255     afExtent[0] = kES.GetEigenvalue(0);
00256     afExtent[1] = kES.GetEigenvalue(1);
00257 }
00258 */
00259 
00260 
00261 /*
00262 bool MgcGaussPointsFit (int iQuantity, const MgcVector2* akPoint,
00263     const bool* abValid, MgcVector2& rkCenter, MgcVector2 akAxis[2],
00264     float afExtent[2])
00265 {
00266     // compute mean of points
00267     rkCenter = MgcVector2::ZERO;
00268     int i, iValidQuantity = 0;
00269     for (i = 0; i < iQuantity; i++)
00270     {
00271         if ( abValid[i] )
00272         {
00273             rkCenter += akPoint[i];
00274             iValidQuantity++;
00275         }
00276     }
00277     if ( iValidQuantity == 0 )
00278         return false;
00279 
00280     float fInvQuantity = 1.0/iValidQuantity;
00281     rkCenter *= fInvQuantity;
00282 
00283     // compute covariances of points
00284     float fSumXX = 0.0, fSumXY = 0.0, fSumYY = 0.0;
00285     for (i = 0; i < iQuantity; i++)
00286     {
00287         if ( abValid[i] )
00288         {
00289             MgcVector2 kDiff = akPoint[i] - rkCenter;
00290             fSumXX += kDiff.x*kDiff.x;
00291             fSumXY += kDiff.x*kDiff.y;
00292             fSumYY += kDiff.y*kDiff.y;
00293         }
00294     }
00295     fSumXX *= fInvQuantity;
00296     fSumXY *= fInvQuantity;
00297     fSumYY *= fInvQuantity;
00298 
00299     // solve eigensystem of covariance matrix
00300     MgcEigen kES(2);
00301     kES.Matrix(0,0) = fSumXX;
00302     kES.Matrix(0,1) = fSumXY;
00303     kES.Matrix(1,0) = fSumXY;
00304     kES.Matrix(1,1) = fSumYY;
00305     kES.IncrSortEigenStuff2();
00306 
00307     akAxis[0].x = kES.GetEigenvector(0,0);
00308     akAxis[0].y = kES.GetEigenvector(1,0);
00309     akAxis[1].x = kES.GetEigenvector(0,1);
00310     akAxis[1].y = kES.GetEigenvector(1,1);
00311 
00312     afExtent[0] = kES.GetEigenvalue(0);
00313     afExtent[1] = kES.GetEigenvalue(1);
00314 
00315     return true;
00316 }
00317 */
00318 
00319 #endif