diff options
author | mwturvey <michael.w.turvey@intel.com> | 2017-03-09 11:46:03 -0700 |
---|---|---|
committer | mwturvey <michael.w.turvey@intel.com> | 2017-03-09 11:46:03 -0700 |
commit | 0ce8c32915d1bebb37f10b1c5d2b63666a5d62a3 (patch) | |
tree | 10a9a35ca585ff4b8820cb5ce6c0c5e069203a9b /tools/lighthousefind_radii/lighthousefind_radii.c | |
parent | b9677a259e3fb090b085a3a3bdee53fb60e5db98 (diff) | |
download | libsurvive-0ce8c32915d1bebb37f10b1c5d2b63666a5d62a3.tar.gz libsurvive-0ce8c32915d1bebb37f10b1c5d2b63666a5d62a3.tar.bz2 |
More Debug stuff
Diffstat (limited to 'tools/lighthousefind_radii/lighthousefind_radii.c')
-rw-r--r-- | tools/lighthousefind_radii/lighthousefind_radii.c | 33 |
1 files changed, 30 insertions, 3 deletions
diff --git a/tools/lighthousefind_radii/lighthousefind_radii.c b/tools/lighthousefind_radii/lighthousefind_radii.c index 2040236..10f20da 100644 --- a/tools/lighthousefind_radii/lighthousefind_radii.c +++ b/tools/lighthousefind_radii/lighthousefind_radii.c @@ -207,6 +207,7 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles FLT newMatchFitness = calculateFitness(angles, point4, pairs, numPairs); + if (newMatchFitness < lastMatchFitness) { //if (logFile) @@ -218,21 +219,47 @@ static RefineEstimateUsingGradientDescent(FLT *estimateOut, SensorAngles *angles memcpy(estimateOut, point4, sizeof(*estimateOut) * numRadii); #ifdef RADII_DEBUG - printf("+ %0.9f (%0.9f): %f, %f, %f, %f\n", newMatchFitness, g, estimateOut[0], estimateOut[1], estimateOut[2], estimateOut[3]); + printf("+ %d %0.9f (%0.9f) ", i, newMatchFitness, g); #endif + g = g * 1.1; } else { #ifdef RADII_DEBUG // printf("-"); - printf("- %0.9f (%0.9f): %f, %f, %f, %f\n", newMatchFitness, g, estimateOut[0], estimateOut[1], estimateOut[2], estimateOut[3]); - + printf("- %d %0.9f (%0.9f) ", i, newMatchFitness, g); #endif // if it wasn't a match, back off on the distance we jump g *= 0.7; } +#ifdef RADII_DEBUG + FLT avg=0; + FLT diffFromAvg[MAX_RADII]; + + for (size_t m = 0; m < numRadii; m++) + { + avg += estimateOut[m]; + } + avg = avg / numRadii; + + for (size_t m = 0; m < numRadii; m++) + { + diffFromAvg[m] = estimateOut[m] - avg;; + } + printf("[avg:%f] ", avg); + + for (size_t x = 0; x < numRadii; x++) + { + printf("%f, ", diffFromAvg[x]); + //printf("%f, ", estimateOut[x]); + } + printf("\n"); + + +#endif + } printf("\ni=%d\n", i); |