在Ego-Planner 中 如何根据索引值来判断有无障碍物

1.Gridmap 地图数据存储

首先我们需要大概了解在Ego-Planner 地图信息是怎么存储的。定义了一个名为MappingData的数据结构，用于存储地图和相关信息

其中 

std::vector<double> occupancy_buffer_;
//存储地图中每个体素（voxel）的占据信息 double类型
std::vector<char> occupancy_buffer_inflate_;
//存储的是膨胀后的占据信息 char类型

完整代码如下：

struct MappingData {
  // main map data, occupancy of each voxel and Euclidean distance

  std::vector<double> occupancy_buffer_;
  std::vector<char> occupancy_buffer_inflate_;

  // camera position and pose data

  Eigen::Vector3d camera_pos_, last_camera_pos_;
  Eigen::Matrix3d camera_r_m_, last_camera_r_m_;
  Eigen::Matrix4d cam2body_;

  // depth image data

  cv::Mat depth_image_, last_depth_image_;
  int image_cnt_;

  // flags of map state

  bool occ_need_update_, local_updated_;
  bool has_first_depth_;
  bool has_odom_, has_cloud_;

  // odom_depth_timeout_
  ros::Time last_occ_update_time_;
  bool flag_depth_odom_timeout_;
  bool flag_use_depth_fusion;

  // depth image projected point cloud

  vector<Eigen::Vector3d> proj_points_;
  int proj_points_cnt;

  // flag buffers for speeding up raycasting

  vector<short> count_hit_, count_hit_and_miss_;
  vector<char> flag_traverse_, flag_rayend_;
  char raycast_num_;
  queue<Eigen::Vector3i> cache_voxel_;

  // range of updating grid

  Eigen::Vector3i local_bound_min_, local_bound_max_;

  // computation time

  double fuse_time_, max_fuse_time_;
  int update_num_;

  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
};

通过 ： 来设置地图尺寸的大小 

node_.param("grid_map/resolution", mp_.resolution_, -1.0);

node_.param("grid_map/map_size_x", x_size, -1.0);

node_.param("grid_map/map_size_y", y_size, -1.0);

node_.param("grid_map/map_size_z", z_size, -1.0);

关于resolution 它指定了地图的分辨率，即在地图中一个体素（voxel）代表的实际空间的大小。分辨率通常以米/体素（meters per voxel）为单位表示 较低的分辨率会导致地图更粗糙，而较高的分辨率会导致地图更细致，

上面这些参数的值可以根据应用需求进行配置。例如，如果你的应用需要一个非常精细的地图，你可以选择较小的分辨率，以及更大的尺寸值，以覆盖更广阔的地理范围。如果你的应用只关注一个小区域的地图，可以选择更大的分辨率和较小的尺寸。

mp_.map_size_ = Eigen::Vector3d(x_size, y_size, z_size);

使用mp_.map_voxel_num_(i)来存储了地图在维度i上的体素（Voxel）数量

for (int i = 0; i < 3; ++i)

mp_.map_voxel_num_(i) = ceil(mp_.map_size_(i) / mp_.resolution_);

occupancy_buffer_ 和 occupancy_buffer_inflate_ 将在后续的地图构建和更新过程中用于存储地图的占据信息和其他相关数据。初始化时，它们都被赋予了初始值，然后随着地图的更新和修改而不断变化。地图的占据信息通常表示了每个体素被占据的程度，可以用于导航、碰撞检测和环境建模等任务。

int buffer_size = mp_.map_voxel_num_(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2); // 计算体积

md_.occupancy_buffer_ = vector<double>(buffer_size, mp_.clamp_min_log_ - mp_.unknown_flag_);

md_.occupancy_buffer_inflate_ = vector<char>(buffer_size, 0); //buffer_size大的一个容器（也就相当于一个数组） 初始值为0 

通过toAddress（）将三维空间的用于将三维空间中的体素坐标 (Eigen::Vector3i 类型) 转换为一维数组中的地址（索引）

inline int GridMap::toAddress(const Eigen::Vector3i& id) {

return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2);

}

2. 检查是否是障碍物

在A star 搜索时 可以 使用checkOccupancy()函数用于检查指定位置是否为障碍物或被占据。没有被占用返回false 

inline bool checkOccupancy(const Eigen::Vector3d &pos) 
	{ 
		return (bool)grid_map_->getInflateOccupancy(pos); 

	}

通过getInflateOccupancy(Eigen::Vector3d pos) 来获取膨胀后的体素占用信息 

inline int GridMap::getInflateOccupancy(Eigen::Vector3d pos) {
  if (!isInMap(pos)) return -1;
    //isInMap(pos) 判断这个点是否处于地图之中

  Eigen::Vector3i id;

  posToIndex(pos, id); //转换为index 

  return int(md_.occupancy_buffer_inflate_[toAddress(id)]);
    //一般栅格地图 0表示 未被占据  1 表示占据 -1 表示为定义
}


inline int GridMap::toAddress(const Eigen::Vector3i& id) {
  return id(0) * mp_.map_voxel_num_(1) * mp_.map_voxel_num_(2) + id(1) * mp_.map_voxel_num_(2) + id(2);
}
// map_voxel_num_    map range in index
//将三维空间点转换为一维数组的索引值