fiesta论文翻译和代码

论文

体素信息结构(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指针。缺点：查找操作数量众多，性能差。

我们：将（块大小)³个体素，作为一块，哈希表仅用于管理块，在根据体素坐标计算块坐标之后，该哈希表将用于查找对应的块。该块中所有体素的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:既添加又删除

  添加时，该点的$dis=0$，然后执行上一步的更新操作

  删除时，该点的$dis=\infty$，查找所有以被删除点cur为最近障碍物的点vox，查找vox的nbr的最近障碍物，最近的那个作为vox的最近障碍物。
  

• 情况3：补丁
  bug：当观察到范围外的点，t0时观察到0，t1时观察到1~3。按步骤1的话，t0时刻不会更新体素1~3，因为他们属于为探测区域；t1时刻，体素3为占据，以它为中心更新它的邻居，第1个体素以为最近的是3。（因为0没发生变化，没再更新障碍0）
  补丁：添加内容->当体素cur到其邻居的最近障碍物的距离，比他自身的近，就换成其邻居的最近障碍物
  

存在的误差：

假设A要被更新，因为$nbr.dis\leftarrow 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_);//直接转换格式放进来

滤波步骤，对深度图每个像素：

• 先判断是否在深度范围内
• 位姿变换：本次camera系 -> world系 -> 上次camera系 -> 上次图像系（u，v）
• 上次该点的depth和本次的depth差值小，就保留

//最后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过了   

其中：

• SetOccupancy:只要被raycast到了，都放到occupancy_queue_，而是否占据通过num_miss和num_hit_判断：num_hit_ >= num_miss_- num_hit_ =>占据的次数>空闲的次数，则记为占据（体素可能包含好几个点云）

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：体素栅格坐标（占）；

• Raycast

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});//广度优先算法,如果这个体素被更新过,下次他的邻居也要被更新

• 更新
  发生过删除和添加的点，都被放到了update_queue_里。这一步要更新这些点的邻居，即这个点变成了障碍物，他的邻居的最近障碍物很可能就需要改变。同时，使用广度优先搜索，邻居的邻居也很可能变化，把他们全部更新直到没有变化。
  先执行第三步的补丁，然后：

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);

---