rbpf_1.c

一个外国人网站上的资料

  pose_t *pose, *old;
  int i = 0;
  for(; i < N; ++i) {
    old = traj_current_pose(i);
    //    mu = *(traj_current_pose(i));
    mu = *old;
    pose = traj_add_new_pose(i);
    do {
      memset(&cov, 0, sizeof(cov));
      motion_model(left, right, &mu, &cov);
    //gaussian_pose(&mu, &cov, pose);
    //    do {
      // HACK: there seem to be occasional bad samples, not sure why
      gaussian_pose(&mu, &cov, pose);
      //} while(/*angular_distance(mu.t, pose->t)*/fp_abs(mu.t-pose->t) > float2fp(0.05) || fp_abs(mu.x-pose->x) > float2fp(0.08) || fp_abs(mu.y-pose->y) > float2fp(0.08));
    } while(angular_distance(old->t,pose->t) > float2fp(0.05) || squared_distance_point_point(old->x,old->y,pose->x,pose->y) > float2fp(0.04));

#ifdef _TESTING
    trajectory[i][traj_count] = *pose;
#endif
  }
#ifdef _TESTING
  ++traj_count;
#endif

#ifndef _TESTING
  tot_predict += TCNT1 - start_predict;
  ++num_predict;
#endif

  // forward filtering with an ekf prediction step for the multiscan
  // expected trajectory
  ++multiscan_count;
  multiscan_mu[multiscan_count] = multiscan_mu[multiscan_count-1];
  multiscan_cov[multiscan_count] = multiscan_cov[multiscan_count-1];
  motion_model
    (left, right, &(multiscan_mu[multiscan_count]),
     &(multiscan_cov[multiscan_count]));

  // store range readings
  // FIXME: it might be good to do most of get_multiscan_points
  // immediately here to save some computation time during the slam
  // update
  memcpy(multiscan_ranges+5*multiscan_count, ir, 5*sizeof(int16_t));

  //
  // if we've collected a complete multiscan, do feature extraction,
  // data association, and a slam update
  //

  if(multiscan_count < M-1)
    return;

  //  rprintf("UPDATE\r\n");

#ifndef _TESTING
  TCNT1 = 0;
  start_update = TCNT1;
#endif

  // get points and range-bearing values for all the range readings
  // (that were valid) in the multiscan
#ifdef _TESTING
  static fixed_t x[5*M], y[5*M], u[5*M];
#else
  fixed_t *x = (fixed_t *)5397;
  fixed_t *y = (fixed_t *)5947;
  fixed_t *u = (fixed_t *)6497;
#endif
  int num_points;
  get_multiscan_points(x, y, u, &num_points);

  //  rprintf("get_multiscan_points()\r\n");

  radial_sort_points(x, y, u, num_points, &(multiscan_mu[multiscan_count]));

  // extract line features from the points
#ifdef _TESTING
  static extent_feature features[MAX_FEATURES_PER_SCAN]; // features in (0,0,0) frame
  static extent_feature xformed[MAX_FEATURES_PER_SCAN];  // transformed to particle frame
  static int            in_range[MAX_LANDMARKS];         // in-range landmarks in the map
#else
  extent_feature *features = (extent_feature *)7047;
  extent_feature *xformed = (extent_feature *)7707;
  int *in_range = (int *)8367;
#endif
  int num_features, num_in_range;
  extract_line_segments(x, y, u, num_points, features, &num_features);

  //  rprintf("extract_line_segments()\r\n");

  fixed_t W = 0; // total weight
  particle_t *p;
  int j, corr;
  fixed_t md_2;
  for(i = 0; i < N; ++i) { // iterate over particles
    p = &(particles[i]);
    pose = traj_current_pose(i);

    // put transformation in mu
    mu.x = pose->x - multiscan_mu[multiscan_count].x;
    mu.y = pose->y - multiscan_mu[multiscan_count].y;
    mu.t = angular_distance(pose->t, multiscan_mu[multiscan_count].t);

    // find a coarse set of in-range landmarks in the particle's map
    num_in_range = 0;
    for(j = 0; j < p->n; ++j)
      if(squared_distance_point_segment
	 (pose->x, pose->y, &(p->map[j])) < IN_RANGE_FOR_DA_2)
	in_range[num_in_range++] = j;

    // process one feature at a time
    for(j = 0; j < num_features; ++j) {
      // transform the feature to the frame of this particle
      transform_segment(&features[j], &mu, &xformed[j]);

      // find nearest-neighbor correspondence
      nn_data_association(&xformed[j], p->map, in_range, num_in_range, &corr, &md_2);
      //corr=-1;
      //md_2=float2fp(10.0);

      if(corr >= 0) {
	// if we found a correspondence, update the observed landmark
	p->w = fp_mul(p->w, likelihood_from_md_2(&xformed[j], &(p->map[corr]), md_2)); // FIXME: beware overflow!
	merge_segments(&(p->map[corr]), &xformed[j]);
      } else {
	// otherwise, add a new landmark
	if(p->n < MAX_LANDMARKS) {
	  p->map[p->n] = xformed[j];
	  ++p->n;
	}
	p->w = fp_mul(p->w, NN_THRESH_LIKELIHOOD); // FIXME: beware over/underflow!
      }

    }

    if(p->w == 0) p->w = fp_epsilon;

    W += p->w;
  }

  //  rprintf("particle filtering loop done\r\n");

  // FIXME: maybe verify that W > 0?

  // normalize particle weights and compute Neff
  fixed_t Neff = 0;
  const fixed_t one_over_W = fp_div(fp_1, W);
  for(i = 0; i < N; ++i) {
    particles[i].w = fp_mul(particles[i].w, one_over_W);
    Neff += fp_mul(particles[i].w, particles[i].w);
  }
  Neff = fp_div(fp_1, Neff);

  //  printfp("Neff", Neff);

  // if Neff is too small, resample
  if(fp_div(Neff, int2fp(N)) < NEFF_MIN)
    resample_random();

  //  rprintf("resample_random()\r\n");

  // reset multiscan storage and set the current pose/scan to the
  // starting pose/scan
  multiscan_count = 0;
  multiscan_mu[0] = multiscan_mu[M-1];
  memset(&multiscan_cov[0], 0, sizeof(cov3_t));
  memcpy(multiscan_ranges, multiscan_ranges+5*(M-1), 5*sizeof(int16_t));

  //  rprintf("reset multiscan\r\n");

#ifndef _TESTING
  tot_update += TCNT1 - start_update;
  ++num_update;
#endif

#if 0
  for(i=0;i<N;++i) {
    printf("%g %g %g ", fp2float(traj_current_pose(i)->x), fp2float(traj_current_pose(i)->y), fp2float(traj_current_pose(i)->t));
  }
  printf("\n");
#endif
}

int best_particle(void)
{
  int i, best = 0;
  for(i = 1; i < N; ++i)
    if(particles[i].w > particles[best].w)
      best = i;
  return best;
}

#ifdef _TESTING
void print_filter_state(void)
{
  int i = 0;

#if 0
  printf("multiscan:\n");
  for(; i < M; ++i)
    printf("%d %g %g %g [%g %g %g %g %g %g] |%g %g %g %g %g|\n", i,
	   fp2float(multiscan_mu[i].x), fp2float(multiscan_mu[i].y), fp2float(multiscan_mu[i].t),
	   fp2float(multiscan_cov[i].xx), fp2float(multiscan_cov[i].xy), fp2float(multiscan_cov[i].xt),
	   fp2float(multiscan_cov[i].yy), fp2float(multiscan_cov[i].yt), fp2float(multiscan_cov[i].tt),
	   fp2float(fp_mul(int2fp(multiscan_ranges[5*i]), CONVERT_IR_RB)),
	   fp2float(fp_mul(int2fp(multiscan_ranges[5*i+1]), CONVERT_IR_RF)),
	   fp2float(fp_mul(int2fp(multiscan_ranges[5*i+2]), CONVERT_IR_F)),
	   fp2float(fp_mul(int2fp(multiscan_ranges[5*i+3]), CONVERT_IR_LF)),
	   fp2float(fp_mul(int2fp(multiscan_ranges[5*i+4]), CONVERT_IR_LB)));
#endif

  int best = 0;
  fixed_t W = 0;
  for(i = 0; i < N; ++i) {
    W += particles[i].w;
    if(particles[i].w > particles[best].w)
      best = i;
  }
  const fixed_t one_over_W = fp_div(fp_1, W);
  fixed_t Neff = 0;
  for(i = 0; i < N; ++i) {
    particles[i].w = fp_mul(particles[i].w, one_over_W);
    Neff += fp_mul(particles[i].w, particles[i].w);
  }
  Neff = fp_div(fp_1, Neff); 
  pose_t *bp = traj_current_pose(best);
#if 0 // neff
  printf("%g\n", fp2float(Neff));
#endif
#if 0
  printf("\nneff: %g\n", fp2float(Neff));
  printf("best: %d\n", best);
  printf("best pose: %g %g %g\n", fp2float(bp->x), fp2float(bp->y), fp2float(bp->t));
  printf("best map: %d landmarks:\n", particles[best].n);
  printf("%g %g %g\n", fp2float(bp->x), fp2float(bp->y), fp2float(bp->t));
#endif
  //#if 0 // best map
  for(i = 0; i < particles[best].n; ++i)
    /*
    printf("%d %g %g [%g %g %g] %g %g (%g %g) (%g %g)\n", i, fp2float(particles[best].map[i].r),
	   fp2float(particles[best].map[i].theta), fp2float(particles[best].map[i].rr),
	   fp2float(particles[best].map[i].rt), fp2float(particles[best].map[i].tt),
	   fp2float(particles[best].map[i].umin), fp2float(particles[best].map[i].umax),
	   fp2float(particles[best].map[i].xmin), fp2float(particles[best].map[i].ymin),
	   fp2float(particles[best].map[i].xmax), fp2float(particles[best].map[i].ymax));
    */
    printf("%g %g %g %g\n", fp2float(particles[best].map[i].xmin), fp2float(particles[best].map[i].ymin),
	   fp2float(particles[best].map[i].xmax), fp2float(particles[best].map[i].ymax));
  //#endif

#if 0 // particles
  for(i = 0; i < N; ++i) {
    pose_t *p = traj_current_pose(i);
    printf("%g %g %g ", fp2float(p->x), fp2float(p->y), fp2float(p->t));
  }
  printf("\n");
#endif
}

// dump the poses and weights for every particle
void write_current_particles(FILE *pfile)
{
  int i;
  for(i = 0; i < N; ++i) {
    pose_t *p = traj_current_pose(i);
    fprintf(pfile, "%g %g %g %g ", fp2float(p->x), fp2float(p->y), fp2float(p->t),
	    fp2float(particles[i].w));
  }
  fprintf(pfile, "\n");
}

void write_best_trajectory(FILE *tfile)
{
  int i, best = best_particle();
  for(i = 0; i < traj_count; ++i) {
    fprintf(tfile, "%g %g %g\n", fp2float(trajectory[best][i].x),
	    fp2float(trajectory[best][i].y), fp2float(trajectory[best][i].t));
  }
}

// write the map associated with the best particle to the map file
void write_best_map(FILE *mfile)
{
  int i, best = best_particle();
  for(i = 0; i < particles[best].n; ++i)
    fprintf(mfile, "%g %g %g %g\n",
	    fp2float(particles[best].map[i].xmin), fp2float(particles[best].map[i].ymin),
	    fp2float(particles[best].map[i].xmax), fp2float(particles[best].map[i].ymax));
}

#else // _TESTING
void print_filter_state(void)
{
  rprintf("Predictions: ");
  rprintfNum(10, 10, 0, ' ', tot_predict);
  rprintf(" at 32150 tics/sec for %d predictions\r\n", num_predict);

  rprintf("Updates: ");
  rprintfNum(10, 10, 0, ' ', tot_update);
  rprintf(" at 32150 tics/sec for %d updates\r\n", num_update);
}
#endif // _TESTING