Ego-planner：三维建图 grid_map.cpp解析

ego避障里面比较重要的功能是对环境进行栅格地图进行避障
其实是通过深度图像或者点云数据来进行转换填充的，（没错，ego也可以利用点云来建图，也就是无需d435i进行输入，可以替换为激光雷达）
下面我们分别对这两个方式的建图方法进行解析

首先我们需要明确几个十分重要的变量：

• buffer_size： 三维栅格地图的体积，是地图尺寸/分辨率获得的
• md_.occupancy_buffer_：初始的栅格地图占据状态，一维数组。他是把空间的3维数据映射到1维去处理的；这个变量只有用深度图像计算占据栅格会用到，直接用点云计算不会用到。
• md_.occupancy_buffer_inflate_：膨胀后的栅格地图占据状态，膨胀指的是在原本的障碍物栅格上增加一圈无人机的体积，这样可以把无人机看成一个质点处理。点云计算直接获得，深度相机通过md_.occupancy_buffer_转换获得
• md_.count_hit_：用于记录raycast过程中栅格被占据的次数
• md_.count_hit_and_miss_：用于记录raycast过程中射线经过该栅格的次数，也就是命中和未命中的总次数
• mp_.local_update_range_：局部更新范围，也就是只在camera的pose周围来进行栅格地图md_.occupancy_buffer_inflate_的更新。
  如在点云处理中（cloudCallback），先对camera_pose的局部范围重置resetBuffer，即栅格地图全都置为0；再利用点云及其膨胀范围确定特定的栅格为1；
  如在深度图像处理中，在raycastProcess中，只对局部地图内的occupancy_buffer_进行更新
• md_.local_bound_min_：其值为(0, 0, 0)
  md_.local_bound_max_：其值为(map_size / resolution - 1, map_size / resolution - 1, map_size / resolution - 1)
  以上两个值代表栅格地图的大小，但其对应的栅格地图数值点云却是随着camerapose移动更新的

深度图像raycast理论：

占据概率（Occupancy Probability）：每个栅格有一个概率值。0 表示栅格完全空闲；1 表示被占据。0.5 表示占据状态未知。

对数似然比（Log-Odds Representation）：计算和更新占据概率的一种常用方法。相比直接使用概率，对数似然比可以更方便地进行递归更新，并避免计算中的数值不稳定问题。

代码流程解析

1. 主函数：initMap(ros::NodeHandle &nh)

// 1.订阅深度图像
  depth_sub_.reset(new message_filters::Subscriber<sensor_msgs::Image>(node_, "/grid_map/depth", 50));

  // 根据pose_type_的值，选择订阅机器人位姿的不同消息类型（PoseStamped或Odometry）
  if (mp_.pose_type_ == POSE_STAMPED)
  {
    pose_sub_.reset(
        new message_filters::Subscriber<geometry_msgs::PoseStamped>(node_, "/grid_map/pose", 25));
    // 使用消息过滤器（message_filters）来同步深度图像与位姿数据，并在接收到同步数据时触发回调函数进行处理
    sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(
        SyncPolicyImagePose(100), *depth_sub_, *pose_sub_));
    sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2));
  }
  else if (mp_.pose_type_ == ODOMETRY)
  {
    odom_sub_.reset(new message_filters::Subscriber<nav_msgs::Odometry>(node_, "/grid_map/odom", 100));

    sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(
        SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_));
    sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));
  }

  // 2.订阅点云图像，相机位姿为Odometry
  indep_cloud_sub_ =
      node_.subscribe<sensor_msgs::PointCloud2>("/grid_map/cloud", 10, &GridMap::cloudCallback, this);
  indep_odom_sub_ =
      node_.subscribe<nav_msgs::Odometry>("/grid_map/odom", 10, &GridMap::odomCallback, this);

  // 3.处理流程，应该只是针对深度图像
  // occ_timer_：创建一个定时器，每隔0.05秒（即50毫秒）触发一次updateOccupancyCallback回调函数，用于更新栅格地图的占据状态。
  // 这个定时器确针对的是深度图像的处理，保地图能够定期更新，以反映传感器数据的变化。
  occ_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::updateOccupancyCallback, this);

  // 4.发布可视化占据栅格地图
  // vis_timer_：创建另一个定时器，同样每隔0.05秒触发一次visCallback回调函数，用于更新地图的可视化数据。
  // 这包括生成并发布栅格地图的可视化信息，使其可以在ROS的可视化工具（如RViz）中查看。
  vis_timer_ = node_.createTimer(ros::Duration(0.05), &GridMap::visCallback, this);

  map_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/occupancy", 10);
  map_inf_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/occupancy_inflate", 10);

  unknown_pub_ = node_.advertise<sensor_msgs::PointCloud2>("/grid_map/unknown", 10);

2. 深度图像转换为占据栅格函数：updateOccupancyCallback(const ros::TimerEvent & /*event*/)

主要有以下三个函数：

  projectDepthImage();
  raycastProcess();
  if (md_.local_updated_)
    clearAndInflateLocalMap();

（1）projectDepthImage()主要用于将 深度图像的有效像素点 => 世界坐标系下的三维点，并存储在md_.proj_points_中。处理方式可以选择滤波或者不滤波，滤波可以加快处理。

（2）raycastProcess()射线投射算法，这种方法通过将射线经过的栅格标记为自由空间，而射线终止的栅格标记为占据空间。
该算法首先利用以上思路标注经过栅格的count_hit_and_miss_和count_hit_，并且将修改的栅格id记录到cache_voxel_中；
然后在局部地图（由mp_.local_update_range_确定）中计算occupancy_buffer_。具体代码如下

while (!md_.cache_voxel_.empty())
  {
    // 从缓存队列中获取栅格索引
    Eigen::Vector3i idx = md_.cache_voxel_.front();
    int idx_ctns = toAddress(idx);
    md_.cache_voxel_.pop();

    // 根据命中情况计算对数似然比更新值
    // md_.count_hit_[idx_ctns] 表示该栅格在射线投射过程中被认为是“被占据”的次数
    // md_.count_hit_and_miss_[idx_ctns] 表示总的射线经过该栅格的次数，包括命中和未命中的情况。
    // 通过判断命中次数是否大于等于未命中次数来决定是否将该栅格的状态更新为“被占据”（使用 mp_.prob_hit_log_）或“空闲”（使用 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_;
    }
    // 对栅格的对数似然比进行更新，并确保其值在最小和最大范围 mp_.clamp_min_log_ 和 mp_.clamp_max_log_ 之间
    // 更新公式：当前时刻似然比 = 上一时刻似然比 + 更新增量(log_odds_update)
    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_);

（3）clearAndInflateLocalMap()主要用于清理地图中无效数据，对障碍物进行膨胀，并为地图添加一个虚拟天花板

该函数首先扩展范围的边界缓冲，是为了后面的障碍物膨胀。注意这里是对全局地图进行扩展

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_);

并将额外扩展的新的栅格标记为未确定的空闲状态

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

最后的目的是完成occupancy_buffer_inflate_数组的填充，即对障碍物膨胀周围，以及天花板栅格都填充为1

3. 发布可视化地图函数visCallback(const ros::TimerEvent & /*event*/)

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

publishMap函数是把md_.occupancy_buffer_中的对应1的值取出来，提取其坐标再转换成物理坐标存入点云cloud，最后包装成PointCloud2消息格式发布的。