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();
}
