视觉SLAM笔记（64） 八叉树地图
视觉SLAM中，点云地图由于其高分辨率和占用大量存储空间而存在缺陷；相比之下，八叉树地图通过将三维空间划分为小方块并动态调整节点状态（占据与否），能够更高效地管理空间资源并减少存储需求。八叉树地图不仅适合静态环境建模，还能处理动态物体的存在；其核心优势在于通过概率对数值（Log-odds）动态更新节点状态，从而适应复杂场景。此外，使用OctoMap库可以方便地构建和管理八叉树地图，并通过RGB-D数据进行实时更新和投影信息处理。
视觉SLAM笔记（64） 八叉树地图
- 1. 点云地图缺陷
- 2. 八叉树
- 3. 八叉树地图
1. 点云地图缺陷
在构建点云地图时, 尽管存在三维结构这一特点, 但通过体素滤波处理来提升数据分辨率. 然而, 在实际应用中发现点云数据仍然存在几个显著的问题.
点云地图通常具有较大的体积，在这种情况下一个PCD文件也会占据大量存储空间。一张分辨率640×480的图像会产生约30万个空间点（即大约3×10^5个数据点），这些数据占用了大量的存储资源。
即使经过一定的过滤处理后（比如去除噪声或冗余信息），PCD文件仍然会保持较大的体积。
值得注意的是，“大”的特性并非是必须的（即并非所有应用都需要如此高的分辨率）。对于像地毯上的褶皱或阴影这类细节而言（如果不在地图中体现这些微小的变化），将其包含在地图中只会浪费存储资源。
由于这些细节所占的空间较大（即占用较多内存资源），除非降低地图的整体分辨率（这会牺牲地图的质量），否则在一个有限内存环境中无法建模一个大规模的空间环境。
然而降低分辨率会导致地图的质量下降（即信息丢失）。此时可以通过某种压缩编码方式将地图数据以更紧凑的形式存储在内存中，并舍弃一些重复或不重要的信息。
- 点云地图在处理运动物体方面存在困难。
由于该方法仅提供了"添加点"的功能，在缺少"当点消失时将其移除"的操作的情况下，在实际应用中难以应对运动物体的普遍性带来的挑战。
因此，在真实环境中这种局限性导致了其不适用性。
2. 八叉树
将三维空间划分为许多体素是一种常见做法。
当一个立方体每个面被均分时会生成8个相同的小立方体。
这一过程可以不断进行直至达到所需精度。
在这一过程中"将一个立方体均分为8个子立方体"可被理解为"一个节点扩展为8个子节点"。
因此整个细分过程形成了一棵八叉树（Octo-tree）。
左侧展示了一个大立方体持续进行均等分割为八个小立方体的过程直至达到最小尺寸。总体结构上类似于根节点的概念。在空间划分中，在树状结构中向上移动一个层级会使覆盖的空间体积增加8倍。
为了便于计算起见：
如果单个叶子节点占据的空间大小是1\ \text{cm}^3，
那么当我们将八叉树的高度限定在10层时，
总体积约为8^{10} = 1,073\ \text{m}^3，
足以建模一间屋子。
随着深度的增长呈指数级增长，
因此使用更大的深度会使建模体积急剧增加。
可能会产生疑问，在点云体素滤波器中，并非仅限于每个体素仅包含一个点。
从点云数据的角度来看，在无信息的情况下可将空位用...来表示
能否更详细地描述这件事？
倾向于以概率形式表征某个节点是否被占据的状态
例如，在区间[0,1]内采用浮点数x来进行表示
初始设定x为0.5值，在持续观察该节点处于被占据状态时，则逐步提升其数值；
反之，在持续观察该节点处于空白状态时，则逐步降低其数值；
通过这种动态更新机制，在地图中实现了障碍物信息的有效建模。
然而，在现有方法中存在一定的局限性：当x持续增大或减小时（即x不断向上或向下变动），可能会导致其超出该区间范围[0,1]的外延延伸情况发生改变，并进而引发处理上的不便。因此不直接采用概率值来表示节点的状态（即是否被占据），而是采用Log-odds值来进行状态的表征。具体而言：定义y为实数域中的Log-odds值（即y∈R），而x则表示一个取值在[0,1]区间的概率值。在这种设定下，则可以通过logit变换将两者联系起来：
具体而言，在这种设定下：定义y为实数域中的Log-odds值（即y∈R），而x则表示一个取值在[0,1]区间的概率值。在这种设定下，则可以通过logit变换将两者联系起来：
其反变换为：
观察得知，在变量y的变化范围内（从-1至+1），变量x的变化范围是从0至1。特别地，在y=0时（即中间状态），x的取值为\frac{1}{2}。基于此观察结果，在系统运行过程中可以选择记录y的值来反映当前节点的状态是否被占用。具体而言，在持续观察发现某节点处于被占用状态时，则将相应的y值增加一个单位；反之，则减少一个单位。
在获取概率值时, 我们可以通过应用逆 logit 变换来将 y 转换至对应概率空间从而得到对应的概率值。
用数学形式表示, 设某节点标记为 n, 观测数据记作 z.
由此可知, 在时间步长从 t 到 t+1 的过程中, 某节点的概率对数值由 L(n|z₁:t) 扩展至 L'(n|z₁:t+1)
如果写成概率形式而不是概率对数形式，就会有一点复杂：
基于对数概率的基础上，在获得 RGB-D 数据后，则能够通过这一数据源来更新完整的八叉树地图结构。
当从 RGB-D 图像中观测到某像素具有深度值 d 时，
表明该空间点处存在被观测占据的数据。
同时，在相机中心位置朝该空间点方向延伸的线段上，
应当不存在其他物体（否则会导致部分遮挡）。
基于此信息分析后可知，
我们可以有效更新八叉树地图模型，
并据此处理物体运动状态的信息。
3. 八叉树地图
首先，安装 octomap 库：
$ git clone https://github.com/OctoMap/octomap
AI助手Octomap 库主要由 octemap 地图和一个用于可视化的辅助程序组成；这些均属于 cmake 工程。
$ cd octomap
$ mkdir build
$ cd build
$ cmake ..
$ make
AI助手或者独立安装octovis
$ sudo apt-get install octovis
AI助手主要依赖项是 doxygen：
$ sudo apt-get install doxygen
AI助手基于前面五张图像生成八叉树地图，并将其可视化。
在代码实现中跳过图像读取步骤
#include <iostream>
#include <fstream>
using namespace std;
#include <opencv2/core/core.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <octomap/octomap.h> // for octomap
#include <Eigen/Geometry>
#include <boost/format.hpp> // for formating strings
int main(int argc, char** argv)
{
vector<cv::Mat> colorImgs, depthImgs; // 彩色图和深度图
vector<Eigen::Isometry3d> poses; // 相机位姿
ifstream fin("./data/pose.txt");
if (!fin)
{
cerr << "cannot find pose file" << endl;
return 1;
}
for (int i = 0; i < 5; i++)
{
boost::format fmt("./data/%s/%d.%s"); //图像文件格式
colorImgs.push_back(cv::imread((fmt%"color" % (i + 1) % "png").str()));
depthImgs.push_back(cv::imread((fmt%"depth" % (i + 1) % "pgm").str(), -1)); // 使用-1读取原始图像
double data[7] = { 0 };
for (int i = 0; i < 7; i++)
{
fin >> data[i];
}
Eigen::Quaterniond q(data[6], data[3], data[4], data[5]);
Eigen::Isometry3d T(q);
T.pretranslate(Eigen::Vector3d(data[0], data[1], data[2]));
poses.push_back(T);
}
// 计算点云并拼接
// 相机内参
double cx = 325.5;
double cy = 253.5;
double fx = 518.0;
double fy = 519.0;
double depthScale = 1000.0;
cout << "正在将图像转换为 Octomap ..." << endl;
// octomap tree
octomap::OcTree tree(0.05); // 参数为分辨率
for (int i = 0; i < 5; i++)
{
cout << "转换图像中: " << i + 1 << endl;
cv::Mat color = colorImgs[i];
cv::Mat depth = depthImgs[i];
Eigen::Isometry3d T = poses[i];
octomap::Pointcloud cloud; // octomap中的点云
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; // 为0表示没有测量到
if (d >= 7000) continue; // 深度太大时不稳定，去掉
Eigen::Vector3d point;
point[2] = double(d) / depthScale;
point[0] = (u - cx)*point[2] / fx;
point[1] = (v - cy)*point[2] / fy;
Eigen::Vector3d pointWorld = T * point;
// 将世界坐标系的点放入点云
cloud.push_back(pointWorld[0], pointWorld[1], pointWorld[2]);
}
// 将点云存入八叉树地图，给定原点，这样可以计算投射线
tree.insertPointCloud(cloud, octomap::point3d(T(0, 3), T(1, 3), T(2, 3)));
}
// 更新中间节点的占据信息并写入磁盘
tree.updateInnerOccupancy();
cout << "saving octomap ... " << endl;
tree.writeBinary("octomap.bt");
return 0;
}
AI助手该库采用octomap::OcTree数据结构来生成完整的地图数据。实际上 octomap 提供了多种类型的八叉树实现：其中一些带有地图数据、另一些则包含位置占据信息。用户可以根据需求自定义每个节点应存储的关键参数。为了简化实现过程，在本方案中采用了最基础的形式——不包含颜色信息的标准八叉树结构。
OCOTMAP系统采用了基于点云的数据结构，并相较于PCL系统稍显简单，在数据存储上仅需关注空间位置信息。基于RGB-D图像数据及相机的姿态信息，在OCOTMAP中首先将各点坐标转换为世界坐标系后存入系统中。随后，在内部应用投影模型更新每个空间单元的状态概率。最终生成的地图文件被命名为octemap.bt文件
现在，调用它打开地图文件
$ octovis octomap.bt
AI助手就能看到地图的实际样子了，显示了构建的地图结果
未将颜色信息纳入地图显示的情况下，默认初始界面呈灰色。
通过按键操作依据高度数据完成染色过程。
右侧存在一个八叉树地深度控制条，在此处可调节地图的整体分辨率。
基于构造时使用的默认设置为 16 层的情况，在此处设置为 16 层即代表最高分辨率。
当将该设置降低一层时，在八叉树结构中叶子节点上移一层，并使每个区域单元的边长增加一倍至 0.1 米。
可以看到，能够很容易地调节地图分辨率以适应不同的场合
Octomap 值得深入挖掘的领域在于其未被充分 explore 的潜力。例如：
- 通过简单的方式获取任意点的位置信息
- 能够基于此方法设计出高效的路径规划策略
- 比较两者的文件容量时可观察到明显的空间效率差异
- 上一节生成的点云地图对应的磁盘文件大小约为7.2\,\text{M}
- 而 octomap 则仅为0.56\,\text{MB}（约0.56MB），远低于点云地图的比例
- 该算法能够高效地模拟大规模的空间环境
参考：
相关推荐：
视觉SLAM笔记（63） 基于红绿色深度相机的稠密三维重建
视觉SLAM笔记（62） 基于单眼摄像头的详细三维模型构建
视觉SLAM笔记（61） 基于单一视点的地图绘制与构建过程分析
视觉SLAM笔记（60） 地图绘制与构建过程分析
视觉SLAM笔记（59） 关键帧之间的相似度评估方法研究
谢谢！