体素信息结构(VIS):
| Name | mean | 符号 |
|---|---|---|
| position | 体素坐标 | pos |
| occupancy | 占用概率 | occ |
| ESDF | 到最近障碍物的欧几里得距离 | dis |
| Closest Obstacle Coordinate | 最近障碍物的体素坐标 | coc |
| observed | 是否曾经观察到这种体素 | obs |
| prev, next, head | DLL用过 | prev, next, head |
| Doubly Linked List（DLL） | 最近障碍物是该体素的所有体素 | dll |
| Neighhoods | 该体素的所有观察到的Neighhood（如26连接） | nbrs |
索引数据结构
该数据结构用来将体素坐标映射到VIS的索引中，可以使用数组的方式或者哈希表的方式。
数组方式：当通过其坐标查询体素时，我们只需要计算其在该数组内的索引，然后返回VIS的相应指针即可。缺点：地图大占内存。
哈希表方式：使用哈希表将体素坐标转换为其相应的VIS指针。缺点：查找操作数量众多，性能差。
我们：将（块大小)3个体素，作为一块，哈希表仅用于管理块，在根据体素坐标计算块坐标之后，该哈希表将用于查找对应的块。该块中所有体素的VIS的所有指针都存储在相对于该块的数组中。
双向链表(DLLs)
使用DLLs可以在体素从占有变为空闲的时候，高效地更新ESDF地图。一个体素vox所对应的dll存放了所有最近障碍物是vox的体素。它是个双向链表，能够快速的遍历。
对于障碍物vox，DLL存储了所有最近障碍物是vox的体素。
过程
在此过程中，所有更改其占用状态的体素均被添加到分别名为insertQueue和deleteQueue的两个队列中。之后，将这两个队列合并为一个名为updateQueue的队列，并使用基于BFS的ESDF更新算法来更新ESDF可能更改的所有体素。
我们使用光线追踪法来将新的占有信息叠加到占有栅格地图中。所有占有状态发生变化的体素会被添加到insertQueue或deleteQueue中。
情况1：假设添加为“占据”的体素，对地图进行更新：
仅有insertQueue， deleteQueue 为空
对于每个被更新过的点cur，处理更新它的临近点nbr，如果临近点nbr之前的esdf 小于 其到cur的最近障碍物的距离，那esdf就更新为其到cur的最近障碍物的距离，把nbr也放到更新过的点里。
大致如图，当某个点占据情况发生变化时，搜索它的邻居，如果这个邻居离该点的距离 比之前的最近距离（esdf）近，就更新成本次距离，并把该邻居也作为生长点，搜索它的邻居进行比较。
情况2:既添加又删除
添加时，该点的 d i s = 0 dis=0 dis=0，然后执行上一步的更新操作
删除时，该点的
d
i
s
=
∞
dis=∞
dis=∞，查找所有以被删除点cur为最近障碍物的点vox，查找vox的nbr的最近障碍物，最近的那个作为vox的最近障碍物。
情况3：补丁
bug：当观察到范围外的点，t0时观察到0，t1时观察到1~3。按步骤1的话，t0时刻不会更新体素1~3，因为他们属于为探测区域；t1时刻，体素3为占据，以它为中心更新它的邻居，第1个体素以为最近的是3。（因为0没发生变化，没再更新障碍0）
补丁：添加内容->当体素cur到其邻居的最近障碍物的距离，比他自身的近，就换成其邻居的最近障碍物
存在的误差：
假设A要被更新，因为 n b r . d i s ← D I S T ( c u r . c o c , n b r . p o s ) nbr.dis ← DIST(cur.coc, nbr.pos) nbr.dis←DIST(cur.coc,nbr.pos)，考虑AC, 其中A是nbr，c是cur。在红蓝之间，离A更近，但离C原，所以A的最近点也会被更新为B。
test_fiesta.cpp：
从test_fiesta.cpp进入，就实例化了一个模板类fiesta，具体实现都在类的构造函数中。
这里可以修改类的输入类型，点云格式：点云还是深度图，位姿格式：path还是odom等。支持的类型回调中可以看到：
int main(int argc, char **argv)
fiesta::Fiesta<sensor_msgs::PointCloud2::ConstPtr, geometry_msgs::TransformStamped::ConstPtr> esdf_map(node);
Fiesta.h：
构造函数：
esdf_map_->SetParameters（）//读入参数
//订阅者，见1、2、
transform_sub_ = node.subscribe("transform", 10, &Fiesta::PoseCallback, this);\
depth_sub_ = node.subscribe("depth", 10, &Fiesta::DepthCallback, this);
//发布者
pass
//定时器,每隔一定时间进入一次回调，见3、
update_mesh_timer_ = node.createTimer(ros::Duration(time), &Fiesta::UpdateEsdfEvent, this);
1、PoseCallback：
//对于不同的输入格式，位姿（平移和旋转）提取出来放到pos、q
if constexpr(std::is_same<PoseMsgType, geometry_msgs::PoseStamped::ConstPtr>::value)
pos = Eigen::Vector3d(msg->pose.position.x, msg->pose.position.y, msg->pose.position.z);
q = Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, msg->pose.orientation.y, msg->pose.orientation.z);
//放到位姿队列transform_queue_
transform_queue_.push(std::make_tuple(msg->header.stamp, pos, q));
2、DepthCallback：
depth_queue_.push(depth_map);//直接放入
SynchronizationAndProcess();//时间同步、depth类型处理、位姿变换、Raycast
SynchronizationAndProcess()：时间同步、depth类型处理、位姿变换、Raycast
//认为位姿比深度图来得密集得多
//从位姿队列找和当前depth最接近的位姿（用迭代的方式），由R、t->Twc 放到transform_
while (transform_queue_.size() > 1 && std::get<0>(transform_queue_.front()) <= depth_time + ros::Duration(time_delay))
sync_pos_ = std::get<1>(transform_queue_.front());
sync_q_ = std::get<2>(transform_queue_.front());
transform_queue_.pop();
//齐次变换T Ra+t => Ta
transform_.block<3, 3>(0, 0) = sync_q_.toRotationMatrix();
transform_.block<3, 1>(0, 3) = sync_pos_; //[R|t]
transform_(3, 0) = transform_(3, 1) = transform_(3, 2) = 0;
transform_(3, 3) = 1;
//求Pc(相机在世界系下的位置)
transform_ = transform_*parameters_.T_D_B_*parameters_.T_B_C_;//Twc = word to depth * depth to body * body to camera (Twc也就是Pc)
raycast_origin_ = Eigen::Vector3d(transform_(0, 3), transform_(1, 3), transform_(2, 3))/transform_(3, 3);//归一
//如果输入的是深度图
//滤波 + 深度图转换到相对于本次camrea系的点云,push到cloud_
if constexpr(std::is_same<DepthMsgType, sensor_msgs::Image::ConstPtr>::value) DepthConversion();
//如果输入的是点云
pcl::fromROSMsg(*tmp, cloud_);//直接转换格式放进来
滤波步骤，对深度图每个像素：
//最后raycast（每帧深度图执行一次）
RaycastMultithread()
RaycastMultithread()：
//if单线程：0：第0个线程；size():处理全部点；tt：第几帧了（用于raycast时减少计算）
RaycastProcess(0, cloud_.points.size(), tt);
//if多线程：i：第i个线程；part：每个线程处理多少点；tt：第几帧了
for (size_t i = 0; i < parameters_.ray_cast_num_thread_; ++i)
integration_threads.emplace_back(&Fiesta::RaycastProcess, this, i, part, tt);
RaycastProcess（）：
for (int idx = part*i; idx < part*(i + 1); idx++)//i个线程处理的点index = part*i ~ part*(i+1)
//得到点云（占据）坐标
pcl::PointXYZ pt = cloud_.points[idx];//深度图中的点变换到camera系后存到cloud_.points中
Eigen::Vector4d tmp = transform_*Eigen::Vector4d(pt.x, pt.y, pt.z, 1);//变到世界系 Pw=Twc*Pc
Eigen::Vector3d point = Eigen::Vector3d(tmp(0), tmp(1), tmp(2))/tmp(3);//Pw
//设置占据
tmp_idx = esdf_map_->SetOccupancy((Eigen::Vector3d) point, 1)//将这个占据点point（相对world）的index(相对world原点)添加到occupancy_queue_,返回index
if (set_occ_[tmp_idx]==tt)//如果这个体素已经设为占据了（如果某体素格更新次数=帧数）（好几个点属于同一个体素），就不raycast了，跳到下一个
continue;
else//如果第一次占据，就设成占据
set_occ_[tmp_idx] = tt;
//raycast
Raycast(raycast_origin_/parameters_.resolution_,//相机原点所属的体素格子
point/parameters_.resolution_,//占据点所属体素格子
parameters_.l_cornor_/parameters_.resolution_,
parameters_.r_cornor_/parameters_.resolution_,//地图边界
&output);//输出
//设置空闲
for (int i = output.size() - 2; i >= 0; i--)
tmp_idx = esdf_map_->SetOccupancy(tmp, 0);//output对应的体素格设为空
//如果某体素格更新次数=帧数，也就意味着本帧在更新过了，再往相机中心的raycast就不用做了（已经做过了）
if (set_free_[tmp_idx]==tt) //在往原点找,已经和其他射线交汇了,就不用再找了.
break；
else
set_free_[tmp_idx] = tt;//赋值，证明已经raycast过了
其中：
Pos2Vox(pos, vox);//相对world的坐标转换为体素栅格坐标
idx = Vox2Idx(vox);//体素栅格坐标转换为（行向量）索引
num_miss_[idx]++;//所有的都++
num_hit_[idx] += occ;// +0(空) 或 +1 (占):即只有占据+1
occupancy_queue_.push(QueueElement{vox, 0.0});//vox：体素栅格坐标（占）；
1、步进时x，y，z增加的方向（ + or - ）。
2、求最小的正t，使start.x()+t*ds为整数（即决定下一步 x+stepX or y+stepY or z+stepZ ）。
3、求变化步长step
4、从相机原点开始，output->push_back(Eigen::Vector3d(x, y, z));
5、更新： x+stepX or y+stepY or z+stepZ ，返回 2
3、UpdateEsdfEvent 回调
//更新占据栅格
esdf_map_->UpdateOccupancy(parameters_.global_update_);
//更新esdf
esdf_map_->UpdateESDF();
//发布
Visualization(esdf_map_, parameters_.global_vis_, "");
UpdateOccupancy()：
while (!occupancy_queue_.empty())//所有raycast过的体素（当然包含顶端占据点）
QueueElement xx = occupancy_queue_.front();
int idx = Vox2Idx(xx.point_);//体素坐标xyz转换为向量的索引位置
int occupy = Exist(idx);//更新前是否占据（用来判断状态是否变化）
double log_odds_update = (num_hit_[idx] >= num_miss_[idx] - num_hit_[idx] ? prob_hit_log_ : prob_miss_log_);//判断是否占据，决定其对数概率值。本帧观测 占据的次数>空闲的次数 就用 prob_hit_log_（0.368），否则用prob_miss_log_（-0.296）
//初始化该体素，注意这里！！！本来初始化地图时，所有体素初始化为负无穷，这里初始化occupancy_queue_里的体素，即在观测范围内的，就把他设为正无穷
//这样更新最近障碍物时,肯定距离<正无穷，就可以更新到了。同时没被观测到的点，距离仍为-无穷，不会更新邻居时被更新。
if (!global_map && !VoxInRange(xx.point_, false)) {
occupancy_buffer_[idx] = 0;//占据概率为0
distance_buffer_[idx] = infinity_;//最近障碍物距离为无穷(10000)
}
//更新占据概率occupancy_buffer_: L' = L + log(p/1-p) 他这儿有一个范围限制
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});
//这个点 占据->空闲 放到delete_queue,距离设置为无穷
else if (!Exist(idx) && occupy)
delete_queue_.push(QueueElement{xx.point_, (double) infinity_});
UpdateESDF()：这部分参阅论文就好了
论文中的几个变量
xx.point_:pos
Exist(idx):occ
distance_buffer_:dis //地图大小,存了所有体素的最近障碍物dis
closest_obstacle_:coc
prev_\next_\head_:prev_\next_\head_
new_pos:nbr
idx：向量中的索引位置
QueueElement xx = insert_queue_.front();
insert_queue_.pop();
int idx = Vox2Idx(xx.point_);
DeleteFromList(Vox2Idx(closest_obstacle_[idx]), idx);//双向链表: A的最近障碍物vox<->以vox为最近障碍物的体素
closest_obstacle_[idx] = xx.point_;//最近障碍物是自己
distance_buffer_[idx] = 0.0;//最近距离为0
InsertIntoList(idx, idx);//自己<->自己
update_queue_.push(xx);
QueueElement xx = delete_queue_.front();
delete_queue_.pop();
int idx = Vox2Idx(xx.point_);
//从头开始,把以idx为最近障碍物的体素obs_vox(索引为obs_idx),全处理一遍
int next_obs_idx;
for (int obs_idx = head_[idx]; obs_idx != undefined_; obs_idx = next_obs_idx)
closest_obstacle_[obs_idx] = Eigen::Vector3i(undefined_, undefined_, undefined_);//最近障碍物:未定义
double distance = infinity_;//距离:无穷
Eigen::Vector3i obs_vox = Idx2Vox(obs_idx);//索引转体素
// 遍历他的邻居,找到有最近障碍物的邻居，把这个最近障碍物给他
for (const auto &dir : dirs_) //dirs_是一个上下左右各+1的东西
Eigen::Vector3i new_pos = obs_vox + dir;
int new_pos_idx = Vox2Idx(new_pos);
if (VoxInRange(new_pos) && closest_obstacle_[new_pos_idx](0) != undefined_ && Exist(Vox2Idx(closest_obstacle_[new_pos_idx]))) //有最近障碍物,且验证它是占据的(避免最近障碍物是刚刚删除的)
double tmp = Dist(obs_vox, closest_obstacle_[new_pos_idx]);//vox到他邻居的最近障碍物的距离
//比较:vox是以删除的障碍物为最近障碍物的点,vox到他邻居的最近障碍物的距离,小于他本来的距离(无穷或上一个邻居的距离)
if (tmp < distance) {//vox到他邻居的最近障碍物的距离,小于他本来的距离
distance = tmp;//修改它到最近障碍物的距离
closest_obstacle_[obs_idx] = closest_obstacle_[new_pos_idx];
break;
// 删除双向链表
prev_[obs_idx] = undefined_;
next_obs_idx = next_[obs_idx];//处理链表指向的下一个
next_[obs_idx] = undefined_;
update_queue_.push(QueueElement{obs_vox, distance});//广度优先算法,如果这个体素被更新过,下次他的邻居也要被更新
int new_obs_idx = Vox2Idx(closest_obstacle_[idx]);//最近障碍物index
for (const auto &dir : dirs_) //dirs_存了邻居的δ，即各个方向+
Eigen::Vector3i new_pos = xx.point_ + dir;//邻居的位置
int new_pos_id = Vox2Idx(new_pos);//邻居的index
double tmp = Dist(new_pos, closest_obstacle_[idx]);//idx邻居到idx的最近障碍物的距离
if (distance_buffer_[new_pos_id] > tmp) {//邻居到最近障碍物的距离比tmp远,需要更新这个邻居.
closest_obstacle_[new_pos_id] = closest_obstacle_[idx];//改他的最近障碍物
distance_buffer_[new_pos_id] = tmp;//改他的最近障碍物距离
//改他的DLL
DeleteFromList(Vox2Idx(closest_obstacle_[new_pos_id]), new_pos_id);
InsertIntoList(new_obs_idx, new_pos_id);
update_queue_.push(QueueElement{new_pos, tmp});//更新过的邻居也放进“更新过的点”的队列，下次他的邻居也更新
4、发布
发布占据栅格地图
esdf_map->GetPointCloud(pc, parameters_.vis_lower_bound_, parameters_.vis_upper_bound_);//遍历地图，把occupancy_buffer_,把占据的点拿出来,放到pc中
occupancy_pub_.publish(pc);
发布距离场
esdf_map->GetSliceMarker(slice_marker, parameters_.slice_vis_level_, 100,//切片高度
Eigen::Vector4d(0, 1.0, 0, 1), parameters_.slice_vis_max_dist_);//遍地图中的每一个x,y,将其最近障碍物的距离从distance_buffer_中取出,发布出去（在范围内的）
slice_pub_.publish(slice_marker);