SOAR: Simultaneous Exploration and Photographing with Heterogeneous UAVs for Fast Autonomous Reconstruction
整体思路：探索 -> 视角生成 -> 任务分配（多机）-> 规划轨迹
Surface Frontier-based Exploration
表面前沿体素 $v_{sf}$ 定义
步骤：
- 对 $v_{sf}$ 聚类，求质心
- 在多个类的质心的某个半径内采样生成不同高度、yaw 的候选视角
- 对每个类上的候选视角选择一个作为代表，标准为视角可以看到最多的 $v_{sf}$
- 使用 LKH 算法求解 ATSP，获得最短的遍历以上选出的视角的路径
以上思路主要借鉴于 FUEL。
尝试改进探索步骤
Incremental Viewpoint Generation
Uncovered Surface Extraction
对已知表面体素进行聚类，不与任何表面前沿接触的（对角线接触也算接触）体素聚类被称作 完全探索表面（Completelt Explored Surface），之后会从这些完全探索表面体素中 提取 原始点云，并将这些体素标记为“已提取”。下一时刻地图更新，会有新的完全探索表面，并对其提取点云，但之前已经提取过点云的体素不会再次提取了。
为了减少非法视角的生成，对提取的点云 $\mathbf{PT}_{new}$ 进行滤波。
\[\mathbf{PT}_{new} \ \mathrm{filtered\ by\ } \mathbf{CV}_{hq} + \mathbf{PT}_{unc,prev}\rightarrow PT_{unc}\]$\mathbf{PT}_{new}$ 的点云无论是从论文还是从代码上来看，都是几块聚类上的点云去提取得到的，之后会在函数
updateSurfaceState中
- 插入到
VR.all_filtered_pts末尾- 也会插入到
VR.new_filtered_pts_，但VR.new_filtered_pts_会在函数surfaceViewpointsGeneration中的末尾进行清空操作VR.new_filtered_pts_.clear();
VR.vps_voxcount_[vp_id]也是全局的
数据结构 VR 里是有对覆盖点独占数和贡献度作区分的，在 updateViewpointInfo 里会对 VR 里的贡献度 VR.vps_contri_num_[vp_id] 作计算更新，这是唯一进行赋值的地方。原版的贡献度是 视角看到的点数，而不是独占数。 而 VR.vps_voxcount_[vp_id] -= 1; 则是分配后的独占数，从这句代码也可以看出来，并且 Prune 里对视角的排序也是基于 VR.vps_voxcount_ 的。
prune 里的 updateViewpointInfo 是用来更新引力筛选后仍然活跃的微调后的视角的信息
利用法向来生成视角，原方法不知道正反向，可以尝试改进成我跟墙中的方法。需要修改的主要函数如下：
void SurfaceCoverage::computeUncoveredViewpoints( const Vector3d& pt_pos, const Vector3d& pt_normal, vector<pcl::PointNormal>& unc_vps)已修改测试完成，主要是利用 sdf 的 grad 来作方向参考
[computeUncoveredViewpoints] cost time: 0.000015 [computeUncoveredViewpointsUsingSdfGrad] cost time: 0.000003 [computeUncoveredViewpoints] total time: 0.002430 --- [computeUncoveredViewpoints] cost time: 0.000016 [computeUncoveredViewpointsUsingSdfGrad] cost time: 0.000002 [computeUncoveredViewpoints] total time: 0.001961 --- [computeUncoveredViewpoints] cost time: 0.000016 [computeUncoveredViewpointsUsingSdfGrad] cost time: 0.000003 [computeUncoveredViewpoints] total time: 0.001898 --- [computeUncoveredViewpoints] cost time: 0.000005 [computeUncoveredViewpointsUsingSdfGrad] cost time: 0.000003 [computeUncoveredViewpoints] total time: 0.000860
代码中的算法和论文的有些出入：采样不是基于法向量，而是表面聚类平均位置周围的圆柱体作采样，然后计算平均 yaw 角（朝向点云，此时生成的视角的 pitch 均设为 0，有点奇怪），最后只把覆盖数最大的视角 push 进去，相当于只对一个 surface 分配一个视角。
为什么加起来不相等？
[ WARN] [1773303869.745404627]: [Coverage Manager] Number of Total Voxels = 2563 [ WARN] [1773303869.745459548]: [Coverage Manager] Seen all voxels num = 2500 [ WARN] [1773303869.745496276]: [Coverage Manager] Number of Uncovered Voxels = 52
vector<bool> new_cover_state_all; vector<int> new_cover_contrib_num_all; vector<int> new_contrib_id_all;
src/planner/heterogeneous_manager/src/coverage_manager.cpp 715这几个 vector 的 idx 都是 subspace-idx，也就是对应VR.new_filtered_pts_这些新提取的点，而不是全局的点云 idx。vector<int> uncovered_all_pt_ids; // find the id corresponding to the global point cloud这句代码则是将未被已有视角覆盖的新点云的“局部 idx” 转换到“全局 idx”。“局部”是指新提取的点云这个集合，“全局”是指程序运行开始后累积下来的所有点云。
多机之间的通信结构如何？
Consistent-MDMTSP for Task Assignment
在 FC-Planner 中是通过骨架来分割空间，对视角进行聚类。而在本方法 SOAR 中，则是通过可见性和距离阈值来对视角进行聚类，再将问题建模成 MDMTSP，即 $N_p$ 架 photographer 去不重复地遍历 $N_{vct}$ 个未访问的 VCTs（viewpoint clustering task）。
FUEL
栅格地图局部更新触发 FIS（Frontier Information Structure）的局部更新，进一步触发分层规划，生成探索轨迹。
INCREMENTAL FRONTIER INFORMATION STRUCTURE
对 cluster 进行主成分分析，如果 first axis 大于阈值，则沿着 first axis 对 cluster 进行分割，分成两个小 cluster，递归进行，不断分割。
生成视角中，对于一个 cluster，不会直接使用其质心作为视角位置，而是以质心为中心的极坐标系中去均匀采样视角位置，然后在每个视角位置上以优化方式求出最佳 yaw，基准是覆盖到最多的 frontier 栅格。
HIERARCHICAL EXPLORATION PLANNING
(1) Global Exploration Tour Planning
构建邻接矩阵，使用 tsp 求解器进行求解。
(2) Local Viewpoint Refinement
相邻的连续几个 cluster 的候选视角组成一个有向无环图（DAG），使用 Dijstra 来查找最短路径，称为 Refinement。