lowmap.c

卡耐基.梅隆大学的机器人仿真软件（Redhat linux 9下安装）

      if (node->array[i].parentGen >= 0)
	node->array[ workingArray[node->array[i].ID] ].parentGen = node->array[i].parentGen;
    }

    // The previously entered entry is outdated. Replace it with this newer one.
    else {
      j = workingArray[node->array[i].ID];
      workingArray[node->array[i].ID] = i;
      if (node->array[j].parentGen >= 0)
	node->array[i].parentGen = node->array[j].parentGen;
    }
    
    // The node identified as "j" is dead. Remove its entry from the list of altered squares in the ancestor tree.
    l_particleID[node->array[j].ID].total--;

    entries = l_particleID[node->array[j].ID].mapEntries;
    source = node->array[j].source;
    last = l_particleID[node->array[j].ID].total;

    if (last != source) {
      entries[source].x = entries[last].x;
      entries[source].y = entries[last].y;
      entries[source].node = entries[last].node;

      // Somewhat confusing- we just removed an entry from the list of altered squares maintained by an ancestor particle (entries)
      // This means moving an entry from the end of that list to the spot which was vacated (entries[l_particleID[node->array[j].ID].total])
      // Therefore, the entry in the map corresponding to that last entry needs to point to the new entry.
      lowMap[ entries[source].x ][ entries[source].y ]->array[ entries[source].node ].source = source;
    }    

    // Now remove the node itself
    node->total--;
    node->dead--;

    if (j != node->total) {
      node->array[j].parentGen = node->array[node->total].parentGen;
      node->array[j].distance = node->array[node->total].distance;
      node->array[j].source = node->array[node->total].source;
      node->array[j].hits = node->array[node->total].hits;
      node->array[j].ID = node->array[node->total].ID;
      // We just moved the last entry in the list to position j. Update it's source entry in the ancestry tree to reflect its new position
      l_particleID[ node->array[j].ID ].mapEntries[ node->array[j].source ].node = j;

      // If the entry we just moved was in workingArray, we need to correct workingArray.
      // Also, we know that since it has been entered already, we don't need to enter it again
      if (workingArray[node->array[j].ID] == node->total)
	workingArray[node->array[j].ID] = j;
      else if (i != node->total) 
	// Final step- add this newly copied node to the working array (we don't want it skipped over)
	AddToWorkingArray(j, node, workingArray);
    }

  }
}


//
// This function is called whenever a grid square in the global map (which has at least one 
// observation associated with it) is accessed for the first time in an iteration. This
// function then creates an entry in the observationArray for this location.  This 
// effectively expands the local map by one grid square, and allows any future accesses to
// this grid square to be completed in constant time. This function itself can take O(P) time.
//
inline void LowBuildObservation(int x, int y, char usage)
{
  TAncestor *lineage;
  PAncestor stack[PARTICLE_NUMBER];
  short int workingArray[ID_NUMBER+1];
  int i, here, topStack;
  char flag = 0;

  // The size of the observationArray is not large enough- we throw out an error
  // message and stop the program
  if (observationID >= AREA) 
    fprintf(stderr, "aRoll over!\n");

  // Grab a slot in the observationArray
  flagMap[x][y] = observationID;
  obsX[observationID] = x;
  obsY[observationID] = y;
  observationID++;
  here = flagMap[x][y];

  // Initialize the slot and the ancestor particles
  for (i=0; i < ID_NUMBER; i++) {
    observationArray[here][i] = -1;
    workingArray[i] = -1;
    l_particleID[i].seen = 0;
  }

  // Fill in the particle entries of the array that made direct observations
  // to this grid square
  for (i=0; i < lowMap[x][y]->total; i++) 
    AddToWorkingArray(i, lowMap[x][y], workingArray);

  // A trick to speed up code when localizing. If an observation has no hits, 
  // then it has a 0% chance of stopping the laser, regardless of any other 
  // info. Marking that specially will remove an extra memory call, which 
  // would almost certainly be a cache miss. Also, if all entries are "empty",
  // then maybe we can skip the whole access to the observationArray entirely.
  if (usage) {
    flag = 1;
    for (i=0; i < lowMap[x][y]->total; i++) 
      if (lowMap[x][y]->array[i].hits > 0) 
	flag = 0;
      else
	workingArray[lowMap[x][y]->array[i].ID] = -2;
  }

  // Fill in the holes in the observation array, by using the value of their parents
  for (i=0; i < l_cur_particles_used; i++) {
    lineage = l_particle[i].ancestryNode;
    topStack = 0;

    // Eventually we will either get to an ancestor that we have already seen,
    // or we will hit the top of the tree (and thus its parent is NULL)
    // We never have to play with the root of the observation tree, because it has no parent
    while ((lineage != NULL) && (lineage->seen == 0)) {
      // put this ancestor on the stack to look at later
      stack[topStack] = lineage;
      topStack++;
      // Note that we already have seen this ancestor, for later lineage searches
      lineage->seen = 1;
      lineage = lineage->parent;  // Advance to this ancestor's parent
    }

    // Now trapse back down the stack, filling in each ancestor's info if need be
    while (topStack > 0) {
      topStack--;
      lineage = stack[topStack];
      // Try to fill in the holes of UNKNOWN. If the parent is also UNKNOWN, we know by construction 
      // that all of the other ancestors are also UNKNOWN, and thus the designation is correct
      if ((workingArray[lineage->ID] == -1) && (lineage->parent != NULL)) {
	workingArray[lineage->ID] = workingArray[lineage->parent->ID];
	// If any particle still has an "unobserved" value for this square, we can't use our cheat to
	// speed up code.
	if (workingArray[lineage->ID] == -1) 
	  flag = 0;
      }
    }
  }

  // If we are only localizing right now (usage) and all particles agree that this grid square
  // is empty (flag), then any access to this grid square doesn't even have to go as far as the
  // observation array- a glance at the flagMap can indicate that the desity is 0, regardless of
  // which particle is making the access.
  if ((usage) && (flag)) 
    flagMap[x][y] = -2;
  else
    for (i=0; i < ID_NUMBER; i++) 
      observationArray[here][i] = workingArray[i];
}




//
// 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;
    }