处理器：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可以选择编译哪份代码。

结果展示：

室内感知地图构建效果：

实时室外感知效果：

代码分析：

启动文件参数理解：

<launch><node pkg="plan_env" name="gridmap_node" type="gridmap_node" output="screen"><param name="grid_map/resolution"      value="0.1" /> <param name="grid_map/local_update_range_x"  value="10" /> <param name="grid_map/local_update_range_y"  value="10" /> <param name="grid_map/local_update_range_z"  value="10" /> <param name="grid_map/obstacles_inflation"     value="0.1" /> <!-- 虚拟地面高度 --><param name="grid_map/virtual_ground"        value="-1.5"/><!-- 是否启动虚拟墙 --><param name="grid_map/enable_virtual_wall"        value="true"/><!-- 该参数控制RViz 中显示的地图高度上限，用于简化可视化：高于此高度的栅格点不会被加入可视化点云。--><param name="grid_map/virtual_ceil"   value="1.8"/> <!-- camera parameter --><param name="grid_map/cx" value="320.2084655761719"/><param name="grid_map/cy" value="174.24594116210938"/><param name="grid_map/fx" value="262.0037536621094"/><param name="grid_map/fy" value="262.0037536621094"/><!-- depth filter --><param name="grid_map/use_depth_filter" value="true"/><param name="grid_map/depth_filter_tolerance" value="0.15"/><param name="grid_map/depth_filter_maxdist"   value="5.0"/><param name="grid_map/depth_filter_mindist"   value="0.2"/><param name="grid_map/depth_filter_margin"    value="2"/><param name="grid_map/k_depth_scaling_factor" value="1000.0"/><param name="grid_map/skip_pixel" value="2"/><!-- local fusion --><param name="grid_map/p_hit"  value="0.65"/><param name="grid_map/p_miss" value="0.35"/><param name="grid_map/p_min"  value="0.12"/><param name="grid_map/p_max"  value="0.90"/><param name="grid_map/p_occ"  value="0.80"/><!-- 忽略距离相机过近的点（如相机本身的支架或噪声点） --><param name="grid_map/min_ray_length" value="0.2"/> <param name="grid_map/show_occ_time"  value="false"/><param name="grid_map/pose_type"     value="2"/>  <param name="grid_map/frame_id"      value="world"/></node><node pkg="rviz" type="rviz" name="rviz" args="-d $(find plan_env)/launch/plan_env.rviz" required="true" /></launch>

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<3, 3>(0, 0).transpose()).block<3, 1>(0, 0);pcl::PointCloud<pcl::PointXYZ> 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 <= ubz; zd += mp_.resolution_){Eigen::Vector3d relative_dir = (Eigen::Vector3d(xd, yd, zd) - md_.camera_pos_);// 只发布相机前方的障碍物// if (heading.dot(relative_dir.normalized()) > 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 20250626if (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<3, 3>(0, 0).transpose()).block<3, 1>(0, 0);pcl::PointCloud<pcl::PointXYZ> 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 < ubz; zd += mp_.resolution_){Eigen::Vector3d relative_dir = (Eigen::Vector3d(xd, yd, zd) - md_.camera_pos_);// 只发布相机前方的障碍物// if (heading.dot(relative_dir.normalized()) > 0.5)// {//   if (md_.occupancy_buffer_inflate_[globalIdx2InfBufIdx(pos2GlobalIdx(Eigen::Vector3d(xd, yd, zd)))])//     cloud.push_back(pcl::PointXYZ(xd, yd, zd));// }// 替换成发布整个局部地图的障碍物 =======LH 20250626if (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_upbound3d_(2)){cloud.push_back(pcl::PointXYZ(xd, yd, mp_.virtual_ceil_));}// 添加虚拟地面（如果启用了虚拟墙）if (mp_.enable_virtual_walll_ && mp_.virtual_ground_ > md_.ringbuffer_lowbound3d_(2)){cloud.push_back(pcl::PointXYZ(xd, yd, mp_.virtual_ground_));}}}}

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

<!-- 虚拟地面高度 -->
<param name="grid_map/virtual_ground"        value="0"/>
<!-- 是否启动虚拟墙 -->
<param name="grid_map/enable_virtual_wall"        value="true"/>
<!-- 该参数控制RViz 中显示的地图高度上限，用于简化可视化：高于此高度的栅格点不会被加入可视化点云。-->
<param name="grid_map/virtual_ceil"   value="5"/> 

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

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

// 将3D更新范围从米转换为网格数
mp_.local_update_range3i_ = (mp_.local_update_range3d_ * mp_.resolution_inv_).array().ceil().cast<int>();
mp_.local_update_range3d_ = mp_.local_update_range3i_.array().cast<double>() * 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<double>(buffer_size, mp_.clamp_min_log_);// 初始化膨胀后的占据缓冲区(用于碰撞检测)md_.occupancy_buffer_inflate_ = vector<uint16_t>(buffer_inf_size, 0);// 初始化统计计数器和标志位md_.count_hit_and_miss_ = vector<short>(buffer_size, 0);md_.count_hit_ = vector<short>(buffer_size, 0);md_.flag_rayend_ = vector<char>(buffer_size, -1);md_.flag_traverse_ = vector<char>(buffer_size, -1);md_.cache_voxel_ = vector<Eigen::Vector3i>(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<sensor_msgs::Image>(node_, "/zed2/zed_node/depth/depth_registered", 50));// 外参订阅(相机到本体的变换)extrinsic_sub_ = node_.subscribe<nav_msgs::Odometry>("/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<3, 3>(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<geometry_msgs::PoseStamped>(node_, "grid_map/pose", 25));// 同步深度图像和姿态消息sync_image_pose_.reset(new message_filters::Synchronizer<SyncPolicyImagePose>(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<nav_msgs::Odometry>(node_, "/vins_estimator/imu_propagate", 100, ros::TransportHints().tcpNoDelay()));// 同步深度图像和里程计消息sync_image_odom_.reset(new message_filters::Synchronizer<SyncPolicyImageOdom>(SyncPolicyImageOdom(100), *depth_sub_, *odom_sub_));sync_image_odom_->registerCallback(boost::bind(&GridMap::depthOdomCallback, this, _1, _2));

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

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

关键组件：

1. 消息过滤器（Message Filters）：ROS 提供的时间同步工具，支持多话题消息的时间对齐。

2. 同步策略（SyncPolicyImageOdom）：定义如何匹配深度图和里程计消息，通常基于时间戳：

3. 常见策略：

   • ApproximateTime：允许时间戳在一定范围内匹配（更常用）。
   • ExactTime：要求时间戳完全一致（严格但不实用）。

4. 缓存大小（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<3, 3>(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<3, 3>(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<3, 3>(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<3, 3>(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<nav_msgs::Odometry>("grid_map/odom", 10, &GridMap::odomCallback, this);indep_cloud_sub_ =node_.subscribe<sensor_msgs::PointCloud2>("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() < 1.0){md_.last_occ_update_time_ = ros::Time::now();}// 如果不需要更新且数据超时，标记错误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! 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;
}

1. 是否需要更新地图：通过md_.occ_need_update_标志判断（该标志在depthOdomCallback中接收到新数据时设置为true）。
2. 数据是否超时：若无需更新地图，进一步检查距离上次更新的时间是否超过阈值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<double>() * mp_.resolution_;Eigen::Vector3i ringbuffer_upbound3i_new = center_new + mp_.local_update_range3i_;Eigen::Vector3d ringbuffer_upbound3d_new = ringbuffer_upbound3i_new.cast<double>() * 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<double>() * 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(0, ringbuffer_upbound3i_new(0));if (center_new(0) > md_.center_last3i_(0))clearBuffer(1, ringbuffer_lowbound3i_new(0));if (center_new(1) < md_.center_last3i_(1))clearBuffer(2, ringbuffer_upbound3i_new(1));if (center_new(1) > md_.center_last3i_(1))clearBuffer(3, ringbuffer_lowbound3i_new(1));if (center_new(2) < md_.center_last3i_(2))clearBuffer(4, ringbuffer_upbound3i_new(2));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_lowbound3i_(i)){md_.ringbuffer_origin3i_(i) += md_.ringbuffer_size3i_(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_lowbound3i_(i)){md_.ringbuffer_inf_origin3i_(i) += md_.ringbuffer_inf_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<double>() * mp_.resolution_;Eigen::Vector3i ringbuffer_upbound3i_new = center_new + mp_.local_update_range3i_;Eigen::Vector3d ringbuffer_upbound3d_new = ringbuffer_upbound3i_new.cast<double>() * 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<double>() * 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(0, ringbuffer_upbound3i_new(0));if (center_new(0) > md_.center_last3i_(0))clearBuffer(1, ringbuffer_lowbound3i_new(0));if (center_new(1) < md_.center_last3i_(1))clearBuffer(2, ringbuffer_upbound3i_new(1));if (center_new(1) > md_.center_last3i_(1))clearBuffer(3, ringbuffer_lowbound3i_new(1));if (center_new(2) < md_.center_last3i_(2))clearBuffer(4, ringbuffer_upbound3i_new(2));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_lowbound3i_(i)){md_.ringbuffer_origin3i_(i) += md_.ringbuffer_size3i_(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_lowbound3i_(i)){md_.ringbuffer_inf_origin3i_(i) += md_.ringbuffer_inf_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<uint16_t>(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<uint16_t>(v) + mp_.depth_filter_margin_;for (int u = mp_.depth_filter_margin_; u < cols - mp_.depth_filter_margin_;u += mp_.skip_pixel_){depth = (*row_ptr) * inv_factor;row_ptr = row_ptr + mp_.skip_pixel_;// filter depth// depth += rand_noise_(eng_);// if (depth > 0.01) depth += rand_noise2_(eng_);if (depth < mp_.depth_filter_mindist_){continue;}// project to world framept_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;// check consistency with last image, disabled...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 < cols && vv >= 0 && vv < rows){if (fabs(md_.last_depth_image_.at<uint16_t>((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<uint16_t>(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<uint16_t>(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 < cols && vv >= 0 && vv < rows){if (fabs(md_.last_depth_image_.at<uint16_t>((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_.cache_voxel_cnt_; ++i){int idx_ctns = globalIdx2BufIdx(md_.cache_voxel_[i]);// 根据命中次数计算占据概率更新值double log_odds_update =md_.count_hit_[idx_ctns] >= 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）更新每个栅格的占据概率。

算法核心原理：

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

初始化与计数器重置：

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_.cache_voxel_cnt_; ++i) {int idx_ctns = globalIdx2BufIdx(md_.cache_voxel_[i]);// 根据命中次数计算对数几率更新值double log_odds_update = md_.count_hit_[idx_ctns] >= 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] < GRID_MAP_OBS_FLAG && md_.occupancy_buffer_[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 2Hzconst double low_thres = mp_.clamp_min_log_ + reduce;ros::Time t0 = ros::Time::now();// 遍历所有网格，衰减占据概率for (size_t i = 0; i < md_.occupancy_buffer_.size(); ++i){if (md_.occupancy_buffer_[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 < md_.occupancy_buffer_.size(); ++i) {if (md_.occupancy_buffer_[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清除对应的膨胀标记（避免机器人仍认为该区域不可通行）。

最终结果：

室内仿真结果：

室外仿真结果：