EGO-Swarm代码解读-地图部分

---

1、参数解读

一、MappingData md_中的参数含义：

local_bound_min_, local_bound_max_ //更新栅格的范围

  //具体占据概率,初始化为-1.99243-0.01=-2.00243(空闲)
  md_.occupancy_buffer_ = 
  vector<double>(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_);
  //栅格占据与否,0表示空闲,1表示占据
  md_.occupancy_buffer_inflate_ = vector<char>(buffer_size, 0);
//统计涉及到的体素的次数,占据和空闲均会+1
  md_.count_hit_and_miss_ = vector<short>(buffer_size, 0);
//统计涉及到的占据体素的次数,占据时会+1
  md_.count_hit_ = vector<short>(buffer_size, 0);
  
  md_.flag_rayend_ = vector<char>(buffer_size, -1);
  
  md_.flag_traverse_ = vector<char>(buffer_size, -1);

  md_.raycast_num_ = 0;

  md_.proj_points_.resize(640 * 480 / mp_.skip_pixel_ / mp_.skip_pixel_);
  
  md_.proj_points_cnt = 0;
  
  md_.cam2body_ << 0.0, 0.0, 1.0, 0.0,
      -1.0, 0.0, 0.0, 0.0,
      0.0, -1.0, 0.0, 0.0,
      0.0, 0.0, 0.0, 1.0;

二、MappingParameters mp_中的参数含义：

//表示栅格地图的索引（index）范围,mp_.resolution_表示地图分表率，默认值为0.1;ceil向上取整
  for (int i = 0; i < 3; ++i)
  {
    mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_);
    }
//缓冲区大小（三维索引 ---> 一维向量）
int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2);

//表示地图的位置（pos）范围
  mp_.map_min_boundary_ = mp_.map_origin_;
  mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;

mp_.local_map_margin_//表示局部栅格地图的更新和清除范围,默认值为10(10*0.1=1m)

  mp_.prob_hit_log_ = logit(mp_.p_hit_);//每判定一次占据,自增mp_.prob_hit_log_ 
  mp_.prob_miss_log_ = logit(mp_.p_miss_);//每判定一次空闲,自增mp_.prob_miss_log_ 
  mp_.clamp_min_log_ = logit(mp_.p_min_);//最终能够判定空闲的阈值
  mp_.clamp_max_log_ = logit(mp_.p_max_);//最终能够判定占据的阈值
  mp_.min_occupancy_log_ = logit(mp_.p_occ_);
  mp_.unknown_flag_ = 0.01;

  cout << "hit: " << mp_.prob_hit_log_ << endl;
  cout << "miss: " << mp_.prob_miss_log_ << endl;
  cout << "min log: " << mp_.clamp_min_log_ << endl;
  cout << "max: " << mp_.clamp_max_log_ << endl;
  cout << "thresh log: " << mp_.min_occupancy_log_ << endl;
  //终端输出结果
  hit: 0.619039
  miss: -0.619039
  min log: -1.99243
  max: 2.19722
  thresh log: 1.38629

三、grid_map.cpp文件中其他参数含义：

x_size, y_size, z_size//代表设定地图的大小
mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size);

//表示地图的原点在地图的左下角，mp_.ground_height_默认值为-0.01
mp_.map_origin_ = Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_);

2、主要函数解读

1. indep_cloud_sub_ =
node_.subscribe(“grid_map/cloud”, 10, &GridMap::cloudCallback, this);

void GridMap::cloudCallback(const sensor_msgs::PointCloud2ConstPtr &img)
{
  // double t_cur = img->header.stamp.toSec();
  // printf("cloudCallback_time: %16f\n",t_cur);

  pcl::PointCloud<pcl::PointXYZ> latest_cloud;
  pcl::fromROSMsg(*img, latest_cloud);//将ROS点云消息类型转换为PCL标准库中点云消息类型

  md_.has_cloud_ = true;

  if (!md_.has_odom_)
  {
    std::cout << "no odom!" << std::endl;
    return;
  }

  if (latest_cloud.points.size() == 0)
    return;

  if (isnan(md_.camera_pos_(0)) || isnan(md_.camera_pos_(1)) || isnan(md_.camera_pos_(2)))
    return;
    
  //md_.occupancy_buffer_inflate_[pos] = 0;
  this->resetBuffer(md_.camera_pos_ - mp_.local_update_range_,
                    md_.camera_pos_ + mp_.local_update_range_);

  pcl::PointXYZ pt;
  Eigen::Vector3d p3d, p3d_inf;
  
  //obstacles_inflation_=0.099,resolution_=0.1,ceil向上取整，inf_step=1.0
  int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_);
  int inf_step_z = 1;

  double max_x, max_y, max_z, min_x, min_y, min_z;
  //map_max_boundary,map_min_boundary表示地图的位置（pos）范围
  min_x = mp_.map_max_boundary_(0);
  min_y = mp_.map_max_boundary_(1);
  min_z = mp_.map_max_boundary_(2);

  max_x = mp_.map_min_boundary_(0);
  max_y = mp_.map_min_boundary_(1);
  max_z = mp_.map_min_boundary_(2);

  for (size_t i = 0; i < latest_cloud.points.size(); ++i)
  {
    pt = latest_cloud.points[i];
    p3d(0) = pt.x, p3d(1) = pt.y, p3d(2) = pt.z;

    /* point inside update range */
    Eigen::Vector3d devi = p3d - md_.camera_pos_;
    Eigen::Vector3i inf_pt;
    //mp_.local_update_range_x=5.5,y=5.5,z=4.5
    if (fabs(devi(0)) < mp_.local_update_range_(0) && fabs(devi(1)) < mp_.local_update_range_(1) &&
        fabs(devi(2)) < mp_.local_update_range_(2))
    {

      /* inflate the point */
      for (int x = -inf_step; x <= inf_step; ++x)
        for (int y = -inf_step; y <= inf_step; ++y)
          for (int z = -inf_step_z; z <= inf_step_z; ++z)
          {
            p3d_inf(0) = pt.x + x * mp_.resolution_;
            p3d_inf(1) = pt.y + y * mp_.resolution_;
            p3d_inf(2) = pt.z + z * mp_.resolution_;
 //第一次进入循环:相当于max_x = p3d_inf(0)
            max_x = max(max_x, p3d_inf(0));
            max_y = max(max_y, p3d_inf(1));
            max_z = max(max_z, p3d_inf(2));
//第一次进入循环:膨胀后的点云的位置与地图最大位置边界---二者取最小(相当于min_x = p3d_inf(0))
            min_x = min(min_x, p3d_inf(0));
            min_y = min(min_y, p3d_inf(1));
            min_z = min(min_z, p3d_inf(2));

            posToIndex(p3d_inf, inf_pt);

            if (!isInMap(inf_pt))
              continue;

            int idx_inf = toAddress(inf_pt);
            //膨胀之后的占据栅格，一个占据点云左右各膨胀0.1(一个栅格)
            md_.occupancy_buffer_inflate_[idx_inf] = 1;
          }
    }
    //三层for循环结束后,max_x的值代表膨胀后的点云的最大位置坐标,
    //min_x的值代表膨胀后的点云的最小位置坐标
  }
//比较膨胀后的点云的最小位置 和 相机的位置, min_x取二者的最小值
  min_x = min(min_x, md_.camera_pos_(0));
  min_y = min(min_y, md_.camera_pos_(1));
  min_z = min(min_z, md_.camera_pos_(2));
//比较膨胀后的点云的最大位置 和 相机的位置, max_x取二者的最大值
  max_x = max(max_x, md_.camera_pos_(0));
  max_y = max(max_y, md_.camera_pos_(1));
  max_z = max(max_z, md_.camera_pos_(2));

  max_z = max(max_z, mp_.ground_height_);
//重置更新栅格的范围
  posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_);
  posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_);
//确保待更新栅格均在地图内.若不在,取地图边界值予以替代
  boundIndex(md_.local_bound_min_);
  boundIndex(md_.local_bound_max_);

  // add virtual ceiling to limit flight height
  /*mp_.virtual_ceil_height_默认值为3.0*/
  if (mp_.virtual_ceil_height_ > -0.5) {
    //floor向下取整
    /*mp_.map_origin_ = 
    Eigen::Vector3d(-x_size / 2.0, -y_size / 2.0, mp_.ground_height_);*/
    //mp_.resolution_inv_ = 1/0.1= 10
    //ceil_id = 30
    int ceil_id = floor((mp_.virtual_ceil_height_ - 
    mp_.map_origin_(2)) * mp_.resolution_inv_);
    /*local_bound_min_ 表示 占据栅格更新范围.一个占据栅格0.1m,
    将障碍物最高设置为30*0.1=3m,防止飞机从障碍物上方飞过*/
    for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
      for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) {
        md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1;
      }
  }
}

void GridMap::resetBuffer(Eigen::Vector3d min_pos, Eigen::Vector3d max_pos)
{

  Eigen::Vector3i min_id, max_id;
  //将位置转换为索引
  posToIndex(min_pos, min_id);
  posToIndex(max_pos, max_id);
  
  //确保最小索引min_id,最大索引max_id均在地图内
  boundIndex(min_id);
  boundIndex(max_id);

  /* reset occ and dist buffer */
  for (int x = min_id(0); x <= max_id(0); ++x)
    for (int y = min_id(1); y <= max_id(1); ++y)
      for (int z = min_id(2); z <= max_id(2); ++z)
      {
        //空闲
        md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0;
      }
}

inline void GridMap::boundIndex(Eigen::Vector3i& id) {
  Eigen::Vector3i id1;
  id1(0) = max(min(id(0), mp_.map_voxel_num_(0) - 1), 0);
  id1(1) = max(min(id(1), mp_.map_voxel_num_(1) - 1), 0);
  id1(2) = max(min(id(2), mp_.map_voxel_num_(2) - 1), 0);
  id = id1;
}

//三维向量 ---> 一维向量
inline int GridMap::toAddress(int& x, int& y, int& z) {
  return x * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + y * mp_.map_voxel_num_(2) + z;
}

2. void GridMap::raycastProcess()

void GridMap::raycastProcess()
{
  // if (md_.proj_points_.size() == 0)
  if (md_.proj_points_cnt == 0)
    return;

  ros::Time t1, t2;
  //md_.raycast_num_初始化为0
  md_.raycast_num_ += 1;

  int vox_idx;
  double length;

  // bounding box of updated region
  //map_max_boundary,map_min_boundary表示地图的位置（pos）范围
  double min_x = mp_.map_max_boundary_(0);
  double min_y = mp_.map_max_boundary_(1);
  double min_z = mp_.map_max_boundary_(2);

  double max_x = mp_.map_min_boundary_(0);
  double max_y = mp_.map_min_boundary_(1);
  double max_z = mp_.map_min_boundary_(2);

  RayCaster raycaster;
  Eigen::Vector3d half = Eigen::Vector3d(0.5, 0.5, 0.5);
  Eigen::Vector3d ray_pt, pt_w;

  for (int i = 0; i < md_.proj_points_cnt; ++i)
  {
    pt_w = md_.proj_points_[i];

    // set flag for projected point
 /*判断该点是否在地图内*/
 /*若该点不在地图内,则在地图范围内找一个与该点靠近的点；又若新找到的点不在射线追踪范围,
 依旧在找一个在射线追踪范围内的新的点.最终设为**空闲***/
    if (!isInMap(pt_w))
    {
      pt_w = closetPointInMap(pt_w, md_.camera_pos_);

      length = (pt_w - md_.camera_pos_).norm();
      if (length > mp_.max_ray_length_)//mp_.max_ray_length_ = 4.5
      {
        pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_;
      }
      vox_idx = setCacheOccupancy(pt_w, 0);
    }
  /* 若该点在地图内:1.不在射线追踪范围,依据该点找一个在射线追踪范围内的新的点,设为**空闲** 
  2.在射线追踪范围,设为**占据** */
    else
    {
      length = (pt_w - md_.camera_pos_).norm();

      if (length > mp_.max_ray_length_)
      {
        pt_w = (pt_w - md_.camera_pos_) / length * mp_.max_ray_length_ + md_.camera_pos_;
        vox_idx = setCacheOccupancy(pt_w, 0);
      }
      else
      {
        vox_idx = setCacheOccupancy(pt_w, 1);
      }
    }
//第一次进入for循环,相当于max_x = pt_w(0),pt_w是点云的位置坐标.之后是取待追踪点云的最小位置坐标
    max_x = max(max_x, pt_w(0));
    max_y = max(max_y, pt_w(1));
    max_z = max(max_z, pt_w(2));
//第一次进入for循环,相当于min_x = pt_w(0).之后是取待追踪点云的最大位置坐标
    min_x = min(min_x, pt_w(0));
    min_y = min(min_y, pt_w(1));
    min_z = min(min_z, pt_w(2));

    // raycasting between camera center and point
    //vox_idx是个一维向量,即是一个数
    //enum { POSE_STAMPED = 1, ODOMETRY = 2, INVALID_IDX = -10000 };
    /*
    if (occ != 1 && occ != 0)
    return INVALID_IDX;
    */
    if (vox_idx != INVALID_IDX)
    {
     // md_.flag_rayend_ = vector(buffer_size, -1);初始化为-1
     //确保一个vox_idx只塞进md_.flag_rayend_一次,用于提高效率
      if (md_.flag_rayend_[vox_idx] == md_.raycast_num_)
      {
        continue;
      }
      else
      {
        md_.flag_rayend_[vox_idx] = md_.raycast_num_;
      }
    }
/*pt_w / mp_.resolution_表示点云的栅格坐标, 
md_.camera_pos_ / mp_.resolution_表示相机的栅格坐标*/
//raycaster.setInput的具体实现见raycast.cpp文件
    raycaster.setInput(pt_w / mp_.resolution_, md_.camera_pos_ / mp_.resolution_);
/*   raycaster.step(ray_pt)=false表示射线追踪到了最后一个点,
即md_.camera_pos_ / mp_.resolution_ .ray_pt表示当前射线追踪点的栅格坐标  */
    while (raycaster.step(ray_pt))
    {
      //tmp表示当前射线追踪点的位置坐标 
      Eigen::Vector3d tmp = (ray_pt + half) * mp_.resolution_;
      length = (tmp - md_.camera_pos_).norm();

      // if (length < mp_.min_ray_length_) break;

      vox_idx = setCacheOccupancy(tmp, 0);

      if (vox_idx != INVALID_IDX)
      {
        if (md_.flag_traverse_[vox_idx] == md_.raycast_num_)
        {
          break;
        }
        else
        {
          md_.flag_traverse_[vox_idx] = md_.raycast_num_;
        }
      }
    }
  }
  //比较点云的最小位置 和 相机的位置, min_x取二者的最小值
  min_x = min(min_x, md_.camera_pos_(0));
  min_y = min(min_y, md_.camera_pos_(1));
  min_z = min(min_z, md_.camera_pos_(2));
  //比较点云的最大位置 和 相机的位置, min_x取二者的最大值
  max_x = max(max_x, md_.camera_pos_(0));
  max_y = max(max_y, md_.camera_pos_(1));
  max_z = max(max_z, md_.camera_pos_(2));
  max_z = max(max_z, mp_.ground_height_);
  //将位置坐标转换为栅格坐标,重置更新栅格的范围
  posToIndex(Eigen::Vector3d(max_x, max_y, max_z), md_.local_bound_max_);
  posToIndex(Eigen::Vector3d(min_x, min_y, min_z), md_.local_bound_min_);
  //确保待更新栅格均在地图内.若不在,取地图边界值予以替代
  boundIndex(md_.local_bound_min_);
  boundIndex(md_.local_bound_max_);
 
 /*if (md_.local_updated_)
   {
     clearAndInflateLocalMap();
  }*/
  md_.local_updated_ = true;

  // update occupancy cached(已缓存) in queue
  Eigen::Vector3d local_range_min = md_.camera_pos_ - mp_.local_update_range_;
  Eigen::Vector3d local_range_max = md_.camera_pos_ + mp_.local_update_range_;

  Eigen::Vector3i min_id, max_id;
  posToIndex(local_range_min, min_id);
  posToIndex(local_range_max, max_id);
  boundIndex(min_id);
  boundIndex(max_id);

  // std::cout << "cache all: " << md_.cache_voxel_.size() << std::endl;
  while (!md_.cache_voxel_.empty())
  {

    Eigen::Vector3i idx = md_.cache_voxel_.front();
    int idx_ctns = toAddress(idx);
    md_.cache_voxel_.pop();
//若判定占据,log_odds_update =mp_.prob_hit_log_;若判定空闲,log_odds_update =mp_.prob_miss_log_
    double log_odds_update =
        md_.count_hit_[idx_ctns] >= md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns] ? mp_.prob_hit_log_ : mp_.prob_miss_log_;

    md_.count_hit_[idx_ctns] = md_.count_hit_and_miss_[idx_ctns] = 0;
    //判定占据
    if (log_odds_update >= 0 && md_.occupancy_buffer_[idx_ctns] >= mp_.clamp_max_log_)
    {
      continue;
    }
    //判定空闲
    else if (log_odds_update <= 0 && md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_)
    {
      md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_;
      continue;
    }

    bool in_local = idx(0) >= min_id(0) && idx(0) <= max_id(0) && idx(1) >= min_id(1) &&
                    idx(1) <= max_id(1) && idx(2) >= min_id(2) && idx(2) <= max_id(2);
    if (!in_local)
    {
      //不在局部更新范围,将体素设定为空闲
      md_.occupancy_buffer_[idx_ctns] = mp_.clamp_min_log_;
    }
     //在局部更新范围,求得正确的占据概率
    md_.occupancy_buffer_[idx_ctns] =
        std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update, mp_.clamp_min_log_),
                 mp_.clamp_max_log_);
  }
}

inline bool GridMap::isInMap(const Eigen::Vector3d& pos) {
  //map_max_boundary,map_min_boundary表示地图的位置（pos）范围
  
  /*mp_.map_min_boundary_ = mp_.map_origin_;
  mp_.map_max_boundary_ = mp_.map_origin_ + mp_.map_size_;*/
  if (pos(0) < mp_.map_min_boundary_(0) + 1e-4 || pos(1) < mp_.map_min_boundary_(1) + 1e-4 ||
      pos(2) < mp_.map_min_boundary_(2) + 1e-4) {
    // cout << "less than min range!" << endl;
    return false;
  }
  if (pos(0) > mp_.map_max_boundary_(0) - 1e-4 || pos(1) > mp_.map_max_boundary_(1) - 1e-4 ||
      pos(2) > mp_.map_max_boundary_(2) - 1e-4) {
    return false;
  }
  return true;
}

Eigen::Vector3d GridMap::closetPointInMap(const Eigen::Vector3d &pt, const Eigen::Vector3d &camera_pt)
{
  Eigen::Vector3d diff = pt - camera_pt;
  Eigen::Vector3d max_tc = mp_.map_max_boundary_ - camera_pt;
  Eigen::Vector3d min_tc = mp_.map_min_boundary_ - camera_pt;

  double min_t = 1000000;

  for (int i = 0; i < 3; ++i)
  {
    if (fabs(diff[i]) > 0)
    {

      double t1 = max_tc[i] / diff[i];
      if (t1 > 0 && t1 < min_t)
        min_t = t1;

      double t2 = min_tc[i] / diff[i];
      if (t2 > 0 && t2 < min_t)
        min_t = t2;
    }
  }

  return camera_pt + (min_t - 1e-3) * diff;
}

int GridMap::setCacheOccupancy(Eigen::Vector3d pos, int occ)
{
  if (occ != 1 && occ != 0)
    return INVALID_IDX;

  Eigen::Vector3i id;
  posToIndex(pos, id);
  int idx_ctns = toAddress(id);
  //体素占据和空闲,md_.count_hit_and_miss_均会自增1
  md_.count_hit_and_miss_[idx_ctns] += 1;
//md_.count_hit_and_miss_[idx_ctns] == 1表示这个体素首次被发现,idx_ctns表示这个体素的一维索引
  if (md_.count_hit_and_miss_[idx_ctns] == 1)
  {
    // md_.cache_voxel_存储体素的三维索引
    md_.cache_voxel_.push(id);
  }

  if (occ == 1)
    md_.count_hit_[idx_ctns] += 1;

  return idx_ctns;
}

3. void GridMap::clearAndInflateLocalMap()

void GridMap::clearAndInflateLocalMap()
{
  /*clear outside local*/
  const int vec_margin = 5;//栅格级别
  //local_bound_min_表示更新栅格的范围,local_map_margin_ = 10 表示局部栅格地图的更新和清除
  Eigen::Vector3i min_cut = md_.local_bound_min_ -
                            Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_);
  Eigen::Vector3i max_cut = md_.local_bound_max_ +
                            Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_);
  boundIndex(min_cut);
  boundIndex(max_cut);

  Eigen::Vector3i min_cut_m = min_cut - Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
  Eigen::Vector3i max_cut_m = max_cut + Eigen::Vector3i(vec_margin, vec_margin, vec_margin);
  boundIndex(min_cut_m);
  boundIndex(max_cut_m);

  // clear data outside the local range(设为空闲)

  for (int x = min_cut_m(0); x <= max_cut_m(0); ++x)
    for (int y = min_cut_m(1); y <= max_cut_m(1); ++y)
    {

      for (int z = min_cut_m(2); z < min_cut(2); ++z)
      {
        int idx = toAddress(x, y, z);
        // md_.occupancy_buffer_[idx] =-1.99243-0.01=-2.00243 < mp_.clamp_min_log_=-1.99243.相当于是设为空闲

        md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
      }

      for (int z = max_cut(2) + 1; z <= max_cut_m(2); ++z)
      {
        int idx = toAddress(x, y, z);
        md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
      }
    }

  for (int z = min_cut_m(2); z <= max_cut_m(2); ++z)
    for (int x = min_cut_m(0); x <= max_cut_m(0); ++x)
    {

      for (int y = min_cut_m(1); y < min_cut(1); ++y)
      {
        int idx = toAddress(x, y, z);
        md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
      }

      for (int y = max_cut(1) + 1; y <= max_cut_m(1); ++y)
      {
        int idx = toAddress(x, y, z);
        md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
      }
    }

  for (int y = min_cut_m(1); y <= max_cut_m(1); ++y)
    for (int z = min_cut_m(2); z <= max_cut_m(2); ++z)
    {

      for (int x = min_cut_m(0); x < min_cut(0); ++x)
      {
        int idx = toAddress(x, y, z);
        md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
      }

      for (int x = max_cut(0) + 1; x <= max_cut_m(0); ++x)
      {
        int idx = toAddress(x, y, z);
        md_.occupancy_buffer_[idx] = mp_.clamp_min_log_ - mp_.unknown_flag_;
      }
    }

  // inflate occupied voxels to compensate robot size
  //obstacles_inflation_=0.099,resolution_=0.1,ceil向上取整，inf_step=1.0
  int inf_step = ceil(mp_.obstacles_inflation_ / mp_.resolution_);
  // int inf_step_z = 1;
  vector<Eigen::Vector3i> inf_pts(pow(2 * inf_step + 1, 3));//inf_pts是27行1列的容器
  
  Eigen::Vector3i inf_pt;

  // clear outdated data
  for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
    for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
      for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z)
      {
        md_.occupancy_buffer_inflate_[toAddress(x, y, z)] = 0;
      }

  // inflate obstacles
  for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
    for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y)
      for (int z = md_.local_bound_min_(2); z <= md_.local_bound_max_(2); ++z)
      {
        //thresh log = mp_.min_occupancy_log_ = 1.38629
    /*  hit: 0.619039
        miss: -0.619039
        min log: -1.99243
        max: 2.19722
        thresh log: 1.38629  */
        //判定为占据
        if (md_.occupancy_buffer_[toAddress(x, y, z)] > mp_.min_occupancy_log_)
        {
 //Eigen::Vector3i(x, y, z)栅格级别,左右各膨胀1*0.1=0.1m,膨胀后的栅格点的坐标存在inf_pts中
          inflatePoint(Eigen::Vector3i(x, y, z), inf_step, inf_pts);

          for (int k = 0; k < (int)inf_pts.size(); ++k)
          {
            inf_pt = inf_pts[k];
            int idx_inf = toAddress(inf_pt);
            if (idx_inf < 0 ||
                idx_inf >= mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2))
            {
              continue;
            }
            md_.occupancy_buffer_inflate_[idx_inf] = 1;
          }
        }
      }

  // add virtual ceiling to limit flight height
  //mp_.virtual_ceil_height_默认值=3.0, mp_.map_origin_(2) = mp_.ground_height_=-0.01
  if (mp_.virtual_ceil_height_ > -0.5) {
    int ceil_id = floor((mp_.virtual_ceil_height_ - mp_.map_origin_(2)) * mp_.resolution_inv_);// ceil_id = 30 是栅格级别,相当于30*0.1=3m
    for (int x = md_.local_bound_min_(0); x <= md_.local_bound_max_(0); ++x)
      for (int y = md_.local_bound_min_(1); y <= md_.local_bound_max_(1); ++y) {
        md_.occupancy_buffer_inflate_[toAddress(x, y, ceil_id)] = 1;
      }
  }
}

inline void GridMap::inflatePoint(const Eigen::Vector3i& pt, int step, vector<Eigen::Vector3i>& pts) {
  int num = 0;
  /* ---------- all inflate ---------- */
  for (int x = -step; x <= step; ++x)
    for (int y = -step; y <= step; ++y)
      for (int z = -step; z <= step; ++z) {
        pts[num++] = Eigen::Vector3i(pt(0) + x, pt(1) + y, pt(2) + z);
      }
}

1.图中红色阴影部分的体素被设为空闲，对应代码中的 //clear data outside the local range.
2.黑色框内的占据体素会被膨胀,左右各膨胀1*0.1=0.1m,而黑色框与蓝色框的边边距离为local_map_margin_*0.1=1m.

  //local_bound_min_表示更新栅格的范围,local_map_margin_ = 10 表示局部栅格地图的更新和清除
  Eigen::Vector3i min_cut = md_.local_bound_min_ -
                            Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_);
  Eigen::Vector3i max_cut = md_.local_bound_max_ +
                            Eigen::Vector3i(mp_.local_map_margin_, mp_.local_map_margin_, mp_.local_map_margin_);

4. vis_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::visCallback, this);

void GridMap::visCallback(const ros::TimerEvent & /*event*/)
{
  publishMapInflate(true);
  publishMap();
}

void GridMap::publishMapInflate(bool all_info)
{

  if (map_inf_pub_.getNumSubscribers() <= 0)
    return;

  pcl::PointXYZ pt;
  pcl::PointCloud<pcl::PointXYZ> cloud;

  Eigen::Vector3i min_cut = md_.local_bound_min_;
  Eigen::Vector3i max_cut = md_.local_bound_max_;

  if (all_info)
  {
    int lmm = mp_.local_map_margin_;//lmm=10,栅格级别
    min_cut -= Eigen::Vector3i(lmm, lmm, lmm);
    max_cut += Eigen::Vector3i(lmm, lmm, lmm);
  }

  boundIndex(min_cut);
  boundIndex(max_cut);

  for (int x = min_cut(0); x <= max_cut(0); ++x)
    for (int y = min_cut(1); y <= max_cut(1); ++y)
      for (int z = min_cut(2); z <= max_cut(2); ++z)
      {
        if (md_.occupancy_buffer_inflate_[toAddress(x, y, z)] == 0)
          continue;

        Eigen::Vector3d pos;
        indexToPos(Eigen::Vector3i(x, y, z), pos);
        if (pos(2) > mp_.visualization_truncate_height_)//visualization_truncate_height_ = 2.9,可视化的最大高度的障碍
          continue;

        pt.x = pos(0);
        pt.y = pos(1);
        pt.z = pos(2);
        cloud.push_back(pt);
      }

  cloud.width = cloud.points.size();
  cloud.height = 1;
  cloud.is_dense = true;
  cloud.header.frame_id = mp_.frame_id_;
  sensor_msgs::PointCloud2 cloud_msg;

  pcl::toROSMsg(cloud, cloud_msg);
  map_inf_pub_.publish(cloud_msg);

  // ROS_INFO("pub map");
}

void GridMap::publishMap()
{

  if (map_pub_.getNumSubscribers() <= 0)
    return;

  pcl::PointXYZ pt;
  pcl::PointCloud<pcl::PointXYZ> cloud;

  Eigen::Vector3i min_cut = md_.local_bound_min_;
  Eigen::Vector3i max_cut = md_.local_bound_max_;

  int lmm = mp_.local_map_margin_ / 2;
  min_cut -= Eigen::Vector3i(lmm, lmm, lmm);
  max_cut += Eigen::Vector3i(lmm, lmm, lmm);

  boundIndex(min_cut);
  boundIndex(max_cut);

  for (int x = min_cut(0); x <= max_cut(0); ++x)
    for (int y = min_cut(1); y <= max_cut(1); ++y)
      for (int z = min_cut(2); z <= max_cut(2); ++z)
      {
        //判定为空闲
        if (md_.occupancy_buffer_[toAddress(x, y, z)] < mp_.min_occupancy_log_)
          continue;

        Eigen::Vector3d pos;
        indexToPos(Eigen::Vector3i(x, y, z), pos);
        if (pos(2) > mp_.visualization_truncate_height_)
          continue;

        pt.x = pos(0);
        pt.y = pos(1);
        pt.z = pos(2);
        cloud.push_back(pt);
      }

  cloud.width = cloud.points.size();
  cloud.height = 1;
  cloud.is_dense = true;
  cloud.header.frame_id = mp_.frame_id_;
  sensor_msgs::PointCloud2 cloud_msg;

  pcl::toROSMsg(cloud, cloud_msg);
  map_pub_.publish(cloud_msg);
}