无人机自主探索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$ ，并更新更新的框内的新边界；
增量边界聚类；
创建视点集 $VP_i$ 以覆盖簇 $F_i$ ；
进行视点检测与筛选，对视点降序排列；
找不到视点时将边界簇移至休眠列表；
重置边界索引；

    void FrontierFinder::searchAndAddFrontiers() {ros::Time t1 = ros::Time::now();//---------------   增量边界检测    ---------------------// Bounding box of updated region  更新区域的AABB框B_iVector3d 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 frontiersEigen::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 numberfor (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 clusteringqueue<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());} elsesplit_frontiers.emplace_back(move(frontier));}frontiers = move(split_frontiers);}

其中FrontierFinder::splitHorizontally ：使用主成分分析，将较大的簇沿第一主轴分成两个均匀的簇（单个的分割步骤）
在这里插入图片描述

- 视点集 $VP_i$ 的采样与覆盖率检测`FrontierFinder::sampleViewpoints`

在void FrontierFinder::searchAndAddFrontiers() 中使用，对应论文 IV.C
对于自由空间内的每个采样点 $\mathbb{p}$ ,通过一个偏航角优化方法，yaw角 $\xi$ 由传感器对簇覆盖的最大角决定；
覆盖度通过符合传感器模型且未被障碍物遮挡的边界单元的数量来估计，
–> 覆盖度超过阈值的视点将被保留并按降序排列
–> 保留 $VP_i$ 中最多 $N_{view}$ 个视点，在第二大部分第2步进行局部更新

// Sample viewpoints around frontier's average position, check coverage to the frontier cellsvoid FrontierFinder::sampleViewpoints(Frontier &frontier) {//通过在原点位于簇中心的柱面坐标系中均匀采样点得到，对应文章fig.4// Evaluate sample viewpoints on circles, find ones that cover most cellsfor (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 yawconst 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}=(\mathbb{p}_{i,j},\xi_{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