removed_ids_存在索引偏移的bug

[linminjy]

removed_ids_ 应该是记录被移除的前沿在原始 frontiers_ 列表中的索引位置（即第几个被删的）的，但是当调用 resetFlag(iter, frontiers_) 之后，iter 已经被 erase() 更新了，新迭代器返回了下一个合法元素，此时 rmv_idx 却没有自增，这会导致后续的 rmv_idx 索引偏移，最终 removed_ids_ 的编号不正确。

` auto resetFlag = [&](list::iterator& iter, list& frontiers) {
Eigen::Vector3i idx;
for (auto cell : iter->cells_) {
edt_env_->sdf_map_->posToIndex(cell, idx);
frontier_flag_[toadr(idx)] = 0;
}
iter = frontiers.erase(iter);
};

// std::cout << "Before remove: " << frontiers_.size() << std::endl;

removed_ids_.clear();
int rmv_idx = 0;
for (auto iter = frontiers_.begin(); iter != frontiers_.end();) {
// haveOverlap(iter->box_min_, iter->box_max_, update_min, update_max)
if (haveAnyOverlap(iter->box_min_, iter->box_max_, mins, maxs) && isFrontierChanged(*iter)) {
resetFlag(iter, frontiers_);
removed_ids_.push_back(rmv_idx);
} else {
++rmv_idx;
++iter;
}
}`