// Create a new filter pf_t *pf_alloc(int min_samples, int max_samples) { int i, j; pf_t *pf; pf_sample_set_t *set; pf_sample_t *sample; pf = calloc(1, sizeof(pf_t)); pf->min_samples = min_samples; pf->max_samples = max_samples; // Control parameters for the population size calculation. [err] is // the max error between the true distribution and the estimated // distribution. [z] is the upper standard normal quantile for (1 - // p), where p is the probability that the error on the estimated // distrubition will be less than [err]. pf->pop_err = 0.01; pf->pop_z = 3; pf->current_set = 0; for (j = 0; j < 2; j++) { set = pf->sets + j; set->sample_count = max_samples; set->samples = calloc(max_samples, sizeof(pf_sample_t)); for (i = 0; i < set->sample_count; i++) { sample = set->samples + i; sample->pose.v[0] = 0.0; sample->pose.v[1] = 0.0; sample->pose.v[2] = 0.0; sample->weight = 1.0 / max_samples; } // HACK: is 3 times max_samples enough? set->kdtree = pf_kdtree_alloc(3 * max_samples); set->cluster_count = 0; set->cluster_max_count = 100; set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t)); } return pf; }
// Create a new filter 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) { int i, j; pf_t *pf; pf_sample_set_t *set; pf_sample_t *sample; srand48(time(NULL)); pf = calloc(1, sizeof(pf_t)); pf->random_pose_fn = random_pose_fn; pf->random_pose_data = random_pose_data; pf->min_samples = min_samples; pf->max_samples = max_samples; // Control parameters for the population size calculation. [err] is // the max error between the true distribution and the estimated // distribution. [z] is the upper standard normal quantile for (1 - // p), where p is the probability that the error on the estimated // distrubition will be less than [err]. pf->pop_err = 0.01; pf->pop_z = 3; pf->dist_threshold = 0.5; pf->current_set = 0; for (j = 0; j < 2; j++) { set = pf->sets + j; set->sample_count = max_samples; set->samples = calloc(max_samples, sizeof(pf_sample_t)); for (i = 0; i < set->sample_count; i++) { sample = set->samples + i; sample->pose.v[0] = 0.0; sample->pose.v[1] = 0.0; sample->pose.v[2] = 0.0; sample->weight = 1.0 / max_samples; } // HACK: is 3 times max_samples enough? set->kdtree = pf_kdtree_alloc(3 * max_samples); set->cluster_count = 0; set->cluster_max_count = max_samples; set->clusters = calloc(set->cluster_max_count, sizeof(pf_cluster_t)); set->mean = pf_vector_zero(); set->cov = pf_matrix_zero(); } pf->w_slow = 0.0; pf->w_fast = 0.0; pf->alpha_slow = alpha_slow; pf->alpha_fast = alpha_fast; //set converged to 0 pf_init_converged(pf); return pf; }