static SpeedAccResult AccBiasTest(ImageData& lut, QueuedTracker *trk, int N, vector3f centerpos, vector3f range, const char *name, int MaxPixelValue, int extraFlags=0) { typedef QueuedTracker TrkType; std::vector<vector3f> results, truepos; int NImg=N;//std::max(1,N/20); std::vector<ImageData> imgs(NImg); const float R=5; int flags= LT_LocalizeZ|LT_NormalizeProfile|extraFlags; if (trk->cfg.qi_iterations>0) flags|=LT_QI; trk->SetLocalizationMode((LocMode_t)flags); Matrix3X3 fisher; for (int i=0;i<NImg;i++) { imgs[i]=ImageData::alloc(trk->cfg.width,trk->cfg.height); vector3f pos = centerpos + range*vector3f(rand_uniform<float>()-0.5f, rand_uniform<float>()-0.5f, rand_uniform<float>()-0.5f)*1; GenerateImageFromLUT(&imgs[i], &lut, trk->cfg.zlut_minradius, trk->cfg.zlut_maxradius, vector3f( pos.x,pos.y, pos.z)); SampleFisherMatrix fm(MaxPixelValue); fisher += fm.Compute(pos, vector3f(1,1,1)*0.001f, lut, trk->cfg.width,trk->cfg.height, trk->cfg.zlut_minradius,trk->cfg.zlut_maxradius); imgs[i].normalize(); if (MaxPixelValue> 0) ApplyPoissonNoise(imgs[i], MaxPixelValue); //if(i==0) WriteJPEGFile(name, imgs[i]); LocalizationJob job(i, 0, 0, 0); trk->ScheduleLocalization((uchar*)imgs[i%NImg].data, sizeof(float)*trk->cfg.width, QTrkFloat, &job); truepos.push_back(pos); } WaitForFinish(trk, N); results.resize(trk->GetResultCount()); for (uint i=0;i<results.size();i++) { LocalizationResult r; trk->FetchResults(&r,1); results[r.job.frame]=r.pos; } for (int i=0;i<NImg;i++) imgs[i].free(); SpeedAccResult r; r.Compute(results, [&](int index) { return truepos[index]; }); fisher *= 1.0f/NImg; r.crlb = sqrt(fisher.Inverse().diag()); return r; }
void cWorldStorage::Stop(void) { WaitForFinish(); }
void TestZRange(const char *name, const char *lutfile, int extraFlags, int clean_lut, RWeightMode weightMode=RWNone, bool biasMap=false, bool biasCorrect=false) { ImageData lut = ReadLUTFile(lutfile); vector3f delta(0.001f,0.001f, 0.001f); if(PathSeperator(lutfile).extension != "jpg"){ WriteJPEGFile(SPrintf("%s-lut.jpg",name).c_str(), lut); } if (clean_lut) { BenchmarkLUT::CleanupLUT(lut); WriteJPEGFile( std::string(lutfile).substr(0, strlen(lutfile)-4).append("_bmlut.jpg").c_str(), lut ); } QTrkComputedConfig settings; settings.qi_iterations = 2; settings.zlut_minradius = 1; settings.qi_minradius = 1; settings.width = settings.height = 100; settings.Update(); float maxVal=10000; std::vector<float> stdv; dbgprintf("High-res LUT range...\n"); SampleFisherMatrix fm( maxVal); QueuedCPUTracker trk(settings); ImageData rescaledLUT; ResampleLUT(&trk, &lut, lut.h, &rescaledLUT); if (biasCorrect) { CImageData result; trk.ComputeZBiasCorrection(lut.h*10, &result, 4, true); WriteImageAsCSV(SPrintf("%s-biasc.txt", name).c_str(), result.data, result.w, result.h); } int f = 0; if (weightMode == RWDerivative) f |= LT_LocalizeZWeighted; else if(weightMode == RWRadial) { std::vector<float> w(settings.zlut_radialsteps); for (int i=0;i<settings.zlut_radialsteps;i++) w[i]= settings.zlut_minradius + i/(float)settings.zlut_radialsteps*settings.zlut_maxradius; trk.SetRadialWeights(&w[0]); } else if (weightMode == RWStetson) trk.SetRadialWeights( ComputeRadialBinWindow(settings.zlut_radialsteps) ); trk.SetLocalizationMode(LT_QI|LT_LocalizeZ|LT_NormalizeProfile|extraFlags|f); uint nstep= InDebugMode ? 20 : 1000; uint smpPerStep = InDebugMode ? 2 : 200; if (biasMap) { smpPerStep=1; nstep=InDebugMode? 200 : 2000; } std::vector<vector3f> truepos, positions,crlb; std::vector<float> stdevz; for (uint i=0;i<nstep;i++) { float z = 1 + i / (float)nstep * (rescaledLUT.h-2); vector3f pos = vector3f(settings.width/2, settings.height/2, z); truepos.push_back(pos); Matrix3X3 invFisherLUT = fm.Compute(pos, delta, rescaledLUT, settings.width, settings.height, settings.zlut_minradius, settings.zlut_maxradius).Inverse(); crlb.push_back(sqrt(invFisherLUT.diag())); ImageData img=ImageData::alloc(settings.width,settings.height); for (uint j=0;j<smpPerStep; j++) { vector3f rndvec(rand_uniform<float>(), rand_uniform<float>(), rand_uniform<float>()); if (biasMap) rndvec=vector3f(); vector3f rndpos = pos + vector3f(1,1,0.1) * (rndvec-0.5f); // 0.1 plane is still a lot larger than the 0.02 typical accuracy GenerateImageFromLUT(&img, &rescaledLUT, settings.zlut_minradius, settings.zlut_maxradius, rndpos, true); img.normalize(); if (!biasMap) ApplyPoissonNoise(img, maxVal); LocalizationJob job(positions.size(), 0, 0, 0); trk.ScheduleImageData(&img, &job); positions.push_back(rndpos); if(j==0 && InDebugMode) { WriteJPEGFile(SPrintf("%s-sampleimg.jpg",name).c_str(), img); } } dbgprintf("[%d] z=%f Min std deviation: X=%f, Y=%f, Z=%f.\n", i, z, crlb[i].x,crlb[i].y,crlb[i].z); img.free(); } WaitForFinish(&trk, positions.size()); std::vector<vector3f> trkmean(nstep), trkstd(nstep); std::vector<vector3f> resultpos(nstep*smpPerStep); for (uint i=0;i<positions.size();i++) { LocalizationResult lr; trk.FetchResults(&lr, 1); resultpos[lr.job.frame]=lr.pos; } for (uint i=0;i<nstep;i++) { for (uint j=0;j<smpPerStep;j ++) { vector3f err=resultpos[i*smpPerStep+j]-positions[i*smpPerStep+j]; trkmean[i]+=err; } trkmean[i]/=smpPerStep; vector3f variance; for (uint j=0;j<smpPerStep;j ++) { vector3f r = resultpos[i*smpPerStep+j]; vector3f t = positions[i*smpPerStep+j];; vector3f err=r-t; err -= trkmean[i]; variance += err*err; if (InDebugMode) { dbgprintf("Result: x=%f,y=%f,z=%f. True: x=%f,y=%f,z=%f\n", r.x,r.y,r.z,t.x,t.y,t.z); } } if (biasMap) trkstd[i]=vector3f(); else trkstd[i] = sqrt(variance / (smpPerStep-1)); } vector3f mean_std; std::vector<float> output; for(uint i=0;i<nstep;i++) { dbgprintf("trkstd[%d]:%f crlb=%f bias=%f true=%f\n", i, trkstd[i].z, crlb[i].z, trkmean[i].z, truepos[i].z); output.push_back(truepos[i].z); output.push_back(trkmean[i].x); output.push_back(trkstd[i].x); output.push_back(trkmean[i].z); output.push_back(trkstd[i].z); output.push_back(crlb[i].x); output.push_back(crlb[i].z); mean_std += trkstd[i]; } dbgprintf("mean z err: %f\n", (mean_std/nstep).z); WriteImageAsCSV( SPrintf("%s_%d_flags%d_cl%d.txt",name, weightMode, extraFlags,clean_lut).c_str(), &output[0], 7, output.size()/7); lut.free(); rescaledLUT.free(); }