//
// Removes an observation from the map at position x,y. Node is the index into the
// dynamic array of which observation needs to be removed. This is typically gotten
// from the ancestry node, since the death of a node is currently the only way to
// call this function.
//
void LowDeleteObservation(short int x, short int y, short int node) {
int total;
// We may have already removed this observation previously in the process of
// resizing the array at some point. In that case, there's nothing to do here.
if ((node == -1) || (lowMap[x][y] == NULL))
return;
// If this is the last observation left at this location in the map, then we can
// revert the whole entry in the map to NULL, indicating that no current particle
// has observed this location.
if (lowMap[x][y]->total - lowMap[x][y]->dead == 1) {
free(lowMap[x][y]->array);
free(lowMap[x][y]);
lowMap[x][y] = NULL;
return;
}
// Look to see if we need to shrink the array
if ((int)((lowMap[x][y]->total - 1 - lowMap[x][y]->dead)*2.5) <= lowMap[x][y]->size) {
// Let resizing the array remove this entry (it's what is considered a "dead" entry
// now, as indicated by the second argument).
LowResizeArray(lowMap[x][y], lowMap[x][y]->array[node].ID);
if (lowMap[x][y]->total == 0) {
free(lowMap[x][y]->array);
free(lowMap[x][y]);
lowMap[x][y] = NULL;
}
return;
}
// If we got this far, we are removing the entry manually. Make a note that we have
// one less entry here.
lowMap[x][y]->total--;
// This variable is here solely to make the code mroe readable by having less
// redirections and indexes.
total = lowMap[x][y]->total;
// If the ibservation was the last one in the array, our work is done. Otherwise,
// we take the last observation in dynamic array, and copy it ontop of the observation
// we are deleting. Since we don't use any of the entries beyond lowMap[x][y].total,
// that last observation is essentially already deleted, and we don't have to worry about
// duplicates.
if (node != lowMap[x][y]->total) {
lowMap[x][y]->array[node].hits = lowMap[x][y]->array[total].hits;
lowMap[x][y]->array[node].distance = lowMap[x][y]->array[total].distance;
lowMap[x][y]->array[node].ID = lowMap[x][y]->array[total].ID;
lowMap[x][y]->array[node].source = lowMap[x][y]->array[total].source;
lowMap[x][y]->array[node].parentGen = lowMap[x][y]->array[total].parentGen;
l_particleID[ lowMap[x][y]->array[node].ID ].mapEntries[ lowMap[x][y]->array[node].source ].node = node;
}
}
//
// Finds the appropriate entry in the designated grid square, and then makes a duplicate of that entry
// modified according to the input.
//
void LowUpdateGridSquare(int x, int y, double distance, int hit, int parentID)
{
TEntryList *tempEntry;
int here, i;
// If the grid square was previously unobserved, then we will need to create a new
// entry in the observationArray for it, so that later accesses can take full advantage
// of constant time access.
if (lowMap[x][y] == NULL) {
// Check to make sure there is still room left in the observation cache.
if (observationID >= AREA)
fprintf(stderr, "bRoll over!\n");
// Display ownership of this slot
flagMap[x][y] = observationID;
obsX[observationID] = x;
obsY[observationID] = y;
observationID++;
// Since the grid square was unobserved previously, we will also need to create a
// new entry into the map at this location, that we can then build on.
// The first step is to create a starter structure, to keep track of the dynamic array
// of observations.
lowMap[x][y] = (TMapStarter *) malloc(sizeof(TMapStarter));
if (lowMap[x][y] == NULL) fprintf(stderr, "Malloc failed in creation of Map Starter at %d %d\n", x, y);
// No dead or obsolete entries yet.
lowMap[x][y]->dead = 0;
// No entries have actually been added to this location yet. We will increment this counter later.
lowMap[x][y]->total = 0;
// We will only have room for one observation in this grid square so far. Later, this can grow.
lowMap[x][y]->size = 1;
// The actual dynamic array is created here, of exactly the size for one entry.
lowMap[x][y]->array = (TMapNode *) malloc(sizeof(TMapNode));
if (lowMap[x][y]->array == NULL) fprintf(stderr, "Malloc failed in making initial map array for %d %d\n", x, y);
// Initialize the slot
for (i=0; i < ID_NUMBER; i++)
observationArray[flagMap[x][y]][i] = -1;
}
// We could have observations here, but this square hasn't been observed yet this iteration.
// In that case, we need to build an entry into the observationArray for constant time access.
else if (flagMap[x][y] == 0)
LowBuildObservation(x, y, 0);
// Note where in the dynamic array of observations we need to look for this one particle's
// relevent observation. This is indicated to us by the observationArray.
here = observationArray[flagMap[x][y]][parentID];
// If the ID of the relevent observation is the same as our altering particle's ID, then the
// new observation is merely an amendment to this data, and noone else is using it yet. Just
// alter the source directly
if ((here != -1) && (lowMap[x][y]->array[here].ID == parentID)) {
lowMap[x][y]->array[here].hits = lowMap[x][y]->array[here].hits + hit;
lowMap[x][y]->array[here].distance = lowMap[x][y]->array[here].distance + distance;
}
// Otherwise, we need to use that relevent observation in order to create a new observation.
// Otherwise, we can corrupt the data for other particles.
else {
// We will be adding a new entry to the list- is there enough room?
if (lowMap[x][y]->size <= lowMap[x][y]->total) {
LowResizeArray(lowMap[x][y], -71);
if (lowMap[x][y]->total == 0) {
free(lowMap[x][y]->array);
free(lowMap[x][y]);
lowMap[x][y] = NULL;
}
}
// Make all changes before incrementing lowMap[x][y]->total, since it's used as an index
// Update the observationArray, to let it know that this ID will have its own special entry
// in the global at this location.
observationArray[flagMap[x][y]][parentID] = lowMap[x][y]->total;
// Add an entry in to the list of altered map squares for this particle
// First check to see if the size of that array is big enough to hold another entry
if (l_particleID[parentID].size == 0) {
l_particleID[parentID].size = 1;
l_particleID[parentID].mapEntries = (TEntryList *) malloc(sizeof(TEntryList));
if (l_particleID[parentID].mapEntries == NULL) fprintf(stderr, "Malloc failed in creation of entry list array\n");
}
else if (l_particleID[parentID].size <= l_particleID[parentID].total) {
l_particleID[parentID].size = (int)(ceil(l_particleID[parentID].total*1.25));
tempEntry = (TEntryList *) malloc(sizeof(TEntryList)*l_particleID[parentID].size);
if (tempEntry == NULL) fprintf(stderr, "Malloc failed in expansion of entry list array\n");
for (i=0; i < l_particleID[parentID].total; i++) {
tempEntry[i].x = l_particleID[parentID].mapEntries[i].x;
tempEntry[i].y = l_particleID[parentID].mapEntries[i].y;
tempEntry[i].node = l_particleID[parentID].mapEntries[i].node;
}
free(l_particleID[parentID].mapEntries);
l_particleID[parentID].mapEntries = tempEntry;
}
// Add the location of this new entry to the list in the ancestry node
l_particleID[parentID].mapEntries[l_particleID[parentID].total].x = x;
l_particleID[parentID].mapEntries[l_particleID[parentID].total].y = y;
l_particleID[parentID].mapEntries[l_particleID[parentID].total].node = lowMap[x][y]->total;
// i is used as a quick reference guide here, in order to make the code easier to read.
i = lowMap[x][y]->total;
// The pointers between the ancestry node's list and the map's observation list need
// to point back towards each other, in order to coordinate data.
lowMap[x][y]->array[i].source = l_particleID[parentID].total;
// Assign the appropriate ID to this new observation.
lowMap[x][y]->array[i].ID = parentID;
// Note that we now have one more observation at this ancestor node
l_particleID[parentID].total++;
// Check to see if this square has been observed by an ancestor
if (here == -1) {
// Previously unknown; clean slate. Update directly with the observed data.
lowMap[x][y]->array[i].hits = hit;
// Include the strength of the prior.
lowMap[x][y]->array[i].distance = distance + L_PRIOR_DIST;
// A value of -2 here indicates that this observation had no preceding observation
// that it was building off of.
lowMap[x][y]->array[i].parentGen = -2;
}
else {
// Include the pertinent info from the old observation in with the new observation.
lowMap[x][y]->array[i].hits = lowMap[x][y]->array[here].hits + hit;
lowMap[x][y]->array[i].distance = distance + lowMap[x][y]->array[here].distance;
lowMap[x][y]->array[i].parentGen = l_particleID[ lowMap[x][y]->array[here].ID ].generation;
}
// Now we can acknowledge that there is another observation at this grid square.
lowMap[x][y]->total++;
}
}
//
// UpdateAncestry
//
// Every iteration, after particles have been resampled and evaluated (ie after Localize has been run),
// we will need to update the ancestry tree. This consists of four main steps.
// a) Remove dead nodes. These are defined as any ancestor node which has no descendents in the current
// generation. This is caused by certain particles not being resampled, or by a node's children all
// dying off on their own. These nodes not only need to be removed, but every one of their observations
// in their observations in the map also need to be removed.
// b) Collapse branches of the tree. We want to restrict each internal node to having a branching factor
// of at least two. Therefore, if a node has only one child, we merge the information in that node with
// the one child node. This is especially common at the root of the tree, as the different hypotheses
// coalesce.
// c) Add the new particles into the tree.
// d) Update the map for each new particle. We needed to wait until they were added into the tree, so that
// the ancestry tree can keep track of the different observations associated with the particle.
//
void UpdateAncestry(TSense sense, TAncestor particleID[])
{
int i, j;
TAncestor *temp, *hold, *parentNode;
TEntryList *entry, *workArray;
TMapStarter *node;
TPath *tempPath, *trashPath;
// Remove Dead Nodes -
// Go through the current particle array, and prune out all particles that did not spawn any particles. We know that the particle
// had to spawn samples, but those samples may not have become particles themselves, through not generating any samples for the
// next generation. Recurse up through there.
for (i=0; i < l_cur_particles_used; i++) {
temp = l_particle[i].ancestryNode;
// This is a "while" loop for purposes of recursing up the tree.
while (temp->numChildren == 0) {
// Free up the memory in the map by deleting the associated observations.
for (j=0; j < temp->total; j++)
LowDeleteObservation(temp->mapEntries[j].x, temp->mapEntries[j].y, temp->mapEntries[j].node);
// Get rid of the memory being used by this ancestor
free(temp->mapEntries);
temp->mapEntries = NULL;
// This is used exclusively for the low level in hierarchical SLAM.
// Get rid of the hypothesized path that this ancestor used.
tempPath = temp->path;
while (tempPath != NULL) {
trashPath = tempPath;
tempPath = tempPath->next;
free(trashPath);
}
temp->path = NULL;
// Recover the ID, so that it can be used later.
cleanID++;
availableID[cleanID] = temp->ID;
temp->generation = curGeneration;
temp->ID = -42;
// Remove this node from the tree, while keeping track of its parent. We need that for recursing
// up the tree (its parent could possibly need to be removed itself, now)
hold = temp;
temp = temp->parent;
hold->parent = NULL;
// Note the disappearance of this particle (may cause telescoping of particles, or outright deletion)
temp->numChildren--;
}
}
// Collapse Branches -
// Run through the particle IDs, checking for those node IDs that are currently in use. Essentially is
// an easy way to pass through the entire tree. If the node is in use, look at its parent.
// If the parent has only one child, give all of the altered map squares of the parent to the child,
// and dispose of the parent. This collapses those ancestor nodes which have a branching factor of only one.
for (i = 0; i < ID_NUMBER-1; i++) {
// These booleans mean (in order) that the ID is in use, it has a parent (ie is not the root of the ancestry tree),
// and that its parent has only one child (which is necessarily this ID)
if ((particleID[i].ID == i) && (particleID[i].parent != NULL) && (particleID[i].parent->numChildren == 1)) {
// This is a special value for redirections. If the node's parent has already participated in a collapse
// during this generation, then that node has already been removed from the tree, and we need to go up
// one more step in the tree.
while (particleID[i].parent->generation == -111)
particleID[i].parent = particleID[i].parent->parent;
parentNode = particleID[i].parent;
// Check to make sure that the parent's array is large enough to accomadate all of the entries of the child
// in addition to its own. If not, we need to increase the dynamic array.
if (parentNode->size < (parentNode->total + particleID[i].total)) {
parentNode->size = (int)(ceil((parentNode->size + particleID[i].size)*1.5));
workArray = (TEntryList *)malloc(sizeof(TEntryList)*parentNode->size);
if (workArray == NULL) fprintf(stderr, "Malloc failed for workArray\n");
for (j=0; j < parentNode->total; j++) {
workArray[j].x = parentNode->mapEntries[j].x;
workArray[j].y = parentNode->mapEntries[j].y;
workArray[j].node = parentNode->mapEntries[j].node;
}
// Note that parentNode->total hasn't changed- that will grow as the child's entries are added in
free(parentNode->mapEntries);
parentNode->mapEntries = workArray;
}
// Change all map entries of the parent to have the ID of the child
// Also check to see if this entry supercedes an entry currently attributed to the parent.
// Since collapses can merge all of the entries between the parent and the current child into the parent, this check is performed
// by comparing to see if the generation of the last observation (before the child's update) is at least as recent as parent's
// generation. If so, note that there is another "dead" entry in the observation array. It will be cleaned up later. If this puts
// the total number of used slot, minus the number of "dead", below the threshold, shrink the array (which cleans up the dead)
entry = particleID[i].mapEntries;
for (j=0; j < particleID[i].total; j++) {
node = lowMap[entry[j].x][entry[j].y];
// Change the ID
node->array[entry[j].node].ID = parentNode->ID;
node->array[entry[j].node].source = parentNode->total;
parentNode->mapEntries[parentNode->total].x = entry[j].x;
parentNode->mapEntries[parentNode->total].y = entry[j].y;
parentNode->mapEntries[parentNode->total].node = entry[j].node;
parentNode->total++;
// Check for pre-existing observation in the parent's list
if (node->array[entry[j].node].parentGen >= parentNode->generation) {
node->array[entry[j].node].parentGen = -1;
node->dead++;
}
}
// We do this in a second pass for a good reason. If there are more than one update for a given grid square which uses the child's
// ID (as a consequence of an earlier collapse), then we want to make certain that the resizing doesn't take place until after all
// entries have changed their ID appropriately.
for (j=0; j < particleID[i].total; j++) {
node = lowMap[entry[j].x][entry[j].y];
if ((node->total - node->dead)*2.5 < node->size)
LowResizeArray(node, -7);
}
// We're done with it- remove the array of updates from the child.
free(particleID[i].mapEntries);
particleID[i].mapEntries = NULL;
// Inherit the path
tempPath = parentNode->path;
while (tempPath->next != NULL)
tempPath = tempPath->next;
tempPath->next = particleID[i].path;
particleID[i].path = NULL;
// Inherit the number of children
parentNode->numChildren = particleID[i].numChildren;
// Subtlety of the ancestry tree: since we only keep pointers up to the parent, we can't exactly change all of the
// descendents of the child to now point to the parent. What we can do, however, is mark the change for later, and
// update all of the ancestor particles in a single go, later. That will take a single O(P) pass
particleID[i].generation = -111;
}
}
// This is the step where we correct for redirections that arise from the collapse of a branch of the ancestry tree
for (i=0; i < ID_NUMBER-1; i++)
if (particleID[i].ID == i) {
while (particleID[i].parent->generation == -111)
particleID[i].parent = particleID[i].parent->parent;
}
// Wipe the slate clean, so that we don't get confused by the mechinations of the previous changes
// from deletes and merges. Updates can make thier own tables, as needed.
LowInitializeFlags();
// Add the current savedParticles into the ancestry tree, and copy them over into the 'real' particle array
j = 0;
for (i = 0; i < cur_saved_particles_used; i++) {
// Check for redirection of parent pointers due to collapsing of branches (see above)
while (savedParticle[i].ancestryNode->generation == -111)
savedParticle[i].ancestryNode = savedParticle[i].ancestryNode->parent;
// A saved particle has ancestryNode denote the parent particle for that saved particle
// If that parent has only this one child, due to resampling, then we want to perform a collapse
// of the branch, but it hasn't been done yet, because the savedParticle hasn't been entered into
// the tree yet. Therefore, we just designate the parent as the "new" entry for this savedParticle.
// We then update the already created ancestry node as if it were the new node.
if (savedParticle[i].ancestryNode->numChildren == 1) {
// Change the generation of the node
savedParticle[i].ancestryNode->generation = curGeneration;
// Now that it represents the new node as well, it no longer is considered to have children.
savedParticle[i].ancestryNode->numChildren = 0;
// We're copying the savedParticles to the main particle array
l_particle[j].ancestryNode = savedParticle[i].ancestryNode;
// Add a new entry to the path of the ancestor node.
trashPath = (TPath *)malloc(sizeof(TPath));
trashPath->C = savedParticle[i].C;
trashPath->D = savedParticle[i].D;
trashPath->T = savedParticle[i].T;
trashPath->dist = savedParticle[i].dist;
trashPath->turn = savedParticle[i].turn;
trashPath->next = NULL;
tempPath = l_particle[i].ancestryNode->path;
while (tempPath->next != NULL)
tempPath = tempPath->next;
tempPath->next = trashPath;
l_particle[j].x = savedParticle[i].x;
l_particle[j].y = savedParticle[i].y;
l_particle[j].theta = savedParticle[i].theta;
l_particle[j].probability = savedParticle[i].probability;
j++;
}
// IF the parent has multiple children, then each child needs its own new ancestor node in the tree
else if (savedParticle[i].ancestryNode->numChildren > 0) {
// Find a new entry in the array of ancestor nodes. This is done by taking an unused ID off of the
// stack, and using that slot.
temp = &(particleID[ availableID[cleanID] ]);
temp->ID = availableID[cleanID];
// That ID on the top of the stack is now being used.
cleanID--;
if (cleanID < 0) {
fprintf(stderr, " !!! Insufficient Number of Particle IDs : Abandon Ship !!!\n");
cleanID = 0;
}
// This new node needs to have its info filled in
temp->parent = savedParticle[i].ancestryNode;
// No updates to the map have been made yet for this node
temp->mapEntries = NULL;
temp->total = 0;
temp->size = 0;
// The generation of this node is important for collapsing branches of the tree. See above.
temp->generation = curGeneration;
temp->numChildren = 0;
temp->seen = 0;
// This is where we add a new entry to this node's hypothesized path for the robot
trashPath = (TPath *)malloc(sizeof(TPath));
trashPath->C = savedParticle[i].C;
trashPath->D = savedParticle[i].D;
trashPath->T = savedParticle[i].T;
trashPath->dist = savedParticle[i].dist;
trashPath->turn = savedParticle[i].turn;
trashPath->next = NULL;
temp->path = trashPath;
// Transfer this entry over to the main particle array
l_particle[j].ancestryNode = temp;
l_particle[j].x = savedParticle[i].x;
l_particle[j].y = savedParticle[i].y;
l_particle[j].theta = savedParticle[i].theta;
l_particle[j].probability = savedParticle[i].probability;
j++;
}
}
l_cur_particles_used = cur_saved_particles_used;
// Here's where we actually go through and update the map for each particle. We had to wait
// until now, so that the appropriate structures in the ancestry had been created and updated.
for (i=0; i < l_cur_particles_used; i++)
AddToWorldModel(sense, i);
// Clean up the ancestry particles which disappeared in branch collapses. Also, recover their IDs.
// We waited until now because we needed to allow for redirection of parents.
for (i=0; i < ID_NUMBER-1; i++)
if (particleID[i].generation == -111) {
particleID[i].generation = -1;
particleID[i].numChildren = 0;
particleID[i].parent = NULL;
particleID[i].mapEntries = NULL;
particleID[i].path = NULL;
particleID[i].seen = 0;
particleID[i].total = 0;
particleID[i].size = 0;
// Recover the ID.
cleanID++;
availableID[cleanID] = i;
particleID[i].ID = -3;
}
}
