From 718471573ce18f2b92feb58f5990f87c9b4b19ac Mon Sep 17 00:00:00 2001 From: tinloaf Date: Mon, 9 Mar 2009 19:28:24 +0000 Subject: Add:Core:Adding cumulative displacement filter git-svn-id: http://svn.code.sf.net/p/navit/code/trunk/navit@2091 ffa7fe5e-494d-0410-b361-a75ebd5db220 --- navit/track.c | 252 +++++++++++++++++++++++++++++++++++++++++++++++++++++++--- 1 file changed, 242 insertions(+), 10 deletions(-) (limited to 'navit/track.c') diff --git a/navit/track.c b/navit/track.c index 848494c3a..7dd44f08b 100644 --- a/navit/track.c +++ b/navit/track.c @@ -19,6 +19,8 @@ #include #include +#include +#include #include "item.h" #include "attr.h" #include "track.h" @@ -47,6 +49,38 @@ struct tracking_line }; +/** + * @brief Conatins a list of previous speeds + * + * This structure is used to hold a list of previously reported + * speeds. This data is used by the CDF. + */ +struct cdf_speed { + struct cdf_speed *next; + int speed; + time_t time; +}; + +/** + * @brief Contains data for the CDF + * + * This structure holds all data needed by the + * cumulative displacement filter. + */ +struct cdf_data { + int extrapolating; + int available; + int first_pos; + int poscount; + int hist_size; + struct cdf_speed *speed_hist; + struct pcoord *pos_hist; + int *dir_hist; + double last_dist; + struct pcoord last_out; + int last_dir; +}; + struct tracking { struct mapset *ms; struct route *rt; @@ -66,6 +100,7 @@ struct tracking { int curr_angle; struct coord last[2]; struct pcoord last_in, last_out; + struct cdf_data cdf; }; @@ -76,6 +111,186 @@ int offroad_limit_pref=5000; int route_pref=300; +static void +tracking_init_cdf(struct cdf_data *cdf, int hist_size) +{ + cdf->extrapolating = 0; + cdf->available = 0; + cdf->poscount = 0; + cdf->last_dist = 0; + cdf->hist_size = hist_size; + + cdf->pos_hist = g_new0(struct pcoord, hist_size); + cdf->dir_hist = g_new0(int, hist_size); +} + +// Variables for finetuning the CDF + +// Minimum average speed +#define CDF_MINAVG 1.f +// Maximum average speed +#define CDF_MAXAVG 6.f // only ~ 20 km/h + // We need a low value here because otherwise we would extrapolate whenever we are not accelerating + +// Mininum distance (square of it..), below which we ignore gps updates +#define CDF_MINDIST 49 // 7 meters, I guess this value has to be changed for pedestrians. + +static void +tracking_process_cdf(struct cdf_data *cdf, struct pcoord *pin, struct pcoord *pout, int dirin, int *dirout, int cur_speed, time_t fixtime) +{ + struct cdf_speed *speed,*sc,*sl; + double speed_avg; + int speed_num,i; + + if (cdf->hist_size == 0) { + printf("No CDF.\n"); + *pout = *pin; + *dirout = dirin; + return; + } + + speed = g_new0(struct cdf_speed, 1); + speed->speed = cur_speed; + speed->time = fixtime; + + speed->next = cdf->speed_hist; + cdf->speed_hist = speed; + + sc = speed; + sl = NULL; + speed_num = 0; + speed_avg = 0; + while (sc && ((fixtime - speed->time) < 4)) { // FIXME static maxtime + speed_num++; + speed_avg += sc->speed; + sl = sc; + sc = sc->next; + } + + speed_avg /= (double)speed_num; + + if (sl) { + sl->next = NULL; + } + + while (sc) { + sl = sc->next; + g_free(sc); + sc = sl; + } + + if (speed_avg < CDF_MINAVG) { + speed_avg = CDF_MINAVG; + } else if (speed_avg > CDF_MAXAVG) { + speed_avg = CDF_MAXAVG; + } + + + if (cur_speed >= speed_avg) { + if (cdf->extrapolating) { + cdf->poscount = 0; + cdf->extrapolating = 0; + } + + cdf->first_pos--; + if (cdf->first_pos < 0) { + cdf->first_pos = cdf->hist_size - 1; + } + + if (cdf->poscount < cdf->hist_size) { + cdf->poscount++; + } + + cdf->pos_hist[cdf->first_pos] = *pin; + cdf->dir_hist[cdf->first_pos] = dirin; + + *pout = *pin; + *dirout = dirin; + } else if (cdf->poscount > 0) { + + double mx,my; // Average position's x and y values + double sx,sy; // Support vector + double dx,dy; // Difference between average and current position + double len; // Length of support vector + double dist; + + mx = my = 0; + sx = sy = 0; + + for (i = 0; i < cdf->poscount; i++) { + mx += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x / cdf->poscount; + my += (double)cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y / cdf->poscount; + + + if (i != 0) { + sx += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].x - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].x; + sy += cdf->pos_hist[((cdf->first_pos + i) % cdf->hist_size)].y - cdf->pos_hist[((cdf->first_pos + i - 1) % cdf->hist_size)].y; + } + + } + + if (cdf->poscount > 1) { + // Normalize the support vector + len = sqrt(sx * sx + sy * sy); + sx /= len; + sy /= len; + + // Calculate the new direction + *dirout = (int)rint(atan(sx / sy) / M_PI * 180 + 180); + } else { + // If we only have one position, we can't use differences of positions, but we have to use the reported + // direction of that position + sx = sin((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI); + sy = cos((double)cdf->dir_hist[cdf->first_pos] / 180 * M_PI); + *dirout = cdf->dir_hist[cdf->first_pos]; + } + + + dx = pin->x - mx; + dy = pin->y - my; + dist = dx * sx + dy * sy; + + if (cdf->extrapolating && (dist < cdf->last_dist)) { + dist = cdf->last_dist; + } + + cdf->last_dist = dist; + cdf->extrapolating = 1; + + pout->x = (int)rint(mx + sx * dist); + pout->y = (int)rint(my + sy * dist); + pout->pro = pin->pro; + + } else { + // We should extrapolate, but don't have an old position available + *pout = *pin; + *dirout = dirin; + } + + if (cdf->available) { + int dx,dy; + + dx = pout->x - cdf->last_out.x; + dy = pout->y - cdf->last_out.y; + + if ((dx*dx + dy*dy) < CDF_MINDIST) { + *pout = cdf->last_out; + *dirout = cdf->last_dir; + } + } + + cdf->last_out = *pout; + cdf->last_dir = *dirout; + + cdf->available = 1; +} + +int +tracking_get_angle(struct tracking *tr) +{ + return tr->curr_angle; +} + struct pcoord * tracking_get_pos(struct tracking *tr) { @@ -348,12 +563,14 @@ tracking_value(struct tracking *tr, struct tracking_line *t, int offset, struct int -tracking_update(struct tracking *tr, struct pcoord *pc, int angle) +tracking_update(struct tracking *tr, struct pcoord *pc, int angle, double *hdop, int speed, time_t fixtime) { struct tracking_line *t; int i,value,min; struct coord lpnt; struct coord cin; + struct pcoord pcf; // Coordinate filtered through the CDF + int anglef; // Angle filtered through the CDF #if 0 int min,dist; int debug=0; @@ -366,19 +583,27 @@ tracking_update(struct tracking *tr, struct pcoord *pc, int angle) *pc=tr->curr_out; return 0; } + + if (*hdop > 3.5f) { // This value has been taken from julien cayzac's CDF implementation + *pc = tr->curr_out; + return 0; + } + + tracking_process_cdf(&tr->cdf, pc, &pcf, angle, &anglef, speed, fixtime); + tr->last_in=tr->curr_in; tr->last_out=tr->curr_out; tr->last[0]=tr->curr[0]; tr->last[1]=tr->curr[1]; - tr->curr_in=*pc; - tr->curr_angle=angle; - cin.x = pc->x; - cin.y = pc->y; - if (!tr->lines || transform_distance_sq_pc(&tr->last_updated, pc) > 250000) { + tr->curr_in=pcf; + tr->curr_angle=anglef; + cin.x = pcf.x; + cin.y = pcf.y; + if (!tr->lines || transform_distance_sq_pc(&tr->last_updated, &pcf) > 250000) { dbg(1, "update\n"); tracking_free_lines(tr); - tracking_doupdate_lines(tr, pc); - tr->last_updated=*pc; + tracking_doupdate_lines(tr, &pcf); + tr->last_updated=pcf; dbg(1,"update end\n"); } @@ -402,14 +627,14 @@ tracking_update(struct tracking *tr, struct pcoord *pc, int angle) tr->curr[1]=sd->c[i+1]; dbg(1,"lpnt.x=0x%x,lpnt.y=0x%x pos=%d %d+%d+%d+%d=%d\n", lpnt.x, lpnt.y, i, transform_distance_line_sq(&sd->c[i], &sd->c[i+1], &cin, &lpnt), - tracking_angle_delta(angle, t->angle[i], 0)*angle_factor, + tracking_angle_delta(anglef, t->angle[i], 0)*angle_factor, tracking_is_connected(tr->last, &sd->c[i]) ? connected_pref : 0, lpnt.x == tr->last_out.x && lpnt.y == tr->last_out.y ? nostop_pref : 0, value ); tr->curr_out.x=lpnt.x; tr->curr_out.y=lpnt.y; - tr->curr_out.pro = pc->pro; + tr->curr_out.pro = pcf.pro; min=value; } } @@ -427,6 +652,13 @@ struct tracking * tracking_new(struct attr *parent, struct attr **attrs) { struct tracking *this=g_new0(struct tracking, 1); + struct attr hist_size; + + if (! attr_generic_get_attr(attrs, NULL, attr_cdf_histsize, &hist_size, NULL)) { + hist_size.u.num = 0; + } + + tracking_init_cdf(&this->cdf, hist_size.u.num); return this; } -- cgit v1.2.1