一小时让你成为点云建图小将（固定帧数法选取关键帧）

创作时间：2021年11月1日

---

摘要

• 当今科技发展速度飞快，无论是无人驾驶，还是虚拟现实，诸多领域都需要进行建图，并在此基础上进行开发处理。地图主要分为稀疏地图、稠密地图和语义地图，它们有着不同的优缺点以及应用，其中稠密地图占据着一个非常重要的位置，它能应用于导航、避障、交互等工作。

• 本文围绕着稠密点云建图这个话题展开，在Ubuntu-18.04的操作系统下利用TUM数据集进行RGBD稠密建图。文章首先简单地说明了稠密建图的原理，然后简述了RGBD相机，接着概述了TUM数据集，讲解了它的一些特点以及如何下载和使用数据集。接着，文章介绍了建图所需要用到的开源库，如OpenCV、Eigen等等，从简介、特点、安装、教程四个方面阐述，并给出学习这些库的网站。之后，文章讲述了本人自己写的一个基于C++的简单基于固定帧数选取法和窗口压缩法建图代码¹的框架，且对部分代码进行了详细地分析，包括说明需要用到上述的哪些开源库，并呈现出建图的效果。最后，文章简单地总结了自己写的建图代码，并给出了未来探索的方向。

• 希望读者在阅读完本篇报告后能有一些收获，成为建图小将，同时，非常高兴读者能指出文章的不足之处，给予宝贵的建议。

---

建图原理

• 稠密建图主要分为单目稠密建图和RGBD稠密建图。其中RGBD稠密建图使用了RGBD相机²进行拍摄，它可以直接测量出像素点的深度，不必像单目稠密建图那样需要通过特征点匹配、三角测量、极线搜索和块匹配对深度进行计算。
• 实时稠密建图是从一张张采集的图片出发，利用像素点的RGB信息和深度信息，将像素点从像素坐标转换为相机坐标，再根据相机的位姿将相机坐标转换为世界坐标，从而生成三维的地图点。
• 建图所生成的点云质量受相机的内参、图片的质量、相机位姿的准确度等方面影响。
• 建图不能也不需要完全利用每张图片的信息，往往只是对关键帧进行处理。

---

RGBD相机

1. 简介

• RGB-D相机深度相机，又称深度相机，是2010年前后兴起的 一种相机，它可以通过红外结构光或ToF 原理测出物体与相机之间的距离（在建图中，物体与相机之间的距离非常重要）。

• 目前常用的 RGB-D 相机包括 Kinect/Kinect V2、Xtion Pro Live、RealSense等，在一些手机上人们也用它来识别人脸。

2. 特点

• 计算量小：RGB-D相机通过物理手段测量出像素的深度，节省了大量的计算资源 。
• 技术尚未成熟：现在多数RGB-D相机还存在测量范围窄、噪声大、 视野小、易受日光干扰、无法测量透射材质等诸多问题，在建图方面，RGB-D相机主要用于室内，室外则较难应用。

---

TUM数据集

1. 简介

• TUM数据集由慕尼黑工业大学的Computer Vision Lab公布。

• TUM数据集的RGB图与深度图由RGBD相机²拍摄得到。

• TUM数据集包含不同结构和纹理的数据、运动物体和3D物体重建。

• TUM数据集的帧数大小不同，运动有快有慢，有手持的相机。

2. 特点

• 数据集非常大，场景多。
• 相机位姿精确：TUM数据集通过运动捕捉系统提供精确的相机位姿。
• 提供了一组python脚本来处理数据集：提供的脚本可将文件进行时间上的对齐、画出轨迹图、比较试验结果和真实轨迹等等。

3. 下载

• 进入网页：

  TUM官方数据集下载

• 下载所需的数据集：

4. *教程

• TUM数据集文件格式
• TUM数据集相机内参
• TUM数据集脚本工具
• TUM数据集非官方速通教程

---

开源库

1. OpenCV

1.1. 简介

• OpenCV提供了大量的开元图像算法，是计算机视觉中使用极广的图像处理算法库。
• OpenCV由一系列 C 函数和少量 C++ 类构成，同时提供了Python、Ruby、MATLAB等语言的接口。
• OpenCV是一个开源计算机视觉和机器学习软件库，可以运行在Linux、Windows、Android和Mac OS操作系统上。

1.2. 特点

• 轻量级。
• 执行、处理速度快。

1.3. 安装

• 首先安装OpenCV的依赖项：

sudo apt-get install build-essential libgtk2.0-dev libvtk7-dev libjpeg-dev libtiff5-dev libjasper-dev libopenexr-dev libtbb-dev

• 若出现这种错误：

errorE: unable to locate libjasper-dev

• 解决方法：

sudo apt-get update
sudo apt-get install software-properties-common
sudo apt install libjasper1 libjasper-dev

• 进入OpenCV官网，选择OpenCV for Linux版本下载：

  OpenCV下载

• 安装OpenCV：

sudo make install
make -j4

1.4. *教程

• OpenCV官方介绍及教程
• OpenCV非官方速通教程

2. Eigen

2.1. 简介

• Eigen是一个基于C++模板的开源库，提供了快速的有关矩阵的线性代数运算，同时还支持数值分析及其相关的算法。
• Eigen支持多种编译环境。

2.2. 特点

• 适用范围广：支持所有矩阵操作、所有标准的数值类型、多种矩阵分解及其几何特征的求解。
• 运行速度很快：表达式模板允许智能地删除临时变量，并在适当时启用延迟计算。
• 可靠性高：算法经过仔细选择，通过了大量测试套件。
• Eigen是一个纯用头文件搭建起来的库，在使用时，只需引入Eigen的头文件即可。

2.3. 安装

• 打开终端，输入：

sudo apt-get install libeigen3-dev
sudo apt-get install libeigen3-dev

2.4. *教程

• Eigen官方介绍及教程

• Eigen非官方速通教程

3. PCL

3.1. 简介

• PCL是一个大型跨平台开源C++编程库，它实现了大量点云相关的通用算法和高效数据结构，涉及到点云获取、滤波、分割、配准、检索、特征提取、识别、追踪、曲面重建、可视化等。
• PCL支持多种操作系统平台，可在Windows、Linux、Android、Mac OS X、部分嵌入式实时系统上运行。

3.2. 特点

• 实时性高：PCL利用高性能计算技术，通过并行化提高程序实时性。
• 使用共享指针：PCL中的所有模块和算法都是通过Boost共享指针来传送数据的，避免了多次复制系统中已存在的数据的需要。

3.3. 安装

• 打开终端，输入：

sudo apt-get install libpcl-dev pcl-tools

3.4. *教程

• PCL官方介绍及教程
• PCL非官方速通教程(1)
• PCL非官方速通教程(2)
• PCL非官方速通教程(3)

---

建图实战

1. 说明

• 为了突出所用到的库，下文所涉及到的库都以空链接的格式标明，真正的链接可按[^脚注]查看。
• 本文的建图用的是TUM数据集³。
• 本文的建图用到iostream库、OpenCV库⁴、Eigen库⁵、PCL库⁶。
• 本文建图选择关键帧的方法是固定帧数选取法。

2. 建图框架

• 本文的建图分为四个大步骤，如下图所示：

3. *框架分析

3.1. 下载数据集

• 下载TUM数据集。
• 打开终端，用associate.py⁷对RGB图、深度图以及相机位姿进行对齐：

python associate.py rgb.txt depth.txt > associate.txt
python associate.py associate.txt groundtruth.txt > associate_with_groundtruth.txt

3.2. 数据集预处理

1. 读取associate_with_groundtruth.txt文件中的信息（iostream）:

ifstream fin("~/associate_with_groundtruth.txt");   // 读取RGB图文件名、深度图文件名、相机位姿对齐的信息
for (int i = 0; i < 12; i++) fin >> data[i];        // 输入对齐的信息

2. 读取对齐的图片和相机位姿:

• 图片的数据类型（OpenCV）、相机位姿的数据类型（Eigen）:

vector<cv::Mat> colorImgs, depthImgs;         // 彩色图和深度图
vector<Eigen::Isometry3d> poses;              // 相机位姿

• 读取RGBD图（OpenCV）和相机位姿（Eigen）:

colorImgs.push_back(cv::imread("RGB图名"));	 // 读取对齐的RGB图
depthImgs.push_back(cv::imread("深度图名"));    // 读取对齐的深度图
poses.push_back(T);							   // 读取对齐的相机位姿

3.3. 计算点云并拼接

1. 定义点和点云的格式（PCL）：

typedef pcl::PointXYZRGB PointT;	          //点类型（世界坐标）
typedef pcl::PointCloud<PointT> PointCloud;	  //点云类型

2. 新建点云（[PCL]()）：

PointCloud::Ptr pointCloud(new PointCloud);	  // 新建一个点云
PointCloud::Ptr slide_win(new PointCloud);	  // 新建一个点云压缩窗口

3. 选择并处理关键帧（Eigen）：

• 均匀地选取图片作为关键帧：

for (int i = 0; i < NImage; i+=IStep) {~}	  //均匀地选取关键

• 计算像素点的相机坐标（Eigen）：

Eigen::Vector3d point;	                     // 像素点的相机坐标
point[2] = double(d) / depthScale;	         // 计算相机坐标
point[0] = (u - cx) * point[2] / fx;
point[1] = (v - cy) * point[2] / fy;

• 计算像素点的世界坐标（Eigen）：

Eigen::Vector3d pointWorld = T * point;		 // 将相机坐标转化为世界坐标

• 将像素点塞进当前关键帧的点云中（PCL）：

current->points.push_back(p);				 //将点塞进current点云中

• 对当前关键帧的点云进行深度滤波（PCL）：

pcl::StatisticalOutlierRemoval<PointT> statistical_filter;	// depth filter and statistical removal （深度滤波与统计去除）
statistical_filter.setInputCloud(current);	 // 将current点云进行深度滤波与统计去除
statistical_filter.filter(*tmp);			 // 将结果赋值给temp点云

• 将滤波后的点云塞入压缩窗口中（PCL）：

(*slide_win) += *tmp;						 // 将滤波后的点云塞入压缩窗口slide_win中

• 对满足条件的压缩窗口进行体素滤波，并塞进点云中（PCL）：

pcl::VoxelGrid<PointT> voxel_filter;		 // voxel filter（体素压缩）
voxel_filter.setInputCloud(slide_win);		 // 将slide_win点云进行体素滤波
voxel_filter.filter(*slide_win);			 // 将结果赋值给slide_win点云
(*pointCloud) += *slide_win;				 // 将滤波后点云塞入点云中

3.4. 展示视频及点云

1. 展示视频（OpenCV）：

cv::imshow("img",colorImgs[i]);				  // 展示视频

2. 展示点云（PCL）：

pcl::visualization::CloudViewer viewer ("Viewer");	// 展示点云的窗口
viewer.showCloud(pointCloud);						// 展示生成的点云

3.5. 最后的处理

1. 对点云进行最后的体素过滤（PCL）：

voxel_filter.setInputCloud(pointCloud);     		//对点云进行最后的全局压缩
voxel_filter.filter(*pointCloud);					//将结果赋值给点云

2. 保存点云文件（PCL）：

pcl::io::savePCDFileBinary("map.pcd", *pointCloud); //保存点云文件

3. 等待展示窗口被关闭，结束程序：

while (!viewer.wasStopped ()){}						//使viewer窗口不自动退出

4. 若程序结束后想看点云，则打开终端：

pcl_viewer map.pcd

---

4. 基于固定帧数选取法和窗口压缩法的建图总代码

#include <iostream>
#include <fstream>

using namespace std;

#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <Eigen/Geometry>
#include <boost/format.hpp>
#include <pcl/point_types.h>
#include <pcl/io/pcd_io.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/visualization/cloud_viewer.h>

int NImage = 2486;		//两次对齐后的图片数量
double NSlide = 1000000;	//点云压缩窗口
int IStep = 360;		//选取关键帧的步长
double resolution = 0.005;		//点云分辨率

int main(int argc, char **argv) {
    vector<cv::Mat> colorImgs, depthImgs;    // 彩色图和深度图
    vector<Eigen::Isometry3d> poses;         // 相机位姿

    // 用fin读取associate_with_groundtruth.txt文件信息（RGB图文件名、深度图文件名、相机位姿对齐的信息）
    ifstream fin("/home/lujunying/rgbd_dataset_freiburg3_long_office_household/associate_with_groundtruth.txt");
    if (!fin) {
        cerr << "cannot find pose file" << endl;
        return 1;
    }

    //读取对齐的图片的信息（RGB图、深度图、相机位姿）
    for (int i = 0; i < NImage; i++) {	// 每张图片进行处理
        string data[12];// 输入一张图的对齐信息
        for (int i = 0; i < 12; i++) fin >> data[i];
        colorImgs.push_back(cv::imread("/home/lujunying/rgbd_dataset_freiburg3_long_office_household/"+data[1])); // 读取对齐的RGB图（数据集的路径）
        depthImgs.push_back(cv::imread("/home/lujunying/rgbd_dataset_freiburg3_long_office_household/"+data[3],-1)); // 读取对齐的深度图（数据集的路径）注意imread函数默认将图片转为RGB类型，故要进行类型转换
        Eigen::Quaterniond q(atof(data[11].c_str()), atof(data[8].c_str()), atof(data[9].c_str()), atof(data[10].c_str()));	// 读取对齐的相机旋转矩阵[四元数（w，x，y，z）]
        Eigen::Isometry3d T(q);
        T.pretranslate(Eigen::Vector3d(atof(data[5].c_str()), atof(data[6].c_str()), atof(data[7].c_str())));	// 读取对齐的相机的平移向量
        poses.push_back(T);	// 对齐的相机的位姿
    }

    // 计算点云并拼接
    // 相机内参（见TUM数据集相机内参）
    double cx = 320.1;
    double cy = 247.6;
    double fx = 535.4;
    double fy = 539.2;
    double depthScale = 5000.0;

    cout << "正在将图像转换为点云..." << endl;

    // 定义点云使用的格式：这里用的是XYZRGB
    typedef pcl::PointXYZRGB PointT;  //点类型（世界坐标）
    typedef pcl::PointCloud<PointT> PointCloud;	//点云类型

    // 新建一个点云
    PointCloud::Ptr pointCloud(new PointCloud);
    // 新建一个点云压缩窗口
    PointCloud::Ptr slide_win(new PointCloud);
    
    // 选择并处理关键帧，生成并展示两次滤波后的点云
    pcl::visualization::CloudViewer viewer ("Viewer");	// 开启展示点云的窗口
    for (int i = 0; i < NImage; i+=IStep) {	// 均匀选择图片作为关键帧，并处理
        PointCloud::Ptr current(new PointCloud);
        cout << "转换关键帧中: " << i + 1 << endl;
        cv::Mat color = colorImgs[i];
        cv::Mat depth = depthImgs[i];
        Eigen::Isometry3d T = poses[i];
        for (int v = 0; v < color.rows; v++)	// 处理图片上的像素点
            for (int u = 0; u < color.cols; u++) {
                unsigned int d = depth.ptr<unsigned short>(v)[u];	//获取深度值
                if (d == 0) continue; // 若没有测量到该像素的深度，则跳转到下一个像素
                Eigen::Vector3d point;	// 像素的相机坐标
                point[2] = double(d) / depthScale;	// d除以scale变为米的单位
                point[0] = (u - cx) * point[2] / fx;
                point[1] = (v - cy) * point[2] / fy;
                Eigen::Vector3d pointWorld = T * point;	// 将相机坐标变为世界坐标
				
                PointT p; // 点（XYZRGB格式）
                p.x = pointWorld[0];
                p.y = pointWorld[1];
                p.z = pointWorld[2];
                p.b = color.data[v * color.step + u * color.channels()];
                p.g = color.data[v * color.step + u * color.channels() + 1];
                p.r = color.data[v * color.step + u * color.channels() + 2];
                current->points.push_back(p);	//将点塞进current点云中
            }

        // depth filter and statistical removal （深度滤波与统计去除）
        cout << "current点云共有" << current->size() << "个点." << endl;
        PointCloud::Ptr tmp(new PointCloud);
        pcl::StatisticalOutlierRemoval<PointT> statistical_filter;
        statistical_filter.setMeanK(50);
        statistical_filter.setStddevMulThresh(1.0);
        statistical_filter.setInputCloud(current);	// 将current点云进行深度滤波与统计去除
        statistical_filter.filter(*tmp);	// 将结果赋值给temp点云
        current.reset();	// 释放current点云内存
        cout << "tmp点云共有" << tmp->size() << "个点." << endl;
        (*slide_win) += *tmp;	// 将滤波后的点云塞入压缩窗口slide_win中
        tmp.reset();  // 释放temp点云内存
        cout << "slide_win点云共有" << slide_win->size() << "个点." << endl;

        if(slide_win->size() > NSlide || slide_win->size() == NSlide || i == 0 || (i+IStep)>( NImage-1)){	//如果slide_win满足体素压缩的条件
             // voxel filter（体素压缩）
             pcl::VoxelGrid<PointT> voxel_filter;
             voxel_filter.setLeafSize(resolution, resolution, resolution);
             voxel_filter.setInputCloud(slide_win);	// 将slide_win点云进行体素滤波
             voxel_filter.filter(*slide_win);	// 将结果赋值给slide_win点云
             cout << "压缩后slide_win点云共有" << slide_win->size() << "个点." << endl;
             (*pointCloud) += *slide_win;	// 将滤波后的slide_win点云塞入点云中
             slide_win.reset(new PointCloud);	// 释放slide_win点云内存
        }

        cout << "pointCloud点云共有" << pointCloud->size() << "个点." << endl;
        cout << "-------------------------------------------------------------------------------" << endl;
		
        // 展示视频与生成的点云
        cv::imshow("img",colorImgs[i]);	// 展示视频
        cv::waitKey(30);
        viewer.showCloud(pointCloud);	// 展示点云
    }
    
    // 对点云进行最后的全局压缩
    cout << "两次滤波后点云共有" << pointCloud->size() << "个点." << endl;
    pointCloud->is_dense = false;	// 这个没什么用
    pcl::VoxelGrid<PointT> voxel_filter;	// 最后的全局压缩
    voxel_filter.setLeafSize(resolution, resolution, resolution);
    voxel_filter.setInputCloud(pointCloud);
    voxel_filter.filter(*pointCloud);
    viewer.showCloud(pointCloud);
    
    //保存生成的点云文件
    cout << "最终两次滤波后点云共有" << pointCloud->size() << "个点." << endl;
    pcl::io::savePCDFileBinary("map.pcd", *pointCloud);
    
    //结束程序
    while (!viewer.wasStopped ()){}	//保持viewer窗口，不自动退出
    return 0; 
}

5. 建图效果

• 下面展示最终生成的点云：

---

现在与未来

• 本文的建图采用窗口压缩的方法建立点云，极大地减少了点云不必要的内存，加大了建图的规模。
• 本文的建图是根据TUM数据集而实时建立的，未来将利用ROS的知识，根据手持摄像头拍摄的视频进行建图。
• 本文对于关键帧的选取采用的是固定帧数选取法，优点是通俗易懂、编程简单，缺点是具有较高的随机性，关键帧的质量一般，比如运动很慢的时候，就会选择大量相似的关键帧，冗余，运动快的时候又丢失了很多重要的帧关键帧。未来将学习ORB-SLAM2^[5]以及GCNv2^[6]的关键帧提取和稠密重建的相关算法，并在此基础上进行改进。

---

参考文献

[1] 高翔,张涛.《视觉SLAM十四讲：从理论到实践》.(第2版).北京:电子工业出版社,2019.

[2] Stephen Prata.《C++ Primer Plus》.(第6版中文版).北京:人民邮电出版社,2020.7.

[3] T. Barfbot, State estimation fbr robotics: A matrix lie group approach, 2016.

[4] R. Mur-Artal, J. M. M. Montiel, and J. D. Tards, “ORB-SLAM: A versatile and accurate monocular slam system,” IEEE Trans. Robot., vol. 31, no. 5, pp. 1147–1163, Oct. 2015.

[5] R. Mur-Artal and J. D. Tardós, “ORB-SLAM2: An open-source SLAM system for monocular, stereo and RGB-D cameras,” IEEE Trans. Robot., vol. 33, no. 5, pp. 1255–1262, Oct. 2017.

[6] J. Tang, J. Folkesson, and P. Jensfelt, “GCNv2: Efficient Correspondence Prediction
for Real-Time SLAM,” IEEE ROBOTICS AND AUTOMATION LETTERS, VOL. 4, NO. 4, OCTOBER 2019.

---

1. 代码见建图实战的第4点。 ↩︎

2. 点我了解RGBD相机 ↩︎ ↩︎

3. 点我了解TUM数据集 ↩︎

4. 点我了解OpenCV ↩︎

5. 点我了解Eigen ↩︎

6. 点我了解PCL ↩︎

7. 点我了解associate.py ↩︎