写在前面，又到了一年的结尾，发现好久没有更新博客了，这次来个大的，也算是对自己近段时间的知识作个总结收尾吧。由于整理量有点大，需要花一段时间才能更新完。

贴在前面，参考文献：
1.参数化
平面表示知乎详解
2.提取
CVPR2014年的Fast Plane Extraction in Organized Point Clouds Using Agglomerative Hierarchical Clustering
ICRA2019年的Incremental Visual-Inertial 3D Mesh Generation with Structural Regularities

PlaneRCNN: 3D Plane Detection and Reconstruction from a Single Image
Single-Image Piece-wise Planar 3D Reconstruction via Associative Embedding

3.优化
Incremental Visual-Inertial 3D Mesh Generation with Structural Regularities
LIC-Fusion 2.0: LiDAR-Inertial-Camera Odometry with Sliding-Window Plane-Feature Tracking
Co-Planar Parametrization for Stereo-SLAM and Visual-Inertial Odometry

一.平面参数化

平面的参数化就是如何去用数学表达式去高效表示平面的公式。公式中包含的参数就是参数化过程。典型的平面用4个参数来表示即 $π=(n,d){\bf {\pi}}=({\bf n}, d)$ ,这里面n是平面的方向（3个自由度），d是平面的距离（一个自由度）。如下图所示就是一个平面最简单的方程图示，假设平面上有一个点，其3D坐标为p，那么平面和点就满足基本的平面方程 $n^Tp+d=0$ 。（当然了，你也可以写成 $n^Tp-d=0$ ）

这种表达非常直观易于理解，比较可惜的是使用4个自由度，但其实平面是一个3D的形式，最小参数化只有3个自由度，所以在做优化问题的时候需要用一个3自由度的表达，目前看来最常用，也是最好用的参数化表达是黄国权老师提出来的最近点表达形式CP（closed point）。下图这种表达形式也可以很好的理解，就是选取平面上距离坐标系点最近的那个点来唯一表示平面，刚好这个点就等于nd，目前还有一个变种的形式就是n/d，这两种形式是相似的，但第一种可以很好的形象化，就是一个点的坐标。但是这种参数化方法有一个问题，就是当d为0的时候平面会有奇异（也就是无法唯一的表示了，你想想d为0，那么无论n为多少，nd都为0，所以无法确定n了，也就无法确定平面了）处理这个问题的方法也比较简单，就是当d为0的时候换一个参考坐标系，那么d就不再为0了。其他的参数化形式详见平面表示知乎详解，但都会有相应的问题，需要特殊处理。这可能也是3D视觉的复杂之处吧！

二.平面提取

平面的提取，应该包含平面的分割和平面参数的计算，目前分为两种方案，一种是传统方法，基本上是基于RANCSAC，聚类等方法，另一种是最近几年较热的DL深度学习的方法。那么我们就按照这两个方法来逐一分析。首先分析前列举下两种方法的优劣：

传统方法：
优势：
（1）精度高，包含边界精度和参数精度
（2）速度相对可以做到较快
劣势：
（1）提取平面的效率低，个数相对较少，小平面难以提取
（2）基本上要靠depth，没有depth图很难获得比较好的平面

DL方法：
优势：
（1）预测的时候不需要depth，单目即可
（2）可以预测大量平面，预测的边界精度高
劣势：
（1）目前参数估计精度还比较差
（2）速度慢，耗时太大，GPU上勉强实时

1.传统方法

传统方法这里细讲两篇论文
CVPR2014年的Fast Plane Extraction in Organized Point Clouds Using Agglomerative Hierarchical Clustering
ICRA2019年的Incremental Visual-Inertial 3D Mesh Generation with Structural Regularities

1.1 CVPR2014年的Fast Plane Extraction in Organized Point Clouds Using Agglomerative Hierarchical Clustering

本文代码在git上的ManhattanSLAM中有复现
这篇文章的最大卖点就是速度很快，平面分割的精度尚可

摘要

本文实现了一个实时快速的点云拟合平面算法。
在图片空间上，统一的将点云分割成不重叠的一组点，一开始构建一个图，图中的节点和边代表了一组点和他们的邻域。在这个图中，采用文中提出的AHC聚类方法，系统性的合并属于同一个平面的节点，直到平面拟合的均方根误差MSE超过一定的阈值。最后使用像素级别的区域增长方式优化平面。可以实时的提取主要平面，在640*480像素下实现35HZ的速率。

简单点讲就是一帧构建一个图，图中的节点进行拟合平面。

算法模型

在图像像素上，使用一个统一尺寸的非重叠区域去划分点，从而构成一个图，图中包含有节点和边。然后使用AHC聚类算法，AHC聚类算法重复如下两个步骤
（1）首先寻找具有最小拟合平面均方差的节点，也就是判断每节点是否是一个平面。
（2）合并该节点周围的节点，通过最小平面均方差。
该算法的时间复杂度是logn，n是节点的大小。
最后还需要优化聚类平面区域的边界，使用的是像素级别的增长方法。
所以整体上有三个步骤
（1）初始化图，得到节点和边
（2）AHClucter聚类
（3）使用像素优化平面边界

整体的系统框架如下

文中提出，想要通过平面估计6自由度的pose，就必须要有三个不同方向的平面，而小于三个平面会出现退化问题，因此需要和点结合使用。

和线段的提取方法类比

在介绍平面AHC算法之前，先说下线段的AHC算法。如图W设置为3，也就是每3个点分成一个node节点。每个node和左右两个node判断是否满足MSE，满足就合并，不满足就不合并，直到左右都没有满足MSE的node就生成一个line。这样最后生成了三条线段，ab，efg和ij。

和线段的提取不同之处

（1）和line的提取不同，提取平面的初始节点之间必须没有相同的点。不然节点合并的时候会出现重叠区域，不好处理点的位置。但是文中又提到没有重叠的区域会导致无法直接获得平面的边界，后面会重点介绍这个缺点的方法。

（2）合并尝试的次数不同，line的node合并只需要和其左右两边的node进行合并判断，而初始的平面node至少要和周围4个node进行合并，随着平面node的合并，范围会变大，因此进行合并的node数量会大量增加。但是后面会分析其平均数量，发现会趋于一个常数，因此时间复杂度也会趋于nlogn。

再次说明下，该平面提取算法分为三个步骤，详见论文中的Algorithm 1：

（1）初始graph的创建G

（2）粗略的AHCLuster平面聚类B, $π\pi$

（3）优化平面边界C， $π′{\pi}'$

F是整个frame，包含了M行N列的点云
B，C分别是coarse和refined的分割，里面是一系列的点。
$π{\pi}$ 和 $π′{\pi}'$ 就是优化前后的平面。
G是初始的graph，u和v是节点，uv是节点之间的没有方向的edge。

graph初始化

整个初始化流程见论文中的模型2
graph的初始化比较简单，就是一个M * N的图像，从左上角开始到右下角，均匀分割成一个个H * W的图像块，这个图像块就是一个节点。每个节点由 $v_i,j$ 表示，i和j分别是对应的横纵位置，每个节点包含有H * W个点。如果靠近右边或者下边的分割没有达到H或者W的大小，就取较小值。模型2中还使用了两种去除不合适节点和边的方法，分别是REJUECTNODE和REJECTEDGE两个函数，这两个函数分别剔除下面四个不适合用来做平面提取的node和edge，也就是outlier

（1）节点本身不是平面区域，应该是通过节点包含的点计算得到的。REJUECTNODE
（2）节点包含又丢失数据的点，比如有些点没有深度。REJUECTNODE
（3）节点内部的点的深度不连续。比如一个平面遮挡了另一个平面，那么在这个量平面遮挡的区域，深度就是不连续的。REJUECTNODE
（4）两个平面区域的边界处。比如墙角。REJECTEDGE

REJUECTNODE处理1-3的问题，REJECTEDGE处理4的问题。

初始化非重叠graph避免计算所有点的normal，默认在一个节点上所有点共有一套参数。

AHCLuster聚类算法

整个聚类在算法3中，首先是构建一个能找到最小的plane拟合MSE的节点的函数，然后使用这个函数重复去找当前最小的平面拟合MSE的节点（在剩下所有的节点中查找），这个节点要不断合并周围其他最好的节点，通过边edge判断是否是周围节点，周围节点会有一个边的连接关系，通过最小的合并后的MSE（两个节点合并后重新计算的MSE）来判断是否是最好节点。如果合并后的节点计算出来的MSE大于给定的阈值 $T_{MSE}$ ,那么就说明最小的MSE都不满足给定的阈值，就不会再有新的周围节点合并到当前的节点上了，因此该平面就不会再merge新的节点了，因此可以直接提取该plane了。如果小于给定的阈值，合并该最好的节点，同时在数据结构E和V中去掉这个节点和相关的边。以此不断循环，不断从周围节点扩大该节点的点数目，把相同平面上的节点合并成一个更大的节点，直到该节点不能在合并周围的节点，同时该节点内的点数满足给定的生成plane点数阈值 $T_{NUM}$ ，就提取该节点为平面。

后面关于合并的时间复杂度被控制在nlogn的原因暂时还没有搞明白
还有一些实现细节可以提高算法的效率和精度。

对分割平面的边界进行优化

这个优化实现起来比较简单，就是一个个像素去判断是否是属于前面提取的平面。
如果需要精确的平面边界和平面方程，上面的coarse分割是不够的，还需要对平面进行优化。
单独使用上面的coarse平面分割方法会有以下3个问题：
（1）平面边界的地方会有锯齿
（2）平面边界的地方会出现遮挡或者丢袭数据节点倒是一些数据节点没有得到使用
（3）两个物体遮挡的地方会出现过分割
第一个问题会引入outlier，后面两个问题会丢失一些inliner。整体上都是边界没有处理好导致的平面边界和参数的精度问题。

模型4详细给出了整个解决方案。然后第二个问题是通过一个像素级别的区域增长来解决的，对k平面区域的每个边界点周围的上下左右4个像素点做平面关联，如果pij 满足和k平面的关联条件，同时该点又不在另外的其他平面上，就直接加入到k平面的区域和边界点，加入边界点是为k平面做进一步的扩展，如果pij 满足和k平面的关联条件，同时该点在另外的其他l平面上，那么这两个平面是相连的，两个平面间（也是两个大的节点node）需要构建新的edge，也就是伪代码里的connect nodes，这时候再判断这个点是否更加满足当前k平面的条件（和l平面相比），如果是的话，需要把该点加入到该k平面的区域和边界点，还需要把该点从l平面里移除。如果不是的话，啥也不用做里，该点pij已经在l平面里了,不需要添加操作了。至此像素级别的边界扩展完成。这时候平面边界就变得非常光滑，同时还构建了新的平面节点 $V^{'}$ 和平面节点之间的边 $E^{'}$ ,这里的节点就是所有的平面，所有的边就是有接触（接触点同时满足两个平面的方程connect nodes）的两个平面之间的连接关系。节点 $V^{'}$ 和边 $E^{'}$ 统一构成了图graph $G^{'}$ 。最后一步就是用这个新的graph调用前面的AHCLuster算法，重新合并平面节点。

1.2 Incremental Visual-Inertial 3D Mesh Generation with Structural Regularities

源码位置
这篇文章的重点是使用稀疏的点云进行平面的拟合，不需要有深度数据即可，单目双目就可以拟合平面。平面拟合的精度和边界要略差于基于深度的拟合方案，但是速度上快如闪电，PC上10毫秒左右就可以完成，是相当的快啊。具体的算法后面有时间仔贴过来，未完待续。。。

2.基于DL的方法

目前比较具有典型的框架分别是:
1.首先是两个比较古老的算法
PlaneNet
PlaneRecover
2.然后是两个CVPR19年的算法
PlaneRCNN
PlanarReconstruction
3.最后是三个2021年比较新的算法
PlaneSegNet
SparsePlanes
PlaneTR

PlaneNet和PlaneRecover文章较老，效果也不好，就不分析
我们先讲下PlaneRCNN和PlanarReconstruction这两篇吧。

2.1 PlaneRCNN

论文开源地址

1 摘要

文章重点:基于深度学习网络框架,利用单张RGB图像实现平面的重建.
(1)利用Mask R-CNN来检测平面参数和平面分割区域.(2)同时联合优化所有的分割区域
相较于PlaneNet(10个平面)和PlaneRecover(5个平面),文章提出的方法不需要固定单张图像重建的平面数量.

2 方法

PlaneRCNN包含有三个主要的模块,分别是平面检测模块,分割区域重优化模块以及一致性的warp模块.

（1）平面检测模块是建立在Mask R-CNN框架下,给定单张RGB图像来检测平面区域同时预测3D平面参数.该模块同时还预测了单张RGB图像的Depth图.

（2）重优化模块是将所有检测到的平面区域放到一起优化,得到更加精细的平面区域.，虽然这里说是把所有的平面放到一起，但其实是分成两个。

（2）warp模块是考虑到平面重建的一致性,将相邻帧之间的平面点重投影优化pose.

2.1 平面检测模块

文章在使用Mask R-CNN的分类只用平面区域和非平面区域两种.同时表明Mask R-CNN在平面提取方面效果意外的好.同时可以处理任意数目的平面.

文章说道已有的CNN方法可以预测深度图和平面法向,以及利用回归的方法可以计算平面距离,但是本文没有采用这些方法,而是用三个步骤解决这些问题(1)预测平面区域的normal (2)估计图像的深度图 (3)利用简单的点面公式计算平面distance

(1)平面normal的估计
1)选择一个锚点normal,2)然后回归normal的残差,3)最后归一化到单位向量
整个过程表示如下:首先通过选取7个锚点normal中的一个来估算平面normal,然后回归3D残差向量.锚点法向是通过正式样本的平面法向量上运行k-means聚类算法确定,回归损失函数选择L1的交叉熵损失函数.大致的意思就是先确定normal的大概值,然后慢慢优化到精确数值.

(2)深度图估计

(3)平面distance估计
在已知平面normal和深度数据的情况下,使用下面的公式计算得到平面distance,其中当点不在该平面上时,mi为0,在该平面上是为1.

2.2 平面分割的重优化模块

该模块就是优化得到更加精细的2D图像上的平面分割.就像下面的图示,输入的平面分割边界模糊,输出的平面分割边界清晰.

文章提到现在主要的挑战是单张图像中平面的数量是不同的,所以文章提出将所有的平面分割结果整合到一起优化,这样就不需要再考虑分割结果的数量了.模型是ConvAccu.ConvAccu模型是联合了一个卷积层和一个累加算术,效果见上图中橙红色画出的框图部分.

优化后的平面分割会和GT中的目标分割做交叉熵损失,注意这里目标分割的生成是选择GT中的分割和预测分割具有较大overlap的分割.如果这个overlap小于预测分割的一半,那么目标分割该部分就会被设置为空.注意这里输入的数据有plane分割,原始图像.

2.3 warp损失模块

该模块就是强制使得相近视角下训练的3D plane具有一致性的结构,使用ScanNet数据进行训练，简单说就是重投影到相邻帧,计算深度残差，最后做的是深度的残差，和我们一般做的重投影的像素残差或者归一化残差有所不同.公式如下:

简单的解释下该过程，一开始输入的有当前帧和相邻帧的depth（GT），相对pose，相机内参，注意没有像素点之间的关联（有的话就不需要像论文里这里复杂的做两次投影）。首先需要根据公式（2）计算相邻帧的像素点 $u^n,v^n)$ 到当前帧的像素点 $u^w,v^w)$ ，这一步骤中需要用到内外参和相邻帧像素点的深度（这是一个GT），这一步骤其实就是做两帧像素点的关联。找到关联关系后，在已知当前帧该像素点附近的深度，就可以通过双线性差值的方法计算 $u^w,v^w)$ 像素点的深度值大小，最后再反投影回相邻帧的像素点 $u^n,v^n)$ 就可以得到一个相邻帧的warp后的深度了。如此再调用公式（3）计算残差。

2.2 PlanarReconstruction

NYU-Toolbox开源代码位置(MATLAB)
开源主页和dataset位置

这篇文章和PlaneRCNN要干的事情基本上一摸一样，只是使用了不同的方法。

一.摘要以及贡献

从一张图片中实现了实时的平面语义分割和3D的平面参数恢复, 提出了基于associative embedding的两个步骤方法.第一个步骤是使用CNN训练得到一个embedding space,属于同一个平面的的像素会有相似的embedding.因此可以通过mean shift clustering模型给embedding vectors分组成平面区域(从而得到了2D图像的平面语义分割).第二个步骤,在已知2d的图像语义平面后,可以通过同时考虑像素级别和实例级别来估计平面参数(n,d).
论文的三个贡献:
(1)提出了两个步骤的深度学习框架来实现分段的平面3D重建.基于associate embedding模型,设计了多分支,端到端的训练网络可以提取不确定数量的平面,同时估计他们的参数.
(2)提出了一个快速的变种的mean shift cluster模型, 用来聚类pixel embedding得到平面分割实例.
(3)在ScanNet和NYUv2数据集上实现

二.模型算法

上图是整个网络模型的框架,主要是三个部分,分两个步骤.第一个步骤包含两个部分,先是CNN训练得到平面和非平面的语义分割地图以及pixel embedding.然后用语义分割来mask pixel embedding,再用mean shift clustering聚类mask后的pixel embedding.第二个步骤训练预测平面参数.所以下面分三个部分介绍对应的方法.

1.平面和非平面的语义分割

这一部分输入是RGB图像，最终输出的结果是平面和非平面的分割
就是一个二分类的问题,使用平衡交叉熵loss:

这里F和B可以理解成平面区域和非平面区域，交叉熵损失函数是DL做分类问题的常用方法。

2.embedding 模型

这一部分的输入是RGB图像，输出是每个像素点的embedding vector（可以理解成描述子）。
主要的功能是使用associative embedding方法预测每一个像素的embedding vector,这是一个高维的特征地图(feature map).embedding vector的具体意义是如果属于一个实例label,那么他们的embedding vectors的距离应该是比较小的.
因此训练的loss函数包含有两个部分,分别是pull和push loss.pull loss是pull每个像素的embedding vector到实例的mean embedding vector(实例的中心点), push loss是push实例中心点远离其他的实例中心点.

C是聚类的个数,也就是GT中平面的个数,Nc是该平面包含的元素的个数.也就是平面包含的点数.xi就是pixel embedding vector.uc是该平面的mean embedding vector.δv和δd是参数,实验部分有给出具体数值.这两个损失函数的意义是,当两个不同平面实例的距离大于δd,那么push loss惩罚就为零,当平面实例的embedding vector到其他实例中心点的距离小于δv,那么pull loss惩罚为零.其他情况下,对应的惩罚会相应的增加.uc(ucA,ucB)是平面的均值.

3.高效快速的mean shift clustering

关于mean shift clustering的文章
在2中已经获得了每个像素点的embedding vector,这里就可以聚类得到平面实例(平面语义分割),标准的mean shift clustering方法对所有的像素点做聚类,非常耗时,这里采用锚点的方法来降低耗时,提高速度.该模型最终是通过锚点输出平面语义分割的地图
首先计算每个像素的embedding vector和锚点的正态概率分布(公式5),来确定该vector属于该锚点的概率,然后通过公式(5)来更新锚点(公式6),这里aj是锚点（可以理解成平面的embedding vector）,xi是第i个像素点embedding vector

通过上面的模型进行迭代计算,最后会得到更加准确的锚点aj,对于相近的aj会进行合并.从而得到最终的平面锚点.
有了平面锚点,我们最后还需要把平面内所有的pixel embedding和聚类的锚点关联,关联的公式也是一种概率公式,表明像素是否属于该锚点,而不是简单的0/1的形式,这样的可微分形式,可以放入到学习中

整个模型的迭代过程如下:

4.平面参数估计

已经有了平面语义分割,现在预测平面参数.该网络分支输入是RGB图像，输出的是HW3的每个像素点的平面参数地图.对于像素级别的损失函数Pixel Plane为:

为什么是像素级别呢,这里的ni是每个像素i的平面参数,优化的是每个像素点的平面参数.ni是预测的平面参数,定义为平面normal/平面distance,ni*是GT.N是像素的个数.同属于一个平面的像素的平面参数并不是非常的一致,所以需要做instance-aware pooling过程,就是先把平面内所有的点的平面参数求一个平均得到nj,就是平面的平均的参数:

对于实例级别的参数损失函数Instance Plane为:

Sij就是前面的(7),为什么说是实例级别的呢,这里是对每个平面和平面上的点做损失,优化的是每个平面实例的平面参数.

论文提出的方法是先聚类出平面语义分割实例,然后再估计平面的3D参数.论文认为该方法更充分，因为分割可以唯一地确定一个实例,最终训练的loss:

三.平面优化与SLAM结合

前面讲了那么多平面的参数化表示和平面生成，其实都是一种3D场景的感知，我们是希望将感知到的平面这种结构化特征用到实际的SLAM跟踪上，可以起到和特征点一样的功能，姑且叫做特征面吧。下面看下特征面是如何使用到优化中的。

特征面在优化中的使用，现在主流的方案有三种方法：
（1）点面公式的使用 $n^Tp+d=0$ ，这种方式是考虑面上的3D点，有多少3D点就有多少个优化约束。
参考论文：Incremental Visual-Inertial 3D Mesh Generation with Structural Regularities
（2）两帧间平面的关联公式，假设一个平面被两个帧观测到，这个平面在两帧中的表达分别是 $π\pi$ 和 $π′\pi'$ ，两帧间的相对pose是T，那么可以构建关联公式 $π−T∗π′=0\pi-T*\pi'=\bf 0$ 。这种优化方式优点像特征点，是把一个面当作一个特征来做的。
参考论文：LIC-Fusion 2.0: LiDAR-Inertial-Camera Odometry with Sliding-Window Plane-Feature Tracking
（3）通过平面方程把点固定到平面上，得到3D点，然后对这个3D点做普通的重投影误差。这种方式同时结合了点面方程和点的重投影约束。
参考论文：Co-Planar Parametrization for Stereo-SLAM and Visual-Inertial Odometry

3D平面SLAM相关总结与思考相关推荐

在结构化场景中基于单目的物体与平面SLAM方案
点云PCL免费知识星球,点云论文速读. 文章:Monocular Object and Plane SLAM in Structured Environments 作者:Shichao Yang, S ...
单目3D物体级SLAM | CubeSLAM: Monocular 3D Object SLAM
点击上方"3D视觉工坊",选择"星标" 干货第一时间送达本文由知乎作者原野寻踪授权转载,不得擅自二次转载.原文链接:https://zhuanlan.zhih ...
ICRA2022 SLAM相关论文整理
目录视觉SLAM相关线.面.3Dfeature 多相机系统 VIO+GPS 其他数据集三维重建语义SLAM Special sensor Lidar相关视觉SLAM相关线.面.3Dfea ...
SLAM学习笔记（十九）开源3D激光SLAM总结大全——Cartographer3D,LOAM,Lego-LOAM,LIO-SAM,LVI-SAM,Livox-LOAM的原理解析及区别
本文为我在浙江省北大信研院-智能计算中心-情感智能机器人实验室-科技委员会所做的一个分享汇报,现在我把它搬运到博客中. 由于参与分享汇报的同事有许多是做其他方向的机器人工程师(包括硬件.控制等各方面并 ...
（汇总篇）语义SLAM相关开源方案| 全球优秀作者与实验室 | SLAM学习资料整理
目录 1 开源方案 1.1 Geometric SLAM (26项) 1. PTAM 2. S-PTAM(双目 PTAM) 3. MonoSLAM 4. ORB-SLAM2 5. DSO 6. LDS ...
[LiteratureReview]CubeSLAM Monocular 3-D Object SLAM
[LiteratureReview]CubeSLAM: Monocular 3-D Object SLAM 出处:2019 IEEE Transactions on Robotics,(截止到2022 ...
一文详解语义SLAM相关工作
作者丨方川@知乎来源丨https://zhuanlan.zhihu.com/p/379243930 编辑丨计算机视觉工坊动态SLAM和语义SLAM 场景中的动态物体不一定是object或不能得到o ...
[转] ICRA2022 SLAM相关论文整理
目录视觉SLAM相关线.面.3Dfeature 多相机系统 VIO+GPS 其他数据集三维重建语义SLAM Special sensor Lidar相关视觉SLAM相关线.面.3Dfea ...
九月SLAM相关论文速递
九月SLAM相关论文速递论文列表 DirectTracker: 3D Multi-Object Tracking Using Direct Image Alignment and Photometr ...
3D激光SLAM:ALOAM:异常点剔除机制代码解析
3D激光SLAM:ALOAM:异常点剔除机制代码解析前言 LOAM中的异常点 Code 前言在LOAM的论文中,作者提到了要剔除两种异常点. 平行点遮挡点但是在ALOAM的代码中并未写相关的功 ...

3D平面SLAM相关总结与思考