Commit 45f72158 authored by David Johnson's avatar David Johnson

Some corrections to smoothing, namely for orientation (cannot average if

positions are on both sides of the 0,2PI boundary).  This fix should solve
that problem.  Also a bunch more debug output.
parent 0efbd929
......@@ -11,6 +11,8 @@
#include "config.h"
#include <stdlib.h>
#include <math.h>
#include <float.h>
#include <assert.h>
......@@ -366,6 +368,7 @@ void vtMatch(struct lnMinList *pool,
struct lnMinList *now)
{
struct vision_track *vt;
int i;
assert(pool != NULL);
assert(prev != NULL);
......@@ -386,8 +389,22 @@ void vtMatch(struct lnMinList *pool,
&distance)) != NULL) {
if (distance <= 0.06) {
vt->vt_userdata = vt_prev->vt_userdata;
//info("COPIED old moving avg data\n");
vt->ma = vt_prev->ma;
info("\nCOPIED old moving avg data\n\n");
//vt->ma = vt_prev->ma;
/* copy the static data by hand to not overwrite ptr */
vt->ma.positions_len = vt_prev->ma.positions_len;
vt->ma.number_valid_positions =
vt_prev->ma.number_valid_positions;
vt->ma.oldest_index = vt_prev->ma.oldest_index;
vt->ma.current_avg = vt_prev->ma.current_avg;
/* copy the positions data w/o overwriting the pointer */
for (i = 0; i < vt->ma.positions_len; ++i) {
vt->ma.positions[i] = vt_prev->ma.positions[i];
}
lnRemove(&vt_prev->vt_link);
lnAddHead(pool, &vt_prev->vt_link);
}
......@@ -489,6 +506,23 @@ struct vision_track *vtFindWiggle(struct lnMinList *start,
return retval;
}
static float orientationToCircle(float o) {
if (o < 0.0f && o >= -M_PI) {
return (o + 2*M_PI);
}
else {
return o;
}
}
static float orientationToSemiCircle(float o) {
if (o > M_PI && o < 2*M_PI) {
return (o - 2*M_PI);
}
else {
return o;
}
}
/**
* Given a moving average, updates it with the data in new and outputs the
......@@ -502,13 +536,18 @@ static void updateMovingAverage(struct moving_average *ma,
struct robot_position *new) {
int i;
int vp;
int quads[4] = {0,0,0,0};
int quad_count = 0;
assert(ma != NULL);
assert(ma->positions != NULL);
assert(new != NULL);
/* setup */
/* update data samples array */
ma->positions[ma->oldest_index] = *new;
/* point to the next piece of data to be replaced */
ma->oldest_index = (ma->oldest_index + 1) % ma->positions_len;
/* if the ring buffer isn't full yet, incr the number of valid data pts */
......@@ -521,19 +560,128 @@ static void updateMovingAverage(struct moving_average *ma,
ma->current_avg.theta = 0.0f;
vp = ma->number_valid_positions;
/* make sure that the theta is BETWEEN 0 and 2PI -- otherwise an improper
* average could be calculated (i.e., if it's flipping between +PI and -PI)
*/
/**
* this gets tricky. here's the algorithm, and it should do well in
* almost every case (certainly, in every case for which incoming data
* is "sensible."
*
* 1) see how many and which quadrants we have data for.
* 2) if |q| > 2, return latest orientation as avg. Can't do much better
* here, except maybe to cut out all data that's not in the two most
* populous quadrants.
* 3) if |q| == 1, calculate the simple average.
* 4) if |q| == 2:
* if q == {2,3}, convert to full circle coords (i.e., convert
* [-PI,0) -> [PI,2PI), and calculate
* simple average. If answer is in [-PI,2PI], convert back to
* [-PI,0).
* if qi are NOT adjacent, return latest orientation as average.
* if qi are adjacent, calculate simple average.
*/
for (i = 0; i < vp; ++i) {
float tt = ma->positions[i].theta;
if (tt >= 0.0f && tt < M_PI_2) {
if (quads[0] == 0) {
++quad_count;
}
++(quads[0]);
}
else if (tt >= M_PI_2 && tt <= M_PI) {
if (quads[1] == 0) {
++quad_count;
}
++(quads[1]);
}
else if (tt >= -M_PI && tt <= -M_PI_2) {
if (quads[2] == 0) {
++quad_count;
}
++(quads[2]);
}
else if (tt > -M_PI_2 && tt < 0.0f) {
if (quads[3] == 0) {
++quad_count;
}
++(quads[3]);
}
else {
info("FATAL ERROR: orientation not in range 0,PI or 0,-PI !!!\n");
assert(0);
}
}
/* handle positions like normal */
for (i = 0; i < vp; ++i) {
ma->current_avg.x += ma->positions[i].x;
ma->current_avg.y += ma->positions[i].y;
}
ma->current_avg.x = ma->current_avg.x / vp;
ma->current_avg.y = ma->current_avg.y / vp;
/* now, handle the various orientations we may have... */
if (quad_count > 2) {
ma->current_avg.theta = new->theta;
info("S WARNING: had to use only latest theta "
"(> 2 quadrants); no avg!\n");
info("S QUADS: q0 = %d, q1 = %d, q2 = %d, q3 = %d\n",
quads[0],quads[1],quads[2],quads[3]);
}
else if (quad_count == 1) {
/* calculate simple average, a la positions */
for (i = 0; i < vp; ++i) {
ma->current_avg.theta += ma->positions[i].theta;
}
ma->current_avg.theta = ma->current_avg.theta / vp;
}
else if (quad_count == 2) {
if (quads[1] && quads[2]) {
float *to = (float *)malloc(sizeof(float)*(vp));
if (debug > 2) {
info("UPDATING moving avg: %d valid positions\n",vp);
/* convert, then calculate avg... then convert avg back */
for (i = 0; i < vp; ++i) {
to[i] = orientationToCircle(ma->positions[i].theta);
ma->current_avg.theta += to[i];
}
ma->current_avg.theta =
orientationToSemiCircle(ma->current_avg.theta / vp);
free(to);
}
else if ((quads[0] && quads[1]) ||
(quads[2] && quads[3]) ||
(quads[0] && quads[3])) {
/* calculate simple avg */
for (i = 0; i < vp; ++i) {
ma->current_avg.theta += ma->positions[i].theta;
}
ma->current_avg.x = ma->current_avg.x / vp;
ma->current_avg.y = ma->current_avg.y / vp;
ma->current_avg.theta = ma->current_avg.theta / vp;
}
else {
/* set current to new orient */
ma->current_avg.theta = new->theta;
info("S WARNING: had to use only latest theta "
"(non-adj); no avg!\n");
}
}
#if 1
//if (debug > 2) {
info("UPDATING moving avg: %d valid positions\n",vp);
//}
#endif
return;
}
......@@ -556,7 +704,7 @@ void vtSmooth(struct lnMinList *tracks) {
updateMovingAverage(ma,
&(vt->vt_position));
if (debug > 2) {
//if (debug > 2) {
info("SMOOTHED position for robot %d from (%f,%f,%f) to "
"(%f,%f,%f)\n",
ro->ro_id,
......@@ -567,7 +715,7 @@ void vtSmooth(struct lnMinList *tracks) {
ma->current_avg.y,
ma->current_avg.theta
);
}
//}
/* figure out how much the posit will change from the last
* one reported... this is important so we know if we mess up
......
......@@ -210,6 +210,39 @@ void dump_vision_list(struct lnMinList *list)
}
}
static void dump_smoothing_info(struct lnMinList *list) {
struct vision_track *vt;
int i;
assert(list != NULL);
vt = (struct vision_track *)(list->lh_Head);
while (vt->vt_link.ln_Succ != NULL) {
if (vt->vt_userdata != NULL) {
struct robot_object *ro;
struct moving_average *ma;
ro = (struct robot_object *)vt->vt_userdata;
ma = &(vt->ma);
info("smooth buffer for robot id %d:\n",ro->ro_id);
/* print the valid positions first */
for (i = 0; i < ma->number_valid_positions; ++i) {
info("(%f,%f,%f),",
ma->positions[i].x,
ma->positions[i].y,
ma->positions[i].theta);
}
info("\n");
}
vt = (struct vision_track *)vt->vt_link.ln_Succ;
}
}
static void dump_info(void)
{
static unsigned long long last_frame_count = 0;
......@@ -250,6 +283,10 @@ static void dump_info(void)
dump_vision_list(&last_frame);
info(" Current frame:\n");
dump_vision_list(&current_frame);
info("Smoothing info:\n");
dump_smoothing_info(&current_frame);
}
static void usage(void)
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment