FIESTA个人理解 五 updateESDF() 删除 上

 //删除：

    QueueElement xx = delete_queue_.front();

    delete_queue_.pop();

    int idx = Vox2Idx(xx.point_); //从delete_queue的头部取出xx 转换成序号idx

    if (!Exist(idx)) { //判断不占据，确认idx体素不再是障碍物

      // doesn't Exist after a whole brunch of updates

      //从头开始,把以idx为最近障碍物的体素obs_vox(索引为obs_idx),全处理一遍

      int next_obs_idx;

      for (int obs_idx = head_[idx]; obs_idx != undefined_; obs_idx = next_obs_idx) {

        //next_obs_idx是obs_idx的下一个索引next_[obs_idx]

        closest_obstacle_[obs_idx] = Eigen::Vector3i(undefined_, undefined_, undefined_);//初始化最近障碍物:未定义

        Eigen::Vector3i obs_vox = Idx2Vox(obs_idx);//索引转体素



        double distance = infinity_;//初始化距离:正无穷

        // find neighborhood whose closest obstacles Exist

        // 目的：遍历他的邻居,找到有最近障碍物的邻居，把这个最近障碍物给他！！！！！

        //由于目前obs_idx的障碍物被删除了，所以需要寻找新障碍物给obs_idx，从他的邻居中找他的障碍物，目前用的是24连通

        for (const auto &dir : dirs_) {//dirs_一个点往上下左右走两步以内能到达的点 遍历邻居？

          Eigen::Vector3i new_pos = obs_vox + dir; //计算两步内所有邻居的位置

          int new_pos_idx = Vox2Idx(new_pos); //转换成邻居的idx，new_pos_idx

          //邻居有最近障碍物,且验证该障碍物是占据的(避免最近障碍物是刚刚删除的)

          if (VoxInRange(new_pos) && closest_obstacle_[new_pos_idx](0) != undefined_

              && Exist(Vox2Idx(closest_obstacle_[new_pos_idx]))) {

            // if in range and closest obstacles Exist

            double tmp = Dist(obs_vox, closest_obstacle_[new_pos_idx]);//当前点obs_idx到他邻居的最近障碍物的距离

      //比较:vox是以删除的障碍物为最近障碍物的点,vox到他邻居的最近障碍物的距离,小于他本来的距离(无穷或上一个邻居的距离)

            if (tmp < distance) {//vox到他邻居的最近障碍物的距离,小于他本来的距离

              distance = tmp;//修改它到最近障碍物的距离

              closest_obstacle_[obs_idx] = closest_obstacle_[new_pos_idx];

            } //存在误差的问题，具体文献中有所体现，无法用一个大圆完全包含多个小圆

            break;

          } // if

        } // for neighborhood