コード例 #1
0
ファイル: pf_vector.c プロジェクト: rsbGroup1/frobo_rsd
// Decompose a covariance matrix [a] into a rotation matrix [r] and a diagonal
// matrix [d] such that a = r d r^T.
void pf_matrix_unitary (pf_matrix_t* r, pf_matrix_t* d, pf_matrix_t a)
{
    int i, j;
    /*
    gsl_matrix *aa;
    gsl_vector *eval;
    gsl_matrix *evec;
    gsl_eigen_symmv_workspace *w;

    aa = gsl_matrix_alloc(3, 3);
    eval = gsl_vector_alloc(3);
    evec = gsl_matrix_alloc(3, 3);
    */

    double aa[3][3];
    double eval[3];
    double evec[3][3];

    for (i = 0; i < 3; i++)
    {
        for (j = 0; j < 3; j++)
        {
            //gsl_matrix_set(aa, i, j, a.m[i][j]);
            aa[i][j] = a.m[i][j];
        }
    }

    // Compute eigenvectors/values
    /*
    w = gsl_eigen_symmv_alloc(3);
    gsl_eigen_symmv(aa, eval, evec, w);
    gsl_eigen_symmv_free(w);
    */

    eigen_decomposition (aa, evec, eval);

    *d = pf_matrix_zero();

    for (i = 0; i < 3; i++)
    {
        //d->m[i][i] = gsl_vector_get(eval, i);
        d->m[i][i] = eval[i];

        for (j = 0; j < 3; j++)
        {
            //r->m[i][j] = gsl_matrix_get(evec, i, j);
            r->m[i][j] = evec[i][j];
        }
    }

    //gsl_matrix_free(evec);
    //gsl_vector_free(eval);
    //gsl_matrix_free(aa);

    return;
}
コード例 #2
0
//void AmclNode::reconfigureCB(AMCLConfig &config, uint32_t level)
void AMCLocalizer::configure() {
    initialize();

    pf_ = pf_alloc(min_particles_, max_particles_,
                   alpha_slow_, alpha_fast_,
                   (pf_init_model_fn_t)AMCLocalizer::uniformPoseGenerator,
                   (void *)map_);

    pf_->pop_err = pf_err_;
    pf_->pop_z = pf_z_;

    // Initialize the filter
    pf_vector_t pf_init_pose_mean = pf_vector_zero();
    pf_init_pose_mean.v[0] = last_published_pose.pose.pose.position.x;
    pf_init_pose_mean.v[1] = last_published_pose.pose.pose.position.y;
    pf_init_pose_mean.v[2] = tf::getYaw(last_published_pose.pose.pose.orientation);
    pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
    pf_init_pose_cov.m[0][0] = last_published_pose.pose.covariance[6*0+0];
    pf_init_pose_cov.m[1][1] = last_published_pose.pose.covariance[6*1+1];
    pf_init_pose_cov.m[2][2] = last_published_pose.pose.covariance[6*5+5];
    pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
    pf_init_ = false;

    // Instantiate Odometry
    delete odom_;
    odom_ = new AMCLOdom();
    if(odom_model_type_ == ODOM_MODEL_OMNI)
        odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
    else
        odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);

    // Instantiate Laser
    delete laser_;
    laser_ = new AMCLLaser(max_beams_, map_);
    if(laser_model_type_ == LASER_MODEL_BEAM)
        laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_, sigma_hit_, lambda_short_, 0.0);
    else  {
        printf("Initializing likelihood field model; this can take some time on large maps...");
        laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_);
        printf("Done initializing likelihood field model.");
    }

    /*
     * ********************* ROS specific ************************************
     */
    //  transform_tolerance_.fromSec(config.transform_tolerance);
    //  odom_frame_id_ = config.odom_frame_id;
    //  base_frame_id_ = config.base_frame_id;
    //  global_frame_id_ = config.global_frame_id;
    //  delete laser_scan_filter_;
    //  laser_scan_filter_ = new tf::MessageFilter<sensor_msgs::LaserScan>(*laser_scan_sub_, *tf_, odom_frame_id_, 100);
    //  laser_scan_filter_->registerCallback(boost::bind(&AmclNode::laserReceived,  this, _1));
    //  initial_pose_sub_ = nh_.subscribe("initialpose", 2, &AmclNode::initialPoseReceived, this);
    /* *********************************************************************** */
}
コード例 #3
0
ファイル: 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;
}
コード例 #4
0
/*
 * This method accepts only initial pose estimates in the global frame, #5148.
 *
 * ************** N.B. This method has been rewritten by DB *****************
 */
void AMCLocalizer::initialPoseReceived(const geometry_msgs::PoseWithCovarianceStampedConstPtr& msg) {
    tf::Pose pose_new;
    tf::poseMsgToTF(msg->pose.pose, pose_new);

    // Re-initialize the filter
    pf_vector_t pf_init_pose_mean = pf_vector_zero();
    pf_init_pose_mean.v[0] = pose_new.getOrigin().x();
    pf_init_pose_mean.v[1] = pose_new.getOrigin().y();
    pf_init_pose_mean.v[2] = getYaw(pose_new);
    pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
    // Copy in the covariance, converting from 6-D to 3-D
    for(int i=0; i<2; i++)
        for(int j=0; j<2; j++)
            pf_init_pose_cov.m[i][j] = msg->pose.covariance[6*i+j];

    pf_init_pose_cov.m[2][2] = msg->pose.covariance[6*5+5];

    delete initial_pose_hyp_;
    initial_pose_hyp_ = new amcl_hyp_t();
    initial_pose_hyp_->pf_pose_mean = pf_init_pose_mean;
    initial_pose_hyp_->pf_pose_cov = pf_init_pose_cov;

    applyInitialPose();
}
コード例 #5
0
ファイル: pf.c プロジェクト: brunoellll/player-git-svn
// Re-compute the cluster statistics for a sample set
void pf_cluster_stats(pf_t *pf, pf_sample_set_t *set)
{
  int i, j, k, c;
  pf_sample_t *sample;
  pf_cluster_t *cluster;

  // Cluster the samples
  pf_kdtree_cluster(set->kdtree);
  
  // Initialize cluster stats
  set->cluster_count = 0;

  for (i = 0; i < set->cluster_max_count; i++)
  {
    cluster = set->clusters + i;
    cluster->count = 0;
    cluster->weight = 0;
    cluster->mean = pf_vector_zero();
    cluster->cov = pf_matrix_zero();

    for (j = 0; j < 4; j++)
      cluster->m[j] = 0.0;
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
        cluster->c[j][k] = 0.0;
  }
  
  // Compute cluster stats
  for (i = 0; i < set->sample_count; i++)
  {
    sample = set->samples + i;

    //printf("%d %f %f %f\n", i, sample->pose.v[0], sample->pose.v[1], sample->pose.v[2]);

    // Get the cluster label for this sample
    c = pf_kdtree_get_cluster(set->kdtree, sample->pose);
    assert(c >= 0);
    if (c >= set->cluster_max_count)
      continue;
    if (c + 1 > set->cluster_count)
      set->cluster_count = c + 1;
    
    cluster = set->clusters + c;

    cluster->count += 1;
    cluster->weight += sample->weight;

    // Compute mean
    cluster->m[0] += sample->weight * sample->pose.v[0];
    cluster->m[1] += sample->weight * sample->pose.v[1];
    cluster->m[2] += sample->weight * cos(sample->pose.v[2]);
    cluster->m[3] += sample->weight * sin(sample->pose.v[2]);

    // Compute covariance in linear components
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
        cluster->c[j][k] += sample->weight * sample->pose.v[j] * sample->pose.v[k];
  }

  // Normalize
  for (i = 0; i < set->cluster_count; i++)
  {
    cluster = set->clusters + i;
        
    cluster->mean.v[0] = cluster->m[0] / cluster->weight;
    cluster->mean.v[1] = cluster->m[1] / cluster->weight;
    cluster->mean.v[2] = atan2(cluster->m[3], cluster->m[2]);

    cluster->cov = pf_matrix_zero();

    // Covariance in linear compontents
    for (j = 0; j < 2; j++)
      for (k = 0; k < 2; k++)
        cluster->cov.m[j][k] = cluster->c[j][k] / cluster->weight -
          cluster->mean.v[j] * cluster->mean.v[k];

    // Covariance in angular components; I think this is the correct
    // formula for circular statistics.
    cluster->cov.m[2][2] = -2 * log(sqrt(cluster->m[2] * cluster->m[2] +
                                         cluster->m[3] * cluster->m[3]));

    //printf("cluster %d %d %f (%f %f %f)\n", i, cluster->count, cluster->weight,
    //       cluster->mean.v[0], cluster->mean.v[1], cluster->mean.v[2]);
    //pf_matrix_fprintf(cluster->cov, stdout, "%e");
  }

  return;
}
コード例 #6
0
ファイル: pf.c プロジェクト: ChingHengWang/andbot_simulator
// 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;
}
コード例 #7
0
/**
 * Convert an OccupancyGrid map message into the internal representation.
 */
void AMCLocalizer::initMap( const nav_msgs::OccupancyGrid& map_msg ) {
    /*
     * Convert an OccupancyGrid map message into the internal representation.
     */
    std::cout << "1" << std::endl;
    freeMapDependentMemory();
    //  Clear queued laser objects because they hold pointers to the existing map, #5202.
    //  lasers_.clear();
    //  lasers_update_.clear();
    std::cout << "2" << std::endl;
    map_t* map = map_alloc();
    //	ROS_ASSERT(map);
    std::cout << "3" << std::endl;
    map->size_x = map_msg.info.width;
    map->size_y = map_msg.info.height;
    map->scale = map_msg.info.resolution;
    map->origin_x = map_msg.info.origin.position.x + (map->size_x / 2) * map->scale;
    map->origin_y = map_msg.info.origin.position.y + (map->size_y / 2) * map->scale;
    // Convert to player format
    std::cout << "4" << std::endl;
    map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y);
    //ROS_ASSERT(map->cells);
    std::cout << "5" << std::endl;
    for(int i=0; i<map->size_x * map->size_y; i++) {
        if(map_msg.data[i] == 0)
            map->cells[i].occ_state = -1;
        else if(map_msg.data[i] == 100)
            map->cells[i].occ_state = +1;
        else
            map->cells[i].occ_state = 0;
    }
    std::cout << "6" << std::endl;
    first_map_received_ = true;

    /*
     * Initialize the particle filter
     */
#if NEW_UNIFORM_SAMPLING
    // Index of free space
    std::cout << "7" << std::endl;
    free_space_indices.resize(0);
    std::cout << "8" << std::endl;
    for(int i = 0; i < map_->size_x; i++)
        for(int j = 0; j < map_->size_y; j++)
            if(map_->cells[MAP_INDEX(map_,i,j)].occ_state == -1)
                free_space_indices.push_back(std::make_pair(i,j));
#endif
    std::cout << "9" << std::endl;
    //	  boost::recursive_mutex::scoped_lock cfl(configuration_mutex_);


    pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_,
                   (pf_init_model_fn_t)AMCLocalizer::uniformPoseGenerator,
                   (void *)map_);
    std::cout << "10" << std::endl;
    pf_->pop_err = pf_err_;
    pf_->pop_z = pf_z_;

    pf_vector_t pf_init_pose_mean = pf_vector_zero();
    pf_init_pose_mean.v[0] = init_pose_[0];
    pf_init_pose_mean.v[1] = init_pose_[1];
    pf_init_pose_mean.v[2] = init_pose_[2];
    pf_matrix_t pf_init_pose_cov = pf_matrix_zero();
    pf_init_pose_cov.m[0][0] = init_cov_[0];
    pf_init_pose_cov.m[1][1] = init_cov_[1];
    pf_init_pose_cov.m[2][2] = init_cov_[2];
    pf_init(pf_, pf_init_pose_mean, pf_init_pose_cov);
    pf_init_ = false;

    /*
     * Instantiate the sensor objects: Odometry
     */
    delete odom_;
    odom_ = new AMCLOdom();
    //  ROS_ASSERT(odom_);
    if(odom_model_type_ == ODOM_MODEL_OMNI)
        odom_->SetModelOmni(alpha1_, alpha2_, alpha3_, alpha4_, alpha5_);
    else
        odom_->SetModelDiff(alpha1_, alpha2_, alpha3_, alpha4_);

    /*
     * Instantiate the sensor objects: Laser
     */
    delete laser_;
    laser_ = new AMCLLaser(max_beams_, map_);
    //  ROS_ASSERT(laser_);
    if(laser_model_type_ == LASER_MODEL_BEAM)
        laser_->SetModelBeam(z_hit_, z_short_, z_max_, z_rand_,
                             sigma_hit_, lambda_short_, 0.0);
    else {
        laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_,
                                        laser_likelihood_max_dist_);
    }
    // In case the initial pose message arrived before the first map,
    // try to apply the initial pose now that the map has arrived.
    applyInitialPose();
}