處理器：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清除對應的膨脹標記（避免機器人仍認為該區域不可通行）。

最終結果：

室內仿真結果：

室外仿真結果：