00001 
00002 
00003 
00004 
00005 
00006 
00007 
00008 
00009 
00010 
00011 
00012 
00013 
00014 
00015 
00016 
00017 
00018 
00019 
00020 
00021 
00022 
00023 
00024 
00025 
00026 
00027 
00028 
00029 
00030 
00031 
00032 
00033 
00034 
00035 
00036 
00037 
00038 
00039 
00040 #ifndef IMAGE_SPRING_H_DEFINED
00041 #define IMAGE_SPRING_H_DEFINED
00042 
00043 #include "Image/Image.H"
00044 #include "Util/Assert.H"
00045 
00046 template <class T> class PixRGB;
00047 
00048 
00049 
00050 
00051 
00052 template<class T>
00053 class ImageSpring : public Image<T>
00054 {
00055 public:
00056 
00057   inline ImageSpring(const int width, const int height,
00058                      const int nbNeighbors);
00059 
00060 
00061 
00062   inline ImageSpring();
00063 
00064 
00065   inline ImageSpring(const ImageSpring<T>& A);
00066 
00067 
00068   inline void init(const int width, const int height,
00069                    const int nbNeighbors);
00070 
00071 
00072   inline void init(const T* a, const float* posX, const float* posY,
00073                    const float** stiff,
00074                    const int width, const int height,
00075                    const int nbNeighbors);
00076 
00077 
00078   inline ~ImageSpring();
00079 
00080 
00081   inline void freeMem();
00082 
00083 
00084   void initClustering(bool initPosMasses);
00085 
00086 
00087 
00088   void computeStiff(void);
00089 
00090 
00091 
00092   void computePos(const float dt);
00093 
00094 
00095 
00096 
00097   void getStats(void);
00098 
00099 
00100 
00101 
00102 
00103   void getStatsDist(void);
00104 
00105 
00106   inline void getXY(const int index, Point2D<int> &pt) const;
00107 
00108 
00109   inline void getIndex(const Point2D<int> point, int &index) const;
00110 
00111 
00112   inline int getNbNeighbors(void) const;
00113 
00114 
00115   float getDistanceMasses(const int index1, const int index2) const;
00116 
00117 
00118 
00119   inline bool getNeighbor(const int index, const int n,
00120                           int& indexNeighbor) const;
00121 
00122 
00123   void getPositions(Image< PixRGB<byte> >& img, const int zoom = 16);
00124 
00125 
00126 
00127   void getClusteredImage(const Image< PixRGB<byte> > &scene,
00128                          Image< PixRGB<byte> > &clusteredImage,
00129                          Point2D<int> &supposedTrackCentroid,
00130                          const Point2D<int>& previousTrackCentroid);
00131 
00132 private:
00133 
00134 
00135 
00136 
00137 
00138 
00139 
00140   void goGraph(Image<int32> &marked, const int currentIndex,
00141                const int32 color, const int begin = 3);
00142 
00143 
00144 
00145   int nbNeighbors;
00146 
00147 
00148 
00149   T mean, stdev;
00150 
00151 
00152 
00153 
00154   double meanDist, stdevDist;
00155 
00156 
00157 
00158 
00159 
00160   T weight;
00161 
00162 
00163 
00164   float *posX, *posY;
00165 
00166 
00167   float *oldPosX, *oldPosY;
00168 
00169 
00170 
00171 
00172   float **stiff;
00173 
00174 
00175   void computeWeight(void);
00176 
00177 
00178   inline void setPos(const Point2D<int> mass, const float x, const float y);
00179 
00180 
00181   inline void setStiff(const Point2D<int> mas, int neighboor, float stiff);
00182 
00183 
00184   const static int nbHeuristic = 500;
00185 };
00186 
00187 
00188 
00189 
00190 
00191 
00192 
00193 
00194 
00195 
00196 template<class T> inline
00197 ImageSpring<T>::ImageSpring(const int width, const int height,
00198                             const int nbN) :
00199   Image<T>(width, height, NO_INIT)
00200 {
00201   init(width, height, nbN);
00202 }
00203 
00204 
00205 template<class T> inline
00206 ImageSpring<T>::ImageSpring() : Image<T>()
00207 { posX = NULL; posY = NULL; oldPosX = NULL; oldPosY = NULL; stiff = NULL; }
00208 
00209 
00210 template<class T> inline
00211 ImageSpring<T>::ImageSpring(const ImageSpring<T>& A) : Image<T>(A)
00212 {
00213   ASSERT(A.initialized());
00214   posX = NULL; posY = NULL; oldPosX = NULL; oldPosY = NULL; stiff = NULL;
00215 
00216   init(A.getArrayPtr(), posX, posY, const_cast<const float**>(A.stiff),
00217        A.getWidth(), A.getHeight(), A.nbNeighbors);
00218 }
00219 
00220 
00221 template<class T> inline
00222 ImageSpring<T>::~ImageSpring()
00223 { freeMem(); }
00224 
00225 
00226 template<class T> inline
00227 void ImageSpring<T>::freeMem()
00228 {
00229   if (stiff) {
00230     for (int index = 0;index < this->getSize(); index++) delete [] stiff[index];
00231     delete [] stiff; stiff = NULL;
00232   }
00233   if (posX) { delete [] posX; posX = NULL; }
00234   if (posY) { delete [] posY; posY = NULL; }
00235   if (oldPosX) { delete [] oldPosX; oldPosX = NULL; }
00236   if (oldPosY) { delete [] oldPosY; oldPosY = NULL; }
00237   Image<T>::freeMem();
00238 }
00239 
00240 
00241 template<class T> inline
00242 void ImageSpring<T>::init(const T* inarray,
00243                           const float* inPosX, const float* inPosY,
00244                           const float** inStiff,
00245                           const int width, const int height,
00246                           const int nbN)
00247 {
00248   freeMem();  
00249   this->resize(width, height);  
00250   nbNeighbors = nbN; int size = this->getSize();
00251 
00252   
00253   posX = new float[size]; memcpy(posX, inPosX, size * sizeof(float));
00254   posY = new float[size]; memcpy(posY, inPosY, size * sizeof(float));
00255   oldPosX = new float[size]; memcpy(oldPosX, inPosX, size * sizeof(float));
00256   oldPosY = new float[size]; memcpy(oldPosY, inPosY, size * sizeof(float));
00257 
00258   
00259   typedef float* floatptr;
00260   stiff = new floatptr[size];
00261   for (int index = 0; index < size; index++)
00262     {
00263       stiff[index] = new float[nbN + 1];
00264       memcpy(stiff[index], inStiff[index], (nbN + 1) * sizeof(float));
00265     }
00266 }
00267 
00268 
00269 template<class T> inline
00270 void ImageSpring<T>::init(const int width, const int height,
00271                           const int nbN)
00272 {
00273   freeMem();  
00274   this->resize(width, height);  
00275   nbNeighbors = nbN; int size = this->getSize();
00276 
00277   
00278   posX = new float[size]; posY = new float[size];
00279   oldPosX = new float[size]; oldPosY = new float[size];
00280 
00281   typedef float* floatptr;
00282   stiff = new floatptr[size];
00283   for (int index = 0; index < size; index++) stiff[index] = new float[nbN + 1];
00284 }
00285 
00286 
00287 template<class T> inline
00288 void ImageSpring<T>::getXY( const int index, Point2D<int> &pt ) const
00289 { pt.i = index % this->getWidth(); pt.j = index / this->getWidth(); }
00290 
00291 
00292 template<class T> inline
00293 void ImageSpring<T>::getIndex( const Point2D<int> point, int &index ) const
00294 { index = point.i + point.j * this->getWidth(); }
00295 
00296 
00297 template<class T> inline
00298 bool ImageSpring<T>::getNeighbor( const int index, const int n,
00299                                   int& indexNeighbor ) const
00300 {
00301   ASSERT(n > 0 && n <= nbNeighbors);
00302 
00303   Point2D<int> pt; getXY(index, pt);
00304 
00305   switch(n)
00306     {
00307     case 0 : break;   
00308 
00309       
00310     case 1 : pt.i--;         break;
00311     case 2 :         pt.j--; break;
00312     case 3 : pt.i++;         break;
00313     case 4 :         pt.j++; break;
00314 
00315       
00316     case 5 : pt.i--; pt.j--; break;
00317     case 6 : pt.i++; pt.j--; break;
00318     case 7 : pt.i--; pt.j++; break;
00319     case 8 : pt.i++; pt.j++; break;
00320 
00321       
00322     case 9 : pt.i-=2; pt.j+=2; break;
00323     case 10: pt.i-=1; pt.j+=2; break;
00324     case 11:          pt.j+=2; break;
00325     case 12: pt.i+=1; pt.j+=2; break;
00326     case 13: pt.i+=2; pt.j+=2; break;
00327     case 14: pt.i+=2; pt.j+=1; break;
00328     case 15: pt.i+=2;          break;
00329     case 16: pt.i+=2; pt.j-=1; break;
00330     case 17: pt.i+=2; pt.j-=2; break;
00331     case 18: pt.i+=1; pt.j-=2; break;
00332     case 19:          pt.j-=2; break;
00333     case 20: pt.i-=1; pt.j-=2; break;
00334     case 21: pt.i-=2; pt.j-=2; break;
00335     case 22: pt.i-=2; pt.j-=1; break;
00336     case 23: pt.i-=2;          break;
00337     case 24: pt.i-=2; pt.j+=1; break;
00338 
00339       
00340     default:
00341       LFATAL("Neighbor out or range");
00342     }
00343 
00344   getIndex( pt, indexNeighbor );
00345 
00346   return this->coordsOk( pt );
00347 }
00348 
00349 
00350 template<class T> inline
00351 int ImageSpring<T>::getNbNeighbors( void ) const
00352 { return nbNeighbors; }
00353 
00354 
00355 template<class T> inline
00356 void ImageSpring<T>::setPos(const Point2D<int> mass, const float x, const float y)
00357 {
00358   ASSERT(this->initialized() && this->coordsOk(mass));
00359   int index; getIndex(mass, index);
00360   posX[index] = x; posY[index] = y;
00361 }
00362 
00363 
00364 template<class T>
00365 void ImageSpring<T>::setStiff(const Point2D<int> mass, int neighbor, float stif)
00366 {
00367   ASSERT(this->initialized() && this->coordsOk(mass) && neighbor <= nbNeighbors);
00368   int index; getIndex(mass, index);
00369   stiff[index][neighbor] = stif;
00370 }
00371 
00372 
00373 
00374 
00375 
00376 
00377 
00378 
00379 
00380 #endif