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}\]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。
个人思考
$\mathbf{PT}_{new}$ 的点云无论是从论文还是从代码上来看，都是几块聚类上的点云去提取得到的，之后会在函数 updateSurfaceState 中
- 插入到
VR.all_filtered_pts末尾 - 也会插入到
VR.new_filtered_pts_，但VR.new_filtered_pts_会在函数surfaceViewpointsGeneration中的末尾进行清空操作VR.new_filtered_pts_.clear();
VR.all_filtered_pts_ VR.cover_state_all_ VR.cover_contrib_num_all_ VR.contrib_id_all_ 的下标是一致的，对应的集合是全局降采样后的点云；VR.all_cloud_pts_ 构造 kdtree 后的下标是真正的全局下标，对应的集合是全局完整的点云。
VR.uncovered_filtered_pt_ids_ 存的是降采样后的点云当中没被覆盖的点云下标，可以称作“半全局点云 idx”；uncovered_all_pt_ids 是函数 surfaceViewpointsGenera中的局部变量，存的则是完整点云当中未被覆盖的点云下标，可以称作“全局点云 idx”。使用 uncovered_all_pt_ids 的目的是能够使用完整的点云信息来生成准确一些的视角法向量。
VR.vps_voxcount_[vp_id] 也是全局的
数据结构 VR 里是有对 覆盖点独占数 VR.vps_voxcount_ 和 贡献度 VR.vps_contri_num_[vp_id] 作区分的，在 updateViewpointInfo 里会对 VR 里的贡献度 VR.vps_contri_num_[vp_id] 作计算更新，这是唯一进行赋值的地方。原版的贡献度是 视角看到的点数，而不是独占数。
而 VR.vps_voxcount_[vp_id] -= 1; 则是分配后的独占数，从这句代码也可以看出来，并且 Prune 里对视角的排序也是基于 VR.vps_voxcount_ 的。
prune 里的 updateViewpointInfo 是用来更新引力筛选后仍然活跃的微调后的视角的信息
难点在于视角的排他控制点云数的更新，解决方法：单独拎出来作增量形式。
两次两层循环（先视角，后点云），可以统计出每个视角的独特性分数。
- 第一次循环，会遍历视角，然后遍历点云，判断点云是否在视野内并作射线投射，为视角记录看到的点云，并为点云记录该视角。
- 第二次循环，同样先遍历视角，之后遍历看到的点云，提取被观测视角数，计算贡献值。
数据结构增加一个 独特性分数，在函数 updateNewPts 中应设置对应点的该属性为 INF。
把控制更新单独设计成增量形式：
- 遍历视角及其预计算分数，作为函数输入，再遍历该视野内未覆盖的点云，判断该点云是否已被覆盖过，
- 若未覆盖，则设置为覆盖，记录该视角及其分数
- 若已覆盖，则考虑本视角分数，与控制该点云的当前视角分数比较，若更大（或当前控制视角已被休眠- 这个点涉及到的历史视角需作分数的变化，假如算上当前视角有 n 个视角， ），则给点云重新分配视角
现在需要把以上算法改进成 视角增量 的形式：
- 来一个视角，遍历点，统计看到的点，同时给这个点统计上该视角，
- 这个点涉及到的历史视角需作分数的变化，假如算上当前视角有 n 个视角，那么历史视角的分数需要变化 $-\frac{1}{n-1} + \frac{1}{n} = -\frac{1}{n(n-1)}$
- 当前视角的分数需要累加 $\frac{1}{n}$
- 遍历 所有发生分数变化的视野 内未覆盖的点云，判断该点云是否已被覆盖过，
- 若未覆盖，则设置为覆盖，记录该视角及其分数
- 若已覆盖，则考虑本次添加引起分数变化的所有视角（换成优先级队列是一个可以改进的点），取其中最大分数者，与历史最大分数者比较，若更大，则给点云重新分配视角
重新考虑后，应当先以批量的形式计算分数，并分配排他覆盖数；在 Prune 内，对引力吸引后保留的视角集合作批量计算分数，再分配排他覆盖数，其可能的bug如下：
原本方法进行引力筛选后，对保留的视角更新排他覆盖数，7654321 -> 7(65)4(3)2(1)，这样当 4 要更新信息（如 VR.cover_contrib_num_）时，如果看到点被已经休眠的 5 看过，则这个点不会被分配给 4，这样就更新不充分
解决方法：在 Prune 里单独使用另一版本的 updateViewpointInfo，写作 updateViewpointInfoDuringPrune，在其中会判断点的控制视角是否已经休眠，如果已经休眠就该释放对该点的控制权。实测该判断未被触发，则说明没有出现如存活的 4 看到休眠的 5 这类情况。
其实发现不会出现这种情况，因为在进行 updateViewpintInfo 前会缓存迭代前的点云信息 cover_state_all_ cover_contrib_num_all cover_contrib_id_all_，计算出初始视角会恢复回去，再在 Prune 中剪枝和更新信息，这样被休眠视角看到的点信息就不会被遗漏更新了，因为已经被还原到上次迭代后本次迭代前的信息了。
计算分数出现 除零 了，待分析。初步结果并没有更好，覆盖率没提升，视角数目反而增加了。
VR.prune_radius_ = 0.7 * vp_dist * tan(30.0 * M_PI / 180.0);
NOTE：把 viewpointsPrune 里的排序准则从 分数 更换成 基于分数分配的排他控制点云数 之后，效果变好
独特性有效的原因：假设是存在至少一个子集，里面的元素互不重叠，且能够覆盖全体，且该子集是最优的，而不会存在一个子集的元素相互覆盖但数量很少。
利用法向来生成视角，原方法不知道正反向，可以尝试改进成我跟墙中的方法。需要修改的主要函数如下：
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] [1774193417.243022835]: [Coverage Manager] Number of Total Voxels = 442
[ WARN] [1774193417.243278926]: [Coverage Manager] Seen all voxels num = 291
[ WARN] [1774193417.243412287]: [Coverage Manager] Seen all voxels num = 291 calculated by cover state
[ WARN] [1774193417.243489047]: [Coverage Manager] Number of Uncovered Voxels = 151
[ WARN] [1774193417.248871288]: [computeUncoveredViewpointsUsingSdfGrad] cost time: 0.000015
[ WARN] [1774193417.250067375]: [computeUncoveredViewpoints] total time: 0.001212
[ WARN] [1774193417.289452518]: [Coverage Manager] Iter_4 Number of Total Voxels = 442
[ WARN] [1774193417.289474286]: [Coverage Manager] Iter_4 Seen all voxels num = 430
[ WARN] [1774193417.289489192]: [Coverage Manager] Iter_4 Seen all voxels num = 431 by cover state
[ WARN] [1774193417.289516590]: [Coverage Manager] Iter_4 Number of Uncovered Points = 11
[ WARN] [1774193417.289546535]: [Coverage Manager] Iter_4 Now final viewpoint num = 9
[ WARN] [1774193417.289578041]: [Coverage Manager] SurfaceViewpointsGeneration Total time = 0.057727
[ WARN] [1774193423.256071025]: [Coverage Manager] Number of Total Voxels = 571
[ WARN] [1774193423.256180814]: [Coverage Manager] Seen all voxels num = 439
[ WARN] [1774193423.256272686]: [Coverage Manager] Seen all voxels num = 440 calculated by cover state
[ WARN] [1774193423.256359032]: [Coverage Manager] Number of Uncovered Voxels = 131
[ WARN] [1774193423.260012010]: [computeUncoveredViewpointsUsingSdfGrad] cost time: 0.000016
[ WARN] [1774193423.261143835]: [computeUncoveredViewpoints] total time: 0.001147
[ WARN] [1774193423.302151497]: [Coverage Manager] Iter_5 Number of Total Voxels = 571
[ WARN] [1774193423.302856387]: [Coverage Manager] Iter_5 Seen all voxels num = 568
[ WARN] [1774193423.303186586]: [Coverage Manager] Iter_5 Seen all voxels num = 570 by cover state
[ WARN] [1774193423.303651271]: [Coverage Manager] Iter_5 Number of Uncovered Points = 1
[ WARN] [1774193423.303953462]: [Coverage Manager] Iter_5 Now final viewpoint num = 16
[ WARN] [1774193423.304427071]: [Coverage Manager] SurfaceViewpointsGeneration Total time = 0.062533
[ WARN] [1774193426.114436366]: [Coverage Manager] Number of Total Voxels = 614
[ WARN] [1774193426.114463933]: [Coverage Manager] Seen all voxels num = 589
[ WARN] [1774193426.114480381]: [Coverage Manager] Seen all voxels num = 591 calculated by cover state
[ WARN] [1774193426.114503761]: [Coverage Manager] Number of Uncovered Voxels = 23
[ WARN] [1774193426.116504021]: [computeUncoveredViewpointsUsingSdfGrad] cost time: 0.000010
[ WARN] [1774193426.116675829]: [computeUncoveredViewpoints] total time: 0.000193
[ WARN] [1774193426.123020280]: drone_3 replan: periodic call============================
[ WARN] [1774193426.123155841]: [Coverage Manager] Iter_6 Number of Total Voxels = 614
[ WARN] [1774193426.123194149]: [Coverage Manager] Iter_6 Seen all voxels num = 605
[ WARN] [1774193426.123229515]: [Coverage Manager] Iter_6 Seen all voxels num = 607 by cover state
[ WARN] [1774193426.123259337]: [Coverage Manager] Iter_6 Number of Uncovered Points = 7
[ WARN] [1774193426.123295548]: [Coverage Manager] Iter_6 Now final viewpoint num = 17
[ WARN] [1774193426.123344911]: [Coverage Manager] SurfaceViewpointsGeneration Total time = 0.031465
为了解释这个问题，对日志输出增加一些内容：
sum_seen = 0;
for (auto final_vp : VR.final_vps_) {
// ROS_WARN("Iter_%d vp_%d : count = %d",
// VR.all_iter_num_, final_vp.vp_id, final_vp.vox_count);
sum_seen += final_vp.vox_count;
}
sum_seen_by_state = 0;
for (auto state: VR.cover_state_all_) {
if (state) sum_seen_by_state++;
}
ROS_WARN(
"\033[1;32m[Coverage Manager] Number of Total Voxels = %d", (int)VR.cover_state_all_.size());
ROS_WARN("\033[1;32m[Coverage Manager] Seen all voxels num = %d", sum_seen);
ROS_WARN("\033[1;32m[Coverage Manager] Seen all voxels num = %d calculated by cover state",
sum_seen_by_state);
ROS_WARN("\033[1;32m[Coverage Manager] Number of Uncovered Voxels = %d",
(int)uncovered_all_pt_ids.size());
...
sum_seen = 0;
for (auto final_vp : VR.final_vps_) {
// ROS_WARN("Iter_%d vp_%d : count = %d",
// VR.all_iter_num_, final_vp.vp_id, final_vp.vox_count);
sum_seen += final_vp.vox_count;
}
sum_seen_by_state = 0;
for (auto state: VR.cover_state_all_) {
if (state) sum_seen_by_state++;
}
ROS_WARN("\033[1;34m[Coverage Manager] Iter_%d Number of Total Voxels = %d", VR.all_iter_num_,
(int)VR.cover_state_all_.size());
ROS_WARN("\033[1;34m[Coverage Manager] Iter_%d Seen all voxels num = %d", VR.all_iter_num_,
sum_seen);
ROS_WARN("\033[1;34m[Coverage Manager] Iter_%d Seen all voxels num = %d by cover state", VR.all_iter_num_,
sum_seen_by_state);
ROS_WARN("\033[1;34m[Coverage Manager] Iter_%d Number of Uncovered Points = %d",
VR.all_iter_num_, (int)uncovered_all_pt_ids.size());
ROS_WARN("\033[1;34m[Coverage Manager] Iter_%d Now final viewpoint num = %d", VR.all_iter_num_,
(int)VR.final_vps_.size());
原因在于，函数 viewpointsPrune 会休眠一些视角，并调整存活的视角，而点云只要被中间状态的视角看过，就会执行 VR.cover_state_all_ = true，但后续如果这个点云因为视角休眠和移动不再被看到，
该标志位不会重新修改为 false，所以 sum_seen_by_state 内仍会统计这个点为已覆盖。而 sum_seen 的统计则是对存活视角的排他控制点云数进行累加得到的。
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”。“局部”是指新提取的点云这个集合，“全局”是指程序运行开始后累积下来的所有点云。
多机之间的通信结构如何？
探索时的视角采样改进，原文是圆柱体采样，可以改称边界簇的法向量来确定
视角簇任务分配，原文使用可视基准和距离阈值来分簇，并使用遗传算法来分配遍历任务，但在隧道场景经常出现一个 photographer 有很多任务，另一架直接无任务，可能是视角分簇的策略在长条状里把所有视角都分成一个簇了。
使用标准求解器，构造增量式矩阵，同时调整矩阵数值来保证一致性，同样能解决遗传算法试图解决的问题，或许结果会更好。