// 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;
}
//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);
/* *********************************************************************** */
}
// 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;
}
/*
* 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();
}
// 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;
}
// 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;
}
/**
* 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();
}