无人机避障——感知篇（Ego_Planner_v2中的滚动窗口实现动态实时感知建图grid_map ROS节点理解与参数调整影响）_egoplannerv2

技术文档

处理器：Orin nx

双目视觉传感器：ZED2

实时感知建图方法：Vins Fusion + Raycast （VIO与射线投影法感知定位加建图方法）

项目地址：https://github.com/ZJU-FAST-Lab/EGO-Planner-v2

【注意】：建图部分的代码存放在EGO-Planner-v2/swarm-playground/main_ws/src/planner/plan_env文件夹中，其中v2有两份建图源码，grid_map.cpp和grid_map_bigmap.cpp改cmakelist可以选择编译哪份代码。

结果展示：

室内感知地图构建效果：

实时室外感知效果：

Vins Fusion & Ray Casting 测试

代码分析：

启动文件参数理解：

virtual_ceil&virtual_ground：

其中参数virtual_ground，enable_virtual_wall，virtual_ceil相互配合使用，实现了虚拟墙功能中的地面高度限制逻辑：

double lbz = mp_.enable_virtual_walll_ ?  max(md_.ringbuffer_lowbound3d_(2), mp_.virtual_ground_) :  md_.ringbuffer_lowbound3d_(2);double ubz = mp_.enable_virtual_walll_ ?  min(md_.ringbuffer_upbound3d_(2), mp_.virtual_ceil_) :  md_.ringbuffer_upbound3d_(2);

代码需要起到的效果是：比如天花板是希望无人机飞得太低或者太高。如下含义所示：

虚拟地面 > 实际下界（如虚拟地面设为 0.0m，实际下界为 -1.0m）：
地图的最低高度被 “抬高” 到 0.0m，低于此高度的区域被视为障碍物（虚拟地面）。

虚拟地面 ≤ 实际下界：虚拟地面不生效，仍使用实际地图下界。

但是egov2以下代码虚拟地面和天花板的代码并没有实现这个功能：

// 发布原始占据地图void GridMap::publishMap(){ // 如果没有订阅者，不发布 if (map_pub_.getNumSubscribers() <= 0) return; // 计算相机朝向 Eigen::Vector3d heading = (md_.camera_r_m_ * md_.cam2body_.block(0, 0).transpose()).block(0, 0); pcl::PointCloud cloud; // 考虑虚拟墙时的高度限制 double lbz = mp_.enable_virtual_walll_ ? max(md_.ringbuffer_lowbound3d_(2), mp_.virtual_ground_) : md_.ringbuffer_lowbound3d_(2); double ubz = mp_.enable_virtual_walll_ ? min(md_.ringbuffer_upbound3d_(2), mp_.virtual_ceil_) : md_.ringbuffer_upbound3d_(2); // 遍历地图范围内的所有网格，收集障碍物点 if (md_.ringbuffer_upbound3d_(0) - md_.ringbuffer_lowbound3d_(0) > mp_.resolution_ && (md_.ringbuffer_upbound3d_(1) - md_.ringbuffer_lowbound3d_(1)) > mp_.resolution_ && (ubz - lbz) > mp_.resolution_) for (double xd = md_.ringbuffer_lowbound3d_(0) + mp_.resolution_ / 2; xd <= md_.ringbuffer_upbound3d_(0); xd += mp_.resolution_) for (double yd = md_.ringbuffer_lowbound3d_(1) + mp_.resolution_ / 2; yd <= md_.ringbuffer_upbound3d_(1); yd += mp_.resolution_) for (double zd = lbz + mp_.resolution_ / 2; zd  0.5) // { // if (md_.occupancy_buffer_[globalIdx2BufIdx(pos2GlobalIdx(Eigen::Vector3d(xd, yd, zd)))] >= mp_.min_occupancy_log_) // cloud.push_back(pcl::PointXYZ(xd, yd, zd)); // }  // 替换成发布整个局部地图的障碍物 =======LH 20250626 if (md_.occupancy_buffer_[globalIdx2BufIdx(pos2GlobalIdx(Eigen::Vector3d(xd, yd, zd)))] >= mp_.min_occupancy_log_) cloud.push_back(pcl::PointXYZ(xd, yd, zd)); } // 准备ROS点云消息 cloud.width = cloud.points.size(); cloud.height = 1; cloud.is_dense = true; cloud.header.frame_id = mp_.frame_id_; sensor_msgs::PointCloud2 cloud_msg; pcl::toROSMsg(cloud, cloud_msg); map_pub_.publish(cloud_msg);}

它只是选择在该虚拟区域内有障碍物的保留，超出该虚拟区域的就不计算在内，虚拟天花板和虚拟地面都没有让其现实为有障碍物，用来限制无人机的规划区域。

因此需要进行修改：

void GridMap::publishMapInflate(){ // 如果没有订阅者，不发布 if (map_inf_pub_.getNumSubscribers() <= 0) return; // 计算相机朝向 Eigen::Vector3d heading = (md_.camera_r_m_ * md_.cam2body_.block(0, 0).transpose()).block(0, 0); pcl::PointCloud cloud; // 考虑虚拟墙时的高度限制(包含膨胀区域) double lbz = mp_.enable_virtual_walll_ ? max(md_.ringbuffer_inf_lowbound3d_(2), mp_.virtual_ground_) : md_.ringbuffer_inf_lowbound3d_(2); double ubz = mp_.enable_virtual_walll_ ? min(md_.ringbuffer_inf_upbound3d_(2), mp_.virtual_ceil_) : md_.ringbuffer_inf_upbound3d_(2); // 遍历膨胀后的地图范围，收集障碍物点 if (md_.ringbuffer_inf_upbound3d_(0) - md_.ringbuffer_inf_lowbound3d_(0) > mp_.resolution_ && (md_.ringbuffer_inf_upbound3d_(1) - md_.ringbuffer_inf_lowbound3d_(1)) > mp_.resolution_ && (ubz - lbz) > mp_.resolution_) { for (double xd = md_.ringbuffer_inf_lowbound3d_(0) + mp_.resolution_ / 2; xd < md_.ringbuffer_inf_upbound3d_(0); xd += mp_.resolution_) { for (double yd = md_.ringbuffer_inf_lowbound3d_(1) + mp_.resolution_ / 2; yd < md_.ringbuffer_inf_upbound3d_(1); yd += mp_.resolution_) { for (double zd = lbz + mp_.resolution_ / 2; zd  0.5) // { // if (md_.occupancy_buffer_inflate_[globalIdx2InfBufIdx(pos2GlobalIdx(Eigen::Vector3d(xd, yd, zd)))]) // cloud.push_back(pcl::PointXYZ(xd, yd, zd)); // } // 替换成发布整个局部地图的障碍物 =======LH 20250626 if (md_.occupancy_buffer_inflate_[globalIdx2InfBufIdx(pos2GlobalIdx(Eigen::Vector3d(xd, yd, zd)))])  cloud.push_back(pcl::PointXYZ(xd, yd, zd));  } // 将虚拟天花板和虚拟地面也显示为障碍物 LH 20250627 // 添加虚拟天花板（如果启用了虚拟墙） if (mp_.enable_virtual_walll_ && mp_.virtual_ceil_  md_.ringbuffer_lowbound3d_(2)) {  cloud.push_back(pcl::PointXYZ(xd, yd, mp_.virtual_ground_)); } } } }

比如我尝试设置如下参数：

加上了虚拟墙的结果如下：

3D 更新范围的单位转换:

// 将3D更新范围从米转换为网格数mp_.local_update_range3i_ = (mp_.local_update_range3d_ * mp_.resolution_inv_).array().ceil().cast();mp_.local_update_range3d_ = mp_.local_update_range3i_.array().cast() * mp_.resolution_;

这是为了防止出现3D更新范围中距离为分辨率以下的浮点型范围出现，一般设置在分辨率以上的数据就行，比如分辨率为0.1，如果设置为10.1，局部更新范围还是10.1，但是如果设置为10.15，那么此时的局部更新范围为10.2，向上取整，一般局部更新范围设置成整数就绝对没有什么问题了。分辨率的话，最好是能够被范围除尽的。

环形缓冲区计算：

// 环形缓冲区大小（包含膨胀区域）md_.ringbuffer_size3i_ = 2 * mp_.local_update_range3i_;md_.ringbuffer_inf_size3i_ = md_.ringbuffer_size3i_ + Eigen::Vector3i(2 * mp_.inf_grid_, 2 * mp_.inf_grid_, 2 * mp_.inf_grid_);

环形缓冲区（Ring Buffer）在代码中扮演着高效管理动态地图内存的核心角色，特别适用于机器人实时导航场景，其中如果更新范围为[15,15,10]:

使用 2 倍的更新范围，确保机器人周围有足够的观察空间
例如：更新范围为 15 米，分辨率 0.1m，则对应 150 个网格
基础缓冲区大小为 300×300×200 网格

包含膨胀区域的缓冲区大小：

mp_.inf_grid_ 是障碍物膨胀的网格数（例如：膨胀 0.1m 对应 1 个网格）
膨胀缓冲区在基础缓冲区周围额外增加空间
例如：膨胀 2 个网格，则最终缓冲区大小为 304×304×204 网格

概率值转换为对数几率（log-odds）的数学变换:

 // 计算概率的对数表示（Log-odds变换） mp_.prob_hit_log_ = logit(mp_.p_hit_); mp_.prob_miss_log_ = logit(mp_.p_miss_); mp_.clamp_min_log_ = logit(mp_.p_min_); mp_.clamp_max_log_ = logit(mp_.p_max_); mp_.min_occupancy_log_ = logit(mp_.p_occ_);

通过Log-odds变换可以将概率转换为对数几率，反之，也可以通过逆变换将对数几率转换回概率，对数几率运算有什么好处呢？

(1) 将概率映射到实数域

概率的取值范围是 (0,1)，而Log-odds变换将其映射到整个实数域 (−∞,+∞)。这使得概率可以用线性方法进行处理，便于数学运算和建模。

(2) 便于概率更新

在贝叶斯推断中，先验概率和后验概率的更新可以通过Log-odds变换简化。例如，假设先验概率为 pprior，观测到某个证据后，后验概率的Log-odds可以通过简单的加法更新：

log-odds(pposterior)=log-odds(pprior)+log-likelihood

其中，log-likelihood 是观测证据的对数似然。

(3) 避免概率值接近0或1时的数值问题

当概率 p 接近0或1时，直接操作概率可能会导致数值计算问题（如下溢或上溢）。Log-odds变换可以缓解这些问题。

基本初始化参数的设置，不做赘述：

 // 初始化数据缓冲区 Eigen::Vector3i map_voxel_num3i = 2 * mp_.local_update_range3i_; int buffer_size = map_voxel_num3i(0) * map_voxel_num3i(1) * map_voxel_num3i(2); int buffer_inf_size = (map_voxel_num3i(0) + 2 * mp_.inf_grid_) * (map_voxel_num3i(1) + 2 * mp_.inf_grid_) * (map_voxel_num3i(2) + 2 * mp_.inf_grid_); md_.ringbuffer_origin3i_ = Eigen::Vector3i(0, 0, 0); md_.ringbuffer_inf_origin3i_ = Eigen::Vector3i(0, 0, 0); // 初始化占据概率缓冲区(对数表示) md_.occupancy_buffer_ = vector(buffer_size, mp_.clamp_min_log_); // 初始化膨胀后的占据缓冲区(用于碰撞检测) md_.occupancy_buffer_inflate_ = vector(buffer_inf_size, 0); // 初始化统计计数器和标志位 md_.count_hit_and_miss_ = vector(buffer_size, 0); md_.count_hit_ = vector(buffer_size, 0); md_.flag_rayend_ = vector(buffer_size, -1); md_.flag_traverse_ = vector(buffer_size, -1); md_.cache_voxel_ = vector(buffer_size, Eigen::Vector3i(0, 0, 0)); // 初始化计数器 md_.raycast_num_ = 0; md_.proj_points_cnt_ = 0; md_.cache_voxel_cnt_ = 0;

相机外参矩阵和外参订阅：

 // 相机到机器人本体的外参矩阵(固定变换) md_.cam2body_ << 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; /* init callback */ /* 初始化订阅者和回调函数 */ // 深度图像订阅 depth_sub_.reset(new message_filters::Subscriber(node_, \"/zed2/zed_node/depth/depth_registered\", 50)); // 外参订阅(相机到本体的变换) extrinsic_sub_ = node_.subscribe( \"/vins_estimator/extrinsic\", 10, &GridMap::extrinsicCallback, this); //sub

代码中外参矩阵和/vins_estimator/extrinsic这个话题是干什么用的？

1. 相机外参矩阵 (`md_.cam2body_`)

作用

坐标系转换：将点从相机坐标系转换到机器人本体坐标系。
固定变换关系：描述相机相对于机器人本体的位置和姿态（旋转 + 平移）。

矩阵解析

md_.cam2body_ << 0.0, 0.0, 1.0, 0.0, -1.0, 0.0, 0.0, 0.0,  0.0, -1.0, 0.0, 0.0,  0.0, 0.0, 0.0, 1.0;

这是一个4×4 齐次变换矩阵，分为：

旋转部分（左上角 3×3）：

R = [ 0 0 1 ] [-1 0 0 ] [ 0 -1 0 ]

表示相机绕 Z 轴旋转 90°，再绕 X 轴旋转 90°（相当于相机朝前，但上下颠倒）。
平移部分（右上角 3×1）：

t = [ 0, 0, 0 ]^T

表示相机安装在机器人本体的原点（无平移偏移）。

实际意义

若相机朝上安装在机器人顶部，这个矩阵会将相机检测到的 “上方” 障碍物转换到本体坐标系的 “前方”。

`/vins_estimator/extrinsic` 话题：

作用

动态外参更新：接收来自 VINS（视觉惯性导航系统）的相机外参估计。
在线校准：在机器人运行过程中实时优化相机与本体的相对位姿。

回调函数 `extrinsicCallback`

当接收到 /vins_estimator/extrinsic 话题的消息时，会调用该函数更新 md_.cam2body_ 矩阵。比如代码中的：

// 外参回调函数(更新相机到本体的变换)void GridMap::extrinsicCallback(const nav_msgs::OdometryConstPtr &odom){ // 从消息中提取四元数并转换为旋转矩阵 Eigen::Quaterniond cam2body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); Eigen::Matrix3d cam2body_r_m = cam2body_q.toRotationMatrix(); // 更新相机到本体的变换矩阵 md_.cam2body_.block(0, 0) = cam2body_r_m; md_.cam2body_(0, 3) = odom->pose.pose.position.x; md_.cam2body_(1, 3) = odom->pose.pose.position.y; md_.cam2body_(2, 3) = odom->pose.pose.position.z; md_.cam2body_(3, 3) = 1.0;}

相机外参矩阵：是将相机数据映射到机器人坐标系的关键变换。
/vins_estimator/extrinsic 话题：提供更精确的动态外参估计，提升地图构建的准确性。

同步深度图像和里程计：

 // 根据姿态类型选择不同的订阅方式 if (mp_.pose_type_ == POSE_STAMPED) { // 包含带时间戳的 3D 位姿信息（位置 + 姿态） pose_sub_.reset( new message_filters::Subscriber(node_, \"grid_map/pose\", 25)); // 同步深度图像和姿态消息 sync_image_pose_.reset(new message_filters::Synchronizer( SyncPolicyImagePose(100), *depth_sub_, *pose_sub_)); sync_image_pose_->registerCallback(boost::bind(&GridMap::depthPoseCallback, this, _1, _2)); } else if (mp_.pose_type_ == ODOMETRY) { odom_sub_.reset(new message_filters::Subscriber(node_, \"/vins_estimator/imu_propagate\", 100, ros::TransportHints().tcpNoDelay())); // 同步深度图像和里程计消息 sync_image_odom_.reset(new message_filters::Synchronizer( SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_)); sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));

根据带时间戳的3D位姿信息进行同步的方式目前没用上，不赘述，对采用里程计和深度图像同步的方式进行理解：

在机器人导航中，深度相机提供环境障碍物信息，而位姿 / 里程计提供机器人自身位置。但两者通常由不同传感器以不同频率发布，时间戳可能不一致。

关键组件：

消息过滤器（Message Filters）：ROS 提供的时间同步工具，支持多话题消息的时间对齐。
同步策略（SyncPolicyImageOdom）：定义如何匹配深度图和里程计消息，通常基于时间戳：
常见策略：
- ApproximateTime：允许时间戳在一定范围内匹配（更常用）。
- ExactTime：要求时间戳完全一致（严格但不实用）。
缓存大小（100）：
存储最近的 100 条消息，用于寻找最佳匹配对。

注册回调函数registerCallback:

当同步器找到匹配的深度图和里程计消息时，会调用depthOdomCallback，这个回调函数是一个处理深度图像和里程计数据的回调函数，用于同时接收和处理这两种传感器数据。

// 深度图像和里程计消息回调函数void GridMap::depthOdomCallback(const sensor_msgs::ImageConstPtr &img, const nav_msgs::OdometryConstPtr &odom){ /* 从里程计消息中提取相机姿态 */ Eigen::Quaterniond body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, odom->pose.pose.orientation.y, odom->pose.pose.orientation.z); Eigen::Matrix3d body_r_m = body_q.toRotationMatrix(); Eigen::Matrix4d body2world; body2world.block(0, 0) = body_r_m; body2world(0, 3) = odom->pose.pose.position.x; body2world(1, 3) = odom->pose.pose.position.y; body2world(2, 3) = odom->pose.pose.position.z; body2world(3, 3) = 1.0; // 计算相机到世界坐标系的变换 Eigen::Matrix4d cam_T = body2world * md_.cam2body_; md_.camera_pos_(0) = cam_T(0, 3); md_.camera_pos_(1) = cam_T(1, 3); md_.camera_pos_(2) = cam_T(2, 3); md_.camera_r_m_ = cam_T.block(0, 0); /* 处理深度图像(与depthPoseCallback类似) */ cv_bridge::CvImagePtr cv_ptr; cv_ptr = cv_bridge::toCvCopy(img, img->encoding); if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1) { (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_); } cv_ptr->image.copyTo(md_.depth_image_); // 初始化投影点容器(仅首次调用) static bool first_flag = true; if (first_flag) { first_flag = false; md_.proj_points_.resize(md_.depth_image_.cols * md_.depth_image_.rows / mp_.skip_pixel_ / mp_.skip_pixel_); } // 标记地图需要更新 md_.occ_need_update_ = true; md_.flag_have_ever_received_depth_ = true;}

mp_.k_depth_scaling_factor_用于将浮点深度值转换为整数深度值，常见值为 1000（从米转换为毫米）
skip_pixel_参数用于控制处理的像素间隔，提高处理效率

姿态信息处理：

/* 从里程计消息中提取相机姿态 */Eigen::Quaterniond body_q = Eigen::Quaterniond(odom->pose.pose.orientation.w, odom->pose.pose.orientation.x, odom->pose.pose.orientation.y, odom->pose.pose.orientation.z);Eigen::Matrix3d body_r_m = body_q.toRotationMatrix();Eigen::Matrix4d body2world;body2world.block(0, 0) = body_r_m;body2world(0, 3) = odom->pose.pose.position.x;body2world(1, 3) = odom->pose.pose.position.y;body2world(2, 3) = odom->pose.pose.position.z;body2world(3, 3) = 1.0;// 计算相机到世界坐标系的变换Eigen::Matrix4d cam_T = body2world * md_.cam2body_;md_.camera_pos_(0) = cam_T(0, 3);md_.camera_pos_(1) = cam_T(1, 3);md_.camera_pos_(2) = cam_T(2, 3);md_.camera_r_m_ = cam_T.block(0, 0);

这部分代码首先从里程计消息中提取四元数表示的姿态，转换为旋转矩阵
构建 4×4 的齐次变换矩阵body2world，表示机器人本体坐标系到世界坐标系的变换
通过cam2body_变换（相机相对于本体的位置），计算相机坐标系到世界坐标系的变换矩阵cam_T
提取相机在世界坐标系中的位置和旋转矩阵，存储在类成员变量中

深度图像处理：

/* 处理深度图像(与depthPoseCallback类似) */cv_bridge::CvImagePtr cv_ptr;cv_ptr = cv_bridge::toCvCopy(img, img->encoding);if (img->encoding == sensor_msgs::image_encodings::TYPE_32FC1){ (cv_ptr->image).convertTo(cv_ptr->image, CV_16UC1, mp_.k_depth_scaling_factor_);}cv_ptr->image.copyTo(md_.depth_image_);

使用cv_bridge将 ROS 图像消息转换为 OpenCV 格式
如果深度图像是 32 位浮点格式（单位通常为米），则将其转换为 16 位无符号整数格式（单位通常为毫米），使用缩放因子k_depth_scaling_factor_
将处理后的深度图像存储在类成员变量中

投影点容器初始化：

// 初始化投影点容器(仅首次调用)static bool first_flag = true;if (first_flag){ first_flag = false; md_.proj_points_.resize(md_.depth_image_.cols * md_.depth_image_.rows / mp_.skip_pixel_ / mp_.skip_pixel_);}

在 SLAM（同步定位与地图构建）或三维重建系统中，投影点容器的初始化是为了存储从深度图像中提取的三维点云数据。这个过程是将二维深度图像转换为三维空间点的关键步骤。

使用静态标志first_flag确保只在首次调用时初始化投影点容器
容器大小根据深度图像尺寸和跳过像素参数skip_pixel_计算，用于存储后续处理中的投影点

状态标记：

// 标记地图需要更新md_.occ_need_update_ = true;md_.flag_have_ever_received_depth_ = true;

设置occ_need_update_标志，表示需要更新占用地图
设置flag_have_ever_received_depth_标志，表示已经接收到过深度图像

独立的里程计和点云订阅（备用数据源）：

 // 独立的里程计和点云订阅(备用数据源) indep_odom_sub_ = node_.subscribe(\"grid_map/odom\", 10, &GridMap::odomCallback, this); indep_cloud_sub_ = node_.subscribe(\"grid_map/cloud\", 10, &GridMap::cloudCallback, this);

主要数据源：通常是通过同步的多传感器数据（如深度图像 + 里程计）实现更精确的建图，例如通过depthOdomCallback处理时间同步的深度图和里程计。
备用数据源：通过独立订阅点云 (PointCloud2) 和里程计 (Odometry) 实现建图，不要求数据严格同步。这种方式灵活性更高，但精度可能较低。
由于没有相对应的备用的点云和里程计的输入，所以这个话题目前是没有用上的。

地图更新定时器：

 occ_timer_ = node_.createTimer(ros::Duration(0.032), &GridMap::updateOccupancyCallback, this); // 地图更新定时器(31.25Hz)

updateOccupancyCallback 是一个定时调用的地图更新函数，主要用于根据深度图像和机器人位姿信息更新占据栅格地图。该函数是移动机器人环境感知和导航的核心模块之一。

// 地图占据状态更新回调函数（定时调用）void GridMap::updateOccupancyCallback(const ros::TimerEvent & /*event*/){ if (!checkDepthOdomNeedupdate()) return; /* update occupancy */ // 更新占据状态 ros::Time t1, t2, t3, t4, t5; t1 = ros::Time::now(); // 移动环形缓冲区（实现动态窗口更新） moveRingBuffer(); t2 = ros::Time::now(); // 将深度图像投影到3D空间 projectDepthImage(); t3 = ros::Time::now(); if (md_.proj_points_cnt_ > 0) { // 执行射线投射处理 raycastProcess(); t4 = ros::Time::now(); // 清除旧数据并膨胀障碍物 clearAndInflateLocalMap(); t5 = ros::Time::now(); // 显示性能统计信息 if (mp_.show_occ_time_) { cout << setprecision(7); cout << \"t2=\" << (t2 - t1).toSec() << \" t3=\" << (t3 - t2).toSec() << \" t4=\" << (t4 - t3).toSec() << \" t5=\" << (t5 - t4).toSec() << endl; static int updatetimes = 0; static double raycasttime = 0; static double max_raycasttime = 0; static double inflationtime = 0; static double max_inflationtime = 0; raycasttime += (t4 - t3).toSec(); max_raycasttime = max(max_raycasttime, (t4 - t3).toSec()); inflationtime += (t5 - t4).toSec(); max_inflationtime = max(max_inflationtime, (t5 - t4).toSec()); ++updatetimes; printf(\"Raycast(ms): cur t = %lf, avg t = %lf, max t = %lf\\n\", (t4 - t3).toSec() * 1000, raycasttime / updatetimes * 1000, max_raycasttime * 1000); printf(\"Infaltion(ms): cur t = %lf, avg t = %lf, max t = %lf\\n\", (t5 - t4).toSec() * 1000, inflationtime / updatetimes * 1000, max_inflationtime * 1000); } } md_.occ_need_update_ = false;}

1、检查是否需要更新条件：

checkDepthOdomNeedupdate 函数是地图更新流程中的数据有效性检查模块，主要用于判断是否有新的深度图像和里程计数据需要处理，以及传感器数据是否超时。

// 检查是否需要更新地图(数据有效性检查)bool GridMap::checkDepthOdomNeedupdate(){ // 初始化最后更新时间 if (md_.last_occ_update_time_.toSec()  mp_.odom_depth_timeout_) { ROS_ERROR(\"odom or depth lost! ros::Time::now()=%f, md_.last_occ_update_time_=%f, mp_.odom_depth_timeout_=%f\", ros::Time::now().toSec(), md_.last_occ_update_time_.toSec(), mp_.odom_depth_timeout_); md_.flag_depth_odom_timeout_ = true; } return false; } // 更新最后更新时间 md_.last_occ_update_time_ = ros::Time::now(); return true;}

1.1、初始化最后更新时间：

if (md_.last_occ_update_time_.toSec() < 1.0) { md_.last_occ_update_time_ = ros::Time::now();}

作用：在系统启动初期（或首次调用时）初始化last_occ_update_time_时间戳，避免因初始值异常导致误判。
判断条件：toSec() < 1.0表示时间戳小于 1 秒（可能为初始默认值）。
意义：确保时间戳在首次调用时被正确设置，为后续超时检测提供基准。

1.2、数据有效性与超时检测：

if (!md_.occ_need_update_) { if (md_.flag_have_ever_received_depth_ && (ros::Time::now() - md_.last_occ_update_time_).toSec() > mp_.odom_depth_timeout_) { ROS_ERROR(\"odom or depth lost! ...\"); md_.flag_depth_odom_timeout_ = true; } return false;}

是否需要更新地图：通过md_.occ_need_update_标志判断（该标志在depthOdomCallback中接收到新数据时设置为true）。
数据是否超时：若无需更新地图，进一步检查距离上次更新的时间是否超过阈值mp_.odom_depth_timeout_。

1.3、时间性能统计，用于调试优化：

 /* update occupancy */ // 更新占据状态 ros::Time t1, t2, t3, t4, t5; t1 = ros::Time::now();

1.4、环形缓冲区移动：

 // 移动环形缓冲区（实现动态窗口更新） moveRingBuffer();

// 移动环形缓冲区(实现动态窗口)void GridMap::moveRingBuffer(){ // 首次调用时初始化地图边界 if (!mp_.have_initialized_) initMapBoundary(); // 计算当前相机位置的网格索引 Eigen::Vector3i center_new = pos2GlobalIdx(md_.camera_pos_); // 计算新的更新范围(网格索引) Eigen::Vector3i ringbuffer_lowbound3i_new = center_new - mp_.local_update_range3i_; Eigen::Vector3d ringbuffer_lowbound3d_new = ringbuffer_lowbound3i_new.cast() * mp_.resolution_; Eigen::Vector3i ringbuffer_upbound3i_new = center_new + mp_.local_update_range3i_; Eigen::Vector3d ringbuffer_upbound3d_new = ringbuffer_upbound3i_new.cast() * mp_.resolution_; ringbuffer_upbound3i_new -= Eigen::Vector3i(1, 1, 1); // 计算包含膨胀区域的边界 const Eigen::Vector3i inf_grid3i(mp_.inf_grid_, mp_.inf_grid_, mp_.inf_grid_); const Eigen::Vector3d inf_grid3d = inf_grid3i.array().cast() * mp_.resolution_; Eigen::Vector3i ringbuffer_inf_lowbound3i_new = ringbuffer_lowbound3i_new - inf_grid3i; Eigen::Vector3d ringbuffer_inf_lowbound3d_new = ringbuffer_lowbound3d_new - inf_grid3d; Eigen::Vector3i ringbuffer_inf_upbound3i_new = ringbuffer_upbound3i_new + inf_grid3i; Eigen::Vector3d ringbuffer_inf_upbound3d_new = ringbuffer_upbound3d_new + inf_grid3d; // 根据相机移动方向清除旧数据 if (center_new(0)  md_.center_last3i_(0)) clearBuffer(1, ringbuffer_lowbound3i_new(0)); if (center_new(1)  md_.center_last3i_(1)) clearBuffer(3, ringbuffer_lowbound3i_new(1)); if (center_new(2)  md_.center_last3i_(2)) clearBuffer(5, ringbuffer_lowbound3i_new(2)); // 更新环形缓冲区索引 for (int i = 0; i < 3; ++i) { while (md_.ringbuffer_origin3i_(i)  md_.ringbuffer_upbound3i_(i)) { md_.ringbuffer_origin3i_(i) -= md_.ringbuffer_size3i_(i); } while (md_.ringbuffer_inf_origin3i_(i)  md_.ringbuffer_inf_upbound3i_(i)) { md_.ringbuffer_inf_origin3i_(i) -= md_.ringbuffer_inf_size3i_(i); } } // 保存当前中心位置和边界 md_.center_last3i_ = center_new; md_.ringbuffer_lowbound3i_ = ringbuffer_lowbound3i_new; md_.ringbuffer_lowbound3d_ = ringbuffer_lowbound3d_new; md_.ringbuffer_upbound3i_ = ringbuffer_upbound3i_new; md_.ringbuffer_upbound3d_ = ringbuffer_upbound3d_new; md_.ringbuffer_inf_lowbound3i_ = ringbuffer_inf_lowbound3i_new; md_.ringbuffer_inf_lowbound3d_ = ringbuffer_inf_lowbound3d_new; md_.ringbuffer_inf_upbound3i_ = ringbuffer_inf_upbound3i_new; md_.ringbuffer_inf_upbound3d_ = ringbuffer_inf_upbound3d_new;}

“移动环形缓冲区”（Moving Ring Buffer）是一种结合了环形缓冲区数据结构和动态窗口机制的地图管理方式，核心目的是在有限内存中高效维护机器人周围的局部环境地图，随机器人移动实时更新有效区域，同时自动丢弃远离的旧数据。

1、确定当前窗口中心

Eigen::Vector3i center_new = pos2GlobalIdx(md_.camera_pos_);

以相机当前位置为中心，将世界坐标（米）转换为地图网格索引（离散坐标），作为动态窗口的中心。
例如：相机在 (2.5, 1.0, 0.5) 米，地图分辨率 0.1 米 / 格 → 中心索引为 (25, 10, 5)。

2、定义窗口边界（有效区域）

 // 计算新的更新范围(网格索引) Eigen::Vector3i ringbuffer_lowbound3i_new = center_new - mp_.local_update_range3i_; Eigen::Vector3d ringbuffer_lowbound3d_new = ringbuffer_lowbound3i_new.cast() * mp_.resolution_; Eigen::Vector3i ringbuffer_upbound3i_new = center_new + mp_.local_update_range3i_; Eigen::Vector3d ringbuffer_upbound3d_new = ringbuffer_upbound3i_new.cast() * mp_.resolution_; ringbuffer_upbound3i_new -= Eigen::Vector3i(1, 1, 1); // 计算包含膨胀区域的边界 const Eigen::Vector3i inf_grid3i(mp_.inf_grid_, mp_.inf_grid_, mp_.inf_grid_); const Eigen::Vector3d inf_grid3d = inf_grid3i.array().cast() * mp_.resolution_; Eigen::Vector3i ringbuffer_inf_lowbound3i_new = ringbuffer_lowbound3i_new - inf_grid3i; Eigen::Vector3d ringbuffer_inf_lowbound3d_new = ringbuffer_lowbound3d_new - inf_grid3d; Eigen::Vector3i ringbuffer_inf_upbound3i_new = ringbuffer_upbound3i_new + inf_grid3i; Eigen::Vector3d ringbuffer_inf_upbound3d_new = ringbuffer_upbound3d_new + inf_grid3d;

基础窗口：由local_update_range3i_定义（如[50,50,20]表示 X/Y/Z 方向各 50/50/20 格），覆盖机器人当前周围的核心感知区域。
膨胀窗口：在基础窗口外扩展inf_grid3i（障碍物安全距离对应的网格数），确保避障时的安全余量。

3、随移动清除旧数据

// 根据相机移动方向清除旧数据 if (center_new(0)  md_.center_last3i_(0)) clearBuffer(1, ringbuffer_lowbound3i_new(0)); if (center_new(1)  md_.center_last3i_(1)) clearBuffer(3, ringbuffer_lowbound3i_new(1)); if (center_new(2)  md_.center_last3i_(2)) clearBuffer(5, ringbuffer_lowbound3i_new(2));

机器人移动时，窗口中心从center_last3i_（上次位置）移到center_new（当前位置）。
对比新旧中心，判断移动方向（如 X 轴左 / 右、Y 轴前 / 后、Z 轴上 / 下），清除 “移出窗口” 的旧数据（这些数据已远离机器人，无需保留）。

4、环形缓冲区的“移动”：索引调整

 // 更新环形缓冲区索引 for (int i = 0; i < 3; ++i) { while (md_.ringbuffer_origin3i_(i)  md_.ringbuffer_upbound3i_(i)) { md_.ringbuffer_origin3i_(i) -= md_.ringbuffer_size3i_(i); } while (md_.ringbuffer_inf_origin3i_(i)  md_.ringbuffer_inf_upbound3i_(i)) { md_.ringbuffer_inf_origin3i_(i) -= md_.ringbuffer_inf_size3i_(i); } }

环形缓冲区有固定大小（ringbuffer_size3i_），通过调整 “原点索引”（ringbuffer_origin3i_）实现 “移动”：

当窗口向右移动，原点索引增加（相当于缓冲区 “向右挪”）。
若原点超出缓冲区边界，通过加减缓冲区大小实现 “回绕”（环形特性），避免内存重新分配

5、保存状态：为下一次移动做准备

 // 保存当前中心位置和边界 md_.center_last3i_ = center_new; md_.ringbuffer_lowbound3i_ = ringbuffer_lowbound3i_new; md_.ringbuffer_lowbound3d_ = ringbuffer_lowbound3d_new; md_.ringbuffer_upbound3i_ = ringbuffer_upbound3i_new; md_.ringbuffer_upbound3d_ = ringbuffer_upbound3d_new; md_.ringbuffer_inf_lowbound3i_ = ringbuffer_inf_lowbound3i_new; md_.ringbuffer_inf_lowbound3d_ = ringbuffer_inf_lowbound3d_new; md_.ringbuffer_inf_upbound3i_ = ringbuffer_inf_upbound3i_new; md_.ringbuffer_inf_upbound3d_ = ringbuffer_inf_upbound3d_new;

更新窗口中心和边界的历史记录，作为下一次调用时判断移动方向的基准。

1.5、将深度图像投影到3D空间

 projectDepthImage();

代码如下：

void GridMap::projectDepthImage(){ md_.proj_points_cnt_ = 0; uint16_t *row_ptr; int cols = md_.depth_image_.cols; int rows = md_.depth_image_.rows; int skip_pix = mp_.skip_pixel_; double depth; Eigen::Matrix3d camera_r = md_.camera_r_m_; if (!mp_.use_depth_filter_) { for (int v = 0; v < rows; v += skip_pix) { row_ptr = md_.depth_image_.ptr(v); for (int u = 0; u < cols; u += skip_pix) { Eigen::Vector3d proj_pt; depth = (*row_ptr) / mp_.k_depth_scaling_factor_; row_ptr = row_ptr + mp_.skip_pixel_; if (depth < 0.1) continue; proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_; proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_; proj_pt(2) = depth; proj_pt = camera_r * proj_pt + md_.camera_pos_; md_.proj_points_[md_.proj_points_cnt_++] = proj_pt; } } } /* use depth filter */ else { if (!md_.has_first_depth_) md_.has_first_depth_ = true; else { Eigen::Vector3d pt_cur, pt_world, pt_reproj; Eigen::Matrix3d last_camera_r_inv; last_camera_r_inv = md_.last_camera_r_m_.inverse(); const double inv_factor = 1.0 / mp_.k_depth_scaling_factor_; for (int v = mp_.depth_filter_margin_; v < rows - mp_.depth_filter_margin_; v += mp_.skip_pixel_) { row_ptr = md_.depth_image_.ptr(v) + mp_.depth_filter_margin_; for (int u = mp_.depth_filter_margin_; u  0.01) depth += rand_noise2_(eng_); if (depth = 0 && uu = 0 && vv < rows) {  if (fabs(md_.last_depth_image_.at((int)vv, (int)uu) * inv_factor - pt_reproj.z()) < mp_.depth_filter_tolerance_)  { md_.proj_points_[md_.proj_points_cnt_++] = pt_world;  } } else {  md_.proj_points_[md_.proj_points_cnt_++] = pt_world; } } } } } } /* maintain camera pose for consistency check */ md_.last_camera_pos_ = md_.camera_pos_; md_.last_camera_r_m_ = md_.camera_r_m_; md_.last_depth_image_ = md_.depth_image_;}

这段代码的核心功能是将二维深度图像（Depth Image）通过相机参数和位姿信息，转换为三维空间中的点云（3D Point Cloud），是从 “平面距离数据” 到 “立体空间结构” 的关键转换步骤。

1、从2D深度图到3D点云

深度图像是一张 “距离地图”：每个像素的数值表示该点到相机的直线距离（深度），但本身是二维的（只有像素坐标(u,v)和深度值z）。这段代码的作用是：通过相机的 “内参”（决定像素与空间位置的映射关系）和 “位姿”（相机在世界中的位置和朝向），将每个像素的(u,v,z)转换为三维空间坐标(X,Y,Z)（世界坐标系下），最终形成三维点云，供机器人理解环境的立体结构。

2、初始化与参数准备

md_.proj_points_cnt_ = 0; // 重置有效点云计数器uint16_t *row_ptr; // 指向深度图像行的指针（快速访问像素）int cols = md_.depth_image_.cols; // 深度图像宽度（列数）int rows = md_.depth_image_.rows; // 深度图像高度（行数）int skip_pix = mp_.skip_pixel_; // 降采样步长Eigen::Matrix3d camera_r = md_.camera_r_m_; // 相机旋转矩阵（当前帧）

作用：初始化点云计数器，获取图像尺寸和处理参数，为后续遍历像素做准备。

3、如果不启用深度滤波（!mp_.use_depth_filter_）

降采样遍历像素：

if (!mp_.use_depth_filter_) { for (int v = 0; v < rows; v += skip_pix) { row_ptr = md_.depth_image_.ptr(v); for (int u = 0; u < cols; u += skip_pix) { Eigen::Vector3d proj_pt; depth = (*row_ptr) / mp_.k_depth_scaling_factor_; row_ptr = row_ptr + mp_.skip_pixel_; if (depth < 0.1) continue; proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_; proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_; proj_pt(2) = depth; proj_pt = camera_r * proj_pt + md_.camera_pos_; md_.proj_points_[md_.proj_points_cnt_++] = proj_pt; } } }

降采样目的：深度图像通常分辨率较高（如 640x480），直接处理所有像素会占用大量计算资源。通过skip_pix间隔采样，在精度和效率间平衡（如skip_pix=2时，点云数量减少为原来的 1/4）。

深度值处理与过滤：

depth = (*row_ptr) / mp_.k_depth_scaling_factor_; // 转换深度单位（毫米→米）row_ptr += mp_.skip_pixel_; // 指针移到下一个待处理像素if (depth < 0.1) continue; // 过滤过近的无效深度（可能是噪声或相机盲区）

单位转换：深度图像的uint16_t值通常以 “毫米” 为单位（如 1000 表示 1 米），除以k_depth_scaling_factor_（如 1000）转换为 “米”，方便后续计算。
有效性过滤：过近的深度（如 < 0.1 米）可能是相机无法聚焦的噪声（如镜头上的灰尘），直接跳过。

像素坐标到相机坐标系3D点：

proj_pt(0) = (u - mp_.cx_) * depth / mp_.fx_; // X坐标（相机坐标系）proj_pt(1) = (v - mp_.cy_) * depth / mp_.fy_; // Y坐标（相机坐标系）proj_pt(2) = depth; // Z坐标（相机坐标系，即深度）

核心原理：基于针孔相机模型的逆运算。

相机坐标系到世界坐标系：

proj_pt = camera_r * proj_pt + md_.camera_pos_;

坐标转换逻辑：相机坐标系下的 3D 点需要通过相机的位姿转换到世界坐标系：

先通过旋转矩阵camera_r旋转（对齐世界坐标系的朝向）；
再通过平移向量camera_pos_平移（对齐世界坐标系的原点）

存储点云：

md_.proj_points_[md_.proj_points_cnt_++] = proj_pt; // 存入点云容器并计数

4、启用深度滤波（mp_.use_depth_filter_）

if (!md_.has_first_depth_) md_.has_first_depth_ = true; // 首次处理时仅标记，无滤波（需上一帧数据参考）else { ... } // 非首次处理时启用滤波

滤波需要 “上一帧的深度图像和相机位姿” 作为参考，因此第一帧不滤波。

边缘过滤与降采样：

for (int v = mp_.depth_filter_margin_; v < rows - mp_.depth_filter_margin_; v += skip_pix) { row_ptr = md_.depth_image_.ptr(v) + mp_.depth_filter_margin_; for (int u = mp_.depth_filter_margin_; u < cols - mp_.depth_filter_margin_; u += skip_pix) { ... }}

边缘过滤：避开图像边缘depth_filter_margin_个像素（边缘像素可能因镜头畸变导致深度不准）。

深度阈值过滤：

if (depth < mp_.depth_filter_mindist_) continue; // 过滤过近深度（阈值可配置）

比无滤波分支更灵活：最小深度阈值depth_filter_mindist_可通过参数配置（而非固定 0.1 米）。

像素坐标点转向三维点云：

pt_cur(0) = (u - mp_.cx_) * depth / mp_.fx_;pt_cur(1) = (v - mp_.cy_) * depth / mp_.fy_;pt_cur(2) = depth;pt_world = camera_r * pt_cur + md_.camera_pos_;md_.proj_points_[md_.proj_points_cnt_++] = pt_world;

可选：与上一帧的一致性检查（当前禁用）：

if (false){ pt_reproj = last_camera_r_inv * (pt_world - md_.last_camera_pos_); double uu = pt_reproj.x() * mp_.fx_ / pt_reproj.z() + mp_.cx_; double vv = pt_reproj.y() * mp_.fy_ / pt_reproj.z() + mp_.cy_; if (uu >= 0 && uu = 0 && vv < rows) { if (fabs(md_.last_depth_image_.at((int)vv, (int)uu) * inv_factor - pt_reproj.z()) < mp_.depth_filter_tolerance_) { md_.proj_points_[md_.proj_points_cnt_++] = pt_world; } } else { md_.proj_points_[md_.proj_points_cnt_++] = pt_world; }}

代码中if (false)包裹的部分是一个未启用的优化逻辑，原理是：

将当前帧的世界坐标系点反投影到上一帧的深度图像中，检查上一帧对应位置的深度是否与当前帧一致。
若不一致（如动态物体移动、噪声），则过滤该点，减少动态干扰。

保存当前帧数据供下一帧参考：

md_.last_camera_pos_ = md_.camera_pos_; // 保存当前相机位置md_.last_camera_r_m_ = md_.camera_r_m_; // 保存当前相机旋转矩阵md_.last_depth_image_ = md_.depth_image_; // 保存当前深度图像

为下一帧的滤波（尤其是一致性检查）提供“上一帧参考数据”。

最终生成的点云md_.proj_points_能直观反映环境的三维结构（如墙壁、障碍物的位置和形状），是机器人 “理解” 周围环境的基础（用于建图、路径规划、避障等）。

1.6、执行射线投射处理

// 执行射线投射处理raycastProcess();

void GridMap::raycastProcess(){ md_.cache_voxel_cnt_ = 0; ros::Time t1, t2, t3; md_.raycast_num_ += 1; RayCaster raycaster; Eigen::Vector3d ray_pt, pt_w; int pts_num = 0; t1 = ros::Time::now(); // 对每个投影点执行射线投射 for (int i = 0; i < md_.proj_points_cnt_; ++i) { int vox_idx; pt_w = md_.proj_points_[i]; // set flag for projected point // 设置投影点的占据状态 if (!isInBuf(pt_w)) { // 如果点不在地图范围内，找到地图边界上的最近点 pt_w = closetPointInMap(pt_w, md_.camera_pos_); pts_num++; vox_idx = setCacheOccupancy(pt_w, 0); } else { pts_num++; vox_idx = setCacheOccupancy(pt_w, 1); } // raycasting between camera center and point // 如果点在缓存中，执行射线投射 if (vox_idx != INVALID_IDX) { if (md_.flag_rayend_[vox_idx] == md_.raycast_num_) { continue; } else { md_.flag_rayend_[vox_idx] = md_.raycast_num_; } } // 初始化射线投射器(相机到点的射线) raycaster.setInput(pt_w / mp_.resolution_, md_.camera_pos_ / mp_.resolution_); // 遍历射线上的每个网格 while (raycaster.step(ray_pt)) { Eigen::Vector3d tmp = (ray_pt + Eigen::Vector3d(0.5, 0.5, 0.5)) * mp_.resolution_; pts_num++; vox_idx = setCacheOccupancy(tmp, 0); if (vox_idx != INVALID_IDX) { if (md_.flag_traverse_[vox_idx] == md_.raycast_num_) { break; } else { md_.flag_traverse_[vox_idx] = md_.raycast_num_; } } } } t2 = ros::Time::now(); // 更新占据概率 for (int i = 0; i = md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns] ? mp_.prob_hit_log_ : mp_.prob_miss_log_; // 重置计数器 md_.count_hit_[idx_ctns] = md_.count_hit_and_miss_[idx_ctns] = 0; // 确保概率在有效范围内 if (log_odds_update >= 0 && md_.occupancy_buffer_[idx_ctns] >= mp_.clamp_max_log_) { continue; } else if (log_odds_update <= 0 && md_.occupancy_buffer_[idx_ctns] <= mp_.clamp_min_log_) { continue; } // 更新占据概率(带上下限约束) md_.occupancy_buffer_[idx_ctns] = std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update, mp_.clamp_min_log_),  mp_.clamp_max_log_); } t3 = ros::Time::now(); // 显示射线投射性能统计 if (mp_.show_occ_time_) { ROS_WARN(\"Raycast time: t2-t1=%f, t3-t2=%f, pts_num=%d\", (t2 - t1).toSec(), (t3 - t2).toSec(), pts_num); }}

这段代码实现了基于 “射线投射（Ray Casting）”的三维栅格地图更新算法，是构建概率占据地图（Probabilistic Occupancy Map）的核心逻辑。其核心思想是：从相机位置出发，向每个观测点发射一条射线，射线经过的栅格标记为 “空闲”，终点栅格标记为 “占据”，最终通过对数几率（Log Odds）更新每个栅格的占据概率。

算法核心原理：

激光束穿透性：射线在遇到障碍物前会一直传播（经过的区域为空闲）。
最后击中原则：射线的终点（深度值对应的点）为障碍物表面（占据）。

初始化与计数器重置：

md_.cache_voxel_cnt_ = 0; // 重置缓存栅格计数器md_.raycast_num_ += 1; // 射线投射次数计数器+1

缓存机制：将待更新的栅格暂存，避免重复操作，提高效率。

对每个投影点进行射线投射：

for (int i = 0; i < md_.proj_points_cnt_; ++i) { pt_w = md_.proj_points_[i]; // 获取投影点(世界坐标系) // 处理投影点本身 if (!isInBuf(pt_w)) { pt_w = closetPointInMap(pt_w, md_.camera_pos_); // 若点超出地图边界，找边界上最近点 vox_idx = setCacheOccupancy(pt_w, 0); // 标记为可能被占据(0表示需进一步处理) } else { vox_idx = setCacheOccupancy(pt_w, 1); // 直接标记为占据(1表示确定占据) } // 初始化射线投射器(从相机到点的射线) raycaster.setInput(pt_w / mp_.resolution_, md_.camera_pos_ / mp_.resolution_); // 遍历射线上的每个栅格 while (raycaster.step(ray_pt)) { Eigen::Vector3d tmp = (ray_pt + Eigen::Vector3d(0.5, 0.5, 0.5)) * mp_.resolution_; vox_idx = setCacheOccupancy(tmp, 0); // 标记为可能空闲 // 避免重复处理已遍历的栅格 if (md_.flag_traverse_[vox_idx] == md_.raycast_num_) { break; } }}

射线步进算法：raycaster.step(ray_pt)使用类似 Bresenham 算法的步进方式，依次访问射线路径上的所有栅格（效率高于逐点计算）。
标记机制：
- setCacheOccupancy(pt, 1)：标记栅格为 “占据”（投影点所在栅格）。
- setCacheOccupancy(pt, 0)：标记栅格为 “可能空闲”（射线路径上的栅格）。

概率更新（对数几率模型）：

for (int i = 0; i = md_.count_hit_and_miss_[idx_ctns] - md_.count_hit_[idx_ctns] ? mp_.prob_hit_log_ : mp_.prob_miss_log_; // 重置计数器 md_.count_hit_[idx_ctns] = md_.count_hit_and_miss_[idx_ctns] = 0; // 带约束的概率更新 md_.occupancy_buffer_[idx_ctns] = std::min(std::max(md_.occupancy_buffer_[idx_ctns] + log_odds_update, mp_.clamp_min_log_),  mp_.clamp_max_log_);}

对数几率模型：将概率p转换为对数表示log(p/(1-p))，更新时直接加减，最后转回概率：
- mp_.prob_hit_log_：观测到占据时的更新值（正数）。
- mp_.prob_miss_log_：观测到空闲时的更新值（负数）。
约束机制：
- mp_.clamp_min_log_和mp_.clamp_max_log_：限制对数几率的范围，避免数值溢出或过度自信。

1.7、清除旧数据并膨胀障碍物

for (int i = 0; i < md_.cache_voxel_cnt_; ++i) { Eigen::Vector3i idx = md_.cache_voxel_[i]; // 获取缓存中的体素索引 int buf_id = globalIdx2BufIdx(idx); // 转换为主地图缓冲区索引 int inf_buf_id = globalIdx2InfBufIdx(idx); // 转换为膨胀地图缓冲区索引 // 情况1：主地图中是障碍物，但膨胀地图尚未标记 → 执行膨胀 if (md_.occupancy_buffer_inflate_[inf_buf_id] = mp_.min_occupancy_log_) { changeInfBuf(true, inf_buf_id, idx); // 标记并膨胀障碍物 } // 情况2：主地图中不再是障碍物，但膨胀地图仍有标记 → 清除膨胀 if (md_.occupancy_buffer_inflate_[inf_buf_id] >= GRID_MAP_OBS_FLAG && md_.occupancy_buffer_[buf_id] < mp_.min_occupancy_log_) { changeInfBuf(false, inf_buf_id, idx); // 清除障碍物及膨胀标记 }}

可视化定时器：

vis_timer_ = node_.createTimer(ros::Duration(0.125), &GridMap::visCallback, this); // 可视化定时器(8Hz)

发布原始地图和膨胀后的地图：

void GridMap::visCallback(const ros::TimerEvent & /*event*/){ if (!mp_.have_initialized_) return; ros::Time t0 = ros::Time::now(); // 发布膨胀后的地图 publishMapInflate(); // 发布原始地图 publishMap(); ros::Time t1 = ros::Time::now(); // 显示可视化耗时 if (mp_.show_occ_time_) { printf(\"Visualization(ms):%f\\n\", (t1 - t0).toSec() * 1000); }}

占据概率衰减定时器：

void GridMap::fadingCallback(const ros::TimerEvent & /*event*/){ // 计算每次衰减的步长 const double reduce = (mp_.clamp_max_log_ - mp_.min_occupancy_log_) / (mp_.fading_time_ * 2); // function called at 2Hz const double low_thres = mp_.clamp_min_log_ + reduce; ros::Time t0 = ros::Time::now(); // 遍历所有网格，衰减占据概率 for (size_t i = 0; i  low_thres) { bool obs_flag = md_.occupancy_buffer_[i] >= mp_.min_occupancy_log_; md_.occupancy_buffer_[i] -= reduce; // 如果障碍物状态因衰减而改变，更新膨胀缓冲区 if (obs_flag && md_.occupancy_buffer_[i] < mp_.min_occupancy_log_) { Eigen::Vector3i idx = BufIdx2GlobalIdx(i); int inf_buf_idx = globalIdx2InfBufIdx(idx); changeInfBuf(false, inf_buf_idx, idx); } } } ros::Time t1 = ros::Time::now(); // 显示衰减处理耗时 if (mp_.show_occ_time_) { printf(\"Fading(ms):%f\\n\", (t1 - t0).toSec() * 1000); }}

fadingCallback 函数是地图动态维护的核心模块，用于定期衰减旧障碍物的占据概率，使地图能够适应动态环境（如移动物体离开、临时障碍物消失等场景），避免因过时的障碍物信息影响机器人导航。

1.1、核心功能与设计目的：

在动态环境中，障碍物可能会移动或消失（如行人走开、临时放置的箱子被移走）。如果地图永久保留这些障碍物的 “占据” 标记，会导致机器人误判环境（如认为已清空的区域仍不可通行）。

fadingCallback 的作用是：通过定期降低旧障碍物的占据概率，让地图 “遗忘” 不再被观测到的障碍物，最终使这些区域的概率低于障碍物阈值，恢复为 “可通行” 状态。

1.2、计算衰减步长：

const double reduce = (mp_.clamp_max_log_ - mp_.min_occupancy_log_) / (mp_.fading_time_ * 2); // 注释：函数被调用的频率是2Hz（每0.5秒一次），因此总调用次数为fading_time_ * 2

衰减步长的意义：每次调用时，对旧障碍物的概率减少reduce，确保在fading_time_内，概率从最大值clamp_max_log_衰减到低于min_occupancy_log_。
示例：若clamp_max_log_=5，min_occupancy_log_=2，fading_time_=5秒（总调用次数 = 5×2=10 次），则reduce=(5-2)/10=0.3，每次衰减 0.3，10 次后从 5 衰减到 5-10×0.3=2（刚好低于阈值）。

1.2、定义低阈值：

const double low_thres = mp_.clamp_min_log_ + reduce;

作用：只对概率高于low_thres的栅格进行衰减（避免对已接近 “非占据” 的栅格过度处理）。
clamp_min_log_是占据概率的最小值（对数几率形式，避免概率无限减小），low_thres确保只处理 “仍有较大概率是障碍物” 的栅格。

1.3、遍历栅格并衰减概率：

for (size_t i = 0; i  low_thres) { // 只处理需要衰减的栅格 bool obs_flag = md_.occupancy_buffer_[i] >= mp_.min_occupancy_log_; // 记录衰减前是否为障碍物 md_.occupancy_buffer_[i] -= reduce; // 衰减概率 // 若衰减后从“障碍物”变为“非障碍物”，清除对应的膨胀标记 if (obs_flag && md_.occupancy_buffer_[i] < mp_.min_occupancy_log_) { Eigen::Vector3i idx = BufIdx2GlobalIdx(i); // 转换为全局栅格索引 int inf_buf_idx = globalIdx2InfBufIdx(idx); // 转换为膨胀地图索引 changeInfBuf(false, inf_buf_idx, idx); // 清除膨胀标记 } }}

衰减逻辑：
对每个栅格，若当前概率高于low_thres，则减去reduce（每次调用衰减一次）。
状态变化处理：
若衰减前栅格是障碍物（obs_flag=true），且衰减后概率低于阈值（ < min_occupancy_log_），说明该障碍物已 “消失”，此时调用changeInfBuf清除对应的膨胀标记（避免机器人仍认为该区域不可通行）。