无人机自主探索FUEL：代码阅读1--边界簇检测更新与视点生成
~/src/catkin_fuel_refactored/fuel_refactored/fuel_planner/bag
bag中包含三个.sh文件，为rosbag指令，给出了录包指令以及有用话题信息
FIS
1、增量边界检测与聚类
- 路径：
~/fuel_planner/active_perception/src/frontier_finder.cpp
主要函数：寻找并更新边界簇 void FrontierFinder::searchAndAddFrontiers()
使用SDF地图，找到该边界簇的轴对齐包围盒（AABB框） B i B_i Bi，并更新更新的框内的新边界；
增量边界聚类；
创建视点集 V P i VP_i VPi以覆盖簇 F i F_i Fi；
进行视点检测与筛选，对视点降序排列；
找不到视点时将边界簇移至休眠列表；
重置边界索引；
void FrontierFinder::searchAndAddFrontiers() {
ros::Time t1 = ros::Time::now();
//--------------- 增量边界检测 ---------------------
// Bounding box of updated region 更新区域的AABB框B_i
Vector3d update_min, update_max;
edt_env_->sdf_map_->getUpdatedBox(update_min, update_max, true);
// ROS_WARN("update_min %lf %lf %lf: ", update_min[0], update_min[1], update_min[2]);
// ROS_WARN("update_max %lf %lf %lf: ", update_max[0], update_max[1], update_max[2]);
// Search new frontier within box slightly inflated from updated box
//从更新的框中搜索稍微膨胀的框内的新边界
Vector3d search_min = update_min - Vector3d(1, 1, 0.5);
Vector3d search_max = update_max + Vector3d(1, 1, 0.5);
Vector3d box_min, box_max; //新的边界
edt_env_->sdf_map_->getBox(box_min, box_max);
for (int k = 0; k < 3; ++k) {
search_min[k] = max(search_min[k], box_min[k]);
search_max[k] = min(search_max[k], box_max[k]);
}
// ROS_WARN("search_min %lf %lf %lf: ", search_min[0], search_min[1], search_min[2]);
// ROS_WARN("search_max %lf %lf %lf: ", search_max[0], search_max[1], search_max[2]);
Eigen::Vector3i min_id, max_id;
edt_env_->sdf_map_->posToIndex(search_min, min_id);
edt_env_->sdf_map_->posToIndex(search_max, max_id);
ROS_WARN("min_id %lf %lf %lf: ", min_id[0], min_id[1], min_id[2]);
ROS_WARN("max_id %lf %lf %lf: ", max_id[0], max_id[1], max_id[2]);
vector<Frontier> tmp_frontiers; //当前边界
for (int x = min_id(0); x <= max_id(0); ++x)
for (int y = min_id(1); y <= max_id(1); ++y)
for (int z = min_id(2); z <= max_id(2); ++z) {
// Scanning the updated region to find seeds of frontiers
Eigen::Vector3i cur(x, y, z);
if (!is_in_frontier_[toAddress(cur)] && knownFree(cur) && isNeighborUnknown(cur)) {
// Expand from the seed cell to find a complete frontier cluster //找到完整边界
Frontier frontier;
if (expandFrontier(cur, frontier)) { //按距离聚类搜索，检查新的簇的可行性
tmp_frontiers.push_back(move(frontier));
}
}
}
//--------------- 增量边界聚类 ---------------------
splitLargeFrontiers(tmp_frontiers); //递归分割边界簇
ROS_WARN_THROTTLE(5.0, "Frontier t: %lf", (ros::Time::now() - t1).toSec());
origin_frontiers_num_ = frontiers_.size();
int new_num = 0;
int new_dormant_num = 0;
//----------------------- 视点生成 ---------------------------
// Try to find viewpoints for each cluster and categorize them according to viewpoint number
for (Frontier &tmp_ftr: tmp_frontiers) {
// Search viewpoints around frontier
//采样边界平均位置周围的视点，检查边界单元的覆盖率
sampleViewpoints(tmp_ftr);
if (!tmp_ftr.viewpoints_.empty()) {
++new_num;
sort(tmp_ftr.viewpoints_.begin(), tmp_ftr.viewpoints_.end(), //降序排列
[](const Viewpoint &v1, const Viewpoint &v2) -> bool {
return v1.visible_num_ > v2.visible_num_;
});
frontiers_.insert(frontiers_.end(), tmp_ftr);
} else {
// Find no viewpoint, move cluster to dormant list 找不到视点，将边界簇移至休眠列表
dormant_frontiers_.push_back(tmp_ftr);
++new_dormant_num;
}
}
// Reset indices of frontiers 重置边界索引
int idx = 0;
for (Frontier &ft: frontiers_) {
ft.id_ = idx++;
}
}
- 按距离聚类搜索，检查新的簇的可行性 FrontierFinder::expandFrontier
bool FrontierFinder::expandFrontier(const Eigen::Vector3i &first, Frontier &frontier) {
// Data for clustering
queue<Eigen::Vector3i> cell_queue;
vector<Eigen::Vector3d> expanded;
Vector3d pos;
edt_env_->sdf_map_->indexToPos(first, pos);
expanded.push_back(pos);
cell_queue.push(first);
is_in_frontier_[toAddress(first)] = true;
// Search frontier cluster based on region growing (distance clustering) 按距离聚类搜索
while (!cell_queue.empty()) {
Vector3i cur_cell = cell_queue.front();
cell_queue.pop();
vector<Vector3i> neighbors = allNeighbors(cur_cell);
for (const Vector3i &next_cell: neighbors) {
// Qualified cell should be inside bounding box and frontier cell not clustered
//合格单元格应位于边界框内，且边界单元格不在簇内
int next_cell_adr = toAddress(next_cell);
if (is_in_frontier_[next_cell_adr] || !edt_env_->sdf_map_->isInBox(next_cell) ||
!(knownFree(next_cell) && isNeighborUnknown(next_cell)))
continue;
edt_env_->sdf_map_->indexToPos(next_cell, pos);
if (pos[2] < 0.4) continue; // Remove noise close to ground 消除噪音
expanded.push_back(pos);
cell_queue.push(next_cell);
is_in_frontier_[next_cell_adr] = true;
}
}
if (expanded.size() > cluster_min_) {
frontier.cells_ = expanded;
computeFrontierInfo(frontier);
return true;
}
return false;
}
- 边界簇的分割FrontierFinder::splitLargeFrontiers
在void FrontierFinder::searchAndAddFrontiers() 中使用，对每个簇都进行主成分分析，如果最大特征值超过阈值，则将该簇沿第一主轴分成两个均匀的簇。（分割是递归地进行的，因此所有大簇都被分成小簇。）
对应论文 IV.B
void FrontierFinder::splitLargeFrontiers(vector<Frontier> &frontiers) {
vector<Frontier> split_frontiers;
for (Frontier &frontier: frontiers) {
vector<Frontier> splits;
if (splitHorizontally(frontier, splits)) {
split_frontiers.insert(split_frontiers.end(), splits.begin(), splits.end());
} else
split_frontiers.emplace_back(move(frontier));
}
frontiers = move(split_frontiers);
}
其中FrontierFinder::splitHorizontally ：使用主成分分析，将较大的簇沿第一主轴分成两个均匀的簇 （单个的分割步骤）
- 视点集 V P i VP_i VPi的采样与覆盖率检测FrontierFinder::sampleViewpoints
在void FrontierFinder::searchAndAddFrontiers() 中使用，对应论文 IV.C
对于自由空间内的每个采样点 p \mathbb{p} p,通过一个偏航角优化方法，yaw角 ξ \xi ξ由传感器对簇覆盖的最大角决定；
覆盖度通过符合传感器模型且未被障碍物遮挡的边界单元的数量来估计，
–> 覆盖度超过阈值的视点 将被保留 并按降序排列
–> 保留 V P i VP_i VPi中最多 N v i e w N_{view} Nview个视点，在第二大部分第2步进行局部更新
// Sample viewpoints around frontier's average position, check coverage to the frontier cells
void FrontierFinder::sampleViewpoints(Frontier &frontier) {
//通过在原点位于簇中心的柱面坐标系中均匀采样点得到，对应文章fig.4
// Evaluate sample viewpoints on circles, find ones that cover most cells
for (double rc = candidate_rmin_, dr = (candidate_rmax_ - candidate_rmin_) / candidate_rnum_;
rc <= candidate_rmax_ + 1e-3; rc += dr)
for (double phi = -M_PI; phi < M_PI; phi += candidate_dphi_) {
const Vector3d sample_pos = frontier.average_ + rc * Vector3d(cos(phi), sin(phi), 0);
//确认视点在AABB框且在无人机可执行的位置
// Qualified viewpoint is in bounding box and in safe region
if (!edt_env_->sdf_map_->isInBox(sample_pos) ||
edt_env_->sdf_map_->getInflateOccupancy(sample_pos) == 1 ||
isNearUnknown(sample_pos)) {
continue;
}
//----------------- 计算视点覆盖度，进行筛选与降序排列 ------------------------
// Compute average yaw
const vector<Vector3d> &cells = frontier.filtered_cells_;
Eigen::Vector3d ref_dir = (cells.front() - sample_pos).normalized();
double avg_yaw = 0.0;
for (size_t i = 1; i < cells.size(); ++i) {
Eigen::Vector3d dir = (cells[i] - sample_pos).normalized();
double yaw = acos(dir.dot(ref_dir));
if (ref_dir.cross(dir)[2] < 0) yaw = -yaw;
avg_yaw += yaw;
}
avg_yaw = avg_yaw / cells.size() + atan2(ref_dir[1], ref_dir[0]);
wrapYaw(avg_yaw); //调整yaw角朝向？
// Compute the fraction of covered and visible cells 计算阈值
int visible_num = countVisibleCells(sample_pos, avg_yaw, cells); //计算有效视点
if (visible_num > min_visible_num_) { //保留部分视点并在V部分优化
Viewpoint vp = {sample_pos, avg_yaw, visible_num};
frontier.viewpoints_.push_back(vp);
}
}
}
- 检测视点 x i , j = ( p i , j , ξ i , j ) x_{i,j}=(\mathbb{p}_{i,j},\xi_{i,j}) xi,j=(pi,j,ξi,j)可行性 FrontierFinder::countVisibleCells
在FrontierFinder::sampleViewpoints 中使用
检测视点与传感器覆盖角度关系，检测视点未落在障碍物内
int FrontierFinder::countVisibleCells(const Eigen::Vector3d &pos, const double &yaw,
const vector<Eigen::Vector3d> &cluster) {
perception_utils_->setPose(pos, yaw);
int visible_num = 0;
Eigen::Vector3i idx;
for (const Vector3d &cell: cluster) {
if (!perception_utils_->insideFOV(cell)) continue; //是否超过传感器对簇覆盖的最大角
// Check if frontier cell is visible (not occluded by obstacles) 检测未落在障碍物内
ray_caster_->input(cell, pos);
bool is_visible = true;
while (ray_caster_->nextId(idx)) {
if (edt_env_->sdf_map_->getInflateOccupancy(idx) == 1 ||
edt_env_->sdf_map_->getOccupancy(idx) == SDFMap::UNKNOWN) {
is_visible = false;
break;
}
}
if (is_visible) visible_num += 1;
}
return visible_num;
}
2. 其他函数
-
移除过时的簇：
FrontierFinder::removeOutDatedFrontiers()
移除改变的簇；移除改变的簇的代价和path；删除已更改的休眠簇 -
计算每对簇之间的连接代价（F_k1,F_k2） ：
FrontierFinder::updateFrontierCostMatrix() -
检测两个AABB框是否重叠：
FrontierFinder::haveOverlap -
对簇进行降采样 ：
FrontierFinder::downSample