From 0ce8c32915d1bebb37f10b1c5d2b63666a5d62a3 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Thu, 9 Mar 2017 11:46:03 -0700 Subject: More Debug stuff --- tools/lighthousefind_radii/lighthousefind_radii.c | 33 ++++++++++++++++++++--- 1 file changed, 30 insertions(+), 3 deletions(-) (limited to 'tools') 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); -- cgit v1.2.3