C++ (Cpp) pf_pdf_gaussian_allocの例

コード例 #1

ファイル: pf.c プロジェクト: brunoellll/player-git-svn

// Initialize the filter using a guassian
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
{
  int i;
  pf_sample_set_t *set;
  pf_sample_t *sample;
  pf_pdf_gaussian_t *pdf;
  
  set = pf->sets + pf->current_set;
  
  // Create the kd tree for adaptive sampling
  pf_kdtree_clear(set->kdtree);

  set->sample_count = pf->max_samples;

  pdf = pf_pdf_gaussian_alloc(mean, cov);
    
  // Compute the new sample poses
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;
    sample->weight = 1.0 / pf->max_samples;
    sample->pose = pf_pdf_gaussian_sample(pdf);

    // Add sample to histogram
    pf_kdtree_insert(set->kdtree, sample->pose, sample->weight);
  }

  pf_pdf_gaussian_free(pdf);
    
  // Re-compute cluster statistics
  pf_cluster_stats(pf, set);

  return;
}

コード例 #2

ファイル: gps.c プロジェクト: uml-robotics/player-2.1.3

// Prepare to initialize the distribution
void gps_init_init(gps_model_t *self)
{
  pf_vector_t x;
  pf_matrix_t xc;
  
  x = pf_vector_zero();
  x.v[0] = self->utm_e - self->utm_base_e;
  x.v[1] = self->utm_n - self->utm_base_n;
  x.v[2] = 0;

  xc = pf_matrix_zero();
  xc.m[0][0] = self->err_horz * self->err_horz;
  xc.m[1][1] = self->err_horz * self->err_horz;
  xc.m[2][2] = M_PI;
  
  self->pdf = pf_pdf_gaussian_alloc(x, xc);

  return;
}