> 项目来源：第四届3D视觉创新应用竞赛 | 作者：蒋永梁
> 硬件平台：NVIDIA Jetson Orin Nano 4G + Orbbec Gemini 2 深度相机
> 开源地址：[3DReconstruction_Gaussian-Splatting]
🧠 项目概述
在具身智能与机器人领域，如何让机器高效地“看懂”并“记住”三维环境，一直是个硬核挑战。传统的 SLAM 方案通常采用隐式体素网格或点云来表示地图，但它们在追求高精度时，往往会变成“算力吞噬兽”。
本项目另辟蹊径，采用前沿的 3D Gaussian Splatting (3DGS) 技术结合 SLAM，让机器人在未知场景中边走边建图。相比传统表征，3DGS 不仅能在保持高保真度的同时大幅降低渲染开销，还能直接输出极具真实感的 3D 场景，为机器人的自主导航与环境感知提供了既好看又好用的空间智能基础。
🏗️ 系统架构
本系统采用“数据采集-预处理-重建-导航”的闭环流水线，依托 ROS 1 (Noetic) 在边缘端实现高效协同：
| 模块层级 | 核心组件 | 功能定位 |
|---|---|---|
| 感知层 | Orbbec Gemini 2 / 激光雷达 | 采集 RGB-D 图像与 2D 激光扫描 |
| 决策与控制层 | ROS Navigation / topic1.yaml |
速度指令下发与路径规划 |
| 定位与滤波层 | robot_pose_ekf / Laser Filters |
融合里程计与激光，输出高鲁棒位姿 |
| 数据处理层 | convert.py |
ROS Bag 数据解包，转存 TUM 标准格式 |
| 重建层 | GS-SLAM | 基于 3DGS 的实时场景重建 |
| 地图层 | map1.yaml (Occupancy Grid) |
生成 2D 栅格地图供导航使用 |
🧩 核心模块说明
1. 🚗 运动控制与位姿追踪
– 速度指令驱动：通过 topic1.yaml 配置机器人的线速度与角速度（如 linear.x: 2.0, angular.z: 2.0），实现“前进-旋转-后退”的扫查式轨迹，确保无死角采集。
– 高精度位姿获取：topic2.yaml 记录了 EKF 融合后的机器人全局位姿（x, y, theta），为 3DGS 重建提供精确的相机外参初始值。
2. 🧹 激光雷达预处理
在 2D 导航中，雷达噪点是避障的致命伤。项目集成了强大的 laser_filters 链式滤波：
– 物理屏蔽：LaserScanBoxFilter 剔除机器人自身 footprint 内的点；
class LaserScanBoxFilter : public filters::FilterBase<sensor_msgs::LaserScan>
{
public:
LaserScanBoxFilter();
bool configure();
bool update(
const sensor_msgs::LaserScan& input_scan,
sensor_msgs::LaserScan& filtered_scan);
protected:
bool inBox(tf::Point &point);
std::string box_frame_;
laser_geometry::LaserProjection projector_;
// tf listener to transform scans into the box_frame
tf::TransformListener tf_;
// defines two opposite corners of the box
tf::Point min_, max_;
bool invert_filter;
bool up_and_running_;
std::shared_ptr<dynamic_reconfigure::Server<BoxFilterConfig>> dyn_server_;
void reconfigureCB(BoxFilterConfig& config, uint32_t level);
boost::recursive_mutex own_mutex_;
BoxFilterConfig config_ = BoxFilterConfig::__getDefault__();
};– 角度裁剪：LaserScanAngularBoundsFilter 只保留前方有效视场；
class LaserScanAngularBoundsFilter : public filters::FilterBase<sensor_msgs::LaserScan>
{
public:
double lower_angle_;
double upper_angle_;
bool configure()
{
lower_angle_ = 0;
upper_angle_ = 0;
if(!getParam("lower_angle", lower_angle_) || !getParam("upper_angle", upper_angle_)){
ROS_ERROR("Both the lower_angle and upper_angle parameters must be set to use this filter.");
return false;
}
return true;
}
virtual ~LaserScanAngularBoundsFilter(){}
bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& filtered_scan){
filtered_scan.ranges.resize(input_scan.ranges.size());
filtered_scan.intensities.resize(input_scan.intensities.size());
double start_angle = input_scan.angle_min;
double current_angle = input_scan.angle_min;
ros::Time start_time = input_scan.header.stamp;
unsigned int count = 0;
//loop through the scan and truncate the beginning and the end of the scan as necessary
for(unsigned int i = 0; i < input_scan.ranges.size(); ++i){
if(input_scan.angle_increment > 0){ //if the laserscanner turns counterclockwise
//wait until we get to our desired starting angle
if(start_angle < lower_angle_){
start_angle += input_scan.angle_increment;
current_angle += input_scan.angle_increment;
start_time += ros::Duration(input_scan.time_increment);
}
else{
filtered_scan.ranges[count] = input_scan.ranges[i];
//make sure that we don't update intensity data if its not available
if(input_scan.intensities.size() > i)
filtered_scan.intensities[count] = input_scan.intensities[i];
count++;
//check if we need to break out of the loop, basically if the next increment will put us over the threshold
if(current_angle + input_scan.angle_increment > upper_angle_){
break;
}
current_angle += input_scan.angle_increment;
}
}
else{ //the laserscanner turns clockwise
//wait until we get to our desired starting angle
if(start_angle > upper_angle_){
start_angle += input_scan.angle_increment;
current_angle += input_scan.angle_increment;
start_time += ros::Duration(input_scan.time_increment);
}
else{
filtered_scan.ranges[count] = input_scan.ranges[i];
//make sure that we don't update intensity data if its not available
if(input_scan.intensities.size() > i)
filtered_scan.intensities[count] = input_scan.intensities[i];
count++;
//check if we need to break out of the loop, basically if the next increment will put us over the threshold
if(current_angle + input_scan.angle_increment < lower_angle_){
break;
}
current_angle += input_scan.angle_increment;
}
}
}
//make sure to set all the needed fields on the filtered scan
filtered_scan.header.frame_id = input_scan.header.frame_id;
filtered_scan.header.stamp = start_time;
filtered_scan.angle_min = start_angle;
filtered_scan.angle_max = current_angle;
filtered_scan.angle_increment = input_scan.angle_increment;
filtered_scan.time_increment = input_scan.time_increment;
filtered_scan.scan_time = input_scan.scan_time;
filtered_scan.range_min = input_scan.range_min;
filtered_scan.range_max = input_scan.range_max;
filtered_scan.ranges.resize(count);
if(input_scan.intensities.size() >= count)
filtered_scan.intensities.resize(count);
ROS_DEBUG("Filtered out %d points from the laser scan.", (int)input_scan.ranges.size() - (int)count);
return true;
}
};– 阴影与强度去噪：ScanShadowsFilter 与 LaserScanIntensityFilter 强强联手，剔除高反与阴影噪点；
class ScanShadowsFilter : public filters::FilterBase<sensor_msgs::LaserScan>
{
public:
double laser_max_range_; // Used in laser scan projection
double min_angle_, max_angle_; // Filter angle threshold
int window_, neighbors_;
bool remove_shadow_start_point_; // Whether to also remove the start point of the shadow
ScanShadowDetector shadow_detector_;
std::shared_ptr<dynamic_reconfigure::Server<laser_filters::ScanShadowsFilterConfig>> dyn_server_;
boost::recursive_mutex own_mutex_;
ScanShadowsFilterConfig param_config;
ScanShadowsFilter();
virtual ~ScanShadowsFilter();
/**@b Configure the filter from XML */
bool configure();
void reconfigureCB(ScanShadowsFilterConfig& config, uint32_t level);
/** brief Filter shadow points based on 3 global parameters: min_angle, max_angle
* and window. {min,max}_angle specify the allowed angle interval (in degrees)
* between the created lines (see getAngleWithViewPoint). Window specifies how many
* consecutive measurements to take into account for one point.
* param scan_in the input LaserScan message
* param scan_out the output LaserScan message
*/
bool update(const sensor_msgs::LaserScan& scan_in, sensor_msgs::LaserScan& scan_out);
private:
float angle_increment_;
std::vector<float> sin_map_;
std::vector<float> cos_map_;
void prepareForInput(const float angle_increment);
};class LaserScanIntensityFilter : public filters::FilterBase<sensor_msgs::LaserScan>
{
public:
LaserScanIntensityFilter();
bool configure();
bool update(const sensor_msgs::LaserScan& input_scan, sensor_msgs::LaserScan& output_scan);
private:
std::shared_ptr<dynamic_reconfigure::Server<IntensityFilterConfig>> dyn_server_;
void reconfigureCB(IntensityFilterConfig& config, uint32_t level);
boost::recursive_mutex own_mutex_;
IntensityFilterConfig config_ = IntensityFilterConfig::__getDefault__();
};– 中值平滑：MultiChannelMedianFilterFloat 抹平突变异常值。
scan_filter_chain:
- type: laser_filters/LaserArrayFilter
name: laser_median_filter
params:
range_filter_chain:
- name: median_5
type: laser_filters/MultiChannelMedianFilterFloat
params:
number_of_observations: 5
unused: 10
intensity_filter_chain:
- name: median_5
type: laser_filters/MultiChannelMedianFilterFloat
params:
number_of_observations: 5
unused: 103. 🔄 数据格式桥梁
GS-SLAM 无法直接消费 ROS Bag，为此专门编写了 convert.py：
– 自动解包：从 Bag 中提取 /robot_pose_ekf/odom_combined 话题，解析四元数与平移向量。
– 标准对齐：将 RGB 图像、深度图和位姿对齐，输出标准的 TUM 数据集格式（timestamp tx ty tz qx qy qz qw），无缝对接 3DGS 训练流。
with open(groundtruth_txt_path, 'w') as txt_file_pose:
with open(rgb_txt_path, 'w') as txt_file_rgb:
with open(depth_txt_path, 'w') as txt_file_depth:
with rosbag.Bag(bag_path, 'r') as bag:
# Write header lines
txt_file_depth.write("# depth mapsn")
txt_file_depth.write(f"# file: {bag_path}n")
txt_file_depth.write("# timestamp filenamen")
txt_file_rgb.write("# rgb mapsn")
txt_file_rgb.write(f"# file: {bag_path}n")
txt_file_rgb.write("# timestamp filenamen")
txt_file_pose.write("# ground truth trajectoryn")
txt_file_pose.write(f"# file: {bag_path}n")
txt_file_pose.write("# timestamp tx ty tz qx qy qz qwn")
num_rgb = 0
num_depth = 0
num_ground =0
for topic,msg,t in bag.read_messages():
if topic == "/robot_pose_ekf/odom_combined":
position = msg.pose.pose.position
orientation = msg.pose.pose.orientation
txt_file_pose.write("{:.6f} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f} {:.4f}n".format(
t.to_sec(), position.x, position.y, position.z, orientation.x, orientation.y, orientation.z, orientation.w
))
num_ground+=1
if topic == "/camera/depth/image_raw":
# 深度图
cv_image = bridge.imgmsg_to_cv2(msg, '16UC1')
# 归一化
# normalized_depth = cv_image.astype(numpy.float32) / 2000 #numpy.max(cv_image)
# normalized_depth *= 255
# print(numpy.max(cv_image))
# cv_image = normalized_depth.astype(numpy.uint8)
timestr = "%.6f" % msg.header.stamp.to_sec() # 时间戳命名
# image_name = timestr# + '.png'# an extension is necessary
# image_name = str(num_depth) + '.png'# 编号命名
# cv2.imwrite(depth_path + image_name, cv_image)
# 实际应用可直接保存为 numpy array
npyPath = os.path.join(depth_path,timestr)
numpy.save(npyPath, cv_image)
# Write timestamp and filename to the text file
txt_file_depth.write("{:.6f} depth/{:.6f}.npyn".format(float(timestr), float(timestr)))
# print(npyPath) # Shell输出
if(num_depth==1):
print(numpy.array(cv_image).shape)
num_depth += 1
if topic == "/camera/color/image_raw":
# RGB
cv_image = bridge.imgmsg_to_cv2(msg, "bgr8")
timestr = "%.6f" % msg.header.stamp.to_sec()
image_name = timestr + '.png'# an extension is necessary
# image_name = str(num_rgb) + '.png'
# print(rgb_path + image_name) # Shell输出
image_path = os.path.join(rgb_path,image_name)
cv2.imwrite(image_path, cv_image)
# Write timestamp and filename to the text file
txt_file_rgb.write("{:.6f} rgb/{:.6f}.pngn".format(float(timestr), float(timestr)))
if(num_rgb==1):
print(numpy.array(cv_image).shape)
num_rgb +=1 4. 🗺️ 双重地图表达
– 3D 高斯地图：用于高逼真度渲染与精细环境理解；
– 2D 栅格地图：通过 map1.yaml（分辨率 0.05m，占用阈值 0.65）为 ROS NavStack 提供经典的 代价地图（Costmap） 导航依据。
📑 设计要点
根据项目设计文档提炼，本系统的核心设计思想如下：
1. 场景痛点：室内复杂环境（家庭、办公、救援）对机器人的感知与操作要求极高，传统 SLAM 难以兼顾精度与算力。
2. 表征革新：用 3D Gaussian Splatting 替代传统点云/体素，不仅内存占用更优，且支持可微渲染，让位姿估计与地图优化更加丝滑。
3. 软硬协同：选择 Jetson Orin Nano 作为边缘算力底座，在有限功耗下榨取 GPU 并行计算潜力，满足 3DGS 实时泼溅渲染的需求。
4. 实用性优先：3D 重建不只是为了“看”，更是为了“用”。系统保留并强化了 2D 激光 SLAM 与导航链路，确保机器人不会在炫技中迷失方向。
🚀 使用示例
以下为系统从数据采集到重建的典型工作流（初学者友好版）：
1. 启动底层与传感器
在 Jetson Orin Nano 上启动 ROS 驱动，获取 RGB-D 与激光数据。
2. 遥控采集数据
通过下发 topic1.yaml 中的速度序列，控制机器人在目标区域巡航，并使用 rosbag record 录制包含图像、深度与 EKF 位姿的数据包。
3. 数据格式转换
运行数据桥接脚本，将 ROS Bag 转换为 GS-SLAM 认识的 TUM 格式：
python convert.py4. 运行 3DGS 重建
喂入转换后的数据，启动 GS-SLAM 开始场景级的高斯泼溅重建，实时查看渲染视角。
5. 2D 导航部署
利用生成的 map1.yaml 栅格地图，配合 ROS move_base 或 nav2，让机器人在重建好的环境中实现自主避障导航。
🧭 后续规划
虽然 3DGS 赋予了系统惊艳的重建能力，但走向真正的具身智能还有几步路要走：
– ⚡ 边缘端极致优化：针对 Jetson 系列优化 3DGS 的 CUDA 算子，探索 TensorRT 加速，压低重建延迟。
– 🔄 闭环检测：目前依赖 EKF 里程计，长距离运行易产生漂移，后续计划引入回环检测修正全局高斯地图。
– 🏃 动态物体抗干扰：3DGS 对动态物体（如走动的人）较为敏感，需结合语义分割剔除动态高斯，打造纯粹的静态先验地图。
– 🤖 Nav2 深度集成：将 3DGS 渲染的深度图直接用于局部代价地图更新，实现基于视觉 3D 重建的精细避障。
> 让机器人不仅能看懂三维世界，更能在这个世界里游刃有余——我们正在路上！