48 double alpha_slow,
double alpha_fast,
58 pf = calloc(1,
sizeof(
pf_t));
75 for (j = 0; j < 2; j++)
80 set->samples = calloc(max_samples,
sizeof(
pf_sample_t));
82 for (i = 0; i <
set->sample_count; i++)
84 sample =
set->samples + i;
85 sample->
pose.
v[0] = 0.0;
86 sample->
pose.
v[1] = 0.0;
87 sample->
pose.
v[2] = 0.0;
88 sample->
weight = 1.0 / max_samples;
94 set->cluster_count = 0;
95 set->cluster_max_count = max_samples;
117 for (i = 0; i < 2; i++)
147 for (i = 0; i <
set->sample_count; i++)
149 sample =
set->samples + i;
183 for (i = 0; i <
set->sample_count; i++)
185 sample =
set->samples + i;
187 sample->
pose = (*init_fn) (init_data);
209 (*action_fn) (action_data,
set);
227 total = (*sensor_fn) (sensor_data,
set);
233 for (i = 0; i <
set->sample_count; i++)
235 sample =
set->samples + i;
240 w_avg /=
set->sample_count;
257 for (i = 0; i <
set->sample_count; i++)
259 sample =
set->samples + i;
260 sample->
weight = 1.0 /
set->sample_count;
289 c = (
double*)malloc(
sizeof(
double)*(set_a->
sample_count+1));
320 if(drand48() < w_diff)
353 if((c[i] <= r) && (r < c[i+1]))
356 assert(i<set_a->sample_count);
360 assert(sample_a->
weight > 0);
367 total += sample_b->
weight;
387 sample_b->
weight /= total;
412 b = 2 / (9 * ((double) k - 1));
413 c = sqrt(2 / (9 * ((
double) k - 1))) * pf->
pop_z;
416 n = (int) ceil((k - 1) / (2 * pf->
pop_err) * x * x * x);
418 if (n < pf->min_samples)
435 double m[4], c[2][2];
443 set->cluster_count = 0;
445 for (i = 0; i <
set->cluster_max_count; i++)
447 cluster =
set->clusters + i;
453 for (j = 0; j < 4; j++)
455 for (j = 0; j < 2; j++)
456 for (k = 0; k < 2; k++)
457 cluster->
c[j][k] = 0.0;
465 for (j = 0; j < 4; j++)
467 for (j = 0; j < 2; j++)
468 for (k = 0; k < 2; k++)
472 for (i = 0; i <
set->sample_count; i++)
474 sample =
set->samples + i;
481 if (cidx >=
set->cluster_max_count)
483 if (cidx + 1 >
set->cluster_count)
484 set->cluster_count = cidx + 1;
486 cluster =
set->clusters + cidx;
497 cluster->
m[2] += sample->
weight * cos(sample->
pose.
v[2]);
498 cluster->
m[3] += sample->
weight * sin(sample->
pose.
v[2]);
506 for (j = 0; j < 2; j++)
507 for (k = 0; k < 2; k++)
515 for (i = 0; i <
set->cluster_count; i++)
517 cluster =
set->clusters + i;
521 cluster->
mean.
v[2] = atan2(cluster->
m[3], cluster->
m[2]);
526 for (j = 0; j < 2; j++)
527 for (k = 0; k < 2; k++)
528 cluster->
cov.
m[j][k] = cluster->
c[j][k] / cluster->
weight -
533 cluster->
cov.
m[2][2] = -2 * log(sqrt(cluster->
m[2] * cluster->
m[2] +
534 cluster->
m[3] * cluster->
m[3]));
542 set->mean.v[0] = m[0] / weight;
543 set->mean.v[1] = m[1] / weight;
544 set->mean.v[2] = atan2(m[3], m[2]);
547 for (j = 0; j < 2; j++)
548 for (k = 0; k < 2; k++)
549 set->cov.m[j][k] = c[j][k] / weight -
set->mean.v[j] *
set->mean.v[k];
553 set->cov.m[2][2] = -2 * log(sqrt(m[2] * m[2] + m[3] * m[3]));
563 double mn, mx, my, mrr;
574 for (i = 0; i <
set->sample_count; i++)
576 sample =
set->samples + i;
585 mean->
v[0] = mx / mn;
586 mean->
v[1] = my / mn;
589 *var = mrr / mn - (mx * mx / (mn * mn) + my * my / (mn * mn));
604 if (clabel >=
set->cluster_count)
606 cluster =
set->clusters + clabel;
608 *weight = cluster->
weight;
609 *mean = cluster->
mean;
pf_init_model_fn_t random_pose_fn
double(* pf_sensor_model_fn_t)(void *sensor_data, struct _pf_sample_set_t *set)
static int pf_resample_limit(pf_t *pf, int k)
void pf_update_sensor(pf_t *pf, pf_sensor_model_fn_t sensor_fn, void *sensor_data)
void pf_get_cep_stats(pf_t *pf, pf_vector_t *mean, double *var)
void pf_kdtree_clear(pf_kdtree_t *self)
void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value)
pf_vector_t(* pf_init_model_fn_t)(void *init_data)
pf_vector_t pf_vector_zero()
void pf_update_resample(pf_t *pf)
pf_t * pf_alloc(int min_samples, int max_samples, double alpha_slow, double alpha_fast, pf_init_model_fn_t random_pose_fn, void *random_pose_data)
void pf_init_model(pf_t *pf, pf_init_model_fn_t init_fn, void *init_data)
void pf_kdtree_free(pf_kdtree_t *self)
ROSCPP_DECL void set(const std::string &key, const XmlRpc::XmlRpcValue &v)
void pf_pdf_gaussian_free(pf_pdf_gaussian_t *pdf)
void pf_update_action(pf_t *pf, pf_action_model_fn_t action_fn, void *action_data)
pf_matrix_t pf_matrix_zero()
TFSIMD_FORCE_INLINE const tfScalar & x() const
pf_pdf_gaussian_t * pf_pdf_gaussian_alloc(pf_vector_t x, pf_matrix_t cx)
static void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
void(* pf_action_model_fn_t)(void *action_data, struct _pf_sample_set_t *set)
int pf_get_cluster_stats(pf_t *pf, int clabel, double *weight, pf_vector_t *mean, pf_matrix_t *cov)
void pf_init(pf_t *pf, pf_vector_t mean, pf_matrix_t cov)
pf_kdtree_t * pf_kdtree_alloc(int max_size)
int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose)
void pf_kdtree_cluster(pf_kdtree_t *self)
pf_vector_t pf_pdf_gaussian_sample(pf_pdf_gaussian_t *pdf)