intpf_resample_limit(pf_t *pf, int k) { double a, b, c, x; int n; if (k <= 1) return pf->max_samples; a = 1; b = 2 / (9 * ((double) k - 1)); // pop_z 为3 c = sqrt(2 / (9 * ((double) k - 1))) * pf->pop_z; x = a - b + c; // pop_err 为0.01 n = (int) ceil( (k - 1) / (2 * pf->pop_err) * x * x * x );
if (n < pf->min_samples) return pf->min_samples; if (n > pf->max_samples) return pf->max_samples; return n; }
boost::recursive_mutex::scoped_lock cfl(configuration_mutex_); // 地图的宽高，分辨率 ROS_INFO("Received a %d X %d map @ %.3f m/pix\n", msg.info.width, msg.info.height, msg.info.resolution); // 如果目前的全局坐标系不是map 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() );
//free相应的指针 freeMapDependentMemory(); // Clear queued laser objects because they hold pointers to the existing map lasers_.clear(); lasers_update_.clear(); frame_to_laser_.clear();
处理获得的地图
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
// AMCL所用的地图类型 typedefstruct { // Map origin; the map is a viewport onto a conceptual larger map. double origin_x, origin_y; double scale; // Map scale (m/cell)
// 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;
// In case the initial pose message arrived before the first map, // try to apply the initial pose now that the map has arrived. 可能初始位姿的信息比第一个map先到达，map到达后apply初始位姿
// Change in odometric pose public: pf_vector_t delta; };
// 单个粒子的采样信息，只有位姿和权重 typedefstruct { // Pose represented by this sample pf_vector_t pose;
// Weight for this pose double weight; } pf_sample_t;
// 从零均值的高斯分布获得随机值，标准差为sigma // We use the polar form of the Box-Muller transformation doublepf_ran_gaussian(double sigma) { double x1, x2, w, r; do { do { r = drand48(); } while (r==0.0); x1 = 2.0 * r - 1.0; do { r = drand48(); } while (r==0.0); x2 = 2.0 * r - 1.0; w = x1*x1 + x2*x2; } while(w > 1.0 || w==0.0); return(sigma * x2 * sqrt(-2.0*log(w)/w)); }
// We want to treat backward and forward motion symmetrically for the // noise model to be applied below. The standard model seems to assume forward motion. // 取了两次旋转角的锐角 delta_rot1_noise = std::min(fabs(angle_diff(delta_rot1,0.0)), fabs(angle_diff(delta_rot1,M_PI))); delta_rot2_noise = std::min(fabs(angle_diff(delta_rot2,0.0)), fabs(angle_diff(delta_rot2,M_PI)));
// 用一定数量的粒子来表征这个区域，然后对这些粒子进行采样来尽可能多的逼近机器人的真实值 // 下面还是书上的公式，我们需要的是sample for (int i = 0; i < set->sample_count; i++) { // 指针形式, 会给set的samples都赋值 pf_sample_t* sample = set->samples + i;
