处理器：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 测试
代码分析：
启动文件参数理解：
bash
<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相互配合使用，实现了虚拟墙功能中的地面高度限制逻辑：
cpp
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以下代码虚拟地面和天花板的代码并没有实现这个功能：
cpp
// 发布原始占据地图
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 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);
}它只是选择在该虚拟区域内有障碍物的保留，超出该虚拟区域的就不计算在内，虚拟天花板和虚拟地面都没有让其现实为有障碍物，用来限制无人机的规划区域。
因此需要进行修改：
cpp
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 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_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_));
}
}
}
}比如我尝试设置如下参数：
bash
<!-- 虚拟地面高度 -->
<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 更新范围的单位转换:
cpp
// 将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，向上取整，一般局部更新范围设置成整数就绝对没有什么问题了。分辨率的话，最好是能够被范围除尽的。
环形缓冲区计算：
cpp
// 环形缓冲区大小（包含膨胀区域）
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）的数学变换:
cpp
// 计算概率的对数表示（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变换可以缓解这些问题。
基本初始化参数的设置，不做赘述：
cpp
// 初始化数据缓冲区
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;相机外参矩阵和外参订阅：
cpp
// 相机到机器人本体的外参矩阵(固定变换)
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_)
作用
- 坐标系转换：将点从相机坐标系转换到机器人本体坐标系。
- 固定变换关系：描述相机相对于机器人本体的位置和姿态（旋转 + 平移）。
矩阵解析
cpp
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）：
cpp
R = [ 0 0 1 ]
[-1 0 0 ]
[ 0 -1 0 ]-
表示相机绕 Z 轴旋转 90°，再绕 X 轴旋转 90°（相当于相机朝前，但上下颠倒）。
-
平移部分（右上角 3×1）：
cpp
t = [ 0, 0, 0 ]^T- 表示相机安装在机器人本体的原点（无平移偏移）。
实际意义
- 若相机朝上安装在机器人顶部，这个矩阵会将相机检测到的 "上方" 障碍物转换到本体坐标系的 "前方"。
/vins_estimator/extrinsic 话题：
作用
- 动态外参更新：接收来自 VINS（视觉惯性导航系统）的相机外参估计。
- 在线校准：在机器人运行过程中实时优化相机与本体的相对位姿。
回调函数 extrinsicCallback
当接收到 /vins_estimator/extrinsic 话题的消息时，会调用该函数更新 md_.cam2body_ 矩阵。比如代码中的：
cpp
// 外参回调函数(更新相机到本体的变换)
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话题：提供更精确的动态外参估计，提升地图构建的准确性。
同步深度图像和里程计：
cpp
// 根据姿态类型选择不同的订阅方式
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位姿信息进行同步的方式目前没用上，不赘述，对采用里程计和深度图像同步的方式进行理解：
在机器人导航中，深度相机提供环境障碍物信息，而位姿 / 里程计提供机器人自身位置。但两者通常由不同传感器以不同频率发布，时间戳可能不一致。
关键组件：
-
消息过滤器（Message Filters）：ROS 提供的时间同步工具，支持多话题消息的时间对齐。
-
同步策略（SyncPolicyImageOdom）：定义如何匹配深度图和里程计消息，通常基于时间戳：
-
常见策略：
ApproximateTime：允许时间戳在一定范围内匹配（更常用）。ExactTime：要求时间戳完全一致（严格但不实用）。
-
缓存大小（100） ：
存储最近的 100 条消息，用于寻找最佳匹配对。
注册回调函数registerCallback:
当同步器找到匹配的深度图和里程计消息时，会调用
depthOdomCallback，这个回调函数是一个处理深度图像和里程计数据的回调函数，用于同时接收和处理这两种传感器数据。
cpp
// 深度图像和里程计消息回调函数
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_参数用于控制处理的像素间隔，提高处理效率
姿态信息处理：
cpp
/* 从里程计消息中提取相机姿态 */
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 - 提取相机在世界坐标系中的位置和旋转矩阵，存储在类成员变量中
深度图像处理：
cpp
/* 处理深度图像(与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_ - 将处理后的深度图像存储在类成员变量中
投影点容器初始化：
cpp
// 初始化投影点容器(仅首次调用)
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_计算，用于存储后续处理中的投影点
状态标记：
cpp
// 标记地图需要更新
md_.occ_need_update_ = true;
md_.flag_have_ever_received_depth_ = true;- 设置
occ_need_update_标志，表示需要更新占用地图 - 设置
flag_have_ever_received_depth_标志，表示已经接收到过深度图像
独立的里程计和点云订阅（备用数据源）：
cpp
// 独立的里程计和点云订阅(备用数据源)
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) 实现建图，不要求数据严格同步。这种方式灵活性更高，但精度可能较低。 - 由于没有相对应的备用的点云和里程计的输入，所以这个话题目前是没有用上的。
地图更新定时器：
cpp
occ_timer_ = node_.createTimer(ros::Duration(0.032), &GridMap::updateOccupancyCallback, this); // 地图更新定时器(31.25Hz)
updateOccupancyCallback是一个定时调用的地图更新函数，主要用于根据深度图像和机器人位姿信息更新占据栅格地图。该函数是移动机器人环境感知和导航的核心模块之一。
cpp
// 地图占据状态更新回调函数（定时调用）
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函数是地图更新流程中的数据有效性检查模块，主要用于判断是否有新的深度图像和里程计数据需要处理，以及传感器数据是否超时。
cpp
// 检查是否需要更新地图(数据有效性检查)
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、初始化最后更新时间：
cpp
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、数据有效性与超时检测：
cpp
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、时间性能统计，用于调试优化：
cpp
/* update occupancy */ // 更新占据状态
ros::Time t1, t2, t3, t4, t5;
t1 = ros::Time::now();1.4、环形缓冲区移动：
cpp
// 移动环形缓冲区（实现动态窗口更新）
moveRingBuffer();
cpp
// 移动环形缓冲区(实现动态窗口)
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、确定当前窗口中心
cpp
Eigen::Vector3i center_new = pos2GlobalIdx(md_.camera_pos_);- 以相机当前位置为中心，将世界坐标（米）转换为地图网格索引（离散坐标），作为动态窗口的中心。
- 例如：相机在 (2.5, 1.0, 0.5) 米，地图分辨率 0.1 米 / 格 → 中心索引为 (25, 10, 5)。
2、定义窗口边界（有效区域）
cpp
// 计算新的更新范围(网格索引)
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、随移动清除旧数据
cpp
// 根据相机移动方向清除旧数据
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、环形缓冲区的"移动"：索引调整
cpp
// 更新环形缓冲区索引
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、保存状态：为下一次移动做准备
cpp
// 保存当前中心位置和边界
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空间
cpp
projectDepthImage();代码如下：
cpp
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 frame
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;
// 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、初始化与参数准备
cpp
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_）**
降采样遍历像素：
cpp
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）。
深度值处理与过滤：
cpp
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点：
cpp
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坐标（相机坐标系，即深度）核心原理：基于针孔相机模型的逆运算。
相机坐标系到世界坐标系：
cpp
proj_pt = camera_r * proj_pt + md_.camera_pos_;坐标转换逻辑：相机坐标系下的 3D 点需要通过相机的位姿转换到世界坐标系：
- 先通过旋转矩阵
camera_r旋转（对齐世界坐标系的朝向）； - 再通过平移向量
camera_pos_平移（对齐世界坐标系的原点）
存储点云：
cpp
md_.proj_points_[md_.proj_points_cnt_++] = proj_pt; // 存入点云容器并计数4、启用深度滤波**（mp_.use_depth_filter_）**
cpp
if (!md_.has_first_depth_)
md_.has_first_depth_ = true; // 首次处理时仅标记，无滤波（需上一帧数据参考）
else { ... } // 非首次处理时启用滤波滤波需要 "上一帧的深度图像和相机位姿" 作为参考，因此第一帧不滤波。
边缘过滤与降采样：
cpp
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_个像素（边缘像素可能因镜头畸变导致深度不准）。
深度阈值过滤：
cpp
if (depth < mp_.depth_filter_mindist_) continue; // 过滤过近深度（阈值可配置）比无滤波分支更灵活：最小深度阈值depth_filter_mindist_可通过参数配置（而非固定 0.1 米）。
像素坐标点转向三维点云：
cpp
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;可选：与上一帧的一致性检查（当前禁用）：
cpp
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)包裹的部分是一个未启用的优化逻辑，原理是：
- 将当前帧的世界坐标系点反投影到上一帧的深度图像中，检查上一帧对应位置的深度是否与当前帧一致。
- 若不一致（如动态物体移动、噪声），则过滤该点，减少动态干扰。
保存当前帧数据供下一帧参考：
cpp
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、执行射线投射处理
cpp
// 执行射线投射处理
raycastProcess();
cpp
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）更新每个栅格的占据概率。
算法核心原理：
- 激光束穿透性：射线在遇到障碍物前会一直传播（经过的区域为空闲）。
- 最后击中原则：射线的终点（深度值对应的点）为障碍物表面（占据）。
初始化与计数器重置：
cpp
md_.cache_voxel_cnt_ = 0; // 重置缓存栅格计数器
md_.raycast_num_ += 1; // 射线投射次数计数器+1缓存机制：将待更新的栅格暂存，避免重复操作，提高效率。
对每个投影点进行射线投射：
cpp
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)：标记栅格为 "可能空闲"（射线路径上的栅格）。
概率更新（对数几率模型）：
cpp
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、清除旧数据并膨胀障碍物
cpp
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); // 清除障碍物及膨胀标记
}
}可视化定时器：
cpp
vis_timer_ = node_.createTimer(ros::Duration(0.125), &GridMap::visCallback, this); // 可视化定时器(8Hz)发布原始地图和膨胀后的地图：
cpp
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);
}
}占据概率衰减定时器：
cpp
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 < 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、计算衰减步长：
cpp
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、定义低阈值：
cpp
const double low_thres = mp_.clamp_min_log_ + reduce;- 作用 ：只对概率高于
low_thres的栅格进行衰减（避免对已接近 "非占据" 的栅格过度处理）。 clamp_min_log_是占据概率的最小值（对数几率形式，避免概率无限减小），low_thres确保只处理 "仍有较大概率是障碍物" 的栅格。
1.3、遍历栅格并衰减概率：
cpp
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清除对应的膨胀标记（避免机器人仍认为该区域不可通行）。
最终结果：
室内仿真结果：
ZED2双目视觉相机进行室内感知定位建图
室外仿真结果：
ZED2双目相机实现Vins Fusion && Ray Casting 感知建立局部导航地图