Predictor.cpp
Go to the documentation of this file.
00001 
00002 #include <blort/Tracker/Predictor.h>
00003 #include <blort/Tracker/Noise.h>
00004 #include <ros/console.h>
00005 
00006 using namespace Tracking;
00007 
00008 Predictor::Predictor(){
00009         m_dTime = 0.0;
00010         c_pred = 0.0;
00011         m_noConvergence = 0.0f;
00012 }
00013 
00014 // *** private ***
00015 float Predictor::noise(float sigma, unsigned int type){
00016     float random = 0.0f;
00017         
00018     // Gaussian noise
00019     if(type == GAUSS){
00020                 for(unsigned i=0; i<10; i++){
00021                         random += float(rand())/RAND_MAX;
00022                 }
00023                 random = 2.0f * (random / 10.0f - 0.5f);
00024     }
00025     
00026     // Uniform distributed noise
00027     if(type == NORMAL){
00028         random = 2.0f * (float(rand())/RAND_MAX - 0.5f);
00029     }
00030     
00031     // Adjusting to range
00032     random = random * sigma;
00033     
00034     return random;
00035 }
00036 
00037 Particle Predictor::genNoise(float sigma, Particle pConstraint, unsigned int type){
00038         if(sigma == 0.0f)
00039                 ROS_DEBUG("[Predictor::genNoise] Warning standard deviation sigma is 0.0");
00040         
00041         Particle epsilon;
00042         Noise N;
00043         
00044         //epsilon.r.x = noise(sigma, type) * pConstraint.r.x;
00045         //epsilon.r.y = noise(sigma, type) * pConstraint.r.y;
00046         //epsilon.r.z = noise(sigma, type) * pConstraint.r.z;
00047         //epsilon.t.x = noise(sigma, type) * pConstraint.t.x;
00048         //epsilon.t.y = noise(sigma, type) * pConstraint.t.y;
00049         //epsilon.t.z = noise(sigma, type) * pConstraint.t.z;
00050         //
00051         //epsilon.z   = noise(sigma, type) * pConstraint.z;
00052         
00053         epsilon.r.x = N.randn_notrig(0.0f, sigma * pConstraint.r.x);
00054         epsilon.r.y = N.randn_notrig(0.0f, sigma * pConstraint.r.y);
00055         epsilon.r.z = N.randn_notrig(0.0f, sigma * pConstraint.r.z);
00056         epsilon.t.x = N.randn_notrig(0.0f, sigma * pConstraint.t.x);
00057         epsilon.t.y = N.randn_notrig(0.0f, sigma * pConstraint.t.y);
00058         epsilon.t.z = N.randn_notrig(0.0f, sigma * pConstraint.t.z);
00059         
00060         epsilon.z = N.randn_notrig(0.0f, sigma * pConstraint.z);
00061                 
00062         return epsilon;
00063 }
00064 
00065 void Predictor::sampleFromGaussian(Distribution& d, int num_particles, Particle mean, Particle variance, float sigma){
00066         Particle p;
00067         Particle epsilon;
00068         
00069         d.push_back(mean);
00070         for(int i=1; i<num_particles; i++){
00071                 
00072                 // particle to sample from
00073                 p = mean;
00074                 
00075                 // move function (gaussian noise)
00076                 epsilon = genNoise(sigma, variance);
00077                 
00078                 // apply movement
00079                 p.Translate(epsilon.t);
00080                 p.RotateAxis(epsilon.r);
00081                 p.Translate( m_cam_view.x * epsilon.z, m_cam_view.y * epsilon.z, m_cam_view.z * epsilon.z);
00082 
00083                 // add particle to distribution
00084                 d.push_back(p);
00085         }       
00086 }
00087 
00088 void Predictor::movePredicted(const TomGine::tgPose& poseIn, TomGine::tgPose& poseOut, float &c){
00089         poseOut = poseIn;
00090         c = 0.0;
00091 }
00092 
00093 // *** public ***
00094 
00095 void Predictor::resample(Distribution& d, int num_particles, Particle variance, bool useMotion){
00096         
00097         if(d.size() <= 0)
00098                 return;
00099         
00100         unsigned n, id;
00101 //      unsigned nid=0;
00102         float sigma = 0.01f;
00103         float c=0.0f;
00104 //      float cn=0.0f;
00105 //      float c_pred_av = 0.0f;
00106         
00107         
00108         if(num_particles<=0){
00109                 ROS_DEBUG("[Distribution::resample] Warning number of particles to low (0)");
00110                 num_particles = d.size();
00111         }
00112         
00113         ParticleList particlelist_tmp;
00114         d.copy(particlelist_tmp);
00115         d.clear();
00116         
00117         //d.push_back(particlelist_tmp[0]);
00118         
00119         // Particles with motion
00120         for(    id=0; 
00121                         id<particlelist_tmp.size() && 
00122                         d.size()<(int)(num_particles*(1.0f - m_noConvergence)); 
00123                         id++)
00124         {
00125                 
00126                 // resampling according to weight
00127                 n = (unsigned)round(particlelist_tmp[id].w * num_particles);
00128                 c = particlelist_tmp[id].c;
00129                 
00130                 // TODO evaluate if this makes sense (higher accuracy/convergence)
00131                 // Remark: Defenitely removes jittering, maybe makes it less robust against fast movements
00132 //              if(d.getMaxC()>0.0f){
00133 //                      cn = c / d.getMaxC();
00134 //                      c = c*(1.0f-c) + cn*c;
00135 //                      //c = 0.8*cn + c*(1.0-cn);
00136 //              }
00137                 
00138                 // Tukey estimator
00139                 // TODO evaluate optimal power of estimator (accuracy / robustness)
00140 //              sigma = (1.0-pow(1.0-pow(1.0-c,2),3));
00141                 sigma = 1.0f - c;
00142                 
00143                 // Prediction motion model
00144 //              if(useMotion){
00145 //                      c_pred = 0.0f;
00146 //                      movePredicted(particlelist_tmp[id], particlelist_tmp[id], c_pred);
00147 //              }
00148 //              if( c_pred>=0.0f && c_pred<=1.0f ){
00149 //                      sigma = sigma * (1.0f - c_pred);
00150 //                      c_pred_av += c_pred;
00151 //              }
00152                 
00153                 // Keep one particle of best matching particles
00154 //              if(id<unsigned(num_particles*(1.0f-m_noConvergence))>>2){
00155 //                      d.push_back(particlelist_tmp[id]);
00156 //                      n--;
00157 //              }
00158                 
00159                 // Noise motion model
00160                 // ensure range of sigma
00161                 if(sigma==0.0) sigma = 0.001f;
00162                 if(sigma>1.0) sigma = 1.0f;
00163                 sampleFromGaussian(d, n, particlelist_tmp[id], variance, sigma);
00164         }
00165         
00167         //for(id=0; id<((int)num_particles*pNoMotion); id++){
00168         //      d.copyParticle(p, id);
00169         //      p.tp = vec3(0.0,0.0,0.0);
00170         //      p.rp = vec3(0.0,0.0,0.0);
00171         //      p.zp = 0.0;
00172         //      d.push_back(p);
00173         //}
00174         
00176         if(m_noConvergence > 0.0f){
00177                 n = num_particles - d.size();
00178 //              Particle p = particlelist_tmp[0] * (1.0f - c_pred_av/id);
00179                 Particle p = particlelist_tmp[0];
00180                 sampleFromGaussian(d, n, p, variance, 1.0f);
00181         }
00182 }
00183 
00184 void Predictor::sample(Distribution& d, int num_particles, Particle mean, Particle variance){
00185         d.clear(); 
00186         sampleFromGaussian(d, num_particles, mean, variance);
00187 }
00188 
00189 void Predictor::updateTime(double dTime){
00190         m_dTime = dTime;
00191 }


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