Localization / Self Localization 5

自己位置推定について説明します。

前回と同様にAMCLを理解していきます。

順番が前後しましたが初期化を見ていきます。

AMCL 1

pf_init()関数を読みます。

handleInitialPose()関数とdynamicParametersCallback()関数から呼び出されます。
ほとんどの場合initialpose topicをsubscribeしてhandleInitialPose()関数が呼び出されてpf_init()関数が呼び出されるという動作と思います。

pf.c

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->w_slow = pf->w_fast = 0.0;

  pf_pdf_gaussian_free(pdf);

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

  // set converged to 0
  pf_init_converged(pf);
}

void pf_init(pf_t * pf, pf_vector_t mean, pf_matrix_t cov)
  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);
  }

必ず理解しておかないといけないのは上記と思います。
最初はnav2のyaml(nav2_bringup/params/nav2_params.yaml)に設定したmax_particlesの値の個数のparticle filterを使っています。
関数名から確率密度関数が正規分布に従うはずなので、initialposeの近くに多くのparticle filterを生成して、遠くに少なくparticle filterを生成している実装のはずです。
正規分布か正規分布みたいなの実装のはずです。

AMCL 1

広告