void Update()
{
double t = GetPreciseTime();
if (t-last_update > 1.0f/update_freq) {
if (fastCMOS) {
int fc = fastCMOS->getFramecount();
dbgprintf("Frames: %d\n", fc);
}
last_update = t;
}
}
void SpeedTest()
{
#ifdef _DEBUG
int N = 20;
#else
int N = 1000;
#endif
int qi_iterations = 7;
int xcor_iterations = 7;
CPUTracker* tracker = new CPUTracker(150,150, 128);
int radialSteps = 64, zplanes = 120;
float zmin = 2, zmax = 8;
float zradius = tracker->xcorw/2;
float* zlut = new float[radialSteps*zplanes];
for (int x=0;x<zplanes;x++) {
vector2f center (tracker->GetWidth()/2, tracker->GetHeight()/2);
float s = zmin + (zmax-zmin) * x/(float)(zplanes-1);
GenerateTestImage(ImageData(tracker->srcImage, tracker->GetWidth(), tracker->GetHeight()), center.x, center.y, s, 0.0f);
tracker->mean = 0.0f;
tracker->ComputeRadialProfile(&zlut[x*radialSteps], radialSteps, 64, 1, zradius, center, false);
}
tracker->SetRadialZLUT(zlut, zplanes, radialSteps, 1,1, zradius, true, true);
delete[] zlut;
// Speed test
vector2f comdist, xcordist, qidist;
float zdist=0.0f;
double zerrsum=0.0f;
double tcom = 0.0, tgen=0.0, tz = 0.0, tqi=0.0, txcor=0.0;
for (int k=0;k<N;k++)
{
double t0 = GetPreciseTime();
float xp = tracker->GetWidth()/2+(rand_uniform<float>() - 0.5) * 5;
float yp = tracker->GetHeight()/2+(rand_uniform<float>() - 0.5) * 5;
float z = zmin + 0.1f + (zmax-zmin-0.2f) * rand_uniform<float>();
GenerateTestImage(ImageData(tracker->srcImage, tracker->GetWidth(), tracker->GetHeight()), xp, yp, z, 0);
double t1 = GetPreciseTime();
vector2f com = tracker->ComputeMeanAndCOM();
vector2f initial(com.x, com.y);
double t2 = GetPreciseTime();
bool boundaryHit = false;
vector2f xcor = tracker->ComputeXCorInterpolated(initial, xcor_iterations, 16, boundaryHit);
if (boundaryHit)
dbgprintf("xcor boundaryhit!!\n");
comdist.x += fabsf(com.x - xp);
comdist.y += fabsf(com.y - yp);
xcordist.x +=fabsf(xcor.x - xp);
xcordist.y +=fabsf(xcor.y - yp);
double t3 = GetPreciseTime();
boundaryHit = false;
vector2f qi = tracker->ComputeQI(initial, qi_iterations, 64, 16,ANGSTEPF, 5,50, boundaryHit);
qidist.x += fabsf(qi.x - xp);
qidist.y += fabsf(qi.y - yp);
double t4 = GetPreciseTime();
if (boundaryHit)
dbgprintf("qi boundaryhit!!\n");
boundaryHit = false;
float est_z = zmin + (zmax-zmin)*tracker->ComputeZ(qi, 64, 0, &boundaryHit, 0) / (zplanes-1);
zdist += fabsf(est_z-z);
zerrsum += est_z-z;
if (boundaryHit)
dbgprintf("computeZ boundaryhit!!\n");
double t5 = GetPreciseTime();
// dbgout(SPrintf("xpos:%f, COM err: %f, XCor err: %f\n", xp, com.x-xp, xcor.x-xp));
if (k>0) { // skip first initialization round
tgen+=t1-t0;
tcom+=t2-t1;
txcor+=t3-t2;
tqi+=t4-t3;
tz+=t5-t4;
}
}
int Nns = N-1;
dbgprintf("Image gen. (img/s): %f\nCenter-of-Mass speed (img/s): %f\n", Nns/tgen, Nns/tcom);
dbgprintf("XCor estimation (img*it/s): %f\n", (Nns*xcor_iterations)/txcor);
dbgprintf("COM+XCor(%d) (img/s): %f\n", xcor_iterations, Nns/(tcom+txcor));
dbgprintf("Z estimation (img/s): %f\n", Nns/tz);
dbgprintf("QI speed: %f (img*it/s)\n", (Nns*qi_iterations)/tqi);
dbgprintf("Average dist: COM x: %f, y: %f\n", comdist.x/N, comdist.y/N);
dbgprintf("Average dist: Cross-correlation x: %f, y: %f\n", xcordist.x/N, xcordist.y/N);
dbgprintf("Average dist: QI x: %f, y: %f\n", qidist.x/N, qidist.y/N);
dbgprintf("Average dist: Z: %f. Mean error:%f\n", zdist/N, zerrsum/N);
delete tracker;
}
types::Double32 PlatformDesktop::GetUpTime()
{
return GetPreciseTime();
}
void UserMain(void * pd) {
int x;
DWORD flag = 0;
int cnt;
InitializeStack();
GetDHCPAddressIfNecessary();
if (EthernetIP == 0) GetDHCPAddress();
OSChangePrio(MAIN_PRIO);
EnableAutoUpdate();
#ifdef _DEBUG
/* InitializeNetworkGDB_and_Wait(); */
InitializeNetworkGDB();
#endif
/* note, this may not work in all cases.
1) all systems might boot at the same time
2) when they get to this point, they may not have an ip yet
*/
/* init our acn stack */
acn_port_protect_startup();
#if CONFIG_NSK
srand(GetPreciseTime() + netx_getmyip(0));
netx_init();
netx_startup();
#endif
#if CONFIG_SLP
slp_init();
slp_open();
slp_active_discovery_start();
#endif
#if CONFIG_RLP
rlp_init();
#if CONFIG_SDT
sdt_init(); /* indirectly calls sdtm_init(), rlp_init(), rlpm_init() */
sdt_startup(true);
#endif
#if CONFIG_DMP
/* dmp_startup(); */
#endif
#endif /* RLP */
CreateTasks();
x = OSChangePrio( DEBUG_TERM_PRIORITY );
if (x) {
PRINTF("%s","OUCH\n");
}
PRINTF("%s","Hello ACN World\n");
ioctl( 0, IOCTL_CLR | IOCTL_RX_ECHO ); /* turn sdtin echo off */
process_keys();
/* shut things down - these require the threads to continue run... */
#if CONFIG_SDT
sdt_shutdown();
#endif
#if CONFIG_SLP
slp_close();
#endif
#if CONFIG_DMP
/* dmp_shutdown() */
#endif
/* shut down receive thread */
OSFlagClear(&recv_flag, 1);
PRINTF("%s","Waiting for task to recv to shut down..");
cnt = 0;
flag = 0;
while (!flag) {
cnt++;
if (cnt == 80) {
cnt = 0;
PRINTF("%s","\n");
}
PRINTF("%s",".");
flag = OSFlagState(&recv_flag);
}
/* shut down tick thread */
OSFlagClear(&tick_flag, 1);
PRINTF("%s","Waiting for task to tick to shut down..");
cnt = 0;
flag = 0;
while (!flag) {
cnt++;
if (cnt == 80) {
cnt = 0;
PRINTF("%s","\n");
}
PRINTF("%s",".");
flag = OSFlagState(&tick_flag);
}
#if CONFIG_NSK
netx_shutdown();
#endif
acn_port_protect_shutdown();
slp_stats();
sdt_stats();
PRINTF("%s","========================\n");
PRINTF("%s","\nDone....\n");
while (1) {
OSTimeDly(20);
}
}
