diff options
Diffstat (limited to 'tools')
| -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); |