Extraction, Update and Optimization of Domain Diretions and Structual Lines

28 Apr 2025

---

本文将介绍在现有的里程计上，如何加入主方向的提取与结构线筛选、主方向的迭代更新、后端中的主方向约束因子。

整体框图如下：

前置内容

2D 线的表示、3D 线的表示以及之间的转换（投影：3D->2D；三角化：2D->3D）见 TODO

对环境结构的假设

考虑的环境是结构化环境，存在一个垂直主方向，多个水平主方向，多个倾斜主方向。所有水平主方向和垂直主方向垂直， 每个倾斜主方向都和某个水平主方向（可以不是同一个水平主方向）垂直。

主方向的表达

什么是主方向呢？我们可以认为，许多条平行的 3D 线的共同方向便是主方向（Domain Direction, 简称 DD） 因此对于一个 3D 空间中的方向，我们可以自然地将其表示为 3 维单位向量。

以上我们都还未引入投影的概念，便能得到主方向的第一种表示。接下来我们将考虑两条 3D 的平行线，被一个相机观测，也就是将 它们投影到相机平面上，那么相机平面上的投影具有什么关系？这两条平行线的投影会交汇于一点，这一点在相机平面上，称为 消失点（Vanishing Point）。 如果我们将相机的光心/焦点与消失点连接起来，那么得到的向量是和两条平行线的方向平行的（fx 与 fy 相等是前提）！ 所以，消失点 + 光心 = 主方向。

总的来说，对于一组 3D 平行线，我们可以将它们的方向定义为主方向，并用 3D 单位向量 或 2D 消失点 来表征这个方向。

对于本文，将统一使用 3D 单位向量来表达主方向。2D 消失点在相机平面上的位置是没有限制的，因此可能会处于图像之外， 而 3D 单位向量是在一个球面上活动，数值上是有限的，因此在计算上更稳定。

投影平面法向量

3D 线投影到相机平面上得到 2D 线，此时 3D 线与光心可以构成一个平面，我们称之为 投影平面（Projection Plane）， 其法向量称之为投影平面法向量。因此一条线可以映射到一个法向量，但一个法向量可能对应多条线，或者说多条不同的线可以映射到 同一个法向量。

在以下的描述中，我们将用这个法向量来表达某一条线。

显而易见，一条线的法向量一定与其主方向垂直，这个是下文需要用到的重要结论。

主方向提取与结构线筛选

原理

首先根据相机系下的重力方向（假设已经通过初始得到），可以筛选出属于垂直主方向（vDD）的 2D 线段； 之后利用余下的法向量确定出水平法向量的数量和朝向，同时筛选出方向是水平的线段； 最后，以同样的方法利用最后剩下的法向量确定出倾斜法向量的数量和朝向。

根据重力方向筛选线段思路比较简单：遍历所有法向量，挑出与相机系下重力向量垂直的法向量，这些法向量对应的 线段的 3D 方向大概率是与重力平行的。至于为什么说是大概率，原因还是前文提到的多条不同的线可以映射到同一个 法向量。

最关键的如何用余下的法向量确定出水平主方向的个数与朝向，确定出朝向后，便可以和用重力筛选垂直线段的方法筛选出 水平线段。首先介绍正向的思路（论文中称为分治）：

• 水平主方向一定与垂直主方向垂直，因此我们可以将可行域限定在 一个平面上的单位向量，显然这个平面和垂直主方向垂直。

• 如何参数化在这一平面上的向量呢，我们可以寻找一个现有的和垂直主方向垂直的向量， 那便是任意一个垂直内点（上一步筛选得到的垂直方向线段的投影平面法向量），然后旋转一定角度， 得到的便是在可行域内的法向量，此时也将待求的法向量参数化为了一个角度 $\theta$

• 如何找到合适的角度呢？可以用分治的方法，先把 360 度分成几份，然后计算每个区间中点对应的向量与多少个法向量垂直（做内积）， 然后选取垂直数量最多的区间，继续分割，重复以上过程

分治的方法需要预先给定主方向的个数，且复杂度为 $O(nl)$，$n$ 为分割个数，$l$ 为分割层数。

如果这时候我们反过来思考，不是去转这个向量然后看有多少法向量和它垂直，而是让每个法向量给出一个 $\theta$ 合法区间， 在这个合法区间内的向量基本与这个法向量垂直，对于一个真实存在的水平主方向，一定和许多属于这个方向的线的法向量垂直， 所以如果有很多个法向量给出的合法区间有重叠，那么则认为存在一个水平主方向，其 $\theta$ 落在这个区间内，如果有多个重叠， 那就有多个水平主方向。这样，水平主方向的数量、方向以及其内点就都确定了！

具体步骤如下：

1. 选择一个垂直内点（是一个法向量）作为水平主方向的起始参考，水平主方向必定由该垂直内点旋转一定角度得到， 因此用 $\theta$ 参数化候选的水平主方向。$n$ 表示

\[\mathbf{h}_n(\theta_n) = \mathbf{R}_{<\mathbf{v}_m, \theta_n>} \mathbf{n}_3\]

1. 水平主方向 垂直于 水平内点

\[\mathbf{h}_n(\theta_n)^T\mathbf{n}_k = 0\]

1. 放松一下上式的要求，不要求完全垂直，然后展开合并同类项（具体推导 TODO）

\[|\mathbf{h}_n(\theta_n)^T\mathbf{n}_k| < \varepsilon\] \[f_k(\theta) = |a_k \cdot \cos \theta_n + b_k \cdot \sin \theta_n| \leq \varepsilon\]

1. 求上面的不等式，得到 $\theta$ 的合法区间

\[\theta \in [-\arctan(\frac{a}{b})-\arcsin(\tilde{\varepsilon}),-\arctan(\frac{a}{b})+\arcsin(\tilde{\varepsilon})]\]

对于余下的每个法向量，我们都可计算这么一个区间，接下来需要考虑如何求出所有的重叠区间。 使用的方法可以称之为 投票法。我们把所有区间放在数轴上，数轴向右为正，然后从左向右遍历每个端点， 如果遇到某个区间的左端点，则计数器加一，表示有一个区间加入了，如果遇到某个区间右端点，则计数器减一， 表示有一个区间退出了。这样，我们得到一个计数的序列，找出每个计数极大值的 $\theta_{l_n}$ 和其后续计数值的 $\theta_{r_n}$，这样就可以得到一个或多个重叠区间 $[\theta_{l_n},\ \theta_{r_n}]$

笔者对该算法做了一些改进：

• 限制不连续区间
• 范围过大的区间不参与投票

实现

先介绍数据结构

再介绍对数据结构的操作（构造、访问、修改）

最后介绍数据结构之间的关系（如果有多个数据结构在这个模块中的话）

主方向的迭代更新

原理

实现

后端优化增加主方向约束

原理

实现

实验评估

介绍指标

结果

介绍批量运行的思路

参考

Hong Kong World: Leveraging Structural Regularity for Line-Based SLAM

Total views. cowboys. Hits