五、源码阅读笔记
同时笔者整理了以下这个建图包的关键流程，只有 2 个关键的函数也不是很复杂。
第一步是订阅点云的回调：
第二步是插入单帧点云构建八叉树，这里就不详细介绍过程了，因为涉及到八叉树库 octomap 的更新原理：
以下是我建图过程的 launch：
/
在这里插入代码片<launch> <node pkg="octomap_server" type="octomap_server_node" name="octomap_server"> <!-- resolution in meters per pixel --> <param name = "resolution" value = "0.15" /> <!-- name of the fixed frame, needs to be "/map" for SLAM --> <!-- 静态全局地图的 frame_id，但在增量式构建地图时，需要提供输入的点云帧和静态全局帧之间的 TF 变换 --> <param name = "frame_id" type = "string" value = "camera_init" /> <!-- set min to speed up! --> <param name = "sensor_model/max_range" value = "15.0" /> <!-- 机器人坐标系 base_link，滤除地面需要该 frame --> <!-- <param name = "base_frame_id" type = "string" value = "base_link" /> --> <!-- filter ground plane, distance value should be big! 项目并不需要滤除地面 --> <!-- <param name = "filter_ground" type = "bool" value = "true" /> --> <!-- <param name = "ground_filter/distance" type = "double" value = "1.0" /> --> <!-- 分割地面的 Z 轴阈值 value 值 --> <!-- <param name = "ground_filter/plane_distance" type = "double" value = "0.3" /> --> <!-- 直通滤波的 Z 轴范围，保留 [-1.0, 10.0] 范围内的点 --> <!-- <param name = "pointcloud_max_z" type = "double" value = "100.0" /> --> <!-- <param name = "pointcloud_min_z" type = "double" value = "-1.0" /> --> <!-- <param name = "filter_speckles" type = "bool" value = "true" /> --> <param name = "height_map" value = "false" /> <param name = "colored_map" value = "true" /> <!-- 增加了半径滤波器 --> <param name = "outrem_radius" type = "double" value = "1.0" /> <param name = "outrem_neighbors" type = "int" value = "10" /> <!-- when building map, set to false to speed up!!! --> <param name = "latch" value = "false" /> <!-- topic from where pointcloud2 messages are subscribed --> <!-- 要订阅的点云主题名称 /pointcloud/output --> <!-- 这句话的意思是把当前节点订阅的主题名称从 cloud_in 变为 /pointcloud/output --> <remap from = "/cloud_in" to = "/fusion_cloud" /> </node> </launch>
六、路径规划
这里给一个链接：https://github.com/Quitino/IndoorMapping，该作者实现了基于ORB-SLAM生成三维密集点云，并使用OctoMap构建室内导航地图。也意味着我们可以在此基础上加入轨迹规划算法实现基于OBR-SLAM的室内规划导航。
具体代码可以参照：https://blog.csdn.net/lovely_yoshino/article/details/105272602中的3D-RRT路径规划方法实现。
七：介绍三维A*在栅格地图中的使用
AngeloG 博文中写了三维 A的相关matlab仿真。为了便于在ROS中对A算法进行开发，这里也提供一下之前我学习的A*算法
#include "Astar_searcher.h" using namespace std; using namespace Eigen; void AstarPathFinder::initGridMap(double _resolution, Vector3d global_xyz_l, Vector3d global_xyz_u, int max_x_id, int max_y_id, int max_z_id) { gl_xl = global_xyz_l(0); gl_yl = global_xyz_l(1); gl_zl = global_xyz_l(2); gl_xu = global_xyz_u(0); gl_yu = global_xyz_u(1); gl_zu = global_xyz_u(2); GLX_SIZE = max_x_id; GLY_SIZE = max_y_id; GLZ_SIZE = max_z_id; GLYZ_SIZE = GLY_SIZE * GLZ_SIZE; GLXYZ_SIZE = GLX_SIZE * GLYZ_SIZE; resolution = _resolution; inv_resolution = 1.0 / _resolution; data = new uint8_t[GLXYZ_SIZE]; memset(data, 0, GLXYZ_SIZE * sizeof(uint8_t)); GridNodeMap = new GridNodePtr **[GLX_SIZE]; for (int i = 0; i < GLX_SIZE; i++) { GridNodeMap[i] = new GridNodePtr *[GLY_SIZE]; for (int j = 0; j < GLY_SIZE; j++) { GridNodeMap[i][j] = new GridNodePtr[GLZ_SIZE]; for (int k = 0; k < GLZ_SIZE; k++) { Vector3i tmpIdx(i, j, k); Vector3d pos = gridIndex2coord(tmpIdx); GridNodeMap[i][j][k] = new GridNode(tmpIdx, pos); } } } } void AstarPathFinder::resetGrid(GridNodePtr ptr) { ptr->id = 0; ptr->cameFrom = NULL; ptr->gScore = inf; ptr->fScore = inf; } void AstarPathFinder::resetUsedGrids() { for (int i = 0; i < GLX_SIZE; i++) for (int j = 0; j < GLY_SIZE; j++) for (int k = 0; k < GLZ_SIZE; k++) resetGrid(GridNodeMap[i][j][k]); } void AstarPathFinder::setObs(const double coord_x, const double coord_y, const double coord_z) { if (coord_x < gl_xl || coord_y < gl_yl || coord_z < gl_zl || coord_x >= gl_xu || coord_y >= gl_yu || coord_z >= gl_zu) return; int idx_x = static_cast<int>((coord_x - gl_xl) * inv_resolution); int idx_y = static_cast<int>((coord_y - gl_yl) * inv_resolution); int idx_z = static_cast<int>((coord_z - gl_zl) * inv_resolution); data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] = 1; } vector<Vector3d> AstarPathFinder::getVisitedNodes() { vector<Vector3d> visited_nodes; for (int i = 0; i < GLX_SIZE; i++) for (int j = 0; j < GLY_SIZE; j++) for (int k = 0; k < GLZ_SIZE; k++) { // if(GridNodeMap[i][j][k]->id != 0) // visualize all nodes in open and // close list if (GridNodeMap[i][j][k]->id == -1) // visualize nodes in close list only visited_nodes.push_back(GridNodeMap[i][j][k]->coord); } ROS_WARN("visited_nodes size : %d", visited_nodes.size()); return visited_nodes; } Vector3d AstarPathFinder::gridIndex2coord(const Vector3i &index) { Vector3d pt; pt(0) = ((double)index(0) + 0.5) * resolution + gl_xl; pt(1) = ((double)index(1) + 0.5) * resolution + gl_yl; pt(2) = ((double)index(2) + 0.5) * resolution + gl_zl; return pt; } Vector3i AstarPathFinder::coord2gridIndex(const Vector3d &pt) { Vector3i idx; idx << min(max(int((pt(0) - gl_xl) * inv_resolution), 0), GLX_SIZE - 1), min(max(int((pt(1) - gl_yl) * inv_resolution), 0), GLY_SIZE - 1), min(max(int((pt(2) - gl_zl) * inv_resolution), 0), GLZ_SIZE - 1); return idx; } Eigen::Vector3d AstarPathFinder::coordRounding(const Eigen::Vector3d &coord) { return gridIndex2coord(coord2gridIndex(coord)); } inline bool AstarPathFinder::isOccupied(const Eigen::Vector3i &index) const { return isOccupied(index(0), index(1), index(2)); } inline bool AstarPathFinder::isFree(const Eigen::Vector3i &index) const { return isFree(index(0), index(1), index(2)); } inline bool AstarPathFinder::isOccupied(const int &idx_x, const int &idx_y, const int &idx_z) const { return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] == 1)); } inline bool AstarPathFinder::isFree(const int &idx_x, const int &idx_y, const int &idx_z) const { return (idx_x >= 0 && idx_x < GLX_SIZE && idx_y >= 0 && idx_y < GLY_SIZE && idx_z >= 0 && idx_z < GLZ_SIZE && (data[idx_x * GLYZ_SIZE + idx_y * GLZ_SIZE + idx_z] < 1)); } inline void AstarPathFinder::AstarGetSucc(GridNodePtr currentPtr, vector<GridNodePtr> &neighborPtrSets, vector<double> &edgeCostSets) { neighborPtrSets.clear(); edgeCostSets.clear(); /* STEP 4: finish AstarPathFinder::AstarGetSucc yourself please write your code below */ // get all neighbours of current node, calculate all the costs, and then save to the point. // should be a 3D search, maxumin 26 nodes, but need to remove the obstacles and boundary Vector3i neighborIdx; for (int dx = -1; dx < 2; dx++) { for (int dy = -1; dy < 2; dy++) { for (int dz = -1; dz < 2; dz++) { if (dx == 0 && dy == 0 && dz == 0) continue; neighborIdx(0) = (currentPtr->index)(0) + dx; neighborIdx(1) = (currentPtr->index)(1) + dy; neighborIdx(2) = (currentPtr->index)(2) + dz; if (neighborIdx(0) < 0 || neighborIdx(0) >= GLX_SIZE || neighborIdx(1) < 0 || neighborIdx(1) >= GLY_SIZE || neighborIdx(2) < 0 || neighborIdx(2) >= GLZ_SIZE) continue; neighborPtrSets.push_back(GridNodeMap[neighborIdx(0)][neighborIdx(1)][neighborIdx(2)]); edgeCostSets.push_back(sqrt(dx * dx + dy * dy + dz * dz)); } } } } double AstarPathFinder::getHeu(GridNodePtr node1, GridNodePtr node2) { /*STEP 1 choose possible heuristic function you want Manhattan, Euclidean, Diagonal, or 0 (Dijkstra) Remember tie_breaker learned in lecture, add it here ? */ // ROS_INFO("[node] calcute Heu"); // return getDiagHeu(node1, node2); double tie_breaker = 1 + 1 / 1000; return tie_breaker * getDiagHeu(node1, node2); } double AstarPathFinder::getEuclHeu(GridNodePtr node1, GridNodePtr node2) { // calculate distance at each dimention double dx = node1->index(0) - node2->index(0); double dy = node1->index(1) - node2->index(1); double dz = node1->index(2) - node2->index(2); double result1 = sqrt(dx * dx + dy * dy + dz * dz); // double result2 = (node2->index - node1->index).norm(); // cout.setf(ios::fixed); // cout << "norm1 = " << setprecision(4) << result1 << endl; // cout << "diff = " << (node2->index - node1->index) << endl; // cout << "norm2 = " << setprecision(4) << result2 << endl; return result1; } double AstarPathFinder::getDiagHeu(GridNodePtr node1, GridNodePtr node2) { double dx = abs(node1->index(0) - node2->index(0)); double dy = abs(node1->index(1) - node2->index(1)); double dz = abs(node1->index(2) - node2->index(2)); double h = 0.0; int diag = min(min(dx, dy), dz); dx -= diag; dy -= diag; dz -= diag; if (dx == 0) h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dy, dz) + 1.0 * abs(dy - dz); if (dy == 0) h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dz) + 1.0 * abs(dx - dz); if (dz == 0) h = 1.0 * sqrt(3.0) * diag + sqrt(2.0) * min(dx, dy) + 1.0 * abs(dx - dy); return h; } double AstarPathFinder::getManhHeu(GridNodePtr node1, GridNodePtr node2) { double dx = abs(node1->index(0) - node2->index(0)); double dy = abs(node1->index(1) - node2->index(1)); double dz = abs(node1->index(2) - node2->index(2)); return dx + dy + dz; } void AstarPathFinder::AstarGraphSearch(Vector3d start_pt, Vector3d end_pt) { ros::Time time_1 = ros::Time::now(); // index of start_point and end_point Vector3i start_idx = coord2gridIndex(start_pt); // what is coord2gridIndex mean? Vector3i end_idx = coord2gridIndex(end_pt); goalIdx = end_idx; // position of start_point and end_point start_pt = gridIndex2coord(start_idx); end_pt = gridIndex2coord(end_idx); // Initialize the pointers of struct GridNode which represent start node and // goal node GridNodePtr startPtr = new GridNode(start_idx, start_pt); GridNodePtr endPtr = new GridNode(end_idx, end_pt); // openSet is the open_list implemented through multimap in STL library openSet.clear(); // currentPtr represents the node with lowest f(n) in the open_list GridNodePtr currentPtr = NULL; GridNodePtr neighborPtr = NULL; // put start node in open set startPtr->gScore = 0; startPtr->fScore = getHeu(startPtr, endPtr); // f = h + g = h + 0 startPtr->id = 1; startPtr->coord = start_pt; openSet.insert(make_pair(startPtr->fScore, startPtr)); // startPtr->cameFrom = startPtr; /** STEP 2: some else preparatory works which should be done before while loop please write your code below, neighbour of start point **/ double tentative_gScore; vector<GridNodePtr> neighborPtrSets; vector<double> edgeCostSets; // this is the main loop while (!openSet.empty()) { /* step 3: Remove the node with lowest cost function from open set to closed set, please write your code below IMPORTANT NOTE!!! This part you should use the C++ STL: multimap, more details can be find in Homework description */ // get the min f node from open set to current currentPtr = openSet.begin()->second; // if the current node is the goal if (currentPtr->index == goalIdx) { ros::Time time_2 = ros::Time::now(); terminatePtr = currentPtr; ROS_WARN("[A*]{sucess} Time in A* is %f ms, path cost is %f m", (time_2 - time_1).toSec() * 1000.0, currentPtr->gScore * resolution); // cout << "final point" << endl << terminatePtr->coord << endl; // cout << "came frome" << endl << terminatePtr->cameFrom->coord << endl; return; } // put to close, and erase it openSet.erase(openSet.begin()); currentPtr->id = -1; // STEP 4: finish AstarPathFinder::AstarGetSucc, get the succetion AstarGetSucc(currentPtr, neighborPtrSets, edgeCostSets); /*** STEP 5: For all unexpanded neigbors "m" of node "n", please finish this for loop please write your code below **/ for (int i = 0; i < (int)neighborPtrSets.size(); i++) { /* Judge if the neigbors have been expanded，please write your code below IMPORTANT NOTE!!! neighborPtrSets[i]->id = -1 : in closed set neighborPtrSets[i]->id = 1 : in open set */ neighborPtr = neighborPtrSets[i]; if (isOccupied(neighborPtr->index) || neighborPtr->id == -1) continue; double edge_cost = edgeCostSets[i]; tentative_gScore = currentPtr->gScore + edge_cost; if (neighborPtr->id != 1) { // discover a new node, which is not in the open set /* STEP 6: As for a new node, do what you need do ,and then put neighbor in open set and record it，please write your code below */ // insert to open set openSet.insert(make_pair(startPtr->fScore, neighborPtrSets[i])); neighborPtr->id = 1; neighborPtr->cameFrom = currentPtr; neighborPtr->gScore = tentative_gScore; neighborPtr->fScore = neighborPtr->gScore + getHeu(neighborPtr, endPtr); neighborPtr->nodeMapIt = openSet.insert(make_pair(neighborPtr->fScore, neighborPtr)); // put neighbor in open set and record it. continue; } else if (neighborPtr->gScore >= tentative_gScore) // compare original f and new f from current // this node is in open set and need to judge if it needs to update, // he "0" should be deleted when you are coding { /* STEP 7: As for a node in open set, update it , maintain the openset, and then put neighbor in open set and record it please write your code below */ neighborPtr->cameFrom = currentPtr; neighborPtr->gScore = tentative_gScore; neighborPtr->fScore = tentative_gScore + getHeu(neighborPtr, endPtr); openSet.erase(neighborPtr->nodeMapIt); neighborPtr->nodeMapIt = openSet.insert(make_pair(neighborPtr->fScore, neighborPtr)); // if change its parents, update the expanding direction for (int i = 0; i < 3; i++) { neighborPtr->dir(i) = neighborPtr->index(i) - currentPtr->index(i); if (neighborPtr->dir(i) != 0) neighborPtr->dir(i) /= abs(neighborPtr->dir(i)); } } } } // if search fails ros::Time time_2 = ros::Time::now(); if ((time_2 - time_1).toSec() > 0.1) ROS_WARN("Time consume in Astar path finding is %f", (time_2 - time_1).toSec()); } vector<Vector3d> AstarPathFinder::getPath() { vector<Vector3d> path; vector<GridNodePtr> gridPath; /* STEP 8: trace back from the curretnt nodePtr to get all nodes along the path please write your code below */ gridPath.push_back(terminatePtr); while (terminatePtr->cameFrom != NULL) { terminatePtr = terminatePtr->cameFrom; gridPath.push_back(terminatePtr); } for (auto ptr : gridPath) path.push_back(ptr->coord); reverse(path.begin(), path.end()); return path; }
