一台 Astra Pro 扫过宿舍，室内 3D 地图竟然“长”出来了？！🧭

1. 项目速览：让相机边走边“记住”房间 🏠

这个项目面向室内三维重建任务，使用 Astra Pro RGB-D 相机采集彩色图像与深度图，在 Zore P1 开发板和 ROS 环境中运行改造版 ORB-SLAM2，并加入在线彩色点云地图模块，实现相机运动过程中的室内空间重建。

工程主干实际采用的是 ORB-SLAM2 的 RGB-D 位姿估计 + 关键帧点云融合：ORB 特征、局部建图、回环检测与图优化负责相机轨迹；点云模块根据关键帧位姿把 RGB-D 数据反投影到统一世界坐标系中。它和 ICP 的目标一致，都是把多帧观测配准到同一个空间里，只是本工程的核心配准依据主要来自视觉 SLAM 位姿，而不是直接调用 PCL ICP。

项目要素	内容
来源	第一届 3D 视觉创新应用竞赛参赛选手苏云征
相机型号	Astra Pro
支持设备	Zore P1 开发板
运行环境	ROS、ORB-SLAM2、OpenCV、PCL、Pangolin、Eigen、DBoW2、g2o
核心输出	相机轨迹、关键帧轨迹、彩色 PCD 点云地图
源代码链接	3DReconstruction_ICP
复现教程	Jetson Orin Nano+Orbbec Femto Mega

2. 为什么最后选 ORB-SLAM2？因为实时重建不只要“拼得上”，还要“跑得动” ⚡

项目报告中提到，团队在时间和质量之间进行权衡后，选择 ORB-SLAM2 作为主要算法框架，并通过 ROS 接口接入 Astra Pro。这个选择很务实：室内三维重建不是单帧拍照，而是持续运动中的多帧对齐；如果每一步都慢吞吞，房间还没扫完，系统先喘了😩。

技术选择	作用	项目意义
ORB 特征	提取稳定、计算较快的图像关键点	支撑实时跟踪
RGB-D 输入	同时提供颜色与深度	让特征点具备空间尺度
ROS 接口	订阅相机 RGB/Depth topic	简化硬件接入
PCL 点云地图	显示与保存重建结果	让重建成果可观察、可复用
g2o 图优化	优化相机位姿与地图结构	降低漂移

3. 系统流程：从 ROS 图像流到 PCD 点云地图 🧩

系统可以拆成三层：传感器层负责“看见”，SLAM 层负责“知道自己在哪”，点云层负责“把看见的东西放到正确位置”。三层各司其职，像一支小型室内测绘队。

层级	输入	处理	输出
采集层	Astra Pro RGB/Depth	ROS topic 发布、时间同步	同步 RGB-D 帧
SLAM 层	RGB-D 帧、相机参数	ORB 特征、跟踪、局部建图、回环	相机位姿与关键帧
点云层	关键帧 RGB-D、位姿	深度反投影、坐标变换、滤波	彩色全局点云
保存层	全局点云与轨迹	`Shutdown()` 后写文件	`result.pcd`、轨迹 txt

4. ROS 接入：把 Astra Pro 的数据送进 SLAM “胃里” 🍽️

ROS 版本的 RGB-D 节点位于 Examples/ROS/ORB_SLAM2/src/ros_rgbd.cc。它会读取配置文件中的 rgb_topic 与 depth_topic，用 message_filters::Synchronizer 对两路图像做近似时间同步，然后把图像传给 TrackRGBD()。

// 节选自 ros_rgbd.cc：订阅 RGB 与 Depth，并进行近似时间同步
string rgb_topic(fsSettings["rgb_topic"]);
string depth_topic(fsSettings["depth_topic"]);

message_filters::Subscriber<sensor_msgs::Image> rgb_sub(nh, rgb_topic, 1);
message_filters::Subscriber<sensor_msgs::Image> depth_sub(nh, depth_topic, 1);

typedef message_filters::sync_policies::ApproximateTime<
    sensor_msgs::Image,
    sensor_msgs::Image
> sync_pol;

message_filters::Synchronizer<sync_pol> sync(sync_pol(10), rgb_sub, depth_sub);
sync.registerCallback(boost::bind(&ImageGrabber::GrabRGBD, &igb, _1, _2));

同步后的回调很直接：ROS 图像消息经 cv_bridge 转成 OpenCV 矩阵，然后送入 ORB-SLAM2。这里的简洁非常可贵，毕竟工程世界里，能少绕路就少掉不少坑。

// 节选自 ros_rgbd.cc：把同步图像送入 ORB-SLAM2
void ImageGrabber::GrabRGBD(
    const sensor_msgs::ImageConstPtr &msgRGB,
    const sensor_msgs::ImageConstPtr &msgD)
{
    cv_bridge::CvImageConstPtr cv_ptrRGB = cv_bridge::toCvShare(msgRGB);
    cv_bridge::CvImageConstPtr cv_ptrD = cv_bridge::toCvShare(msgD);

    mpSLAM->TrackRGBD(
        cv_ptrRGB->image,
        cv_ptrD->image,
        cv_ptrRGB->header.stamp.toSec()
    );
}

5. 相机参数：重建的第一把尺子 📏

配置文件 astra.yaml 中保存了 Astra Pro 的内参、畸变参数、图像尺寸、帧率、深度尺度、ORB 参数与点云分辨率。对于三维重建来说，这些参数不是“装饰品”，而是坐标计算的地基。

# 节选自 astra.yaml：Astra Pro 相机与点云参数
Camera.fx: 608.175105
Camera.fy: 608.398081
Camera.cx: 332.108897
Camera.cy: 241.674149

Camera.width: 640
Camera.height: 480
Camera.fps: 30.0

DepthMapFactor: 1.0
PointCloudMapping.Resolution: 0.005

rgb_topic: /camera/rgb/image_raw
depth_topic: /camera/depth/image

参数类别	代表字段	作用
内参	`fx/fy/cx/cy`	像素点反投影为三维坐标
畸变	`k1/k2/p1/p2`	修正镜头成像误差
深度尺度	`DepthMapFactor`	将深度图转为实际距离
点云分辨率	`PointCloudMapping.Resolution`	控制体素滤波精细程度
ROS topic	`rgb_topic/depth_topic`	指定相机数据来源

6. SLAM 主干：ORB-SLAM2 如何稳住相机轨迹 🧠

ORB-SLAM2 的任务是估计相机在室内运动时的姿态。它主要依赖 ORB 特征：先在图像中找角点，再计算描述子，随后通过匹配、PnP、局部 BA、回环检测等步骤持续修正轨迹。报告中也总结了 ORB-SLAM2 的几个优势：实时性较好，支持共视图与 Essential Graph，具备回环检测能力，对重定位和短时跟踪丢失更鲁棒。

ORB-SLAM2 组件	解决的问题	对重建的贡献
Tracking	当前帧位姿估计	决定每帧点云放在哪里
Local Mapping	局部地图点维护	提高局部位姿稳定性
Loop Closing	回环检测与位姿校正	减少长时间扫描漂移
ORB Vocabulary	场景识别与重定位	支持闭环和跟踪恢复
g2o Optimizer	图优化	让轨迹更一致

源码中 Tracking::GrabImageRGBD() 会接收 RGB-D 图像，处理颜色通道与深度尺度，然后创建当前帧。

// 节选自 Tracking.cc：RGB-D 图像进入 Tracking
cv::Mat Tracking::GrabImageRGBD(
    const cv::Mat &imRGB,
    const cv::Mat &imD,
    const double &timestamp)
{
    mImGray = imRGB;
    mImDepth = imD;

    if ((fabs(mDepthMapFactor - 1.0f) > 1e-5) ||
        mImDepth.type() != CV_32F)
    {
        mImDepth.convertTo(mImDepth, CV_32F, mDepthMapFactor);
    }

    mCurrentFrame = Frame(
        mImGray, mImDepth, timestamp,
        mpORBextractorLeft, mpORBVocabulary,
        mK, mDistCoef, mbf, mThDepth
    );

    Track();
    return mCurrentFrame.mTcw.clone();
}

7. “ICP 配准”怎么理解？这里更像“SLAM 位姿驱动的点云配准” 🧲

传统 ICP 会直接最小化两帧点云之间的几何距离，常见形式如下：

// ICP 配准伪代码：直接让两帧点云几何对齐
T = Identity()
repeat until convergence:
    matched_pairs = findNearestNeighbors(source_cloud, target_cloud)
    T_delta = solveRigidTransform(matched_pairs)
    source_cloud = transform(source_cloud, T_delta)
    T = T_delta * T
return T

本项目源码没有把 PCL ICP 作为核心调用，而是让 ORB-SLAM2 先估计关键帧位姿，再用关键帧位姿把点云变换到全局坐标系。这相当于：配准矩阵不是直接由点云最近邻优化得来，而是由视觉 SLAM 的相机轨迹提供。它的工程好处是实时性更好，也能利用 ORB-SLAM2 的回环与图优化能力；不足是点云质量强烈依赖视觉跟踪是否稳定。

对齐方式	主要依据	优点	局限
传统 ICP	点云几何最近邻	几何解释直观	计算量较大，易受初值影响
本项目主线	ORB-SLAM2 关键帧位姿	实时性好，支持回环	依赖纹理与特征跟踪
可扩展方案	SLAM 位姿 + ICP 微调	精度潜力更高	工程复杂度增加

8. 关键帧点云：把二维图像“搬”进三维世界 🌍

当系统决定创建新关键帧时，源码会把当前 RGB 图像和深度图存进关键帧对象，再通知 PointCloudMapping 更新点云。

// 节选自 Tracking.cc：创建关键帧时保存 RGB-D 数据并通知点云模块
pKF->img0 = this->mImGray.clone();
pKF->img1 = this->mImDepth.clone();

mpLocalMapper->InsertKeyFrame(pKF);

// insert Key Frame into point cloud viewer
mpPointCloudMapping->insertKeyFrame(pKF);

点云生成的核心在 PointCloudMapping::generatePointCloud()。它遍历深度图，根据相机内参把像素点转成三维点，并把颜色写入点云。为了减小计算量，源码每隔 3 个像素采样一次，这属于典型的工程取舍：少一点密度，换一点速度。

// 节选自 pointcloudmapping.cc：由关键帧 RGB-D 生成彩色点云
for (int m = 0; m < depth.rows; m += 3)
{
    for (int n = 0; n < depth.cols; n += 3)
    {
        float d = depth.ptr<float>(m)[n];
        if (d < 0.01 || d > 10)
            continue;

        PointT p;
        p.z = d;
        p.x = (n - kf->cx) * p.z / kf->fx;
        p.y = (m - kf->cy) * p.z / kf->fy;

        p.b = color.ptr<uchar>(m)[n * 3];
        p.g = color.ptr<uchar>(m)[n * 3 + 1];
        p.r = color.ptr<uchar>(m)[n * 3 + 2];

        tmp->points.push_back(p);
    }
}

随后，系统会根据关键帧位姿把点云从相机坐标系变换到世界坐标系。这个步骤就是“多帧拼接”的关键：如果位姿准，点云就乖乖归队；如果位姿漂，点云就开始表演空间错位。

// 节选自 pointcloudmapping.cc：利用关键帧位姿变换点云
Eigen::Isometry3d T =
    ORB_SLAM2::Converter::toSE3Quat(kf->GetPose());

PointCloud::Ptr cloud(new PointCloud);
pass.setInputCloud(tmp);
pass.filter(*cloud);
tmp->swap(*cloud);

pcl::transformPointCloud(
    *tmp,
    *cloud,
    T.inverse().matrix()
);

9. 点云滤波与地图更新：别让远处噪声来“凑热闹” 🧹

点云地图模块使用了两种简单但有效的 PCL 滤波：PassThrough 删除过远点，VoxelGrid 进行体素降采样。项目 README 中也提到，加入 PassThrough 是为了去除远处不准确的点；而 voxel 滤波则用于压缩点云规模，让显示和保存更稳定。

// 节选自 pointcloudmapping.cc：构造点云模块时设置滤波器
PointCloudMapping::PointCloudMapping(float resolution_, Map *map)
    : mpMap(map)
{
    this->resolution = resolution_;
    voxel.setLeafSize(resolution, resolution, resolution);

    pass.setFilterFieldName("z");
    pass.setFilterLimits(0.0, 3.0);

    globalMap = boost::make_shared<PointCloud>();
    viewerThread = make_shared<thread>(
        bind(&PointCloudMapping::viewer, this)
    );
}

全局地图更新时，系统会重新取出所有关键帧，逐帧生成点云并累加，最后做体素滤波。这样可以在回环优化后重新生成更一致的地图，避免“轨迹改了，点云还留在旧位置”的尴尬。

// 节选自 pointcloudmapping.cc：重新生成全局点云地图
void PointCloudMapping::generateMap()
{
    keyframes = mpMap->GetAllKeyFrames();
    globalMap->clear();

    for (size_t i = 0; i < keyframes.size(); i++)
    {
        PointCloud::Ptr p = generatePointCloud(
            keyframes[i],
            keyframes[i]->img0,
            keyframes[i]->img1
        );
        *globalMap += *p;
    }

    PointCloud::Ptr tmp(new PointCloud());
    voxel.setInputCloud(globalMap);
    voxel.filter(*tmp);
    globalMap->swap(*tmp);
}

10. 可视化与保存：点云终于露脸了 🖥️

项目文件夹中已有一张效果图：pic/宿舍.png。图中可以看到 PCD viewer 显示的宿舍彩色点云，床、墙面、家具与局部空间结构已经被重建出来，同时也能看到点云稀疏和局部缺失，这正是 RGB-D 重建非常真实的一面：它能快速还原空间，但不会假装自己是完美扫描仪。

系统停止时会保存点云和轨迹。ROS 节点订阅 /RGBD/cmd，收到 "stop" 后关闭；Shutdown() 会关闭点云线程，点云模块最终保存 result.pcd。

// 节选自 ros_rgbd.cc：通过 topic 停止运行
void cmdCallback(const std_msgs::String::ConstPtr &cmd)
{
    if (cmd->data == "stop")
        ros::shutdown();
}

// 节选自 pointcloudmapping.cc：退出时保存 PCD 文件
void PointCloudMapping::viewer()
{
    pcl::visualization::CloudViewer viewer("PointCloud Viewer");

    while (true)
    {
        if (shutDownFlag)
            break;

        generateMap();
        viewer.showCloud(globalMap);
    }

    savePointCloud("result.pcd");
}

运行说明中也提供了保存命令：

rostopic pub /RGBD/cmd std_msgs/String "data: 'stop'"
pcl_viewer result.pcd

11. 工程文件怎么分工？一张表看懂 🗂️

文件/目录	作用
`README.md`	说明项目基于 ORB-SLAM2 点云地图版本，并列出改动
`CMakeLists.txt`	编译 ORB-SLAM2 主库、RGB-D 示例与点云依赖
`Examples/ROS/ORB_SLAM2`	ROS RGB-D 节点、配置和运行脚本
`astra.yaml`	Astra Pro 相机参数、ORB 参数、ROS topic
`src/Tracking.cc`	RGB-D 输入处理、跟踪、关键帧创建
`src/pointcloudmapping.cc`	关键帧点云生成、滤波、地图显示与保存
`include/pointcloudmapping.h`	点云地图模块的数据结构与接口
`Thirdparty/DBoW2`	ORB 词袋，用于回环与重定位
`Thirdparty/g2o`	图优化库，用于位姿优化
`pic/宿舍.png`	项目点云重建效果图

12. 实验现象与问题：能重建，但别拿它当魔法棒 🛠️

项目报告中提到，系统可以通过 ROS topic 获取相机数据并完成简单室内三维重建；但在开发板上运行时，受性能限制影响，运动需要尽量缓慢，点云地图显示和保存也曾出现卡顿、无法保存等问题。后续通过移除或调整部分 voxel 滤波、修复 ROS 编译依赖、添加 PCL 依赖等方式逐步解决。

现象	原因	建议
小范围移动效果较好	帧间重叠高，位姿估计稳定	采集时缓慢移动
远处点云不准	深度传感器远距离噪声增大	使用 PassThrough 限制深度范围
点云显示卡顿	点云过密、开发板算力有限	提高体素尺寸或降低关键帧频率
回环后点云需更新	位姿图优化改变关键帧位姿	重新遍历关键帧生成全局点云
纹理弱区域跟踪困难	ORB 特征不足	增加环境纹理或引入几何 ICP 微调

13. 技术亮点：这套系统不花哨，但很工程 🧪

亮点	解读
ROS 接入简洁	直接订阅 RGB/Depth topic，便于连接 Astra Pro
ORB-SLAM2 框架成熟	跟踪、建图、回环和优化链路完整
在线点云地图	扫描过程中即可看到彩色三维点云
支持 PCD 保存	结果可用 `pcl_viewer` 二次查看
兼顾小型设备	面向 Zore P1 这类开发板进行工程尝试
保留扩展空间	可继续叠加 ICP 微调、TSDF 融合、稠密网格化

14. 后续优化方向：从“能看见房间”到“看得又稳又细” 🔧

优化方向	具体做法	预期收益
ICP 微调	在 ORB-SLAM2 位姿基础上，对相邻关键帧点云做局部 ICP	提升几何对齐精度
动态关键帧策略	根据运动幅度、跟踪质量决定是否建图	减少冗余点云
深度质量过滤	根据深度范围、边缘、空洞剔除低质量点	降低噪声
TSDF 融合	将点云堆叠升级为空间体素融合	生成更连续模型
地图分块	分区域保存与加载点云	降低内存压力
轻量化部署	调整 ORB 特征数、点云分辨率、显示频率	更适配开发板

15. 总结：所谓室内重建，就是让每一帧都找到自己的“座位” 🎯

这个系统的核心价值在于把 Astra Pro 的 RGB-D 数据、ROS 工程接口、ORB-SLAM2 实时位姿估计和 PCL 彩色点云地图串成了一条可运行链路。它不是单纯展示算法公式，而是把“相机怎么接入、轨迹怎么估计、点云怎么生成、结果怎么保存”这些工程问题都走了一遍。

如果用一句话概括：它让相机一边移动，一边把看到的室内空间放进三维地图里。虽然现阶段在开发板性能、点云密度、弱纹理场景和几何精度上仍有优化空间，但作为一个基于 RGB-D SLAM 的室内三维重建原型，它已经把“能跑、能看、能存”的主流程打通了。室内空间被一点点“拼”出来的那一刻，确实很有 3D 视觉的快乐。