Commit 41e5c140 authored by David Johnson's avatar David Johnson

Finally, the smoothing (via moving average) changes have been tested

enough and seem to work.  Currently, the moving average window size is set
to 5 frames (in testbed/event/sched/event-sched.c); a window size of 10
produced too much position lag.
parent 594516b1
......@@ -159,6 +159,9 @@ static void sighup(int sig)
execlp("vmcd",
"vmcd",
"-d",
"-S",
"-w",
"5",
"-l",
"logs/vmcd.log",
"-U",
......@@ -457,6 +460,9 @@ main(int argc, char *argv[])
execlp("vmcd",
"vmcd",
"-d",
"-S",
"-w",
"5",
"-l",
"logs/vmcd.log",
"-U",
......
......@@ -17,6 +17,7 @@
#include "log.h"
#include "robotObject.h"
#include "visionTrack.h"
extern int debug;
......@@ -182,6 +183,15 @@ int vtUpdate(struct lnMinList *now,
vt->vt_client = vc;
vt->vt_age = 0;
vt->vt_userdata = NULL;
/* zero out the smoothing data for this
* track!
*/
//vt->ma.oldest_index = 0;
//vt->ma.number_valid_positions = 0;
lnAddTail(now, &vt->vt_link);
/* Adjust the cameras viewable area based on this track. */
......@@ -376,6 +386,8 @@ 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;
lnRemove(&vt_prev->vt_link);
lnAddHead(pool, &vt_prev->vt_link);
}
......@@ -476,3 +488,113 @@ struct vision_track *vtFindWiggle(struct lnMinList *start,
return retval;
}
/**
* Given a moving average, updates it with the data in new and outputs the
* calculated average in avg.
*
* @param ma The moving average metadata.
* @param new The data to add to the moving average.
* @param avg The output from this function; contains the latest moving avg.
*/
static void updateMovingAverage(struct moving_average *ma,
struct robot_position *new) {
int i;
int vp;
assert(ma != NULL);
assert(ma->positions != NULL);
assert(new != NULL);
/* 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 */
if (ma->number_valid_positions < ma->positions_len) {
++(ma->number_valid_positions);
}
ma->current_avg.x = 0.0f;
ma->current_avg.y = 0.0f;
ma->current_avg.theta = 0.0f;
vp = ma->number_valid_positions;
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.theta += ma->positions[i].theta;
}
if (debug > 2) {
info("UPDATING moving avg: %d valid positions\n",vp);
}
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;
return;
}
void vtSmooth(struct lnMinList *tracks) {
struct vision_track *vt = NULL;
struct robot_object *ro = NULL;
struct moving_average *ma = NULL;
float distance_delta = 0.0f;
float theta_delta = 0.0f;
assert(tracks != NULL);
vt = (struct vision_track *)tracks->lh_Head;
while (vt->vt_link.ln_Succ != NULL) {
if ((ro = vt->vt_userdata) != NULL) {
ma = &(vt->ma);
/* using the latest position in ro->position, calc moving avg */
updateMovingAverage(ma,
&(vt->vt_position));
if (debug > 2) {
info("SMOOTHED position for robot %d from (%f,%f,%f) to "
"(%f,%f,%f)\n",
ro->ro_id,
vt->vt_position.x,
vt->vt_position.y,
vt->vt_position.theta,
ma->current_avg.x,
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
* wiggles
*/
distance_delta = hypotf(vt->vt_position.x - ma->current_avg.x,
vt->vt_position.y - ma->current_avg.y);
theta_delta = (float)abs(vt->vt_position.theta -
ma->current_avg.theta);
if (debug > 2) {
info(" DELTAS were %f dist and %f angle.\n",
distance_delta,
theta_delta);
}
/* update the position... this is a bit sneaky... but this way
* we don't have to change oodles of code.
*/
vt->vt_position.x = ma->current_avg.x;
vt->vt_position.y = ma->current_avg.y;
vt->vt_position.theta = ma->current_avg.theta;
// don't update the timestamp!
}
vt = (struct vision_track *)vt->vt_link.ln_Succ;
}
return;
}
......@@ -34,6 +34,18 @@
* lost.
*/
#define MAX_TRACK_AGE 5
/**
* Structure used to keep a moving average of last N positions.
*/
struct moving_average {
struct robot_position *positions; /*< ring buffer of most recent posits */
int positions_len; /*< length of buffer */
int number_valid_positions; /*< the buffer may not be full... */
/* don't want to shift the contents of the buffer every time, doh */
int oldest_index; /*< this is the one we should replace */
struct robot_position current_avg; /*< the last calc'd avg */
};
/**
* The maximum distance, in meters, that will be tolerated between an initial
......@@ -56,6 +68,7 @@ struct vision_track {
struct vmc_client *vt_client; /*< Camera that detected the track. */
int vt_age; /*< Age of the track (lower=younger) */
void *vt_userdata; /*< Data attached to the track. */
struct moving_average ma; /*< smooth out the positions... */
};
/**
......@@ -152,4 +165,12 @@ void vtUnknownTracks(struct lnMinList *unknown, struct lnMinList *mixed);
struct vision_track *vtFindWiggle(struct lnMinList *start,
struct lnMinList *now);
/**
* Taking the latest position, augment the moving average for each track. This
* should be done AFTER a vtMatch so that we only avg on valid data. Doing
* smoothing after everything else will allow the current matching algorithms
* to continue to work.
*/
void vtSmooth(struct lnMinList *tracks);
#endif
......@@ -74,6 +74,13 @@ static struct lnMinList last_frame, current_frame, wiggle_frame;
static struct robot_object *wiggle_bot = NULL;
#define DEFAULT_SMOOTH_WINDOW 10
/* by default, smoothing is off */
static int smoothing = 1;
static int smoothing_window = DEFAULT_SMOOTH_WINDOW;
/**
* Here's how the algorithm will lay out: when we get update_position packets
* from a vmc-client, we will attempt to find a preexisting track for the given
......@@ -247,20 +254,24 @@ static void dump_info(void)
static void usage(void)
{
fprintf(stderr,
"Usage: vmcd [-hd] [-l logfile] [-e emchost] [-p emcport]\n"
" [-c clienthost] [-P clientport]\n"
"Usage: vmcd [-hdS] [-l logfile] [-e emchost] [-p emcport]\n"
" [-c clienthost] [-P clientport] [-s #samples]\n"
"Vision master control daemon\n"
" -h\tPrint this message\n"
" -d\tTurn on debugging messages and do not daemonize\n"
" -S\tTurn on position smoothing\n"
" -l logfile\tSpecify the log file to use\n"
" -i pidfile\tSpecify the pid file name\n"
" -e emchost\tSpecify the host name where emc is running\n"
" -p emcport\tSpecify the port that emc is listening on\n"
" -c clienthost\tSpecify the host name where a vmc-client is\n"
" -P clientport\tSpecify the port the client is listening on\n"
" -w #samples\tSmooth over the last #samples positions (default "
" \tis %d)\n"
"\n"
"Example:\n"
" $ vmcd -c foo -P 7070 -- -c foo -P 7071\n");
" $ vmcd -c foo -P 7070 -- -c foo -P 7071\n",
DEFAULT_SMOOTH_WINDOW);
}
static int parse_client_options(int *argcp, char **argvp[])
......@@ -274,8 +285,22 @@ static int parse_client_options(int *argcp, char **argvp[])
argc = *argcp;
argv = *argvp;
while ((c = getopt(argc, argv, "hdp:U:l:i:e:c:P:")) != -1) {
while ((c = getopt(argc, argv, "hdSp:U:l:i:e:c:P:w:")) != -1) {
switch (c) {
case 'S':
smoothing = 1;
fprintf(stdout,"DATA WILL BE SMOOTHED!\n");
break;
case 'w':
smoothing_window = atoi(optarg);
if (smoothing_window < 2) {
fprintf(stdout,
"WARNING: smoothing window must be at least 2; setting"
" to default of %d.\n",
DEFAULT_SMOOTH_WINDOW);
smoothing_window = DEFAULT_SMOOTH_WINDOW;
}
break;
case 'h':
usage();
exit(0);
......@@ -364,6 +389,14 @@ static int wiggle_complete(struct robot_object *ro, mtp_packet_t *mp)
roMoveRobot(ro, ROS_LOST);
}
else {
/* zero out the smoothing data for this
* track!
*/
info("WC: RESET ma data\n");
vt->ma.oldest_index = 0;
vt->ma.number_valid_positions = 0;
vt->vt_userdata = wiggle_bot;
roMoveRobot(ro, ROS_KNOWN);
}
......@@ -462,6 +495,21 @@ int main(int argc, char *argv[])
retval = parse_client_options(&argc, &argv);
} while ((argc > 0) && strcmp(argv[0], "--") == 0);
/* set up smoothing */
if (smoothing) {
for (lpc = 0; lpc < TRACK_POOL_SIZE; ++lpc) {
vt_pool_nodes[lpc].ma.positions = (struct robot_position *)
malloc(sizeof(struct robot_position)*smoothing_window);
bzero(vt_pool_nodes[lpc].ma.positions,
sizeof(struct robot_position)*smoothing_window);
vt_pool_nodes[lpc].ma.positions_len = smoothing_window;
vt_pool_nodes[lpc].ma.number_valid_positions = 0;
vt_pool_nodes[lpc].ma.oldest_index = 0;
}
}
if (debug) {
loginit(0, logfile);
}
......@@ -658,6 +706,13 @@ int main(int argc, char *argv[])
/* Match the current frame to the last frame. */
vtMatch(&vt_pool, &last_frame, &current_frame);
/* (maybe) Smooth the matched bot posits */
if (smoothing) {
vtSmooth(&current_frame);
}
/* Reset the list of known/unknown bots. */
lnAppendList(&ro_data.rd_lists[ROS_UNKNOWN],
&ro_data.rd_lists[ROS_KNOWN]);
......
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