vcglib: fitting3.h Source File

00001 /****************************************************************************
00002 * VCGLib                                                            o o     *
00003 * Visual and Computer Graphics Library                            o     o   *
00004 *                                                                _   O  _   *
00005 * Copyright(C) 2004                                                \/)\/    *
00006 * Visual Computing Lab                                            /\/|      *
00007 * ISTI - Italian National Research Council                           |      *
00008 *                                                                    \      *
00009 * All rights reserved.                                                      *
00010 *                                                                           *
00011 * This program is free software; you can redistribute it and/or modify      *   
00012 * it under the terms of the GNU General Public License as published by      *
00013 * the Free Software Foundation; either version 2 of the License, or         *
00014 * (at your option) any later version.                                       *
00015 *                                                                           *
00016 * This program is distributed in the hope that it will be useful,           *
00017 * but WITHOUT ANY WARRANTY; without even the implied warranty of            *
00018 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the             *
00019 * GNU General Public License (http://www.gnu.org/licenses/gpl.txt)          *
00020 * for more details.                                                         *
00021 *                                                                           *
00022 ****************************************************************************/
00023 /****************************************************************************
00024   History
00025 
00026 $Log: not supported by cvs2svn $
00027 Revision 1.2  2005/10/13 14:59:57  ganovelli
00028 versione con svd
00029 
00030 Revision 1.1  2005/03/14 17:04:24  ganovelli
00031 created
00032 
00033 
00034 ****************************************************************************/
00035 
00036 #ifndef __VCGLIB_FITTING3
00037 #define __VCGLIB_FITTING3
00038 
00039 #include <vector>
00040 #include <vcg/space/plane3.h>
00041 #include <vcg/math/matrix44.h>
00042 #include <vcg/math/matrix33.h>
00043 #include <vcg/math/lin_algebra.h>
00044 
00045 
00046 namespace vcg {
00047 
00048 template <class S>
00049 Point3<S> PlaneFittingPoints(const std::vector< Point3<S> > & samples, Plane3<S> & p, Point4<S> & eval, Matrix44<S> & evec)
00050 {
00051         Matrix44<S> m;m.SetZero();
00052         typename std::vector< Point3<S> >::const_iterator it;
00053 
00054         Point3<S> c; c.SetZero();
00055         for(it = samples.begin(); it != samples.end(); ++it)
00056         {
00057                 c += *it;
00058         }
00059         c /= samples.size();
00060 
00061         for(it = samples.begin(); it != samples.end(); ++it)
00062         {
00063                 Point3<S> p = (*it) - c;
00064                 for(int j = 0 ; j < 3;++j)
00065                         *(Point3<S>*)&m[j][0] += p * p[j];
00066         }
00067 
00068         m[0][3] = m[1][3] = m[2][3] = S(0);
00069         m[3][3] = S(1);
00070         m[3][0] = m[3][1] = m[3][2] = S(0);
00071 
00072         int n;
00073         Point3<S> d;
00074         Jacobi(m, eval, evec, n);
00075 
00076         //Sort eigenvalues (tarinisort)
00077         Point4<S> e;
00078         e[0] = S(fabs(eval[0]));
00079         e[1] = S(fabs(eval[1]));
00080         e[2] = S(fabs(eval[2]));
00081 
00082         int maxi, mini, medi;
00083         if (e[1]    > e[0]) { maxi = 1; mini = 0; } else { maxi = 0; mini = 1;}
00084         if (e[maxi] < e[2]) { maxi = 2;           } else if (e[mini] > e[2]) { mini = 2; };
00085         medi = 3 - maxi -mini;
00086 
00087         d[0] = evec[0][mini];
00088         d[1] = evec[1][mini];
00089         d[2] = evec[2][mini];
00090 
00091         const S norm = d.Norm();
00092         p.SetOffset(c.dot(d)/norm);
00093         p.SetDirection(d/norm);
00094 
00095         return Point3<S>(e[mini], e[medi], e[maxi]);
00096 }
00097 
00098 template <class S>
00099 Point3<S> PlaneFittingPoints(const std::vector< Point3<S> > & samples, Plane3<S> & p)
00100 {
00101         Point4<S>   eval;
00102         Matrix44<S> evec;
00103         return PlaneFittingPoints(samples, p, eval, evec);
00104 }
00105 
00106 template<class S>
00107 inline double FIT_VExp( const Point3<S> & x, const int i )
00108 {
00109         assert(i>=0);
00110         assert(i<4);
00111         if(i==0) return 1;
00112         else     return x[i-1];
00113 }
00114 
00118 template<class S>
00119 bool PlaneFittingPointsOld(  std::vector< Point3<S> > & samples, Plane3<S> & p )
00120 {
00121         Point3<S> d;
00122 
00123   const int N = 4;
00124         S P[N][N];              // A = s' . s
00125         S U[N][N];
00126         int i,j,k,n;
00127 
00128         n = (int)samples.size();
00129         if(n<3)
00130                 return false;
00131 
00132         //printf("\n p_prima: %f %f %f %f \n",p.Offset(),p.Direction()[0],p.Direction()[1],p.Direction()[2]);
00133 
00134         for(i=0;i<N;++i)
00135         {
00136                 for(j=i;j<N;++j)
00137                 {
00138                         P[i][j] = 0;
00139                         for(k=0;k<n;++k)
00140                                 P[i][j] += FIT_VExp(samples[k],i) * FIT_VExp(samples[k],j);
00141                 }
00142                 for(j=0;j<i;++j)
00143                         P[i][j] = P[j][i];
00144         }
00145 
00146         //printf("D \n");
00147         //for(i=0;i<N;++i){
00148         //      printf("\n");
00149  //             for(j=0;j<N;++j)
00150         //              printf("%2.3f\t",P[i][j]);
00151         //}
00152         //
00153         Matrix44<S> m;
00154         for(i=0;i<N;++i)
00155                 for(j=0;j<N;++j)
00156                         m[i][j]=P[i][j];
00157 
00158 
00159 //      Point4<S> s;s.SetZero();
00160 //
00161 //      s.Normalize();
00162 //      printf("\n RES %f %f %f %f \n",s[0],s[1],s[2],s[3]);
00163 //printf("\n GJ \n");
00164 //      for(i=0;i<N;++i){
00165 //              printf("\n");
00166 //              for(j=0;j<N;++j)
00167 //                      printf("%2.3f\t",m[i][j]);
00168 //      }
00169         for(i=0;i<N;++i)
00170         {
00171                 U[i][i] = 1.0;
00172                 for(j=0;j<i;++j)
00173                         U[i][j] = 0.0;
00174                 for(j=i+1;j<N;++j)
00175                 {
00176                         if(P[i][i]==0.0)
00177                                 return false;
00178 
00179                         U[i][j] = P[i][j]/P[i][i];
00180                         for(k=j;k<N;++k)
00181                                 P[j][k] -= U[i][j]*P[i][k];
00182                 }
00183         }
00184 
00185         //printf("\n U \n");
00186         //for(i=0;i<N;++i){
00187         //      printf("\n");
00188  //             for(j=0;j<N;++j)
00189         //              printf("%2.3f\t",U[i][j]);
00190         //}
00191 
00192         
00193         S norm = Point3<S>(U[1][2]*U[2][3]-U[1][3],-U[2][3],1).Norm();
00194 
00195         p.SetDirection(Point3<S>(U[1][2]*U[2][3]-U[1][3],-U[2][3],1));
00196         p.SetOffset(-(U[0][2]*U[2][3]-U[0][3]+U[0][1]*U[1][3]-U[0][1]*U[1][2]*U[2][3])/norm);
00197 
00198  
00199         //printf("\n p: %f %f %f %f \n",p.Offset(),p.Direction()[0],p.Direction()[1],p.Direction()[2]);
00200 
00201         return true;
00202 }
00203 } // end namespace
00204 
00205 #endif