lowmap.c

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

    }

    for (y = (int) (starty) + incY; y != endy; y = y + incY) {
      overflow = overflow - slope;
      if (overflow < 0)
	distance = (overflow+slope)*secant;
      else
	distance = fabs(slope)*secant;

      LowUpdateGridSquare(x, y, distance, 0, parentID);

      if (overflow < 0.0) {
	x = x + incX;
	distance = -overflow*secant;
	overflow = overflow + 1.0;
	LowUpdateGridSquare(x, y, distance, 0, parentID);
      }
    }

    if (addEnd) {
      if (incY < 0)
	distance = fabs(((y+1) - dy)/sin(theta));
      else
	distance = fabs((dy - y)/sin(theta));
      LowUpdateGridSquare(endx, endy, distance, 1, parentID);
    }
  }

}





//
// Inputs: x, y- starting point for the trace
//         theta- angle for the trace
//         measuredDist- the observed distance for this trace
//         parentID- the ID of the most recent member of the ancestry for the particle being considered
//         hit- really an output, this will be filled with the total probability that this laser cast
//              hit an obstruction before reaching the maximum range of the sensor
// Output: The total evaluated probability for this laser cast (unnormalized). 
//
double LowLineTrace(double startx, double starty, double theta, double MeasuredDist, int parentID, float culling) {
  double overflow, slope; // Used for actually tracing the line
  int x, y, incX, incY, endx, endy;
  double dx, dy;
  double totalProb; // Total probability that the line trace should have stopped before this step in the trace
  double eval;      // Total raw probability for the observation given this line trace through the map
  double prob, distance, error;
  double secant, cosecant;   // precomputed for speed
  double xblock, yblock;
  double xMotion, yMotion;
  double standardDist;

  // eval is the total probability for this line trace. Since this is a summation, eval starts at 0
  eval = 0.0;
  // totalProb is the total probability that the laser scan could travel this far through the map.
  // This starts at 1, and decreases as possible objects are passed.
  totalProb = 1.0;
  // a couple of variables are precomuted for speed.
  secant = 1.0/fabs(cos(theta));
  cosecant = 1.0/fabs(sin(theta));

  // If you look at Localize funtion in low.c, you can see that there are two different line traces
  // performed. The second trace is the full evaluation of the scan. The first trace is a kind of
  // approximate scan, only covering a small section near the percieved endpoint of the scan. The 
  // first trace is used as a way of culling out obviously bad particles without having to do too
  // much on them. For this "culling" trace, we specify directly how much further past the endpoint
  // we want to trace. When that culling number is set to zero, we know that it is the full trace
  // we are looking at, and we can as far out as 20 grid squares beyond the endpoint (anything further
  // has essentially zero probability to add to the scan.
  if (culling)
    distance = MeasuredDist+culling;
  else
    distance = MIN(MeasuredDist+20.0, MAX_SENSE_RANGE);

  // The endpoint of the scan, in both float and int.
  dx = (startx + (cos(theta) * distance));
  dy = (starty + (sin(theta) * distance));
  endx = (int) (dx);
  endy = (int) (dy);

  // Decide which x and y directions the line is travelling.
  if (startx > dx) {
    incX = -1;
    xblock = -startx;
  }
  else {
    incX = 1;
    xblock = 1.0-startx;
  }
  
  if (starty > dy) {
    incY = -1;
    yblock = -starty;
  }
  else {
    incY = 1;
    yblock = 1.0-starty;
  }
  
  // Two copies of the same basic code, swapping the roles of x and y, depending on which one is the primary 
  // direction of motion in the line trace.
  if (fabs(startx - dx) > fabs(starty - dy)) {
    y = (int) (starty);

    // 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 off-center amount, in order to make steps in the y direction at the right places.
    overflow = starty - y;
    // Code is simpler if overflow is always decreasing towards zero. Note that slope is forced to be postive
    if (incY == 1) 
      overflow = 1.0 - overflow;

    slope = fabs(tan(theta));
    if (slope > 1.0) 
      slope = fabs((starty - dy) / (startx - dx));

    // 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. 
    dx = fabs((int)(startx)+xblock);
    dy = fabs(tan(theta)*dx);
    // The first step is actually in the y direction, due to the proximity of starty 
    // to the y axis. 
    if (overflow - dy < 0.0) {
      y = y + incY;
      overflow = overflow - dy + 1.0;
    }
    // Our first step is in fact in the x direction in this case. Set up for the overflow to 
    // be our starting offset plus this little extra we travel in the y direction.
    else 
      overflow = overflow - dy;

    // Most of the scans will be the same length across a grid square, entering and exiting on opposite
    // sides. Precompute this amount to save a little time.
    standardDist = slope*cosecant;

    // These two numbers help determine just how far away the endpoint of the scan is from the current
    // position in the scan. xMotion keeps track of the distance from the endpoint of the scan to next 
    // point where the line trace will cross the x-axis. Since each step in this loop moves us across 
    // exactly one grid square, this number will change by 1/cosine(theta) (ie secant) each iteration.
    // yMotion obviously does the same for the y axis.
    xMotion = -fabs(fabs(( ((int) (startx)) +xblock) * secant) - MeasuredDist);
    yMotion = -fabs(fabs((y+yblock) * cosecant) - MeasuredDist);

    for (x = (int) (startx) + incX; x != endx; x = x + incX) {
      // Update our two running counts.
      xMotion = xMotion + secant;
      overflow = overflow - slope;

      // Establish the distance travelled by the laser through this square. Note that this amount is
      // less than normal if the slope has overflowed, implying that the y-axis has been crossed.
      if (overflow < 0.0) 
	distance = (overflow+slope)*cosecant;
      else 
	distance = standardDist;

      // Compute the probability of the laser stopping in the square, given the particle's unique map.
      // Keep in mind that the probability of even getting this far in the trace is likely less than 1.
      prob = totalProb * LowComputeProbability(x, y, distance, parentID);
      if (prob > 0) {
	// If the scan had actually been stopped by an object in the map at this square,
	// how much error would there be in the laser? Determine which axis will be crossed 
	// next, and compute from there. (This value is actually kept as a running total now).
	if (overflow < 0.0)
	  error = fabs(yMotion);
	else
	  error = fabs(xMotion);

	// Increase the probability of the scan by the probability of stopping here, multiplied by the 
	// probability that a scan which stopped here could produce the error observed.
	// If the error is too large, the net effect on probability of the scan is essentially zero.
	// We can save some time by not computing this exponential.
	if (error < 20.0) 
	  eval = eval + (prob * exp(-(error*error)/(2*LOW_VARIANCE)));

	// Correspondingly decrease the probability that laser has continued.
	totalProb = totalProb - prob;
      }
    
      // If the overflow has dipped below zero, then the trace has crossed the y-axis, and we need to compute
      // everything for a single step in the y direction. 
      if (overflow < 0.0) {
	y += incY;
	yMotion = yMotion + cosecant;

	distance = -overflow*cosecant;
	overflow = overflow + 1.0;

	prob = totalProb * LowComputeProbability(x, y, distance, parentID);
	if (prob > 0) {
	  // There is no question about which axis will be the next crossed, since we just crossed the y-axis, 
	  // and x motion is dominant
	  error = fabs(xMotion);
	  if (error < 20.0) 
	    eval = eval + (prob * exp(-(error*error)/(2*LOW_VARIANCE)));
	}
	totalProb = totalProb - prob;
      }
    
    }
  }

  // ...second verse, same as the first...
  // Pretty much a direct copy of 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)
    dy = fabs((int)(starty)+yblock);
    dx = fabs(dy/tan(theta));
    if (overflow - dx < 0) {
      x = x + incX;
      overflow = overflow - dx + 1.0;
    }
    else 
      overflow = overflow - dx;

    standardDist = slope*secant;
    xMotion = -fabs(fabs((x+xblock) * secant) - MeasuredDist);
    yMotion = -fabs(fabs(( ((int) (starty)) +yblock) * cosecant) - MeasuredDist);

    for (y = (int) (starty) + incY; y != endy; y = y + incY) {
      yMotion = yMotion + cosecant;
      overflow = overflow - slope;

      if (overflow < 0.0) 
	distance = (overflow+slope)*secant;
      else 
	distance = standardDist;

      prob = totalProb * LowComputeProbability(x, y, distance, parentID);
      if (prob > 0) {
	if (overflow < 0.0) 
	  error = fabs(xMotion);
	else 
	  error = fabs(yMotion);
	if (error < 20.0) 
	  eval = eval + (prob * exp(-(error*error)/(2*LOW_VARIANCE)));
      }
      totalProb = totalProb - prob;
    
      if (overflow < 0.0) {
	x += incX;
	xMotion = xMotion + secant;

	distance = -overflow*secant;
	overflow = overflow + 1.0;

	prob = totalProb * LowComputeProbability(x, y, distance, parentID);
	if (prob > 0) {
	  error = fabs(yMotion);
	  if (error < 20.0) 
	    eval = eval + (prob * exp(-(error*error)/(2*LOW_VARIANCE)));
	}
	totalProb = totalProb - prob;
      }

    }
  }

  // If the laser reported a range beyond the maximum range allowed, any left-over probability that
  // the laser has not yet been stopped all has a probability of 1 to get the measured reading. We
  // therefore just add this remaining probability to the evaluation.
  if (MeasuredDist >= MAX_SENSE_RANGE) 
    return (eval + totalProb);

  // Otherwise, we know that the total probability of the laser being stopped at some point during 
  // the scan is 1. Normalize the evaluation to enforce this. 
  if (totalProb == 1)
    return 0;
  return (eval / (1.0 - totalProb));
}