Commit c8defd13 authored by Timothy Stack's avatar Timothy Stack

More tweaks and some comments.

parent 443582dc
...@@ -9,46 +9,38 @@ ...@@ -9,46 +9,38 @@
#include "visionTrack.h" #include "visionTrack.h"
static int vtCompare(struct vision_track *vt1, /**
struct vision_track *vt2, * Find the track in a list that is the minimum distance from a given track.
float tolerance) *
{ * @param vt The track to compare against the other tracks in the list.
float distance; * @param curr The track in a list where the search should start. The function
int retval; * will continue the search until it reaches the end of the list.
* @param distance_out Reference to a float where the minimum distance found
assert(vt1 != NULL); * should be stored.
assert(vt2 != NULL); */
distance = hypotf(vt1->vt_position.x - vt2->vt_position.x,
vt1->vt_position.y - vt2->vt_position.y);
retval = (distance < tolerance);
if (!retval) {
printf("miss %f %f\n", distance, tolerance);
}
return retval;
}
static struct vision_track *vtFindMin(struct vision_track *vt, static struct vision_track *vtFindMin(struct vision_track *vt,
struct lnMinList *list, struct vision_track *curr,
float *distance_out) float *distance_out)
{ {
struct vision_track *curr, *retval = NULL; struct vision_track *retval = NULL;
assert(vt != NULL); assert(vt != NULL);
assert(list != NULL); assert(curr != NULL);
assert(distance_out != NULL); assert(distance_out != NULL);
*distance_out = FLT_MAX; *distance_out = FLT_MAX;
curr = (struct vision_track *)list->lh_Head;
while (curr->vt_link.ln_Succ != NULL) { while (curr->vt_link.ln_Succ != NULL) {
float distance; float distance;
distance = hypotf(vt->vt_position.x - curr->vt_position.x, distance = hypotf(vt->vt_position.x - curr->vt_position.x,
vt->vt_position.y - curr->vt_position.y); vt->vt_position.y - curr->vt_position.y);
printf(" min %f %f - %f %f = %f\n",
vt->vt_position.x,
vt->vt_position.y,
curr->vt_position.x,
curr->vt_position.y,
distance);
if (distance < *distance_out) { if (distance < *distance_out) {
retval = curr; retval = curr;
*distance_out = distance; *distance_out = distance;
...@@ -95,6 +87,7 @@ int vtUpdate(struct lnMinList *now, ...@@ -95,6 +87,7 @@ int vtUpdate(struct lnMinList *now,
vt->vt_userdata = NULL; vt->vt_userdata = NULL;
lnAddTail(now, &vt->vt_link); lnAddTail(now, &vt->vt_link);
/* Adjust the cameras viewable area based on this track. */
if (mup->position.x < vc->vc_left) if (mup->position.x < vc->vc_left)
vc->vc_left = mup->position.x; vc->vc_left = mup->position.x;
if (mup->position.x > vc->vc_right) if (mup->position.x > vc->vc_right)
...@@ -138,7 +131,11 @@ void vtCoalesce(struct lnMinList *extra, ...@@ -138,7 +131,11 @@ void vtCoalesce(struct lnMinList *extra,
vt = (struct vision_track *)now->lh_Head; vt = (struct vision_track *)now->lh_Head;
while (vt->vt_link.ln_Succ != NULL) { while (vt->vt_link.ln_Succ != NULL) {
int in_camera_count = 0, lpc; int in_camera_count = 0, lpc;
/*
* Figure out how many cameras this track might be viewable in so we
* can coalesce them.
*/
for (lpc = 0; lpc < vc_len; lpc++) { for (lpc = 0; lpc < vc_len; lpc++) {
#if 0 #if 0
printf("vc2 %f %f -- %f %f %f %f\n", printf("vc2 %f %f -- %f %f %f %f\n",
...@@ -160,27 +157,23 @@ void vtCoalesce(struct lnMinList *extra, ...@@ -160,27 +157,23 @@ void vtCoalesce(struct lnMinList *extra,
assert(in_camera_count > 0); assert(in_camera_count > 0);
if (in_camera_count > 1) { if (in_camera_count > 1) {
struct vision_track *vt_extra, *vt_succ; struct vision_track *vt_extra;
float distance;
vt_extra = (struct vision_track *)vt->vt_link.ln_Succ; while ((in_camera_count > 1) &&
while (vt_extra->vt_link.ln_Succ != NULL) { ((vt_extra = vtFindMin(vt,
vt_succ = (struct vision_track *)vt_extra->vt_link.ln_Succ; (struct vision_track *)
vt->vt_link.ln_Succ,
printf("try coalesce %f %f -- %f %f %d %d\n", &distance)) != NULL) &&
(distance < 0.35)) {
printf("coalesce %f %f -- %f %f\n",
vt->vt_position.x, vt->vt_position.y, vt->vt_position.x, vt->vt_position.y,
vt_extra->vt_position.x, vt_extra->vt_position.y, vt_extra->vt_position.x, vt_extra->vt_position.y);
vt->vt_client->vc_port, vt_extra->vt_client->vc_port);
if ((vt->vt_client != vt_extra->vt_client) &&
vtCompare(vt, vt_extra, 0.35)) {
printf("coalesce %f %f -- %f %f\n",
vt->vt_position.x, vt->vt_position.y,
vt_extra->vt_position.x, vt_extra->vt_position.y);
lnRemove(&vt_extra->vt_link);
lnAddHead(extra, &vt_extra->vt_link);
}
vt_extra = vt_succ; lnRemove(&vt_extra->vt_link);
lnAddHead(extra, &vt_extra->vt_link);
in_camera_count -= 1;
} }
} }
...@@ -198,12 +191,18 @@ void vtMatch(struct lnMinList *pool, ...@@ -198,12 +191,18 @@ void vtMatch(struct lnMinList *pool,
assert(prev != NULL); assert(prev != NULL);
assert(now != NULL); assert(now != NULL);
/*
* Walk through the tracks in the current frame trying to find tracks in
* the previous frame, within some threshold.
*/
vt = (struct vision_track *)now->lh_Head; vt = (struct vision_track *)now->lh_Head;
while (vt->vt_link.ln_Succ != NULL) { while (vt->vt_link.ln_Succ != NULL) {
struct vision_track *vt_prev; struct vision_track *vt_prev;
float distance; float distance;
if ((vt_prev = vtFindMin(vt, prev, &distance)) != NULL) { if ((vt_prev = vtFindMin(vt,
(struct vision_track *)prev->lh_Head,
&distance)) != NULL) {
if (distance < 0.30) { if (distance < 0.30) {
vt->vt_userdata = vt_prev->vt_userdata; vt->vt_userdata = vt_prev->vt_userdata;
lnRemove(&vt_prev->vt_link); lnRemove(&vt_prev->vt_link);
...@@ -214,6 +213,10 @@ void vtMatch(struct lnMinList *pool, ...@@ -214,6 +213,10 @@ void vtMatch(struct lnMinList *pool,
vt = (struct vision_track *)vt->vt_link.ln_Succ; vt = (struct vision_track *)vt->vt_link.ln_Succ;
} }
/*
* Walk through the previous frame copying any young tracks to the current
* frame.
*/
vt = (struct vision_track *)prev->lh_Head; vt = (struct vision_track *)prev->lh_Head;
while (vt->vt_link.ln_Succ != NULL) { while (vt->vt_link.ln_Succ != NULL) {
struct vision_track *vt_next; struct vision_track *vt_next;
...@@ -259,12 +262,22 @@ struct vision_track *vtFindWiggle(struct lnMinList *start, ...@@ -259,12 +262,22 @@ struct vision_track *vtFindWiggle(struct lnMinList *start,
assert(start != NULL); assert(start != NULL);
assert(now != NULL); assert(now != NULL);
/*
* Walk through the current frame searching for tracks that have a small
* displacement from the start frame and large orientation change.
*/
vt = (struct vision_track *)now->lh_Head; vt = (struct vision_track *)now->lh_Head;
while ((vt->vt_link.ln_Succ != NULL) && (retval == NULL)) { while ((vt->vt_link.ln_Succ != NULL) && (retval == NULL)) {
struct vision_track *vt_start; struct vision_track *vt_start;
float distance; float distance;
if ((vt_start = vtFindMin(vt, start, &distance)) == NULL) { printf("check %f %f %f\n",
vt->vt_position.x,
vt->vt_position.y,
vt->vt_position.theta);
if ((vt_start = vtFindMin(vt,
(struct vision_track *)start->lh_Head,
&distance)) == NULL) {
} }
else if (distance < 0.05) { else if (distance < 0.05) {
float diff; float diff;
...@@ -285,6 +298,8 @@ struct vision_track *vtFindWiggle(struct lnMinList *start, ...@@ -285,6 +298,8 @@ struct vision_track *vtFindWiggle(struct lnMinList *start,
printf(" found it %p\n", retval); printf(" found it %p\n", retval);
} }
} }
printf("dist %p %f\n", vt_start, distance);
vt = (struct vision_track *)vt->vt_link.ln_Succ; vt = (struct vision_track *)vt->vt_link.ln_Succ;
} }
......
...@@ -15,22 +15,71 @@ struct vision_track { ...@@ -15,22 +15,71 @@ struct vision_track {
void *vt_userdata; void *vt_userdata;
}; };
/**
* Update the list of tracks for the current frame with the tracks from a given
* vmc-client.
*
* @param now The track list for the current frame, any new tracks will be
* added to this.
* @param client The reference to the vmc-client that produced a track.
* @param mp The packet received from the client.
* @param pool The pool of nodes to draw vision_track objects from.
* @return True if all of the tracks have been received from this client for
* the current frame.
*/
int vtUpdate(struct lnMinList *now, int vtUpdate(struct lnMinList *now,
struct vmc_client *client, struct vmc_client *client,
struct mtp_packet *mp, struct mtp_packet *mp,
struct lnMinList *pool); struct lnMinList *pool);
/**
* Coalesce tracks that are in an area where cameras are overlapping.
*
* @param extra The list to add any duplicate tracks found in the current
* frame.
* @param now The list of tracks from all of the cameras.
* @param vc The array of cameras, coalescing is based on the camera's viewable
* area.
* @param vc_len The length of the vc array.
*/
void vtCoalesce(struct lnMinList *extra, void vtCoalesce(struct lnMinList *extra,
struct lnMinList *now, struct lnMinList *now,
struct vmc_client *vc, struct vmc_client *vc,
unsigned int vc_len); unsigned int vc_len);
/**
* Match any tracks from the current frame against the previous frame. Any
* matches will have their vt_userdata value copied over. Any tracks from the
* previous frame that weren't matched, will have their vt_age value
* incremented and be appended to the current frame if their age value is less
* than 5. By keeping older frames around, we can deal with tracks that skip
* a few frames, which seems to happen at the edge of the camera's viewable
* area.
*
* @param pool The pool to add any older, or matched nodes, to.
* @param prev The previous frame to match.
* @param now The current frame.
*/
void vtMatch(struct lnMinList *pool, void vtMatch(struct lnMinList *pool,
struct lnMinList *prev, struct lnMinList *prev,
struct lnMinList *now); struct lnMinList *now);
/**
* Move any tracks whose vt_userdata == NULL from one list to another.
*
* @param unknown The destination for any tracks that have no userdata value.
* @param mixed The list of tracks to sort.
*/
void vtUnknownTracks(struct lnMinList *unknown, struct lnMinList *mixed); void vtUnknownTracks(struct lnMinList *unknown, struct lnMinList *mixed);
/**
* Find a vision_track that changed 180 degrees from a previous frame to the
* current frame.
*
* @param start A snapshot of unknown tracks from before the wiggle started.
* @param now The current frame.
* @param return The matched vision_track or NULL if none could be found.
*/
struct vision_track *vtFindWiggle(struct lnMinList *start, struct vision_track *vtFindWiggle(struct lnMinList *start,
struct lnMinList *now); struct lnMinList *now);
......
...@@ -201,8 +201,8 @@ static int parse_client_options(int *argcp, char **argvp[]) ...@@ -201,8 +201,8 @@ static int parse_client_options(int *argcp, char **argvp[])
int main(int argc, char *argv[]) int main(int argc, char *argv[])
{ {
struct lnMinList known_bots, unknown_bots, wiggling_bots, lost_bots;
struct lnMinList last_frame, current_frame, wiggle_frame; struct lnMinList last_frame, current_frame, wiggle_frame;
struct lnMinList known_bots, unknown_bots, wiggling_bots;
int lpc, current_client = 0, retval = EXIT_SUCCESS; int lpc, current_client = 0, retval = EXIT_SUCCESS;
struct robot_object *wiggle_bot = NULL; struct robot_object *wiggle_bot = NULL;
fd_set readfds; fd_set readfds;
...@@ -220,6 +220,7 @@ int main(int argc, char *argv[]) ...@@ -220,6 +220,7 @@ int main(int argc, char *argv[])
lnNewList(&known_bots); lnNewList(&known_bots);
lnNewList(&unknown_bots); lnNewList(&unknown_bots);
lnNewList(&wiggling_bots); lnNewList(&wiggling_bots);
lnNewList(&lost_bots);
do { do {
retval = parse_client_options(&argc, &argv); retval = parse_client_options(&argc, &argv);
...@@ -324,6 +325,38 @@ int main(int argc, char *argv[]) ...@@ -324,6 +325,38 @@ int main(int argc, char *argv[])
} }
} }
/*
* The main loop consists of:
*
* 1. Gathering all of the tracks from the cameras;
* 2. Coalescing the separate tracks into a single "frame" that has all
* of the tracks without any duplicates caused by overlapping cameras.
* 3. Matching the tracks in the current frame to those in the last
* frame, and copying the robot references from the old frame into
* the new frame.
* 4. Sort the list of robots into known and unknown based on whether
* or not they are referenced in the current frame.
* 5. Send MTP_WIGGLE_START to any unknown robots to stop them.
* 6. Move the current frame to the last frame and start reading tracks
* from the cameras again.
*
* Notes:
*
* + We will always get a packet from the vmc-clients, if there are no
* tracks, it will send an error. This behavior allows us to keep all
* of the cameras in sync.
*
* + I don't actually sync the cameras to the same frame, so there might
* be a few frames difference between tracks received from one camera
* and another.
*
* + We read the tracks from each camera in the same order each time so
* the coalesce operation always picks the same track when merging.
*
* + XXX I don't actually do anything with lost_bots at the moment, they
* should be rewiggled after a short timeout.
*/
FD_SET(vmc_clients[0].vc_handle->mh_fd, &readfds); FD_SET(vmc_clients[0].vc_handle->mh_fd, &readfds);
/* handle input from emc and vmc */ /* handle input from emc and vmc */
...@@ -336,25 +369,29 @@ int main(int argc, char *argv[]) ...@@ -336,25 +369,29 @@ int main(int argc, char *argv[])
int unknown_track_count = 0; int unknown_track_count = 0;
struct robot_object *ro; struct robot_object *ro;
printf("got whole frame\n"); /* We've got all of the camera tracks, start processing. */
vtCoalesce(&vt_pool, vtCoalesce(&vt_pool,
&current_frame, &current_frame,
vmc_clients, vmc_clients,
vmc_client_count); vmc_client_count); // Get rid of duplicates
/*
* Match the current frame to the last frame and throw any old
* frames (vt_age > 5) in the pool.
*/
vtMatch(&vt_pool, &last_frame, &current_frame); vtMatch(&vt_pool, &last_frame, &current_frame);
/* Reset the list of known/unknown bots. */
lnAppendList(&unknown_bots, &known_bots); lnAppendList(&unknown_bots, &known_bots);
/*
* Detect the robots that have references in the current frame and
* send an MTP_UPDATE_POSITION. All others are left in the
* unknown_bots list.
*/
vt = (struct vision_track *)current_frame.lh_Head; vt = (struct vision_track *)current_frame.lh_Head;
while (vt->vt_link.ln_Succ != NULL) { while (vt->vt_link.ln_Succ != NULL) {
#if 0
if (vt->vt_userdata == NULL) {
fake_userdata += 1;
vt->vt_userdata = fake_userdata;
}
#else
if ((ro = vt->vt_userdata) != NULL) { if ((ro = vt->vt_userdata) != NULL) {
struct mtp_packet ump; struct mtp_packet ump;
...@@ -373,7 +410,6 @@ int main(int argc, char *argv[]) ...@@ -373,7 +410,6 @@ int main(int argc, char *argv[])
unknown_track_count += 1; unknown_track_count += 1;
vt_unknown = vt; vt_unknown = vt;
} }
#endif
if (debug > 1) { if (debug > 1) {
printf("track %f %f %f - %d - %p %s\n", printf("track %f %f %f - %d - %p %s\n",
...@@ -398,6 +434,10 @@ int main(int argc, char *argv[]) ...@@ -398,6 +434,10 @@ int main(int argc, char *argv[])
} }
#endif #endif
/*
* Send MTP_WIGGLE_STARTs for any unknown robots, this should stop
* them and cause an IDLE wiggle_status to come back.
*/
ro = (struct robot_object *)unknown_bots.lh_Head; ro = (struct robot_object *)unknown_bots.lh_Head;
while (ro->ro_link.ln_Succ != NULL) { while (ro->ro_link.ln_Succ != NULL) {
if (!(ro->ro_flags & RF_WIGGLING)) { if (!(ro->ro_flags & RF_WIGGLING)) {
...@@ -417,18 +457,32 @@ int main(int argc, char *argv[]) ...@@ -417,18 +457,32 @@ int main(int argc, char *argv[])
} }
ro = (struct robot_object *)ro->ro_link.ln_Succ; ro = (struct robot_object *)ro->ro_link.ln_Succ;
} }
/* Throw any frames left from the last frame in the pool and */
lnAppendList(&vt_pool, &last_frame); lnAppendList(&vt_pool, &last_frame);
/*
* ... move the current frame to the last frame for the next
* iteration.
*/
lnMoveList(&last_frame, &current_frame); lnMoveList(&last_frame, &current_frame);
FD_SET(vmc_clients[0].vc_handle->mh_fd, &readfds); FD_SET(vmc_clients[0].vc_handle->mh_fd, &readfds);
current_client = 0; current_client = 0;
} }
/*
* Check if we need to identify any bots, assuming we're not in the
* process of finding one.
*/
if ((wiggle_bot == NULL) && !lnEmptyList(&wiggling_bots)) { if ((wiggle_bot == NULL) && !lnEmptyList(&wiggling_bots)) {
struct mtp_packet wmp; struct mtp_packet wmp;
/*
* Take a snapshot of the tracks with no robot attached from the
* last frame so we can compare it against the frame when the robot
* has finished its wiggle.
*/
vtUnknownTracks(&wiggle_frame, &last_frame); vtUnknownTracks(&wiggle_frame, &last_frame);
wiggle_bot = (struct robot_object *)lnRemHead(&wiggling_bots); wiggle_bot = (struct robot_object *)lnRemHead(&wiggling_bots);
mtp_init_packet(&wmp, mtp_init_packet(&wmp,
...@@ -460,7 +514,6 @@ int main(int argc, char *argv[]) ...@@ -460,7 +514,6 @@ int main(int argc, char *argv[])
struct robot_object *ro; struct robot_object *ro;
float distance; float distance;
/* XXX Need to handle update_id msgs here. */
if (debug > 0) { if (debug > 0) {
mtp_print_packet(stdout, &mp); mtp_print_packet(stdout, &mp);
} }
...@@ -471,25 +524,49 @@ int main(int argc, char *argv[]) ...@@ -471,25 +524,49 @@ int main(int argc, char *argv[])
switch (mws->status) { switch (mws->status) {
case MTP_POSITION_STATUS_IDLE: case MTP_POSITION_STATUS_IDLE:
/* Got a response to an MTP_WIGGLE_START. */
if ((ro = roFindRobot(&unknown_bots, if ((ro = roFindRobot(&unknown_bots,
mws->robot_id)) == NULL) { mws->robot_id)) == NULL) {
fatal("unknown bot %d\n", mws->robot_id); fatal("unknown bot %d\n", mws->robot_id);
} }
else { else {
/*
* Add him to the list of robots ready
* to be wiggled.
*/
lnRemove(&ro->ro_link); lnRemove(&ro->ro_link);
lnAddTail(&wiggling_bots, &ro->ro_link); lnAddTail(&wiggling_bots, &ro->ro_link);
} }
break; break;
case MTP_POSITION_STATUS_COMPLETE: case MTP_POSITION_STATUS_COMPLETE:
/*
* The robot finished his wiggle, check the
* current frame against the snapshot we took
* to find a robot in the same position, but
* with a 180 degree difference in orientation.
*/
if ((vt = vtFindWiggle(&wiggle_frame, if ((vt = vtFindWiggle(&wiggle_frame,
&last_frame)) == NULL) { &last_frame)) == NULL) {
fatal("no wiggle found!\n"); /*
* Couldn't locate the robot, add him to
* the list of "lost" robots which we'll
* try to rediscover after a short time.
*/
lnAddTail(&lost_bots,
&wiggle_bot->ro_link);
wiggle_bot = NULL;
} }
else { else {
info("found wiggle ! %p\n", vt); info("found wiggle ! %p\n", vt);
vt->vt_userdata = wiggle_bot; vt->vt_userdata = wiggle_bot;
wiggle_bot->ro_flags &= ~RF_WIGGLING; wiggle_bot->ro_flags &= ~RF_WIGGLING;
lnAddTail(&known_bots,
&wiggle_bot->ro_link);
wiggle_bot = NULL; wiggle_bot = NULL;
/*
* Return the frame we snapshotted awhile
* ago to the pool.
*/
lnAppendList(&vt_pool, &wiggle_frame); lnAppendList(&vt_pool, &wiggle_frame);
} }
break; break;
...@@ -524,8 +601,13 @@ int main(int argc, char *argv[]) ...@@ -524,8 +601,13 @@ int main(int argc, char *argv[])
printf("got frame from client %s:%d\n", printf("got frame from client %s:%d\n",
vc->vc_hostname, vc->vc_hostname,
vc->vc_port); vc->vc_port);
/*
* Got all of the tracks from this client, clear
* his bit and
*/
FD_CLR(vc->vc_handle->mh_fd, &readfds); FD_CLR(vc->vc_handle->mh_fd, &readfds);
/* ... try to wait for the next one. */