之前看AMCL只是关注粒子滤波的流程，对于地图数据怎么转换没仔细看。现在有空就简单理理。
1、处理地图数据的入口是函数：handleMapMessage。ROS地图数据转换、粒子滤波器的初始化、创建传感器对象及其模型参数的初始化，均在处理地图数据的时候进行。
void AmclNode::handleMapMessage(const nav_msgs::OccupancyGrid& msg) { boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); ROS_INFO("Received a %d X %d map @ %.3f m/pix ", msg.info.width, msg.info.height, msg.info.resolution); if(msg.header.frame_id != global_frame_id_) ROS_WARN("Frame_id of map received:'%s' doesn't match global_frame_id:'%s'. This could cause issues with reading published topics", msg.header.frame_id.c_str(), global_frame_id_.c_str()); freeMapDependentMemory(); // Clear queued laser objects because they hold pointers to the existing // map, #5202. lasers_.clear(); lasers_update_.clear(); frame_to_laser_.clear(); map_ = convertMap(msg); #if NEW_UNIFORM_SAMPLING //默认值为1，把地图上的空的点存起来 // Index of free space free_space_indices.resize(0); 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 // Create the particle filter pf_ = pf_alloc(min_particles_, max_particles_, alpha_slow_, alpha_fast_, (pf_init_model_fn_t)AmclNode::uniformPoseGenerator,//函数指针 (void *)map_); //这里得到的是一个均匀分布采样的向量//void *是无类型指针，可以指向任何数据 pf_->pop_err = pf_err_; pf_->pop_z = pf_z_; // Initialize the filter updatePoseFromServer(); 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_); odom_->SetModel( odom_model_type_, alpha1_, alpha2_, alpha3_, alpha4_, alpha5_ ); // 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 if(laser_model_type_ == LASER_MODEL_LIKELIHOOD_FIELD_PROB){ ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); laser_->SetModelLikelihoodFieldProb(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_, do_beamskip_, beam_skip_distance_, beam_skip_threshold_, beam_skip_error_threshold_); ROS_INFO("Done initializing likelihood field model."); } else { ROS_INFO("Initializing likelihood field model; this can take some time on large maps..."); laser_->SetModelLikelihoodField(z_hit_, z_rand_, sigma_hit_, laser_likelihood_max_dist_); ROS_INFO("Done initializing likelihood field model."); } // In case the initial pose message arrived before the first map, // try to apply the initial pose now that the map has arrived. applyInitialPose(); }
2、ROS下的地图数据：nav_msgs/OccupancyGrid Message
看官方解释：http://docs.ros.org/en/kinetic/api/nav_msgs/html/msg/OccupancyGrid.html
std_msgs/Header header nav_msgs/MapMetaData info int8[] data
注意：data是一维数组，存储方式是一行一行存储。按序号取对应像素点的值。左下角是地图序号的起点(0,0)，别问怎么知道，自己去写一个map的数据调用测试一下便知。
AMCL中地图的数据格式
typedef struct { // Map origin; the map is a viewport onto a conceptual larger map. 地图起点在世界坐标下的值，地图起点位于地图的左下角 double origin_x, origin_y; // Map scale (m/cell) 尺度 double scale; // Map dimensions (number of cells) 栅格数目 int size_x, size_y; // The map data, stored as a grid 像素点数据 map_cell_t *cells; // Max distance at which we care about obstacles, for constructing // likelihood field 障碍物的最大距离 double max_occ_dist; } map_t;
3、convertMap(msg)地图数据的转换，要知道占用的表示：其中100表示障碍物occupy，0表示未占用free，-1表示未知状态unknown
map_t* AmclNode::convertMap( const nav_msgs::OccupancyGrid& map_msg ) { map_t* map = map_alloc(); ROS_ASSERT(map); 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 map->cells = (map_cell_t*)malloc(sizeof(map_cell_t)*map->size_x*map->size_y); ROS_ASSERT(map->cells); for(int i=0;i<map->size_x * map->size_y;i++) { // 将0，free 转成 -1 if(map_msg.data[i] == 0) map->cells[i].occ_state = -1; // 将100，占用 转成 +1 else if(map_msg.data[i] == 100) map->cells[i].occ_state = +1; // 将其他情况 转成 0 else map->cells[i].occ_state = 0; } return map; }
地图点的格式，用结构体表示。转换后的map数据存在一块内存中，也是map->cells数组来存储。
// Description for a single map cell. typedef struct { // Occupancy state (-1 = free, 0 = unknown, +1 = occ) int occ_state; // Distance to the nearest occupied cell 记录的是该栅格与最近的被占据栅格的距离值 double occ_dist; // Wifi levels //int wifi_levels[MAP_WIFI_MAX_LEVELS]; } map_cell_t;
但是在地图数据转换的时候，并没有看到occ_dist的数据的初始化。这就要看另一个函数map_update_cspace了。
4、地图坐标系的像素点和世界坐标系下的空间点的转换关系
// Convert from map index to world coords #define MAP_WXGX(map, i) (map->origin_x + ((i) - map->size_x / 2) * map->scale) #define MAP_WYGY(map, j) (map->origin_y + ((j) - map->size_y / 2) * map->scale) // Convert from world coords to map coords 世界坐标系转换成地图坐标系 #define MAP_GXWX(map, x) (floor((x - map->origin_x) / map->scale + 0.5) + map->size_x / 2) //floor取最大的整数，+0.5后是四舍五入 #define MAP_GYWY(map, y) (floor((y - map->origin_y) / map->scale + 0.5) + map->size_y / 2) // Test to see if the given map coords lie within the absolute map bounds.是否在地图范围内的点 #define MAP_VALID(map, i, j) ((i >= 0) && (i < map->size_x) && (j >= 0) && (j < map->size_y)) // Compute the cell index for the given map coords. 计算(i,j)像素点以int[]数组存储，下面是数组的索引计算方式，索引是从左下角为起始点 #define MAP_INDEX(map, i, j) ((i) + (j) * map->size_x)
从MAP_INDEX就可以看出索引的方式是一行一行遍历的。
5、地图转换好了，用途是什么呢？再看pf_alloc初始化滤波器的时候，调用的uniformPoseGenerator，用于在地图的空白处随机的生成一个的粒子位姿。
pf_vector_t AmclNode::uniformPoseGenerator(void* arg) //做的事情是按均匀分布采样,根据地图获取一个随机的位姿 { map_t* map = (map_t*)arg; #if NEW_UNIFORM_SAMPLING//如果是新的均匀采样 unsigned int rand_index = drand48() * free_space_indices.size(); //均匀采样drand48()，随机找到地图上空白的某个像素点 std::pair<int,int> free_point = free_space_indices[rand_index]; pf_vector_t p; p.v[0] = MAP_WXGX(map, free_point.first); //地图像素点转换成世界坐标系下的位姿 p.v[1] = MAP_WYGY(map, free_point.second); // Convert from map index to world coords p.v[2] = drand48() * 2 * M_PI - M_PI; //0-180角度均匀采样 #else /***省略else部分***/ return p; } //这里是地图上free区域上随机的采点，满足均匀分布
5、最后执行applyInitialPose。有种情况是，当机器人给定了一个初始值时，此时的初始Pose值就会覆盖上面随机生成的Pose值，调用pf_init，用设定的Pose值来初始化滤波器。
void AmclNode::applyInitialPose() { boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); if( initial_pose_hyp_ != NULL && map_ != NULL ) { pf_init(pf_, initial_pose_hyp_->pf_pose_mean, initial_pose_hyp_->pf_pose_cov); pf_init_ = false; delete initial_pose_hyp_; initial_pose_hyp_ = NULL; } }
需要注意的是，另开一个线程的意义：是否能支持，只要新来一个Pose初值，是不是就可以重新生成一个滤波器呢？待思考和验证。
6、讲讲pf_init初始化，和随机初始化的区别是什么？
先看一下假设的初始值的数据结构
// Pose hypothesis typedef struct { // Total weight (weights sum to 1) double weight; // Mean of pose esimate pf_vector_t pf_pose_mean; // Covariance of pose estimate pf_matrix_t pf_pose_cov; } amcl_hyp_t;
这里的weight是什么？为什么是0-1？
然后看pf_init，这部分以前没仔细看，所以没有注释。因为涉及到更多粒子滤波器的函数，后面看完了之后再补充与地图相关的内容。
// Initialize the filter using a guassian 每次位姿估计后，用高斯分布初始化一次的粒子 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); return; }
7、初始化完成后，还需设置传感器模型。其中重点分析SetModelLikelihoodField函数，因为涉及到map中max_occ_dist的更新。
void AMCLLaser::SetModelLikelihoodField(double z_hit, double z_rand, double sigma_hit, double max_occ_dist) { this->model_type = LASER_MODEL_LIKELIHOOD_FIELD; this->z_hit = z_hit; this->z_rand = z_rand; this->sigma_hit = sigma_hit; map_update_cspace(this->map, max_occ_dist); }
其中map_update_cspace函数：主要是计算每个occ_dist的值，计算每个障碍点周围4个点的障碍点的距离来更新occ_dist
// Update the cspace distance values void map_update_cspace(map_t *map, double max_occ_dist) { unsigned char* marked; std::priority_queue<CellData> Q; marked = new unsigned char[map->size_x*map->size_y]; memset(marked, 0, sizeof(unsigned char) * map->size_x*map->size_y); map->max_occ_dist = max_occ_dist; CachedDistanceMap* cdm = get_distance_map(map->scale, map->max_occ_dist); // Enqueue all the obstacle cells CellData cell; cell.map_ = map; for(int i=0; i<map->size_x; i++) { cell.src_i_ = cell.i_ = i; for(int j=0; j<map->size_y; j++) { if(map->cells[MAP_INDEX(map, i, j)].occ_state == +1) { map->cells[MAP_INDEX(map, i, j)].occ_dist = 0.0; cell.src_j_ = cell.j_ = j; marked[MAP_INDEX(map, i, j)] = 1; Q.push(cell); } else map->cells[MAP_INDEX(map, i, j)].occ_dist = max_occ_dist; } } while(!Q.empty()) { CellData current_cell = Q.top(); if(current_cell.i_ > 0) enqueue(map, current_cell.i_-1, current_cell.j_, current_cell.src_i_, current_cell.src_j_, Q, cdm, marked); if(current_cell.j_ > 0) enqueue(map, current_cell.i_, current_cell.j_-1, current_cell.src_i_, current_cell.src_j_, Q, cdm, marked); if((int)current_cell.i_ < map->size_x - 1) enqueue(map, current_cell.i_+1, current_cell.j_, current_cell.src_i_, current_cell.src_j_, Q, cdm, marked); if((int)current_cell.j_ < map->size_y - 1) enqueue(map, current_cell.i_, current_cell.j_+1, current_cell.src_i_, current_cell.src_j_, Q, cdm, marked); Q.pop(); } delete[] marked; }
目前还不清楚这个变量的用途。 max_occ_dist通过参数设置的，表示障碍物点的最大膨胀距离。