Distribution.cpp
Go to the documentation of this file.
00001 #include <ros/console.h>
00002 #include <blort/Tracker/Distribution.h>
00003 #include <blort/TomGine/tgError.h>
00004 
00005 using namespace Tracking;
00006 
00007 // *** private ***
00008 void Distribution::updateQueries(){
00009         
00010         if(queryMatches.empty()){
00011                 queryMatches.assign(m_particlelist.size(), 0);
00012                 glGenQueriesARB(m_particlelist.size(), &queryMatches[0]);
00013         }
00014                 
00015         if(queryEdges.empty()){
00016                 queryEdges.assign(m_particlelist.size(), 0);
00017                 glGenQueriesARB(m_particlelist.size(), &queryEdges[0]);
00018         }
00019         
00020         if(     m_particlelist.size() != queryMatches.size() ){
00021          glDeleteQueriesARB(queryMatches.size(), &queryMatches[0]);
00022          queryMatches.resize(m_particlelist.size(), 0);
00023          glGenQueriesARB(m_particlelist.size(), &queryMatches[0]);
00024         }
00025          
00026         if( m_particlelist.size() != queryEdges.size() ){
00027                 glDeleteQueriesARB(queryEdges.size(), &queryEdges[0]);
00028                 queryEdges.resize(m_particlelist.size(), 0);
00029                 glGenQueriesARB(m_particlelist.size(), &queryEdges[0]);
00030         }
00031 }
00032 
00033 void Distribution::calcMean(){
00034         m_meanParticle = Particle(0.0f);
00035         vec3 maxAxis = vec3(0.0f, 0.0f, 0.0f);
00036         float maxAngle = 0.0f;
00037         vec3 axis;
00038         float angle;
00039         w_sum = 0.0f;
00040         c_mean = 0.0f;
00041         int id;
00042         
00043         int num_particles = m_particlelist.size()>>1;
00044         
00045         // Normalise particles
00046         for(id=0; id<num_particles; id++)
00047                 w_sum += m_particlelist[id].w;
00048 
00049         for(id=0; id<num_particles && w_sum > 0.0; id++)
00050                 m_particlelist[id].w = m_particlelist[id].w / w_sum;
00051         
00052         // Weighted sum over all particles
00053         for(id=0; id<num_particles; id++){
00054 //              m_meanParticle.r  += m_particlelist[id].r * m_particlelist[id].w;
00055                 m_meanParticle.t += m_particlelist[id].t * m_particlelist[id].w;
00056                 c_mean  += m_particlelist[id].c;
00057                 
00058                 m_particlelist[id].q.getAxisAngle(axis, angle);
00059                 maxAxis += axis * m_particlelist[id].w;
00060                 maxAngle += angle * m_particlelist[id].w;
00061         }
00062         m_meanParticle.q.fromAxis(maxAxis, maxAngle);
00063 
00064         if(!m_particlelist.empty())
00065                 c_mean = c_mean / num_particles;
00066         
00067         m_meanParticle.c = c_mean;
00068         m_meanParticle.w = w_max;
00069 }
00070 
00071 // *** public ***
00072 Distribution::Distribution(){
00073         v_max = 1;
00074         w_sum = 0.0f;
00075 }
00076 
00077 Distribution::~Distribution(){
00078         if(!queryMatches.empty())
00079                 glDeleteQueriesARB(m_particlelist.size(), &queryMatches[0]);
00080         if(!queryEdges.empty())
00081                 glDeleteQueriesARB(m_particlelist.size(), &queryEdges[0]);
00082 }
00083 
00084 double Distribution::getVarianceC(){
00085         double mean = 0.0;
00086         double var = 0.0;
00087         
00088         // Evaluate mean
00089         for(unsigned id=0; id<m_particlelist.size(); id++)
00090                 mean += m_particlelist[id].c;
00091         mean = mean / m_particlelist.size();
00092         
00093         // Evaluate standard variation
00094         for(unsigned id=0; id<m_particlelist.size(); id++)
00095                 var += pow(m_particlelist[id].c - mean, 2);
00096         var = var / m_particlelist.size();
00097         
00098         return var;
00099 }
00100 
00101 double Distribution::getVariancePt(){
00102         double var = 0.0;
00103         
00104         // Evaluate mean
00105         Particle mean = getMean();
00106         
00107         // Evaluate standard variation
00108         for(unsigned id=0; id<m_particlelist.size(); id++){
00109                 vec3 d = m_particlelist[id].t - mean.t;
00110                 var += (d.x*d.x + d.y*d.y + d.z*d.z);
00111         }
00112         var = sqrt(var / m_particlelist.size());
00113         
00114         return var;
00115 }
00116 
00117 void Distribution::normalizeW(){
00118         float dw_sum = 0.0;
00119         if(w_sum>0.0){
00120                 dw_sum = 1.0f/w_sum;
00121                 for(unsigned id=0; id<m_particlelist.size(); id++){
00122                         m_particlelist[id].w = m_particlelist[id].w * dw_sum;
00123                         if(m_particlelist[id].w > w_max)
00124                                 w_max = m_particlelist[id].w;
00125                 }
00126         }else{
00127                 dw_sum = 1.0f/m_particlelist.size();
00128                 for(unsigned id=0; id<m_particlelist.size(); id++){
00129                         m_particlelist[id].w = dw_sum;
00130                 }
00131                 w_max = dw_sum;
00132         }
00133 }
00134 
00135 // Measurement
00136 bool sortfunction(Particle p1, Particle p2){ return (p1.w > p2.w); }
00137 
00138 void Distribution::drawParticlesEdges(TrackerModel& model, Shader* shadeCompare, bool showparticles){
00139         model.setTexture(0);
00140         glEnable(GL_DEPTH_TEST);
00141         
00142         for(unsigned i=0; i<m_particlelist.size(); i++){
00143                 m_particlelist[i].Activate();
00144                 
00145                 glColorMask(0,0,0,0); glDepthMask(1);
00146                 glClear(GL_DEPTH_BUFFER_BIT);
00147                 model.drawFaces();
00148                 
00149                 glDepthMask(0);
00150                 if(showparticles)
00151                         glColorMask(1,1,1,1);
00152                 
00153                 glBeginQueryARB(GL_SAMPLES_PASSED_ARB, queryEdges[i]);
00154                 model.drawEdges();
00155                 glEndQueryARB(GL_SAMPLES_PASSED_ARB);
00156                                 
00157                 glColorMask(0,0,0,0);
00158                 shadeCompare->bind();
00159                 shadeCompare->setUniform("analyze", false);
00160                 glBeginQueryARB(GL_SAMPLES_PASSED_ARB, queryMatches[i]);
00161                 model.drawEdges();
00162                 glEndQueryARB(GL_SAMPLES_PASSED_ARB);
00163                 shadeCompare->unbind();
00164                 
00165                 m_particlelist[i].Deactivate();
00166         }
00167         // Reset render masks
00168         glColorMask(1,1,1,1); glDepthMask(1);
00169 }
00170 
00171 void Distribution::drawParticlesTextured(TrackerModel& model, Shader* shader, bool showparticles)
00172 {
00173         m_particlelist.size();
00174         
00175         // Set perspective mode and smaller viewport
00176         glColorMask(0,0,0,0); glDepthMask(0);
00177         glEnable(GL_DEPTH_TEST);
00178                 
00179         // Draw particles and count pixels
00180         shader->bind();
00181         shader->setUniform("analyze", false);
00182         for(unsigned i=0; i<m_particlelist.size(); i++){
00183                 m_particlelist[i].Activate();
00184                 
00185                 // Draw all model edge pixels
00186                 glBeginQueryARB(GL_SAMPLES_PASSED_ARB, queryEdges[i]);
00187                 shader->setUniform("compare", false);
00188                 if(showparticles)
00189                         glColorMask(1,1,1,1);
00190                 model.drawTexturedFaces();
00191                 model.drawUntexturedFaces();
00192                 glEndQueryARB(GL_SAMPLES_PASSED_ARB);
00193                 
00194                 glColorMask(0,0,0,0);
00195                 
00196                 // Draw matching model edge pixels using a shader
00197                 glBeginQueryARB(GL_SAMPLES_PASSED_ARB, queryMatches[i]);
00198                 shader->setUniform("compare", true);
00199                 shader->setUniform("textured", true);
00200                 model.drawTexturedFaces();
00201                 shader->setUniform("textured", false);
00202                 model.drawUntexturedFaces();
00203                 glEndQueryARB(GL_SAMPLES_PASSED_ARB);
00204 
00205                 m_particlelist[i].Deactivate();
00206         }
00207         shader->unbind();
00208         glColorMask(1,1,1,1); glDepthMask(1);   
00209 }
00210 
00211 float Distribution::confidenceFunction(const float &m, const float &e)
00212 {
00213         float c = 0.0f;
00214         
00215         if( e!=0 && v_max!=0 ){
00216                 // TODO evaluate weights and convergence factor (w.r.t. robustness / accuracy / anti-locking)
00217                 // TODO TODO TODO !!! HARDCODING !!!
00218                 c = (0.8f * float(m)/float(e) + 0.2f * float(m)/float(v_max));
00219 //              c = ( 0.5f * float(d)/float(v) + 0.5f * float(d)/float(v_max));
00220 //              c = (1.0f * float(d)/float(v) + 0.0f * float(d)/float(v_max));
00221 //              c = (float(d)/float(v));
00222         }else{
00223                 c = 0.0;
00224         }
00225         
00226         return c;
00227 }
00228 
00229 void Distribution::calcLikelihood(int convergence){
00230         int v, d;
00231         int v_max_tmp = 0;
00232         w_sum = 0.0;
00233         c_max = 0.0;
00234         w_max = 0.0;
00235         c_min = 1.0;
00236         
00237         for(unsigned id=0; id<m_particlelist.size(); id++){
00238                 // Get number of pixels from Occlusion Query
00239                 glGetQueryObjectivARB(queryEdges[id], GL_QUERY_RESULT_ARB, &v);
00240                 glGetQueryObjectivARB(queryMatches[id], GL_QUERY_RESULT_ARB, &d);
00241                 
00242                 // Get maximum edge pixels (view dependent)
00243                 if(v>v_max_tmp)
00244                         v_max_tmp = v;
00245                 
00246                 // avoids  (d/v_max) > 1.0
00247                 if(v>v_max)
00248                         v_max = v;
00249                 
00250                 // Likelihood calculation formula
00251                 m_particlelist[id].c = confidenceFunction(d, v);
00252                 m_particlelist[id].w = pow(m_particlelist[id].c, float(convergence)*(1.0f-m_particlelist[id].c));
00253                 
00254                 if(m_particlelist[id].c > c_max)
00255                         c_max = m_particlelist[id].c;
00256                 
00257                 if(m_particlelist[id].c < c_min)
00258                         c_min = m_particlelist[id].c;
00259                 
00260                 // sum of likelihood over all particles
00261                 w_sum += m_particlelist[id].w;
00262         }       
00263         
00264         // Update v_max (in case of decreasing v_max_tmp)
00265         v_max = v_max_tmp;
00266         
00267         if(c_max<=0.0){
00268                 for(unsigned id=0; id<m_particlelist.size(); id++){
00269                         m_particlelist[id].c = 0.01f;
00270                 }
00271                 c_max=0.01f;
00272         }
00273 }
00274 
00275 void Distribution::updateLikelihood(TrackerModel& model, Shader* shadeCompare, bool textured, int convergence, bool showparticles){
00276         
00277         // no particles to update
00278         if(m_particlelist.empty()){
00279                 return;
00280         }
00281                 
00282         updateQueries();
00283         
00284         if(textured)
00285                 drawParticlesTextured(model, shadeCompare, showparticles);
00286         else
00287                 drawParticlesEdges(model, shadeCompare, showparticles);
00288         
00289 //      printf("Distribution::updateLikelihood A: %f p: ", m_particlelist[0].c); m_particlelist[0].Print();
00290         
00291         calcLikelihood(convergence);
00292         
00293         // normalize weights
00294         normalizeW();
00295 
00296 //      printf("Distribution::updateLikelihood B: %f p: ", m_particlelist[0].c); m_particlelist[0].Print();
00297         
00298         // sort particles by likelihood
00299         std::stable_sort(m_particlelist.begin(), m_particlelist.end(), sortfunction);
00300         
00301 //      printf("Distribution::updateLikelihood C: %f p: ", m_particlelist[0].c); m_particlelist[0].Print();
00302         
00303         // calculate mean of distribution
00304         calcMean();
00305 }
00306 
00307 void Distribution::updateLikelihood(TrackerModel& model, Shader* shadeCompare, TomGine::tgCamera* cam_persp, Texture* tex_edge, int res)
00308 {
00309         if(m_particlelist.empty()){
00310                 return;
00311         }
00312         
00313         updateQueries();
00314         
00315         ImageProcessor *ip = g_Resources->GetImageProcessor();
00316         
00317         TomGine::tgPose p = getMean();
00318         int minX, maxX, minY, maxY;
00319         int fbo_res = ip->avgGetResolution();
00320         int segs = fbo_res / res;
00321         
00322         model.getBoundingBox2D( ip->getWidth(), ip->getHeight(), p, cam_persp, minX, maxX, minY, maxY );
00323         
00324         int w2 = ip->getWidth() >> 1;
00325         int h2 = ip->getHeight() >> 1;
00326         
00327         int width = 1.2 * (maxX-minX); // TODO hardcoded error tolerance (variance)
00328         int height = 1.2 * (maxY-minY); // TODO hardcoded error tolerance (variance)
00329         
00330         int minXsq = minX - ((width-(maxX-minX))>>1);
00331         int minYsq = minY - ((height-(maxY-minY))>>1);
00332         if(minXsq < 0) minXsq = 0;
00333         if(minYsq < 0) minYsq = 0;
00334 
00335         ip->setCamOrtho();
00336         glColor3f(1,1,1);
00337         glBegin(GL_LINE_LOOP);
00338                 glTexCoord2f(0,0); glVertex3f(minX-w2,minY-h2,0);
00339                 glTexCoord2f(1,0); glVertex3f(maxX-w2,minY-h2,0);
00340                 glTexCoord2f(1,1); glVertex3f(maxX-w2,maxY-h2,0);
00341                 glTexCoord2f(0,1); glVertex3f(minX-w2,maxY-h2,0);
00342         glEnd();
00343         
00344         cam_persp->Activate();
00345         
00346         p.Activate();
00347         glColor3f(1,0,0);
00348         model.drawEdges();
00349         p.Deactivate();
00350         
00351         float fResW = float(res)/width;
00352         float fResH = float(res)/height;
00353         
00354         unsigned steps = (unsigned)ceil(float(m_particlelist.size())/(segs*segs));
00355 //      printf("%d %d %d\n", m_particlelist.size(), segs, steps);
00356         
00357         float* avgs = (float*)malloc(sizeof(float)*segs*segs);
00358         int* pxs = (int*)malloc(sizeof(int)*segs*segs);
00359         for(int i=0; i<segs*segs; i++){
00360                 avgs[i] = -1;
00361                 pxs[i] = 0;
00362         }
00363         unsigned i=0;
00364         shadeCompare->bind();
00365         shadeCompare->setUniform("analyze", false);
00366         for(unsigned s=0; s<steps; s++){
00367                 unsigned j=i;
00368                 
00369 //              ip->avgActivate();
00370                 tex_edge->bind(0);
00371                 for(int r=0; r<segs; r++){
00372                         for(int c=0; c<segs; c++){
00373                                 if(i<m_particlelist.size()){
00374                                         glViewport(res*c-minXsq*fResW,res*r-minYsq*fResH, ip->getWidth()*fResW, ip->getHeight()*fResH);
00375                                 
00376                                         m_particlelist[i].Activate();
00377                                         glBeginQueryARB(GL_SAMPLES_PASSED_ARB, queryEdges[i]);
00378                                         
00379                                         shadeCompare->setUniform("compare", true);
00380                                         shadeCompare->setUniform("textured", true);
00381                                         model.drawTexturedFaces();
00382                                         shadeCompare->setUniform("textured", false);
00383                                         model.drawUntexturedFaces();
00384                                         
00385                                         glEndQueryARB(GL_SAMPLES_PASSED_ARB);
00386                                         m_particlelist[i].Deactivate();
00387                                 }
00388                                 i++;
00389                         }
00390                 }
00391                 ip->avgGet(avgs, 2);
00392                 ip->avgDeactivate();
00393                 
00394                 unsigned k=j;
00395                 float c;
00396                 float w;
00397 //              w_sum = 0.0;
00398 //              c_max = 0.0;
00399 //              w_max = 0.0;
00400 //              c_min = 1.0;
00401                 for(int i=0; i<segs*segs; i++){
00402                         if(k<m_particlelist.size()){
00403                                 glGetQueryObjectivARB(queryEdges[k], GL_QUERY_RESULT_ARB, &pxs[i]);
00404                                 tgCheckError("Distribustion::updateLikelihood glGetQueryObjectivARB: ");
00405                                 
00406                                 
00407                                 c = avgs[i]*float(res*res)/pxs[i];
00408                                 w = pow(c,3.0*(1.0f-c));
00409                                 
00410 //                              w_sum += w;
00411 //                              
00412 //                              if(w > w_max)
00413 //                                      w_max = w;
00414 //                              
00415 //                              if(c > c_max)
00416 //                                      c_max = c;
00417 //                              
00418 //                              if(c < c_min)
00419 //                                      c_min = c;
00420                                 
00421                                 if(i%segs==0) printf("\n");
00422                                 ROS_DEBUG("%f %f %f %d | ", c, m_particlelist[k].c, w, pxs[i]);
00423 //                              m_particlelist[k].c = c;
00424 //                              m_particlelist[k].w = w;
00425                         }
00426                         k++;
00427                 }
00428 //              printf("\n %f %f %f %f\n", w_sum, w_max, c_max, c_min);
00429         
00430         }
00431         shadeCompare->unbind();
00432         glColorMask(1,1,1,1); glDepthMask(1);
00433         
00434 //      // normalize weights
00435 //      normalizeW();
00436 // 
00437 //      // sort particles by likelihood and average most likely particles
00438 //      std::sort(m_particlelist.begin(), m_particlelist.end(), sortfunction);
00439 //      
00440 //      // calculate mean of distribution
00441 //      calcMean();
00442         
00443         cam_persp->Activate();
00444         glViewport(0,0,ip->getWidth(), ip->getHeight());
00445 }
00446 
00447 
00448 


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:25