FIESTA个人理解 一

//根据点云判断体素占据情况，并更新占据概率，放入insert或delete中

bool fiesta::ESDFMap::UpdateOccupancy(bool global_map) {

#ifdef PROBABILISTIC

  cout << "Occupancy Update" << ' ' << occupancy_queue_.size() << '\t';

  while (!occupancy_queue_.empty()) {//所有raycast过的体素（当然包含顶端占据点）

    QueueElement xx = occupancy_queue_.front();

    occupancy_queue_.pop();//从头取出一个放在 xx 中

    int idx = Vox2Idx(xx.point_);//体素坐标xyz转换为向量的索引位置

    int occupy = Exist(idx);//更新前是否占据（用来判断状态是否变化）

//判断是否占据，决定其对数概率值。

//本帧观测 占据的次数>空闲的次数 就用 prob_hit_log_（0.368 = e^-1），否则用prob_miss_log_（-0.296）

//一帧信息内 一个体素中可能有很多点云信息，所以存在占据和空闲！！！！！！！！

    //对数更新概率？

    double log_odds_update = (num_hit_[idx] >= num_miss_[idx] - num_hit_[idx] ? prob_hit_log_ : prob_miss_log_);



    num_hit_[idx] = num_miss_[idx] = 0;

    if (distance_buffer_[idx] < 0) { //处于undefined的状态

      distance_buffer_[idx] = infinity_;

      InsertIntoList(reserved_idx_4_undefined_, idx); //idx插入到最后，障碍物距离为无穷大？ reserved = 总网格大小

      //在观测范围内的，就把他设为正无穷

    }

    if ((log_odds_update >= 0 &&

        occupancy_buffer_[idx] >= clamp_max_log_) ||

        (log_odds_update <= 0 &&

            occupancy_buffer_[idx] <= clamp_min_log_)) {

      continue;

    }



//初始化该体素，注意这里！！！本来初始化地图时，所有体素初始化为负无穷，这里初始化occupancy_queue_里的体素，即在观测范围内的，就把他设为正无穷

//这样更新最近障碍物时,肯定距离<正无穷，就可以更新到了。同时没被观测到的点，距离仍为-无穷，不会更新邻居时被更新。

    if (!global_map && !VoxInRange(xx.point_, false)) {

      occupancy_buffer_[idx] = 0; //设为不占用

      distance_buffer_[idx] = infinity_; //距离设为正无穷大

    }

    //更新占据概率occupancy_buffer_: L' = L + log(p/1-p) 已经计算好了 log_odds_update  

    //他这儿有一个范围clamp_max - clamp_min的限制

    occupancy_buffer_[idx] = std::min(

        std::max(occupancy_buffer_[idx] + log_odds_update, clamp_min_log_),

        clamp_max_log_);

    //这个点 空闲->占据 放到insert_queue,距离设置为0 

    if (Exist(idx) && !occupy) {//现在占据&&之前不占据

      insert_queue_.push(QueueElement{xx.point_, 0.0});

    } else if (!Exist(idx) && occupy) {//这个点 占据->空闲 放到delete_queue,距离设置为无穷    

      delete_queue_.push(QueueElement{xx.point_, (double) infinity_});

    }

  }

#endif

  return !insert_queue_.empty() || !delete_queue_.empty();

}