$search
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 }