lowmap.c

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

    // Now we can acknowledge that there is another observation at this grid square.
    lowMap[x][y]->total++;
  }
}




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



//
// Input: x, y- location of a grid square
//        distance- the length of a line passing through the square
//        variance- an output, which will be filled the variance for errors in the lasercast that 
//                  should stop here.
// Output: returns the probability of trace of the given length through this square will be stopped by an obstacle
//
inline double LowComputeProbability(int x, int y, double distance, int parentID) 
{
  // If there are no entries at this location in the map, we know that the observation
  // for any particle is UNKNOWN. Use the density of our prior for unknown grid squares
  if (lowMap[x][y] == NULL) 
    return (1.0 - exp(L_PRIOR * distance));

  // If this grid square has been observed already this iteration, the flagMap will show
  // how to get constant time access. If that value is set to 0, we know that this location
  // has yet to be accessed this iteration, and we have build the observation array entry 
  // for this square
  if (flagMap[x][y] == 0) 
    LowBuildObservation(x, y, 1);

  // If the flagMap is set to the constant -2, all particles agree that this location is
  // empty. We can avoid significant pointer redirection and memory accesses, and just
  // acknowledge that an empty square has probability 0 of stopping a scan.
  if (flagMap[x][y] == -2)
    return 0;

  // If the observationArray does not have an entry for this particle (as indicated by
  // the index of -1) then this location is considered UNKNOWN for this particle, and
  // we can use our prior value for density.
  if (observationArray[flagMap[x][y]][parentID] == -1)
    return (1.0 - exp(L_PRIOR * distance));
  // This value of -2 is a constant used to indicate that the square is empty, and
  // it is not necessary to access the lowMap, and risk a cache miss.
  if (observationArray[flagMap[x][y]][parentID] == -2)
    return 0;
  // If there is an entry in the observationArray, then we use that entry as an index
  // into the global map at the relevent location, and retrieve the information 
  // appropriate to compute the density, specifically the number of observed laser stops
  // in that grid square (hits) and the total distance of laser scans that we have
  // observed passing through that grid square (distance). density = hits/distance.
  // Note that if no laser scan have been observed to stop in this square, density is
  // zero, and no matter what the distance currently being observed to pass through the 
  // square, there is no chance that it will stop the scan. 
  if (lowMap[x][y]->array[ observationArray[flagMap[x][y]][parentID] ].hits == 0)
    return 0;
  return (1.0 - exp(-(lowMap[x][y]->array[ observationArray[flagMap[x][y]][parentID] ].hits/
		      lowMap[x][y]->array[ observationArray[flagMap[x][y]][parentID] ].distance) * distance));
}



// 
// This function performs the exact same function as the one above, except that it can work
// without the observation array. This is useful for printing out the map or doing debugging
// or similar investigation in areas which are not within the area currently being observed.
//
double LowComputeProb(int x, int y, double distance, int ID) 
{
  int i;

  if (lowMap[x][y] == NULL) 
    return UNKNOWN;

  while (1) {
    for (i=0; i < lowMap[x][y]->total; i++) {
      if (lowMap[x][y]->array[i].ID == ID) {
	if (lowMap[x][y]->array[i].hits == 0)
	  return 0;
	return (1.0 - exp(-(lowMap[x][y]->array[i].hits/lowMap[x][y]->array[i].distance) * distance));
      }
    }

    if (l_particleID[ID].parent == NULL)
      return UNKNOWN;
    else 
      ID = l_particleID[ID].parent->ID;
  }

  return UNKNOWN;
}




//
// Takes as input the parameters of a laser scan, and updates the world map appropriately
// startx and stary are the origins of the laser scan, MeasuredDist is how far it was
// was percieved to travel (the length of the line trace), and theta was the angle of the
// line (in radians). parentID lets us know which particle ID this update is associated with, 
// and addEnd = 1 when the laser scan was stopped by an object (instead of just travelling
// maximum range without seeing anything) indicating that the last grid square needs to be
// updated as occupied.
//
void LowAddTrace(double startx, double starty, double MeasuredDist, double theta, int parentID, int addEnd) {
  double overflow, slope; // Used for actually tracing the line
  int x, y, incX, incY, endx, endy;
  int xedge, yedge;       // Used in computing the midpoint. Recompensates for which edge of the square the line entered from
  double dx, dy;
  double distance, error;
  double secant, cosecant;   // precomputed for speed

  // Precomute a few numbers for speed.
  secant = 1.0/fabs(cos(theta));
  cosecant = 1.0/fabs(sin(theta));

  // This allows the user to limit the effective range of the sensor
  distance = MIN(MeasuredDist, MAX_SENSE_RANGE);

  // Mark the final endpoint of the line trace, so that we know when to stop.
  // We keep the endpoint as both float and int.
  dx = (startx + (cos(theta) * distance));
  dy = (starty + (sin(theta) * distance));
  endx = (int) (dx);
  endy = (int) (dy);

  // Ensures that we don't enter the loop: ray-tracing is done here. 
  if (endx == (int)startx && endy == (int)starty)
    return; 

  // Decide which x and y directions the line is travelling.
  // inc tells us which way to increment x and y. edge indicates whether we are computing
  // distance from the near or far edge.
  if (startx > dx) {
    incX = -1;
    xedge = 1;
  }
  else {
    incX = 1;
    xedge = 0;
  }
  
  if (starty > dy) {
    incY = -1;
    yedge = 1;
  }
  else {
    incY = 1;
    yedge = 0;
  }

  // Figure out whether primary motion is in the x or y direction. 
  // The two sections of code look nearly identical, with x and y reversed.
  if (fabs(startx - dx) > fabs(starty - dy)) {
    // The given starting point is non-integer. The line therefore starts at some point partially set in to the starting
    // square. Overflow starts at this offcenter amount, in order to make steps in the y direction at the right places.
    y = (int) (starty);
    overflow =  starty - y;
    // We always use overflow as a decreasing number- therefore positive y motion needs to 
    // adjust the overflow value accordingly.
    if (incY == 1)
      overflow = 1.0 - overflow;
    // Compute the effective slope of the line.
    slope = fabs(tan(theta));

    // The first square is a delicate thing, as we aren't doing a full square traversal in
    // either direction. So we figure out this strange portion of a step so that we can then
    // work off of the axes later. 
    // NOTE: we aren't computing the probability of this first step. Its a technical issue for
    // simplicity, and the odds of the sensor sitting on top of a solid object are sufficiently 
    // close to zero to ignore this tiny portion of a step. 
    error = fabs(((int)(startx)+incX+xedge)-startx);
    overflow = overflow - (slope*error);
    // The first step is actually in the y direction, due to the proximity of starty to the y axis. 
    if (overflow < 0.0) {
      y = y + incY;
      overflow = overflow + 1.0;
    }

    // Now we can start the actual line trace.
    for (x = (int) (startx) + incX; x != endx; x = x + incX) {
      overflow = overflow - slope;

      // Compute the distance travelled in this square
      if (overflow < 0.0)
	distance = (overflow+slope)*cosecant;
      else
	distance = fabs(slope)*cosecant;
      // Update every grid square we cross as empty...
      LowUpdateGridSquare(x, y, distance, 0, parentID);

      // ...including the overlap in the minor direction
      if (overflow < 0) {
	y = y + incY;
	distance = -overflow*cosecant;
	overflow = overflow + 1.0;
	LowUpdateGridSquare(x, y, distance, 0, parentID);
      }
    }

    // Update the last grid square seen as having a hit.
    if (addEnd) {
      if (incX < 0)
	distance = fabs((x+1) - dx)*secant;
      else
	distance = fabs(dx - x)*secant;
      LowUpdateGridSquare(endx, endy, distance, 1, parentID);
    }

  }

  // This is the same as the previous block of code, with x and y reversed.
  else {
    x = (int) (startx);
    overflow = startx - x;
    if (incX == 1)
      overflow = 1.0 - overflow;
    slope = 1.0/fabs(tan(theta));

    // (See corresponding comments in the previous half of this function)
    error = fabs(((int)(starty)+incY+yedge)-starty);
    overflow = overflow - (error*slope);
    if (overflow < 0.0) {
      x = x + incX;
      overflow = overflow + 1.0;