FIESTA

related work

地图有grid map, octomap, point cloud, topomap(slam feature), sparse map(esdf based topomap), nanomap(local)

motion plan 不需要一直计算esdf. 应该fuse map to global。

对于voxblox改进：

• 减小了两种误差. 第二种没了，但第一种还有。
• 不用TSDF，更容易量化和改进。

data structure

如果内存够，直接把voxel存到数组中。内存不够，需要有从voxel coord 到Voxel information structure的指针的一个映射，就是hashmap. 但是查找需要很多时间。
一个tradeoff就是用voxel hashing, 只有block才用hash，这样会更快。使用(bitwise会更快，把block_size=2,4,8,)。

双向链表

所有VIS都在DLL中。先根据坐标查找VIS，再增加或者删除VIS，总共是O(1)时间。
为啥用DLL？ 所有新发现但是没有被更新(esdf)的voxel都加到DLL中（插到头部。）一般先来的都是vironoi 里边的，后来的（靠近头部）都是外围的。
每一个DLL都对应一个特定的obs, DLL中所有元素到该OBS都最短，也就是obs是vironoi中心。

算法

更新occupancy

alg2:
新发现的voxel，更新occupancy, 产生insert queue和delete queue，二者生成需要Update esdf的update queue。
alg1: 根据update queue来更新esdf
update queue一开始是obs，之后成为BFS的frontier。uodate queue中弹出curr,使用BFS来更新cur的nbr的最近obs信息，如果nbr的最近obs被更新，那么将其push到update queue中，后面再更新nbr的nbr

alg3：要考虑没有observe，但是以前observe过的obs的影响，要用alg3来处理update queue。具体是，从update queue弹出一个cur，如果cur的邻居的最近obs到cur的距离比cur到自己的最近obs的距离要小，那么说明cur记录的最近obs信息并不准确，不能直接用作更新EDF, 需要更新cur的最近obs信息，再将cur加到update queue中，跳过下面步骤continue，直到cur的最近obs的信息完全正确，再转到alg1 line4。

代码细节

block_bit=3
block_=8
block_size_=8x8x8

在收到depth时候，将其和pos一起同步，之后进行多线程的raycast（用了std::list< std::thread >.emplace_back），但没有生成occupancy map, 而只是在setOccupancy中放到了occupancyQueue中。在更新ESDF的时候才更新occupancy map.

ESDF是固定频率更新的。更新occupancy map,再更新ESDF，用的就是上面说的3个alg啦。

voxel_hashing代码再ESDFMap.cpp:732. ，findandInsert里面。这个函数作用就是一个从vox到内存中idx的映射。具体是计算vox在block中的位置idx_in_block,再计算vox所属于block的id(block_id)，根据这两个id来计算内存中的id。但这个内存在哪里分配的呢？
答案是，都是在ESDFMap中分配的，每一个都分配的reserve_size（默认是1000000）的vector。注意在每次findandinsert开始，都需要检查现在的block_size*block_num是不是>reserve_size，是的话说明不够，vector内存也就是reserve_size要x2。

hash_table定义是这样紫：
param 1是hash_key，用来查找的。
param 2传入的是count，也就是index of the Voxel Information Structure.
param 3传入的是hash的方法，如何把3D voxel coord map到index。

 std::unordered_map<Eigen::Vector3i, int, MatrixHash<Eigen::Vector3i>> hash_table_;
 

MatrixHash是这样子：

template<typename transform_>
struct MatrixHash : std::unary_function<transform_, size_t> {
  std::size_t operator()(transform_ const &matrix) const {
    // Note that it is oblivious to the storage order of Eigen matrix (column- or
    // row-major). It will give you the same hash value for two different matrices if they
    // are the transpose of each other in different storage order.
    size_t seed = 0;
    for (size_t i = 0; i < matrix.size(); ++i) {
      auto elem = *(matrix.data() + i);
      seed ^= std::hash<typename transform_::Scalar>()(elem) + 0x9e3779b9 + (seed << 6) + (seed >> 2);
    }
    return seed;
  }
};

这个transform_太迷惑了，不是表示位姿，而是一个形参，跟T一样。
unary_function将这个hash函数变成一个一元运算符（这样说不专业）。专业的说，operator成员函数继承unary_function这个基类，成为一个实例被调用。具体可看csdn. 感觉就是语法特殊了点，没啥优越性？

如果当前更新的vox坐标对应的block_id在hash table中找不到，那么要在hash table中加一个block。block中每一个vox都赋值到vox_buffer(内存)里，這是爲了在可視化以及debug的時候方便寻秩访问。最后返回vox在内存中的idx。
如果找到了，那么返回在内存中的地址。方法是

return tmp->second + idx_in_block;

tmp->second就是block的首地址，加上偏置以后就是vox的id，在vox_buffer里面查找一下就是值了。