fgbgsegment.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2011, Mårten Björkman (celle@csc.kth.se) 
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are
00007  * met:
00008  *
00009  *  1.Redistributions of source code must retain the above copyright
00010  *    notice, this list of conditions and the following disclaimer.
00011  *  2.Redistributions in binary form must reproduce the above
00012  *    copyright notice, this list of conditions and the following
00013  *    disclaimer in the documentation and/or other materials provided
00014  *    with the distribution.  
00015  *  3.The name of Mårten Björkman may not be used to endorse or
00016  *    promote products derived from this software without specific
00017  *    prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00020  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00021  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
00022  * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
00023  * HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
00024  * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
00025  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
00026  * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
00027  * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00028  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
00029  * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00030  */
00031 
00032 #undef USE_TBB
00033 
00034 #include <iostream>
00035 #include <cstdlib>
00036 #include <cstdio>
00037 #include <vector>
00038 #ifdef USE_TBB
00039 #include <tbb/tbb.h>
00040 #endif // USE_TBB
00041 //#include <boost/thread/thread.hpp>
00042 #include "pyra/tpimageutil.h"
00043 
00044 #include "belief.h"
00045 #include "largest.h"
00046 typedef unsigned int uint;
00047 #include "cudasegment.h" 
00048 #include "fgbgsegment.h"
00049 #include "timercpu.h"
00050 #include <ros/assert.h>
00051 
00052 FgBgSegment::FgBgSegment(int w, int h, int r, float b, 
00053                          float w_size, float b_size) 
00054   :  withSurface(true)
00055   ,  withColors(true)
00056   ,  withDisparities(true)
00057   ,  withColorHoles(true)
00058   ,  uniform(false)
00059   ,  verbose(0)
00060   ,  ground(*this, w, h)
00061   ,  surface(*this, w, h)
00062   ,  hue(w, h)
00063   ,  saturation(w, h)
00064   ,  grey(w, h)
00065   ,  width(w)
00066   ,  height(h)
00067   ,  drange(r)
00068   ,  gradWeight(b)
00069   ,  windowSize(w_size)
00070   ,  ballSize(b_size)
00071   ,  gpuopt(true)
00072 { 
00073 #ifdef USE_CUDA
00074   cudaSegment = new CudaSegment(w, h);
00075 #endif // USE_CUDA  
00076 }
00077 
00078 FgBgSegment::~FgBgSegment()
00079 {
00080   for (unsigned int i=0;i<figures.size();i++)
00081     delete figures[i];
00082 #ifdef USE_CUDA
00083   delete cudaSegment;
00084 #endif // USE_CUDA  
00085 }
00086 
00087 void FgBgSegment::UseGPU(bool gpuopt_)
00088 {
00089   gpuopt = gpuopt_;
00090 }
00091 
00092 FgBgSegment::ColorModel::ColorModel(FgBgSegment &segm_) : 
00093   segm(segm_), prior(NULL)
00094 {
00095   for (int i=0;i<hist_size*hist_size;i++) 
00096     histogram[i] = 0.0f; 
00097 }
00098 
00099 FgBgSegment::ColorModel::ColorModel(const ColorModel &model) : 
00100   segm(model.segm), prior(NULL)
00101 {
00102   // std::cout << "Copy colors" << std::endl;
00103   for (int i=0;i<hist_size*hist_size;i++) {
00104     histogram[i] = model.histogram[i]; 
00105     colorcost[i] = model.colorcost[i];
00106   }
00107 }
00108 
00109 FgBgSegment::ColorModel &FgBgSegment::ColorModel::operator=(const ColorModel &model)
00110 {
00111   // std::cout << "Copy colors" << std::endl;
00112   for (int i=0;i<hist_size*hist_size;i++) {
00113     histogram[i] = model.histogram[i]; 
00114     colorcost[i] = model.colorcost[i];
00115   }
00116 }
00117 
00118 FgBgSegment::ColorModel::~ColorModel()
00119 {
00120 
00121 }
00122 
00123 FgBgSegment::FlatSurface::FlatSurface(FgBgSegment &segm, 
00124                                       int width, int height) 
00125   : ColorModel(segm)
00126   , alpha(0.0f)
00127   , beta(0.0f)
00128   , disp(1000.0f)
00129   , spread_d(1.0f)
00130   , probabilities(width, height)
00131 {
00132   const int numFigures = segm.figures.size();
00133   Fill(probabilities, 1.0f/((float)numFigures+2.0f));
00134   //  Fill(probabilities, 1.0f/3.0f);
00135 }
00136 
00137 FgBgSegment::Foreground::Foreground(FgBgSegment &segm, 
00138                                     int width, int height) 
00139   : ColorModel(segm)
00140   , window_size(0.20f)
00141   , ball_size(0.20f)
00142   , combined(false)
00143   , probabilities(width, height)
00144 {
00145 
00146   const int numFigures = segm.figures.size();
00147   Fill(probabilities, 1.0f/((float)numFigures+2.0f));
00148   //  Fill(probabilities, 1.0f/3.0f);
00149 }
00150 
00151 FgBgSegment::Background::Background(FgBgSegment &segm, 
00152                                     int width, int height) 
00153   : ColorModel(segm)
00154   , spread_d(16.0f)
00155   , probabilities(width, height)
00156 {
00157   const int numFigures = segm.figures.size();
00158   Fill(probabilities, 1.0f/((float)numFigures+2.0f));
00159   //  Fill(probabilities, 1.0f/3.0f);
00160 }
00161 
00162 //=================== Initialization routines =====================//
00163 
00164 void FgBgSegment::FlatSurface::Initialize()
00165 {
00166   float *dimd = segm.disparities->GetData();
00167   int drange = segm.drange;
00168   int width = segm.width;
00169   int height = segm.height;
00170 
00171   // reset plane limits
00172   min_x = width;
00173   max_x = 0;
00174   min_y = height;
00175   max_y = 0;
00176 
00177   float minbeta = 0.08;
00178   // Find dominating disparity for each y-value
00179   int *hist = new int[height*drange];
00180   int *totals = new int[height];
00181   for (int y=0;y<height;y++) {
00182     int *histy = &hist[y*drange];
00183     for (int i=0;i<drange;i++) 
00184       histy[i] = 0;
00185     for (int x=0;x<width;x++) {
00186       int d = (int)dimd[y*width + x];
00187       if (d>=0 && d<drange)
00188         histy[d] ++;
00189     }
00190     for (int i=0;i<drange-2;i++)
00191       histy[i] = histy[i] + 2*histy[i+1] + histy[i+2];
00192     for (int i=drange-1;i>1;i--)
00193       histy[i] = histy[i] + 2*histy[i-1] + histy[i-2];
00194     totals[y] = 0;
00195     for (int i=0;i<drange;i++)
00196       totals[y] += histy[i];
00197   }
00198   // Find best line using random sampling
00199   float maxwei = 0.0f; 
00200   alpha = 0.0f; 
00201   beta = 0.0f; 
00202   disp = drange/2;
00203   for (int l=0;l<1000;l++) {
00204     int idx1 = rand()%height;
00205     int idx2 = rand()%height;
00206     while (idx1==idx2)
00207       idx2 = rand()%height;
00208     if (!totals[idx1] || !totals[idx2])
00209       continue;
00210     int cnt1 = rand()%totals[idx1];
00211     int cnt2 = rand()%totals[idx2];
00212     int disp1 = 0, disp2 = 0;
00213     for (int sum1=0;sum1<cnt1;disp1++) 
00214       sum1 += hist[idx1*drange+disp1];
00215     for (int sum2=0;sum2<cnt2;disp2++) 
00216       sum2 += hist[idx2*drange+disp2];
00217     disp1--;
00218     disp2--;
00219     float dgra = (float)(disp2 - disp1) / (idx2 - idx1);
00220     float dzer = disp2 - dgra*idx2;
00221     float sumwei = 0.0f;
00222     for (int y=0;y<height;y++) {
00223       for (int dd=-3;dd<=3;dd++) {
00224         int d = (int)(dgra*y + dzer + 0.5f) + dd;
00225         if (d<0 || d>=drange) 
00226           continue;
00227         float er = d - (dgra*y + dzer);
00228         sumwei += hist[y*drange + d] / (4.0f + er*er);
00229       }
00230     }
00231     if (sumwei>maxwei && dgra>minbeta) {
00232       maxwei = sumwei;
00233       beta = dgra;
00234       disp = dzer;
00235     }
00236   }
00237   //std::cout << "### Init surface: " << maxwei/4 << " " << alpha << " " << beta << " " << disp << std::endl;
00238   // Improve line (depends only on y) using m-estimator
00239   for (int l=0;l<3;l++) {
00240     float syy = 0.0, sy1 = 0.0f, s11 = 0.0f;
00241     float sdy = 0.0, sd1 = 0.0f;
00242     for (int y=0;y<height;y++) {
00243       for (int dd=-3;dd<=3;dd++) {
00244         int d = (int)(beta*y + disp + 0.5f) + dd;
00245         if (d<0 || d>=drange) 
00246           continue;
00247         float er = d - (beta*y + disp);
00248         float w = hist[y*drange + d] / (4.0f + er*er);
00249         syy += w*y*y;
00250         sy1 += w*y;
00251         s11 += w;
00252         sdy += w*d*y;
00253         sd1 += w*d;
00254       }
00255     }
00256     float det = syy*s11 - sy1*sy1;
00257     beta = s11*sdy - sy1*sd1;
00258     disp = syy*sd1 - sy1*sdy;
00259     if (det!=0.0f) {
00260       beta /= det;
00261       disp /= det;
00262     }
00263     //std::cout << "### Init surface: " << s11/4 << " " << alpha << " " << beta << " " << disp << std::endl;
00264   }
00265   disp += 0.5f;
00266   // Improve plane (depends on both x and y) using m-estimator
00267   for (int l=0;l<3;l++) {
00268     float sxx = 0.0, sx1 = 0.0f, s11 = 0.0f;
00269     float sdx = 0.0, sd1 = 0.0f;
00270     for (int y=0;y<height;y++) {
00271       for (int x=0;x<width;x++) {
00272         if (dimd[y*width+x]>0.0f) {
00273           float d = dimd[y*width+x] - beta*y;
00274           float er = d - (alpha*x + disp);
00275           float w = 1.0f / (1.0f + er*er);
00276           sxx += w*x*x;
00277           sx1 += w*x;
00278           s11 += w;
00279           sdx += w*d*x;
00280           sd1 += w*d;
00281         }
00282       }
00283     }
00284     float det = sxx*s11 - sx1*sx1;
00285     alpha = s11*sdx - sx1*sd1;
00286     disp = sxx*sd1 - sx1*sdx;
00287     if (det!=0.0f) {
00288       alpha /= det;
00289       disp /= det;
00290     }
00291   }
00292   int num = 0;
00293   float vard = 0.0f;
00294   for (int y=0;y<height;y++) {
00295     for (int x=0;x<width;x++) {
00296       int i = y*width + x;
00297       float d = dimd[i];
00298       if (d>=0.0f && d<drange) {
00299         float er = alpha*x + beta*y + disp - d;
00300         if (er*er<4*spread_d) {
00301           
00302           if(y>max_y)
00303             max_y = y;;
00304           if(y<min_y)
00305             min_y = y;
00306           
00307           if(x>max_x)
00308             max_x = x;
00309           if(x<min_x)
00310             min_x = x;
00311           
00312           vard += er*er;
00313           num++;
00314         }
00315       }
00316     }
00317   }
00318   spread_d = (num ? vard/num : 1e-3);
00319   delete [] hist;
00320   delete [] totals;
00321   if (segm.verbose)
00322     std::cout << "### Flat surface: " << alpha << " " << beta << " " << disp << " " << spread_d << std::endl;
00323 }  
00324 
00325 void FgBgSegment::Foreground::Initialize(int startx, int starty)
00326 {
00327   //  const float window_size = 0.10f; //%%%%
00328   //  const float ball_size = 0.10f;
00329   float *dimd = segm.disparities->GetData();
00330   int drange = segm.drange;
00331   int width = segm.width;
00332   int height = segm.height;
00333   // Find center disparity through histograms
00334   int *hist1 = new int[drange];
00335   int *hist2 = new int[drange];
00336   for (int i=0;i<drange;i++)
00337     hist1[i] = hist2[i] = 0;
00338   int size = (int)(window_size*height);
00339   int miny = std::max(starty - size/2, 0);
00340   int maxy = std::min(starty + size/2, height);
00341   int minx = std::max(startx - size/2, 0);
00342   int maxx = std::min(startx + size/2, width);
00343   for (int y=miny;y<maxy;y++) {
00344     for (int x=minx;x<maxx;x++) {
00345       int i = y*width + x;
00346       if (dimd[i]>0.0f && dimd[i]<drange) 
00347         hist1[(int)dimd[i]]++;
00348     }
00349   }
00350   for (int i=2;i<drange-2;i++)
00351     hist2[i] = (hist1[i-2] + hist1[i+2]) + 4*(hist1[i-1] + hist1[i+1]) + 6*hist1[i];
00352   for (int i=2;i<drange-2;i++)
00353     hist1[i] = (hist2[i-2] + hist2[i+2]) + 4*(hist2[i-1] + hist2[i+1]) + 6*hist2[i];
00354   int maxhist = 0;
00355   int d = 0;
00356   for (int i=2;i<drange-2;i++) {
00357     if (hist1[i]>maxhist) {
00358       maxhist = hist1[i]; 
00359       d = i;
00360     }
00361   }
00362   int meand = d;
00363   if (hist1[d+1]<hist1[d] && hist1[d-1]<hist1[d])
00364     meand += 0.5f*(hist1[d+1] - hist1[d-1]) / (2.0f*hist1[d] - hist1[d+1] - hist1[d-1]);
00365   // Find spread of disparity peak
00366   int maxd = d+1;
00367   while (maxd<drange-1 && hist1[maxd]<hist1[maxd-1]) 
00368     maxd++;
00369   int mind = d-1;
00370   while (mind>0 && hist1[mind]<hist1[mind+1]) 
00371     mind--;
00372   float d2 = 0.0f;
00373   int di = 0;
00374   for (int i=mind;i<=maxd;i++) {
00375     d2 += hist1[i]*(i-meand)*(i-meand);
00376     di += hist1[i];
00377   }
00378   float vard = d2/di + 1.0f;
00379   float stdd = sqrt(vard); 
00380   std::cout << "meand: " << meand << "  vard: " << vard << std::endl;
00381   delete [] hist1;
00382   delete [] hist2;
00383   // Find largest region within disparity drange and not on surface
00384   Image<uint8_t> region(width, height);
00385   uint8_t *regd = region.GetData();
00386 #if 1
00387   segm.MakeSegmentImage(region);
00388   for (int y=0;y<height;y++) {
00389     for (int x=0;x<width;x++) {
00390       int i = y*width + x;
00391       if (!regd[i]) {
00392         int dx = x - startx;
00393         int dy = y - starty;
00394         if (segm.withDisparities) {
00395           if ((dx*dx + dy*dy)>(height*height*ball_size*ball_size) || dimd[i]<(meand-stdd) || dimd[i]>(meand+stdd)) 
00396             regd[i] = 1;
00397         } else {
00398           if ((dx*dx + dy*dy)>(height*height*ball_size*ball_size)) 
00399             regd[i] = 1;
00400         }
00401       }
00402     }
00403   }
00404 #else
00405   for (int y=0;y<height;y++) {
00406     for (int x=0;x<width;x++) {
00407       int i = y*width + x;
00408       regd[i] = 1;
00409       if (segm.withDisparities) {
00410         int dx = x - startx;
00411         int dy = y - starty;
00412         if ((dx*dx + dy*dy)<(height*height*ball_size*ball_size)) {
00413           if (dimd[i]>0.0f && dimd[i]<drange) {
00414             float er = segm.surface.alpha*x + segm.surface.beta*y + segm.surface.disp - dimd[i];
00415             if (er*er>2*segm.surface.spread_d && dimd[i]>(meand-stdd) && dimd[i]<(meand+stdd))
00416               regd[i] = 0;
00417           } 
00418         }
00419       } else {
00420         int dx = x - startx;
00421         int dy = y - starty;
00422         if ((dx*dx + dy*dy)<(height*height*ball_size*ball_size)) 
00423           regd[i] = 0;
00424       }
00425     }
00426   }
00427 #endif
00428   //region.Store("region.pgm", true, false);
00429   int num = 0;
00430   if (combined) {
00431     Matrix3 var3d;
00432     Vector3 pos3d;
00433     float var00 = 0.0f, var01 = 0.0f, var02 = 0.0f;
00434     float var11 = 0.0f, var12 = 0.0f, var22 = 0.0f;
00435     float pos0 = 0.0f, pos1 = 0.0f, pos2 = 0.0f;
00436     for (int y=0;y<height;y++) {
00437       for (int x=0;x<width;x++) {
00438         int i = y*width + x;
00439         if (regd[i]==0) {
00440           float d = dimd[i];
00441           var00 += x*x;
00442           var01 += x*y;
00443           var02 += x*d;
00444           var11 += y*y;
00445           var12 += y*d;
00446           var22 += d*d;
00447           pos0 += x;
00448           pos1 += y;
00449           pos2 += d;
00450           num++;
00451         }
00452       }  
00453     }
00454     if (num>0) {
00455       var3d(0,0) = var00;
00456       var3d(0,1) = var01;
00457       var3d(0,2) = var02;
00458       var3d(1,1) = var11;
00459       var3d(1,2) = var12;
00460       var3d(2,2) = var22;
00461       pos3d(0) = pos0;
00462       pos3d(1) = pos1;
00463       pos3d(2) = pos2;
00464       var3d *= (num>0.0f ? 1.0/num : 1.0f);
00465       pos3d *= (num>0.0f ? 1.0/num : 1.0f);
00466       var3d(0,0) -= pos3d(0)*pos3d(0);
00467       var3d(0,1) -= pos3d(0)*pos3d(1);
00468       var3d(0,2) -= pos3d(0)*pos3d(2);
00469       var3d(1,1) -= pos3d(1)*pos3d(1);
00470       var3d(1,2) -= pos3d(1)*pos3d(2);
00471       var3d(2,2) -= pos3d(2)*pos3d(2);
00472       var3d(1,0) = var3d(0,1);
00473       var3d(2,0) = var3d(0,2);
00474       var3d(2,1) = var3d(1,2);
00475       position3d = pos3d;
00476       spread3d = var3d;
00477     }
00478   } else {
00479     Matrix2 var_p;
00480     Vector2 position;
00481     float disp = 0.0f;
00482     float var_d = 0.0f;
00483     float var00 = 0.0f, var01 = 0.0f, var11 = 0.0f;
00484     float pos0 = 0.0f, pos1 = 0.0f;
00485     for (int y=0;y<height;y++) {
00486       for (int x=0;x<width;x++) {
00487         int i = y*width + x;
00488         if (regd[i]==0) {
00489           var00 += x*x;
00490           var01 += x*y;
00491           var11 += y*y;
00492           pos0 += x;
00493           pos1 += y;
00494           disp += dimd[i];
00495           var_d += dimd[i]*dimd[i];
00496           num++;
00497         }
00498       }  
00499     }
00500     if (num>0) {
00501       double inum = (num>0.0 ? 1.0/num : 1.0f);
00502       var_p(0,0) = var00;
00503       var_p(0,1) = var01;
00504       var_p(1,1) = var11;
00505       position(0) = pos0;
00506       position(1) = pos1;
00507       var_p *= inum;
00508       position *= inum;
00509       var_p(0,0) -= position(0)*position(0);
00510       var_p(0,1) -= position(0)*position(1);
00511       var_p(1,1) -= position(1)*position(1);
00512       var_p(1,0) = var_p(0,1);
00513       var_d *= inum; 
00514       disp *= inum;
00515       var_d -= disp*disp;
00516       position3d(0) = position(0);
00517       position3d(1) = position(1);
00518       position3d(2) = disp;
00519       spread3d.identity();
00520       spread3d(0,0) = var_p(0,0);
00521       spread3d(0,1) = var_p(0,1);
00522       spread3d(1,0) = var_p(1,0);
00523       spread3d(1,1) = var_p(1,1);
00524       spread3d(2,2) = var_d;
00525     }
00526   }
00527   if (segm.verbose) {
00528     std::cout << "### Foreground position3d: " << position3d << " " << num << std::endl;
00529     std::cout << "### Foreground spread3d: " << spread3d << std::endl;
00530   }
00531 }
00532 
00533 void FgBgSegment::Foreground::SetInitParams( float l_window_size, 
00534                                              float l_ball_size)
00535 {
00536   window_size = l_window_size;
00537   ball_size   = l_ball_size;
00538 }
00539 
00540 
00541 void FgBgSegment::Background::Initialize()
00542 {
00543   const int numFigures = segm.figures.size();
00544   float *dimd = segm.disparities->GetData();
00545   int drange = segm.drange;
00546   int width = segm.width;
00547   int height = segm.height;
00548   float mean = 0.0f;
00549   float vari = 0.0f;
00550   int num = 0;
00551   float ivar[numFigures][6]; 
00552   for (int f=0;f<numFigures;f++) {
00553     Matrix3 invs = segm.figures[f]->spread3d;
00554     invs = invs.invert();
00555     ivar[f][0] = invs(0,0), ivar[f][1] = invs(0,1), ivar[f][2] = invs(1,1);
00556     ivar[f][3] = invs(0,2), ivar[f][4] = invs(1,2), ivar[f][5] = invs(2,2);
00557   }
00558   for (int y=0;y<height;y++) {
00559     for (int x=0;x<width;x++) {
00560       int i = y*width + x;
00561       if (dimd[i]>0.0f && dimd[i]<drange) {
00562         float d = dimd[i];
00563         bool used = false;
00564         for (int f=0;f<numFigures;f++) {
00565           float er_x = x - segm.figures[f]->position3d(0);
00566           float er_y = y - segm.figures[f]->position3d(1);
00567           float er_d = d - segm.figures[f]->position3d(2);
00568           float er_f = er_x*er_x*ivar[f][0] + 2*er_x*er_y*ivar[f][1] + er_y*er_y*ivar[f][2];
00569           er_f += er_d*(2*er_x*ivar[f][3] + 2*er_y*ivar[f][4] + er_d*ivar[f][5]);
00570           used |= (er_f<16.0f);
00571         }
00572         float diff = (segm.surface.alpha*x + segm.surface.beta*y + segm.surface.disp - dimd[i]);
00573         float er_s = diff*diff/segm.surface.spread_d;
00574         used |= (er_s<16.0f);
00575         if (!used) {
00576           mean += d;
00577           vari += d*d;
00578           num++;
00579         }
00580       }
00581     }
00582   }
00583   if (num>0) {
00584     mean /= num;
00585     vari /= num;
00586     disp = mean;
00587     spread_d = vari - mean*mean;
00588   } else {
00589     disp = drange/2;
00590     spread_d = disp*disp/4.0f;
00591   }    
00592   if (segm.verbose)
00593     std::cout << "### Clutter spread: " << disp << " " << spread_d << std::endl;
00594 }
00595 
00596 void FgBgSegment::InitSegmentation()
00597 {
00598   const int numFigures = figures.size();
00599   float *dimd = disparities->GetData();
00600   float *reg_g = ground.probabilities.GetData();
00601   float *reg_s = surface.probabilities.GetData();
00602   /*
00603   Fill(ground.probabilities, 1.0f/3.0f);
00604   Fill(surface.probabilities, 1.0f/3.0f);
00605   */
00606   Fill(ground.probabilities, 1.0f/((float)numFigures+2.0f));
00607   Fill(surface.probabilities, 1.0f/((float)numFigures+2.0f));
00608   
00609   float er_g = 2.0f*log((float)width*height*drange);
00610   float const_s = log(surface.spread_d) + 2.0f*log((float)width*height);
00611   float *reg_f[numFigures], const_f[numFigures], prob_f[numFigures];
00612   float ivar[numFigures][6]; 
00613   for (int f=0;f<numFigures;f++) {
00614 
00615     //  Fill(figures[f]->probabilities, 1.0f/3.0f);
00616     Fill(figures[f]->probabilities, 1.0f/((float)numFigures+2.0f));
00617 
00618     reg_f[f] = figures[f]->probabilities.GetData();
00619     const_f[f] = log(figures[f]->spread3d.determinant());
00620     Matrix3 invs = figures[f]->spread3d;
00621     invs = invs.invert();
00622     ivar[f][0] = invs(0,0), ivar[f][1] = invs(0,1), ivar[f][2] = invs(1,1);
00623     ivar[f][3] = invs(0,2), ivar[f][4] = invs(1,2), ivar[f][5] = invs(2,2);
00624   }
00625   for (int y=0;y<height;y++) {
00626     for (int x=0;x<width;x++) {
00627       int i = y*width + x;
00628       float d = dimd[i];
00629       if (d>0.0f && d<drange) {
00630         float prob_g = (withSurface ? exp(-0.5f*er_g) : eps); 
00631         float diff = d - (surface.alpha*x + surface.beta*y + surface.disp);
00632         float er_s = diff*diff/surface.spread_d;
00633         er_s += const_s;
00634         float prob_s = exp(-0.5f*er_s); 
00635         float sumprob = prob_g + prob_s;
00636         for (int f=0;f<numFigures;f++) {
00637           float difx = x - figures[f]->position3d(0);
00638           float dify = y - figures[f]->position3d(1);
00639           float difd = d - figures[f]->position3d(2);
00640           float er_f = difx*difx*ivar[f][0] + 2*difx*dify*ivar[f][1] + dify*dify*ivar[f][2];
00641           er_f += difd*(2*difx*ivar[f][3] + 2*dify*ivar[f][4] + difd*ivar[f][5]);
00642           er_f += const_f[f];
00643           prob_f[f] = exp(-0.5f*er_f); 
00644           sumprob += prob_f[f];
00645         }
00646         reg_g[i] = prob_g / sumprob;
00647         reg_s[i] = prob_s / sumprob;
00648         for (int f=0;f<numFigures;f++) 
00649           reg_f[f][i] = prob_f[f] / sumprob;
00650       }
00651     }
00652   }
00653 } 
00654 
00655 //================== Update routines =====================//
00656 
00657 void FgBgSegment::FlatSurface::Update()
00658 { 
00659   float *dimd = segm.disparities->GetData();
00660   int drange = segm.drange;
00661   int width = segm.width;
00662   int height = segm.height;
00663   // reset plane limits
00664   min_x = width;
00665   max_x = 0;
00666   min_y = height;
00667   max_y = 0;
00668   float *reg_s = probabilities.GetData();
00669   float xx00 = 0.0f, xx01 = 0.0f, xx02 = 0.0f;
00670   float xx11 = 0.0f, xx12 = 0.0f, xx22 = 0.0f;
00671   float xd0 = 0.0f, xd1 = 0.0f, xd2 = 0.0f;
00672   for (int y=0;y<height;y++) {
00673     for (int x=0;x<width;x++) {
00674       int i = y*width + x;
00675       float d = dimd[i];
00676       if (d>0.0f && d<drange) {
00677         float w = reg_s[i];
00678         xx00 += x*x*w;
00679         xx01 += x*y*w;
00680         xx02 += x*w;
00681         xx11 += y*y*w;
00682         xx12 += y*w;
00683         xx22 += w;
00684         xd0 += x*d*w;
00685         xd1 += y*d*w;
00686         xd2 += d*w;
00687       }
00688     }
00689   }
00690   Matrix3 xx;
00691   Vector3 xd;
00692   xx(0,0) = xx00;
00693   xx(0,1) = xx01;
00694   xx(0,2) = xx02;
00695   xx(1,1) = xx11;
00696   xx(1,2) = xx12;
00697   xx(2,2) = xx22;
00698   xx(1,0) = xx(0,1);
00699   xx(2,0) = xx(0,2);
00700   xx(2,1) = xx(1,2);
00701   xd(0) = xd0;
00702   xd(1) = xd1;
00703   xd(2) = xd2;
00704   float num = xx(2,2);
00705   xx *= (num>0.0f ? 1.0/num : 1.0f);
00706   xd *= (num>0.0f ? 1.0/num : 1.0f);
00707   xx(0,0) += spread_d*weight_a;
00708   xx(1,1) += spread_d*weight_b;
00709   xx(2,2) += spread_d*weight_d;
00710   xd(0) += alpha * spread_d*weight_a;
00711   xd(1) += beta  * spread_d*weight_b;
00712   xd(2) += disp  * spread_d*weight_d;
00713   xx(0,0) += eps;
00714   xx(1,1) += eps;
00715   xx(2,2) += eps;
00716   Vector3 p = xx.invert()*xd;
00717   float sumer = 0.0f;
00718   num = 0.0f;
00719   for (int y=0;y<height;y++) {
00720     for (int x=0;x<width;x++) {
00721       int i = y*width + x;
00722       float d = dimd[i];
00723       if (d>0.0f && d<drange) {
00724         float er = alpha*x + beta*y + disp - d;
00725         float w = reg_s[i];
00726         sumer += w*er*er;
00727         num += w;
00728 
00729         if (er*er<4*spread_d) {
00730           if(y>max_y)
00731               max_y = y;;
00732             if(y<min_y)
00733               min_y = y;
00734 
00735             if(x>max_x)
00736               max_x = x;
00737             if(x<min_x)
00738               min_x = x;
00739           }
00740 
00741       }
00742     }
00743   }
00744   alpha = p(0);
00745   beta  = p(1);
00746   disp = p(2);
00747   spread_d = ((num>0.0f ? sumer/num : 1.0f) + strength*spread_d) / (1 + strength);
00748   //if (spread_d>1.0f) //%%%%
00749   //  spread_d = 1.0f; //%%%%
00750   if (segm.verbose)
00751     std::cout << "### Flat surface: " << alpha << " " << beta << " " << disp << " " << spread_d << std::endl;
00752 }
00753 
00754 void FgBgSegment::Foreground::Update()
00755 {
00756   float *dimd = segm.disparities->GetData();
00757   int drange = segm.drange;
00758   int width = segm.width;
00759   int height = segm.height;
00760   float *reg_f = probabilities.GetData();
00761   double num = 0.0f;
00762   if (combined) {
00763     Matrix3 var3d;
00764     Vector3 pos3d;
00765     var3d = 0.0;
00766     pos3d = 0.0;
00767     for (int y=0;y<height;y++) {
00768       for (int x=0;x<width;x++) {
00769         int i = y*width + x;
00770         if (dimd[i]>0.0f && dimd[i]<drange) {
00771           float w = reg_f[i];
00772           float d = dimd[i];
00773           var3d(0,0) += x*x*w;
00774           var3d(0,1) += x*y*w;
00775           var3d(0,2) += x*d*w;
00776           var3d(1,1) += y*y*w;
00777           var3d(1,2) += y*d*w;
00778           var3d(2,2) += d*d*w;
00779           pos3d(0) += x*w;
00780           pos3d(1) += y*w;
00781           pos3d(2) += d*w;
00782           num += w;
00783         }
00784       }
00785     }
00786     if (num>0.0f) {
00787       double inum = (num>0.0f ? 1.0f/num : 1.0f);
00788       var3d *= inum;
00789       pos3d *= inum;
00790       var3d(0,0) -= pos3d(0)*pos3d(0);
00791       var3d(0,1) -= pos3d(0)*pos3d(1);
00792       var3d(0,2) -= pos3d(0)*pos3d(2);
00793       var3d(1,1) -= pos3d(1)*pos3d(1);
00794       var3d(1,2) -= pos3d(1)*pos3d(2);
00795       var3d(2,2) -= pos3d(2)*pos3d(2);
00796       var3d(1,0) = var3d(0,1);
00797       var3d(2,0) = var3d(0,2);
00798       var3d(2,1) = var3d(1,2);
00799       Matrix3 weight;
00800       weight = 0.0;
00801       weight(0,0) = weight_p;
00802       weight(1,1) = weight_p;
00803       weight(2,2) = weight_d;
00804       Matrix3 scale = spread3d*weight; 
00805       scale(0,0) += 1.0;
00806       scale(1,1) += 1.0;
00807       scale(2,2) += 1.0;
00808       pos3d += spread3d*(weight*position3d);
00809       pos3d = scale.invert()*pos3d;
00810       var3d += spread3d*strength;
00811       var3d *= 1.0/(1.0 + strength) ;
00812       position3d = pos3d;
00813       spread3d = var3d;
00814     }
00815   } else {
00816     Matrix2 var_p;
00817     Vector2 position;
00818     double disp = 0.0;
00819     double var_d = 1.0;
00820     var_p = 0.0;
00821     position = 0.0;
00822     for (int y=0;y<height;y++) {
00823       for (int x=0;x<width;x++) {
00824         int i = y*width + x;
00825         if (dimd[i]>0.0f && dimd[i]<drange) {
00826           float w = reg_f[i];
00827           var_p(0,0) += x*x*w;
00828           var_p(0,1) += x*y*w;
00829           var_p(1,1) += y*y*w;
00830           position(0) += x*w;
00831           position(1) += y*w;
00832           disp += dimd[i]*w;
00833           var_d += dimd[i]*dimd[i]*w;
00834           num += w;
00835         }
00836       }
00837     }
00838     if (num>0.0f) {
00839       double inum = (num>0.0 ? 1.0/num : 1.0);
00840       var_p *= inum;
00841       position *= inum;
00842       var_p(0,0) -= position(0)*position(0);
00843       var_p(0,1) -= position(0)*position(1);
00844       var_p(1,1) -= position(1)*position(1);
00845       var_p(1,0) = var_p(0,1); 
00846       Matrix2 spread_p = spread3d.submatrix(0,0);
00847       Vector2 figure_p;
00848       figure_p(0) = position3d(0); 
00849       figure_p(1) = position3d(1); 
00850       Matrix2 scale_p = spread_p*weight_p; 
00851       scale_p(0,0) += 1.0;
00852       scale_p(1,1) += 1.0;
00853       position += ((spread_p*figure_p)*weight_p);  
00854       position = scale_p.invert()*position;
00855       var_p += spread_p*strength;
00856       var_p *= 1.0/(1.0 + strength);
00857       double spread_d = spread3d(2,2);
00858       double figure_d = position3d(2);
00859       var_d *= inum; 
00860       disp *= inum;
00861       var_d -= disp*disp;
00862       var_d += 1.00; 
00863       disp += spread_d*weight_d*figure_d;
00864       disp /= (1.0 + spread_d*weight_d);
00865       var_d += strength*spread_d;
00866       var_d /= (1.0 + strength);
00867       position3d(0) = position(0);
00868       position3d(1) = position(1);
00869       position3d(2) = disp;
00870       spread3d.identity();
00871       spread3d(0,0) = var_p(0,0);
00872       spread3d(0,1) = var_p(0,1);
00873       spread3d(1,0) = var_p(1,0);
00874       spread3d(1,1) = var_p(1,1);
00875       spread3d(2,2) = var_d;
00876     }
00877   }
00878   if (segm.verbose) {
00879     std::cout << "### Foreground position3d: " << position3d << " " << num << std::endl;
00880     std::cout << "### Foreground spread3d: " << spread3d << std::endl;
00881   }
00882 }
00883 
00884 void FgBgSegment::Background::Update()
00885 {
00886   float *reg_g = probabilities.GetData();
00887   float *dimd = segm.disparities->GetData();
00888   int drange = segm.drange;
00889   int width = segm.width;
00890   int height = segm.height;
00891   float mean = 0.0f;
00892   float vari = 0.0f;
00893   float num = 0.0f;
00894   for (int y=0;y<height;y++) {
00895     for (int x=0;x<width;x++) {
00896       int i = y*width + x;
00897       if (dimd[i]>0.0f && dimd[i]<drange) {
00898         float w = reg_g[i];
00899         mean += dimd[i]*w;
00900         vari += dimd[i]*dimd[i]*w;
00901         num += w;
00902       }
00903     }
00904   }
00905   if (num>0) {
00906     mean /= num;
00907     vari /= num;
00908     vari -= mean*mean;
00909     mean += spread_d*weight_d*disp;
00910     mean /= (1.0f + spread_d*weight_d);
00911     vari += strength*spread_d;
00912     vari /= (1.0f + strength);
00913     disp = mean;
00914     spread_d = vari;
00915   }
00916   if (segm.verbose)
00917     std::cout << "### Clutter spread: " << disp << " " << spread_d << std::endl;
00918 }
00919 
00920 float FgBgSegment::ColorModel::CreateHistogram( Image<float> &probabilities, 
00921                                                 bool allPoints)
00922 {
00923   int count = 0;
00924 
00925   float *reg_g = probabilities.GetData();
00926   float *dimd = segm.disparities->GetData();
00927   int drange = segm.drange;
00928   int width = segm.width;
00929   int height = segm.height;
00930   uint8_t *himd = segm.hue.GetData();
00931   uint8_t *simd = segm.saturation.GetData();
00932   uint8_t *gimd = segm.grey.GetData();
00933   float hist[hist_size*hist_size];
00934   float grey[hist_size];
00935 
00936   for (int i=0;i<hist_size*hist_size;i++) 
00937     hist[i] = 0.0f;
00938 
00939   for (int i=0;i<hist_size;i++) 
00940     grey[i] = 0.0f;
00941 
00942   float sumProb = 0.0f;
00943   for (int i=0;i<width*height;i++) {
00944     float prob = reg_g[i]; 
00945     sumProb += prob;
00946     if (allPoints || (dimd[i]>0.0f && dimd[i]<drange)) {
00947       if(!segm.withColorHoles || (dimd[i]>0.0f && dimd[i]<drange)){
00948         int ix = hist_size*himd[i]/256;
00949         int iy = hist_size*simd[i]/256;
00950         int idx = iy*hist_size + ix;
00951         hist[idx] += prob;
00952       } else if(!segm.uniform){
00953         ROS_ASSERT(!(dimd[i]>0.0f && dimd[i]<drange));
00954         int g = hist_size*gimd[i]/256;
00955         grey[g] += prob;
00956         count ++;
00957       }
00958     }
00959   }
00960 
00961   ROS_DEBUG("%d pixels with invalid colour information in histogram creation", count);
00962 
00963   float *phist = NULL;
00964   if (prior!=NULL)
00965     phist = prior->histogram;
00966   
00967   SmoothAndNormalizeHist( hist, phist, hist_size*hist_size, 
00968                           histogram, colorcost);
00969   if(!segm.uniform) {
00970     if (prior!=NULL)
00971       phist = prior->greyhist;
00972     else 
00973       phist = NULL;
00974     SmoothAndNormalizeHist( grey, phist, hist_size, greyhist, greycost);
00975   }
00976   /*
00977   float fac_old = 1.0f / ColorModel::weight;
00978   float fac_pri = 2.0f*fac_old;
00979   float num = 0.0f;
00980   for (int i=0;i<hist_size*hist_size;i++)
00981     num += hist[i];
00982   float inum = (num>0.0f ? 1.0/num : 1.0);
00983   if (prior!=NULL) {
00984     float *phist = prior->histogram;
00985     for (int i=0;i<hist_size*hist_size;i++) 
00986       histogram[i] = (hist[i]*inum + fac_old*histogram[i] + fac_pri*phist[i])/(1.0f + fac_old + fac_pri);
00987   } else {
00988     for (int i=0;i<hist_size*hist_size;i++) 
00989       histogram[i] = (hist[i]*inum + fac_old*histogram[i])/(1.0f + fac_old);
00990   }
00991   num = 0.0f;
00992   for (int i=0;i<hist_size*hist_size;i++) 
00993     num += histogram[i];
00994   inum = (num>0.0f ? 1.0/num : 1.0);
00995   for (int i=0;i<hist_size*hist_size;i++) {
00996     histogram[i] *= inum;
00997     colorcost[i] = -2.0f*log(std::max(histogram[i], eps/hist_size/hist_size));
00998   }
00999   */
01000   
01001   return sumProb;
01002 }
01003 
01004 void FgBgSegment::ColorModel::SmoothAndNormalizeHist( float const* hist,
01005                                                       float const* phist,
01006                                                       int size, 
01007                                                       float* const histogram,
01008                                                       float* const cost)
01009 {
01010   
01011   float fac_old = 1.0f / ColorModel::weight;
01012   float fac_pri = 4.0f*fac_old;
01013   float num = 0.0f;
01014   for (int i=0;i<size;i++)
01015     num += hist[i];
01016   float inum = (num>0.0f ? 1.0/num : 1.0);
01017   if (phist!=NULL) {
01018     //  if (prior!=NULL) {
01019     //  float *phist = prior->histogram;
01020     for (int i=0;i<size;i++) 
01021       histogram[i] = (hist[i]*inum + fac_old*histogram[i] 
01022                       + fac_pri*phist[i])/(1.0f + fac_old + fac_pri);
01023   } else {
01024     for (int i=0;i<size;i++) 
01025       histogram[i] = (hist[i]*inum 
01026                       + fac_old*histogram[i])/(1.0f + fac_old);
01027   }
01028 
01029   NormalizeHist(histogram, cost, size);
01030 
01031   /*
01032     num = 0.0f;
01033     for (int i=0;i<hist_size*hist_size;i++) 
01034     num += histogram[i];
01035     inum = (num>0.0f ? 1.0/num : 1.0);
01036     for (int i=0;i<hist_size*hist_size;i++) {
01037     histogram[i] *= inum;
01038     cost[i] = -2.0f*log(std::max(histogram[i], eps/hist_size/hist_size));
01039     }
01040   */
01041 }
01042 
01043 void FgBgSegment::ColorModel::NormalizeHist( float* const histogram,
01044                                              float* const cost,
01045                                              int size)
01046 {
01047   float num = 0.0f;
01048   for (int i=0;i<size;i++) 
01049     num += histogram[i];
01050   float inum = (num>0.0f ? 1.0/num : 1.0);
01051   for (int i=0;i<size;i++) {
01052     histogram[i] *= inum;
01053     cost[i] = -2.0f*log(std::max(histogram[i], eps/(float)size));
01054   }
01055 }
01056 
01057 void FgBgSegment::ColorModel::CreateHistogram(Image<uint8_t> &mask, 
01058                                               bool allPoints)
01059 {
01060   int count = 0;
01061 
01062   uint8_t *masd = mask.GetData();
01063   float   *dimd = segm.disparities->GetData();
01064   uint8_t *himd = segm.hue.GetData();
01065   uint8_t *simd = segm.saturation.GetData();
01066   uint8_t *gimd = segm.grey.GetData();
01067   int width = segm.width;
01068   int height = segm.height;
01069 
01070   for (int i=0;i<hist_size*hist_size;i++) 
01071     histogram[i] = 0.0f;
01072 
01073   if(!segm.uniform)
01074     for (int i=0;i<hist_size;i++) 
01075       greyhist[i] = 0.0f;
01076 
01077   int num  = 0;
01078   for (int i=0;i<width*height;i++) {
01079     if (masd[i]>0) {
01080       if (allPoints || (dimd[i]>0.0f && dimd[i]<segm.drange)) {
01081         if(!segm.withColorHoles || 
01082            (dimd[i]>0.0f && dimd[i]<segm.drange)){
01083           int ix = hist_size*himd[i]/256;
01084           int iy = hist_size*simd[i]/256;
01085           int idx = iy*hist_size + ix;
01086           histogram[idx] ++;
01087           num ++;
01088         } else if(!segm.uniform){
01089           ROS_ASSERT(!(dimd[i]>0.0f && dimd[i]<segm.drange));
01090           int g = hist_size*gimd[i]/256;
01091           greyhist[g] ++;
01092           count++;
01093         }
01094       }
01095     }
01096   }
01097 
01098   ROS_DEBUG("%d Pixels used for creating the histogram with mask", count);
01099 
01100   NormalizeHist(histogram, colorcost, hist_size*hist_size);
01101   if(!segm.uniform)
01102     NormalizeHist(greyhist, greycost, hist_size);
01103   /*
01104   double inum = (num>0 ? 1.0/num : 1.0);
01105   for (int i=0;i<hist_size*hist_size;i++) {
01106     histogram[i] *= inum;
01107     colorcost[i] = -2.0f*log(std::max(histogram[i], eps/hist_size/hist_size));
01108   }
01109   */
01110 }
01111 
01112 void FgBgSegment::CreateHistograms(bool allPoints)
01113 {
01114   ground.CreateHistogram(ground.probabilities, allPoints);
01115   surface.CreateHistogram(surface.probabilities, allPoints);
01116   for (unsigned int f=0;f<figures.size();f++)
01117     figures[f]->CreateHistogram(figures[f]->probabilities, allPoints);
01118 }
01119 
01120 void FgBgSegment::SetNewForeground(int startx, int starty, 
01121                                    Image<float> &dimg, int drange_)
01122 {
01123   Foreground *model = new Foreground(*this, width, height);
01124   model->SetInitParams( windowSize, ballSize);
01125   figures.push_back(model);
01126   model->Initialize(startx, starty);
01127   Image<float> probs(width, height);
01128   float *prod = probs.GetData();
01129   float ivar[6];  
01130   Matrix3 invs = model->spread3d;
01131   if (invs.determinant()!=0.0)
01132     invs = invs.invert();
01133   ivar[0] = invs(0,0), ivar[1] = invs(0,1), ivar[2] = invs(1,1);
01134   ivar[3] = invs(0,2), ivar[4] = invs(1,2), ivar[5] = invs(2,2);
01135   Image<uint8_t> mask(width, height);
01136   MakeSegmentImage(mask);
01137   uint8_t *masd = mask.GetData();
01138   float *dimd = dimg.GetData();
01139   for (int y=0;y<height;y++) {
01140     for (int x=0;x<width;x++) {
01141       int i = y*width + x;
01142       if (!masd[i]) {
01143         float d = dimd[i];
01144         float er_x = x - model->position3d(0);  
01145         float er_y = y - model->position3d(1);
01146         float er_d = d - model->position3d(2);
01147         float er_f = er_x*er_x*ivar[0] + 2*er_x*er_y*ivar[1] + er_y*er_y*ivar[2];
01148         if (dimd[i]>0.0f && dimd[i]<drange_) 
01149           er_f += er_d*(2*er_x*ivar[3] + 2*er_y*ivar[4] + er_d*ivar[5]);
01150         prod[y*width + x] = exp(-0.5f*er_f);
01151       } else
01152         prod[y*width + x] = 0.0f;
01153     }
01154   }
01155   model->CreateHistogram(probs, true);
01156 }
01157 
01158 void FgBgSegment::SetNewForeground(Image<uint8_t> &mask, 
01159                                    Image<float> &dimg, 
01160                                    int drange_, bool reuseLast)
01161 {
01162   if (!reuseLast)
01163     figures.push_back(new Foreground(*this, width, height));
01164   Foreground &fig = *figures.back();
01165   uint8_t *masd = mask.GetData();
01166   float *dimd = dimg.GetData();
01167   Matrix2 var_p;
01168   Vector2 position;
01169   double disp = 0.0;
01170   double var_d = 1.0;
01171   double num = 0.0, numd = 0.0;
01172   var_p = 0.0;
01173   position = 0.0;
01174   for (int y=0;y<height;y++) {
01175     for (int x=0;x<width;x++) {
01176       int i = y*width + x;
01177       if (masd[i]>0) {
01178         var_p(0,0) += x*x;
01179         var_p(0,1) += x*y;
01180         var_p(1,1) += y*y;
01181         position(0) += x;
01182         position(1) += y;
01183         num += 1.0;
01184         float d = dimd[i];
01185         if (d>0.0 && d<drange_) {
01186           disp += d;
01187           var_d += d*d;
01188           numd += 1.0;
01189         }
01190       }
01191     }
01192   }
01193   if (num>0.0) {
01194     double inum = (num>0.0 ? 1.0/num : 1.0);
01195     var_p *= inum;
01196     position *= inum;
01197     var_p(0,0) -= position(0)*position(0);
01198     var_p(0,1) -= position(0)*position(1);
01199     var_p(1,1) -= position(1)*position(1);
01200     var_p(1,0) = var_p(0,1); 
01201     inum = (numd>0.0 ? 1.0/numd : 1.0);
01202     var_d *= inum; 
01203     disp *= inum;
01204     var_d -= disp*disp;
01205     fig.position3d(0) = position(0);
01206     fig.position3d(1) = position(1);
01207     fig.position3d(2) = disp;
01208     fig.spread3d.identity();
01209     fig.spread3d(0,0) = var_p(0,0);
01210     fig.spread3d(0,1) = var_p(0,1);
01211     fig.spread3d(1,0) = var_p(1,0);
01212     fig.spread3d(1,1) = var_p(1,1);
01213     fig.spread3d(2,2) = var_d;
01214   }
01215   if (figures.size()<=colorPriors.size()) {
01216     ColorModel &prior = colorPriors[figures.size()-1];
01217     for (int i=0;i<hist_size*hist_size;i++) {
01218       fig.histogram[i] = prior.histogram[i];
01219       fig.colorcost[i] = prior.colorcost[i];
01220     }
01221     fig.prior = &prior;
01222   } else
01223     fig.CreateHistogram(mask, true);
01224   if (verbose){
01225     std::cout << "Figure position: " << fig.position3d << std::endl;
01226     std::cout << "Figure spread: " << std::endl << fig.spread3d << std::endl;
01227     std::cout << "Foregrounds: " << figures.size() << "  Priors: " << colorPriors.size() << std::endl;
01228   }
01229 }
01230 
01231 template <int numFigures>
01232 void FgBgSegment::PixSegment(FgBgSegment &segm)
01233 {
01234   int width = segm.width;
01235   int height = segm.height;
01236   int drange = segm.drange;
01237   float *dimd = segm.disparities->GetData();
01238   uint8_t *himd = segm.hue.GetData();
01239   uint8_t *simd = segm.saturation.GetData();
01240   uint8_t *gimd = segm.grey.GetData();
01241   
01242   Fill(segm.ground.probabilities, 1.0f/((float)numFigures+2.0f));
01243   Fill(segm.surface.probabilities, 1.0f/((float)numFigures+2.0f));
01244   /*
01245   Fill(segm.ground.probabilities, 1.0f/3.0f);
01246   Fill(segm.surface.probabilities, 1.0f/3.0f);
01247   */
01248   float const_sd = log(segm.surface.spread_d);
01249   float const_sp = 2.0f*log((float)width*height/1); 
01250   float const_s0 = -2.0*log(0.45f); 
01251   float const_su = -2.0*log(0.40f);
01252   float const_gd = 2.0f*log((float)drange/2); 
01253   float const_gp = 2.0f*log((float)width*height/1); 
01254   float const_g0 = -2.0*log(0.45f); 
01255   float const_gu = -2.0*log(0.40f);
01256 
01257   float const_fd[numFigures], const_fp[numFigures]; 
01258   float const_f0 = -2.0*log(0.10f);
01259   float const_fu = -2.0*log(0.20f);
01260   float ivar[numFigures][6]; 
01261   for (int f=0;f<numFigures;f++) {
01262     
01263     //Fill(segm.figures[f]->probabilities, 1.0f/3.0f);
01264     Fill(segm.figures[f]->probabilities, 1.0f/((float)numFigures+2.0f));
01265 
01266     const_fd[f] = log(segm.figures[f]->spread3d(2,2)); 
01267     const_fp[f] = log(segm.figures[f]->spread3d.determinant()) - const_fd[f];
01268     Matrix3 invs = segm.figures[f]->spread3d;
01269     invs = invs.invert();
01270     ivar[f][0] = invs(0,0), ivar[f][1] = invs(0,1), ivar[f][2] = invs(1,1);
01271     ivar[f][3] = invs(0,2), ivar[f][4] = invs(1,2), ivar[f][5] = invs(2,2);
01272   }
01273 
01274   BeliefProp<numFigures+2> belief(width, height);
01275   belief.SetGradientCosts(segm.grey, segm.gradWeight); 
01276   float **priors = belief.GetPriors();
01277 
01278   for (int y=0;y<height;y++) {
01279     for (int x=0;x<width;x++) {
01280       int i = y*width + x;
01281       int ix = hist_size*himd[i]/256;
01282       int iy = hist_size*simd[i]/256;
01283       int idx = iy*hist_size + ix;
01284       for (int f=0;f<numFigures;f++) {
01285         float er_f = const_f0;
01286         float difx = x - segm.figures[f]->position3d(0);
01287         float dify = y - segm.figures[f]->position3d(1);
01288         float difp = difx*difx*ivar[f][0] + 2*difx*dify*ivar[f][1] + dify*dify*ivar[f][2];
01289         er_f += const_fp[f] + (difp<9.0f ? difp : 100.0f);
01290         if (segm.withDisparities) {
01291           float dcostf = const_fu;
01292           if (dimd[i]>0.0f && dimd[i]<drange) {
01293             float difd = dimd[i] - segm.figures[f]->position3d(2);
01294             dcostf = difd*(2*difx*ivar[f][3] + 2*dify*ivar[f][4] + difd*ivar[f][5]) + const_fd[f];
01295           }
01296           er_f += dcostf;
01297         }
01298         if (segm.withColors && !segm.withColorHoles) {
01299           er_f += segm.figures[f]->colorcost[idx];
01300         } else if(segm.withColors && segm.withColorHoles){
01301           // in case colour of this pixel not defined 
01302           // (hue & saturation ==0) equal probability for each label
01303           if(!(dimd[i]>0.0f && dimd[i]<drange)){
01304             if(segm.uniform){
01305               
01306               float norm = numFigures+2.0f;
01307               er_f += -2.0f*log(1.0f/norm);
01308               
01309             } else {
01310               
01311               int idx = hist_size*gimd[i]/256;
01312               er_f += segm.figures[f]->greycost[idx];
01313 
01314             }
01315           } else {
01316             er_f += segm.figures[f]->colorcost[idx];
01317           }
01318         }
01319         priors[2+f][i] = 0.5f*er_f;
01320       }
01321       float er_g = const_g0; 
01322       float er_s = const_s0;
01323       er_g += const_gp;
01324       er_s += const_sp;
01325       if (segm.withDisparities) {
01326         float dcosts = const_su; 
01327         float dcostg = const_gu;
01328         if (dimd[i]>0.0f && dimd[i]<drange) {
01329           float diff = dimd[i] - (segm.surface.alpha*x 
01330                                   + segm.surface.beta*y 
01331                                   + segm.surface.disp);
01332           dcosts = diff*diff/segm.surface.spread_d + const_sd;
01333           dcostg = const_gd;
01334         } 
01335         er_s += dcosts;
01336         er_g += dcostg;
01337       } 
01338       if (segm.withColors && !segm.withColorHoles) { 
01339         er_g += segm.ground.colorcost[idx];
01340         er_s += segm.surface.colorcost[idx];
01341       } else if(segm.withColors && segm.withColorHoles){
01342         // in case colour of this pixel not defined 
01343         // (hue & saturation ==0) equal probability for each label
01344         if (!(dimd[i]>0.0f && dimd[i]<drange)){
01345           if(segm.uniform){
01346             
01347             float norm = numFigures+2.0f;
01348             er_g += -2.0f*log(1.0f/norm);
01349             er_s += -2.0f*log(1.0f/norm);
01350             
01351           } else {
01352             
01353             int idx = hist_size*gimd[i]/256;
01354             er_g += segm.ground.greycost[idx];
01355             er_s += segm.surface.greycost[idx];
01356             
01357           }
01358           
01359         } else {
01360           er_g += segm.ground.colorcost[idx];
01361           er_s += segm.surface.colorcost[idx];
01362         }
01363       }
01364 
01365       priors[0][i] = 0.5f*er_g;
01366       priors[1][i] = 0.5f*er_s;
01367     }
01368   }
01369 
01370   float **beliefs = belief.GetBeliefs();
01371   TimerCPU timer(2800);
01372   //belief.Execute(10);
01373   belief.Execute(5, 3);
01374   std::cout << "Belief: " <<  timer.read() << " ms" << std::endl;
01375   //belief.ComputeMAP(segm.grey); //%%%
01376   float *reg_g = segm.ground.probabilities.GetData();
01377   float *reg_s = segm.surface.probabilities.GetData();
01378   float *reg_f[numFigures]; 
01379   float prob_f[numFigures];
01380   for (int f=0;f<numFigures;f++) 
01381     reg_f[f] = segm.figures[f]->probabilities.GetData();
01382   for (int i=0;i<width*height;i++) {
01383     float minbelief = (beliefs[0][i]<beliefs[1][i] ? beliefs[0][i] : beliefs[1][i]);
01384     for (int f=0;f<numFigures;f++) 
01385       minbelief = (beliefs[2+f][i]<minbelief ? beliefs[2+f][i] : minbelief);
01386     float prob_g = exp(minbelief - beliefs[0][i]) + eps;
01387     float prob_s = exp(minbelief - beliefs[1][i]) + eps;
01388     float sumprob = prob_g + prob_s;
01389     for (int f=0;f<numFigures;f++) {
01390       prob_f[f] = exp(minbelief - beliefs[2+f][i]) + eps;
01391       sumprob += prob_f[f];
01392     }
01393     reg_g[i] = prob_g / sumprob;
01394     reg_s[i] = prob_s / sumprob;
01395     for (int f=0;f<numFigures;f++) 
01396       reg_f[f][i] = prob_f[f] / sumprob;
01397   }  
01398   if (segm.verbose>1) {
01399     Image<unsigned char> mask(width, height);
01400     segm.MakeSegmentImage(mask);
01401     mask.Store("mask.pgm", true, false);
01402     if (numFigures) {
01403       Image<float>(width, height, reg_g).Store("probsg.pgm", true, false);
01404       Image<float>(width, height, reg_s).Store("probss.pgm", true, false);
01405       Image<float>(width, height, reg_f[0]).Store("probsf1.pgm", true, false);
01406     }
01407   }
01408 }
01409 
01410 void FgBgSegment::RGBToHSV(Image<uint8_t> &cimg) 
01411 {  
01412   uint8_t *srcd = cimg.GetData();
01413   uint8_t *himd = hue.GetData();
01414   uint8_t *simd = saturation.GetData();
01415   uint8_t *vimd = grey.GetData();
01416   for (int i=0;i<width*height;i++) {
01417     short r = srcd[3*i+0];
01418     short g = srcd[3*i+1];
01419     short b = srcd[3*i+2];
01420     int minv = (r<g ? r : g);
01421     minv = (b<minv ? b : minv);
01422     int maxv = (r>g ? r : g);
01423     maxv = (b>maxv ? b : maxv);
01424     int diff = maxv - minv;
01425     int dif6 = diff*6;
01426     if (diff==0) 
01427       himd[i] = 0;
01428     else if (maxv==r) {
01429       himd[i] = (1536*diff + 256*(g - b))/dif6 & 255;
01430     } else if (maxv==g)
01431       himd[i] =  (512*diff + 256*(b - r))/dif6;
01432     else 
01433       himd[i] = (1024*diff + 256*(r - g))/dif6;
01434     if (maxv==0)
01435       simd[i] = 0;
01436     else
01437       simd[i] = 255*(maxv - minv)/maxv;
01438     vimd[i] = maxv;
01439   }
01440 }
01441 
01442 void FgBgSegment::GetSurfaceParameters( float &alpha, 
01443                                         float &beta, 
01444                                         float &disp)
01445 {
01446   alpha = surface.alpha;
01447   beta  = surface.beta;
01448   disp  = surface.disp;
01449   
01450 }
01451 
01452 
01453 void FgBgSegment::GetSurfaceMinMax( float &min_x,
01454                                     float &min_y,
01455                                     float &max_x,
01456                                     float &max_y )
01457 {
01458   min_x = surface.min_x;
01459   min_y = surface.min_y;
01460   max_x = surface.max_x;
01461   max_y = surface.max_y;
01462 }
01463 
01464 
01465 void FgBgSegment::SetWithColors( bool val)
01466 {
01467   withColors = val;
01468 }
01469 
01470 bool FgBgSegment::GetWithColors(){ return withColors; }
01471 
01472 void FgBgSegment::SetWithColorHoles( bool val)
01473 {
01474   withColorHoles = val;
01475 }
01476 
01477 bool FgBgSegment::GetWithColorHoles(){ return withColorHoles;}
01478 
01479 void FgBgSegment::SetUniform( bool val)
01480 {
01481   uniform = val;
01482 }
01483 
01484 bool FgBgSegment::GetUniform(){ return uniform;}
01485 
01486 void FgBgSegment::SetWithSurface( bool val)
01487 {
01488   withSurface = val;
01489 }
01490 
01491 bool FgBgSegment::GetWithSurface(){ return withSurface;}
01492 
01493 void FgBgSegment::SetWithDisparities( bool val)
01494 {
01495   withDisparities = val;
01496 }
01497 
01498 bool FgBgSegment::GetWithDisparities(){return withDisparities;}
01499 
01500 void FgBgSegment::SetGradWeight( float val) 
01501 {
01502   gradWeight = val;
01503 }
01504 
01505 float FgBgSegment::GetGradWeight() {return gradWeight;}
01506 
01507 void FgBgSegment::MakeSegmentImage(Image<uint8_t> &image)
01508 {
01509   const int numFigures = figures.size();
01510   assert(image.GetWidth()==width && image.GetHeight()==height);
01511   float *reg_g = ground.probabilities.GetData();
01512   float *reg_s = surface.probabilities.GetData();
01513   float *reg_f[numFigures];
01514   uint8_t *imgd = image.GetData();
01515   for (int f=0;f<numFigures;f++)
01516     reg_f[f] = figures[f]->probabilities.GetData();
01517   for (int i=0;i<width*height;i++) {
01518     float maxprob = reg_g[i];
01519     uint8_t col = 0;
01520     if (reg_s[i]>maxprob) {
01521       maxprob = reg_s[i];
01522       col = 1;
01523     }
01524     for (int f=0;f<numFigures;f++) {
01525       if (reg_f[f][i]>maxprob) {
01526         maxprob = reg_f[f][i];
01527         col = f + 2;
01528       }
01529     }
01530     imgd[i] = col; 
01531   }
01532 }
01533  
01534 void FgBgSegment::MakeMaskImage(Image<uint8_t> &image, int val, int obj)
01535 {
01536   const int numFigures = figures.size();
01537   assert(image.GetWidth()==width && image.GetHeight()==height);
01538   float *reg_g = ground.probabilities.GetData();
01539   float *reg_s = surface.probabilities.GetData();
01540   float *reg_f[numFigures]; 
01541   obj = std::min(numFigures-1, obj);
01542   uint8_t *imgd = image.GetData();
01543   for (int f=0;f<numFigures;f++)
01544     reg_f[f] = figures[f]->probabilities.GetData();
01545   for (int i=0;i<width*height;i++) {
01546     float maxprob = std::max(reg_g[i], reg_s[i]); 
01547     for (int f=0;f<numFigures;f++)
01548       maxprob = std::max(maxprob, reg_f[f][i]);
01549     imgd[i] = (reg_f[obj][i]==maxprob ? val : 0); 
01550   }
01551 }
01552  
01553 void FgBgSegment::MakeBorderImage(Image<uint8_t> &image)
01554 {
01555   Image<uint8_t> segm(width, height);
01556   uint8_t *segd = segm.GetData();
01557   MakeMaskImage(segm, 1);
01558   FillHoles(segm); //%%%%
01559   KeepLargestSegment(segm, 1, 0, 1000); //%%%%
01560   uint8_t *imgd = image.GetData();
01561   for (int i=0;i<3*width*height;i++)  
01562     imgd[i] = 160*imgd[i]/256;
01563   for (int y=2;y<height-2;y++) {  
01564     for (int x=2;x<width-2;x++) {
01565       int p = y*width + x;
01566       int sum = segd[p] + segd[p+1] + segd[p-1] + segd[p+1+width] + segd[p-1+width] + 
01567         segd[p+width] + segd[p-width] + segd[p+1-width] + segd[p-1-width] + 
01568         segd[p+2*width] + segd[p-2*width] + segd[p+2] + segd[p-2]; 
01569       if (sum>0 && sum<13)
01570         imgd[3*p+0] = imgd[3*p+1] = imgd[3*p+2] = 255;
01571     }
01572   } 
01573 }
01574  
01576 class ModelWorker {
01578   FgBgSegment::ColorModel &model;
01580   Image<float> &probs;
01582   float &sumProb;
01583 public:
01585 
01589   ModelWorker(FgBgSegment::ColorModel &m, 
01590               Image<float> &p, float &s) : 
01591     model(m), probs(p), sumProb(s) { }
01593   void operator()() {
01594     model.Update(); 
01595     sumProb = model.CreateHistogram(probs, true);
01596   }
01598   void runModel() {
01599     model.Update(); 
01600     sumProb = model.CreateHistogram(probs, true);
01601   }
01602 };
01603 
01604 
01605 void FgBgSegment::Execute(Image<uint8_t> &image, 
01606                           Image<float> &disp, 
01607                           bool initialize, int loops, int startx, int starty)
01608 {
01609   bool cudaopt = gpuopt; 
01610   typedef void(*func)(FgBgSegment&);
01611   static const func PixSegment[7] = { &FgBgSegment::PixSegment<0>, 
01612                                       &FgBgSegment::PixSegment<1>, 
01613                                       &FgBgSegment::PixSegment<2>, 
01614                                       &FgBgSegment::PixSegment<3>, 
01615                                       &FgBgSegment::PixSegment<4>, 
01616                                       &FgBgSegment::PixSegment<5>, 
01617                                       &FgBgSegment::PixSegment<6>};
01618   disparities = &disp;
01619   RGBToHSV(image);
01620 
01621   for (int i=0;i<loops;i++) {
01622     
01623     if (initialize) { 
01624       colorPriors.clear();
01625       for (int i=0;i<(int)figures.size();i++) {
01626         colorPriors.push_back(*figures[i]);
01627         delete figures[i];
01628       }
01629       figures.clear();
01630       if (withSurface)
01631         surface.Initialize();
01632       ground.Initialize();
01633       InitSegmentation(); 
01634       CreateHistograms(false);
01635     }    
01636 
01637     int nFigures = figures.size();
01638     if (cudaopt) 
01639 #ifdef USE_CUDA
01640       cudaSegment->Execute(*this, image, disp, nFigures, i==0);
01641 #else 
01642     std::cout << "Cuda not available!"<< std::endl;
01643 #endif //USE_CUDA
01644     else 
01645       PixSegment[nFigures](*this);
01646 
01647     //    ROS_INFO("How many foreground figures? %d", nFigures);
01648     
01649     // Boost equivalent to tbb task group
01650     /* 
01651       boost::thread_group grp;
01652       
01653       std::vector<ModelWorker*> workers;
01654       float sumProbs[8];
01655       if (withSurface)
01656       workers.push_back(new ModelWorker(surface, surface.probabilities, sumProbs[1]));
01657       for (int i=0;i<nFigures;i++) 
01658       workers.push_back(new ModelWorker(*figures[i], figures[i]->probabilities, sumProbs[i+2]));
01659       for (int i=0;i<workers.size();i++){
01660       grp.create_thread(*workers[i]);
01661       }
01662       
01663       grp.join_all( );
01664     */
01665     
01666 #ifdef USE_TBB
01667     tbb::task_group g;
01668 #endif // USE_TBB
01669     
01670     std::vector<ModelWorker*> workers;
01671     float sumProbs[8];
01672     if (withSurface)
01673       workers.push_back(new ModelWorker(surface, 
01674                                         surface.probabilities, 
01675                                         sumProbs[1]));
01676 
01677     for (int i=0;i<nFigures;i++) 
01678       workers.push_back(new ModelWorker(*figures[i], 
01679                                         figures[i]->probabilities, 
01680                                         sumProbs[i+2]));
01681 
01682     for (int i=0;i<(int)workers.size();i++){
01683 #ifdef USE_TBB
01684       g.run(*workers[i]);
01685 #else
01686       workers[i]->runModel();
01687 #endif // USE_TBB
01688     }
01689     
01690     ground.Update();
01691     sumProbs[0] = ground.CreateHistogram(ground.probabilities, true);
01692 
01693 #ifdef USE_TBB
01694     g.wait();
01695 #endif // USE_TBB
01696     
01697     for (int i=0;i<(int)workers.size();i++)
01698       delete workers[i];
01699     
01700     if (verbose>1) {
01701       std::cout << "SumProbs: ";
01702       for (int i=0;i<(int)figures.size()+2;i++) 
01703         std::cout << (int)sumProbs[i] << " ";
01704       std::cout << std::endl;
01705     }
01706     initialize = false;
01707   }  
01708 
01709   if (verbose || true) 
01710     std::cout << "Surface spread: " << surface.spread_d << std::endl;
01711   //PixSegment[figures.size()](*this);
01712 }
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends Defines


active_realtime_segmentation
Author(s): Mårten Björkman. Maintained by Jeannette Bohg
autogenerated on Wed Jan 23 2013 15:46:43