pcl 学习论文阅读（Towards 3D Object Maps for Autonomous Household Robots）

Towards 3D Object Maps for Autonomous Household Robots，2007
室内环境的3D地图构建

摘要

• 描述了室内环境下的绘图系统，如何获取室内目标模型
• 系统分为3维分割和几何重建
• 门、桌子、抽屉、货架、眼镜盘子是该系统中重要的对象
• 该系统可以在复杂环境中辨识目标，并对新出现的物体进行分类
• 该算法关键部分
  1. 提供了一种稳定、准确且高效的算法：使用局部视角点云，构造完整的对象模型。
  2. 基于特征点的辨识桌子、门等对象。
  3. 自动推理目标对象和类别，可以在复杂或者真实环境下运行

简介

• 地图可以使节气人更好的完成它们的工作比如：自身运动和环境中的物体
• 目前大多数机器人具备定位和导航功能，只有少数提供地图辨识：地图不提供与机器人工作相关的对象，大多数情况下只提供几何原语（线、面），使地图过于抽象，在实际生活/工作环境中，需要考虑对象的性质并且同时识别在地图上的目标。
• 论文目标：，在map上找到一种描述符，能够提供物体识别，使自主机器人更加有效的工作

理论基础

• 从机器人的角度概念化的描述一个厨房的工作：比如摆桌子、准备饭菜、打扫卫生，并将这些概念转化为地图，再考虑相关计算问题，实现程序。

人眼中的厨房

1 . 室内地图

• 将地图转换为RG（Reigion-Gateway 区域互联）
• 用$<R,G>$表示地图，R:区域的集合，G：表示区域集合中，两个区域互相转换的可能性
• 一个区域$R$具有如下属性：
  1. office-like/hallway-like (办公室类/楼道类)
  2. 一个描述紧凑程度的几何描述符
  3. 一个边界框
  4. 1或2个主轴
  5. 相关联区域列表
  6. 关联对象$O$(指桌子等)
• 对象$O$:的表示和获取
  1. 对向$O:$用元组$<F,OL>$表示：$F:$可移动家具对象，$OL:$对象库
  2. $F$:可移动家具对象，包括：
     1. 3D模型集合
     2. 坐标点集
     3. 方向
     4. 对象类列表：箱体、抽屉、架子、桌子、障碍物
     箱体：包含所有可能为矩形的容器：烤箱、冰箱、刷碗机
     桌子：用来在表面放置物体，其矩形直方图臀高位置，可以拟合出一些已知尺寸和长度的对象
     抽屉和架子作为可移动对象
     作者实现库实现了一些日常使用对象的库：配料、盘子、罐子、等等，包含相应的状态
• 地图问题
  如何在建图过程中，获取最优的语义地图？
  • 传感器数据队列$<v_i,pos{e}_i>$
    • $v_1:$点云坐标系x,y,z,或者颜色/强度(点云有足够的分辨率并且对齐环境)
    • $pos{e}_i:$传感器当前位姿（位置、姿态），有可能是未知的
  • 输出
    • 环境：一系列紧凑的障碍物
    • 可以识别橱柜和抽屉，并标记其打开和关闭状态
• 变为具体问题：
  1. 给定：一些列传感器数据 { < v 1 , p o s e 1 > , . . . , < v n , p o s e n > } \{<v_1,pose_1>,...,<v_n,pose_n>\} {<v1​,pose1​>,...,<vn​,posen​>}
  2. 求解：如何得到基于矩形的简单几何组合表示：$v_1\cup ...\cup v_n$
  3. 再推理：目标模型桌子、抽屉、橱柜

\
2. 系统构成
论文中的地图系统：

• 第一步:原始点处理（论文中获取点模型）：
  1. 获得其在各个视角方向的，全面而精确的点模型:
     • 测量值处理：测量值是否异常：超出边界、视场角、离群点 等
     • 集成，聚类并提取特征（具有角度不变性）
     • 集成：通过对应点，进行最小二乘迭代求解
  2. 对象和环境模型：
     均存储在分割地图
     • 对象模型：平滑目标曲面并使用重采样进行曲面重建，得到多边形结构的点云、对象类别、本质特征
     • 环境模型：计算多边形描述符，对大的矩形进行辨别和解析：利用特殊的方式进行感知：比如利用被遮挡的矩形点云，识别大型矩形平面（橱柜、桌子、架子）
  3. 再用对象模型和环境映射模块处理得到分割的map。

\
3. 鲁棒性点云模型获取（点云原始数据处理）
原始数据:传感器在不同视角下获得多个点云

• 基于匹配的目标重建方法：

  • 求解问题：计算多视角下的map和传感器位置，同时计算视角之间的转换关系
  • 输出：移除了传感器噪声，并补全缺失信息的，多边形重构模型
  • 限制：计算问题的困难：数据量巨大，不能直接计算，部分点云不重合，等

• 一种稳健的建图计算方法

  • 通过对齐视图，利用迭代，不断求解最新的$P^d$,直到点云对齐同一对象和场景，称点云模型为$Q^d$,特征点最少需要50%真实重叠，

  • 适应度函数：第一个次扫描与后面扫描，对应点的距离

  • 计算步骤：

    1. 初始化
       1. 移除噪声：处理原始数据$P_{raw}^d$,使用统计学手段移除李群点，并减少噪声
       2. 计算相对位姿：$P^d:$位于模型点$Q^d$的收敛区域内，同时使用传感器位姿，传感器位姿不可用时，$P^d$的位姿使用主成分（特征分解PCA）表示：
          $p_i:$归一化的姿态$q_i^{\prime }:$归一化的位置
          $C_p:$位置协防差矩阵
          $C_q:$姿态协防差矩阵
          $T:$转换矩阵
          $$\{\begin{array}{l}{p}_i^{\prime }=p_i-\frac{1}{n}{\sum }_{i=0}^n{p}_i,q_i^{\prime }=q_i-\frac{1}{m}{\sum }_{i=0}^m{q}_i\\ C_p=P^T\ast P^{SVD}=U_p\ast S_p\ast V_p^T\\ C_q=Q^T\ast Q^{SVD}=U_q\ast S_q\ast V_q^T\\ T=Q^d-(U_q\ast U_p^T)\ast P^d\end{array}$$
    2. 筛选特征并对齐点云
       使用多种几何属性，提取特征（比如平面和法线特征）
       
    3. 使用聚类（用质心代替点云簇）以加速搜索
    4. 陪准多个视图形成完整的点云

  • 点云寄存器
    目的：将来自传感器不同位置的数据组合，以便得到目标的有效模型

    • 存在多种方法，均使用刚性变换，找到最小的旋转和平移时距离误差最小（ICP求解），即：
      $P|Q:$相邻的两个点云
      $R:$坐标转换矩阵
      $t:$列向量
      转 换 矩 阵 ： T ( P , Q ) = { R ∗ P + t ∣ R = [ 3 × 3 ] , t = [ 1 × 3 ] } 误 差 函 数 : e ( R , t ) = 1 N ∑ i = 1 N θ ∣ ∣ q i − R ∗ p i − t ∣ ∣ 2 转换矩阵：T(P,Q)=\{R*P+t |R=[3×3],t=[1×3]\}\\ 误差函数:e(R,t)=\frac{1}{N}\sum_{i=1}^N\theta ||q_i-R*p_i-t||^2 转换矩阵：T(P,Q)={R∗P+t∣R=[3×3],t=[1×3]}误差函数:e(R,t)=N1​i=1∑N​θ∣∣qi​−R∗pi​−t∣∣2

    • 提出了类似ICP算法（nD-ICP）

      1. 提取两个点云的特征用于对齐过程

      2. 在对齐时，使用传感器提供额外的纬度（6D：XYZRGB, 9D:XYZRGB,nx,ny,nz（法线））用于缩短匹配时间

      3. 具体步骤：

         1. 移除噪声和离群点;

         2. 提供使点云$P^d$与$Q^d$尽可能对齐的初值，如果传感器不能提供初值，进行PCA/特征分析得到特征值最大的向量作为初始迭代值;

         3. 估计几何特这并计算特征（法向量或者曲率） ;

         4. 将点云模型$Q^d$聚类，形成k个簇 C k = { c 1 , . . . , c k } C_k=\{c_1,...,c_k\} Ck​={c1​,...,ck​}

         5. 从原始点$P^d$选择最优的特征点集（子集） P f d = { p i d ∈ P d ∣ F p i > u + d F ∗ σ } P^d_f=\{p_i^d \in P^d |F_{p_i}>u+d_F*\sigma\} Pfd​={pid​∈Pd∣Fpi​​>u+dF​∗σ}

         6. 重复如下步骤直到最大迭代次数或者误差小于预设值：
            1. 寻找$Q_n^d,P_f^d$中的对应点云:
            { Q n d = { q n 1 d , . . . , q n s d } d i s t ( P f , Q n ) = m i n ∑ i = 1 d θ ( P f i , Q n i ) 2 \begin{cases}Q_n^d=\{q^d_{n1},...,q_{ns}^d\}\\ dist(P_f,Q_n)=min\sqrt{\sum_{i=1}^d\theta(P^i_f,Q^i_n)^2}\end{cases} {Qnd​={qn1d​,...,qnsd​}dist(Pf​,Qn​)=min∑i=1d​θ(Pfi​,Qni​)2 ​​
            2. 移除不配点$RANSC$
            3. 使用SVD计算转换矩阵$T=(R,T)$
            4. 将转换矩阵应用到$P_f^d$，更新点的坐标:$P_{f_j}^d=R\ast P_{f_{j-1}^d}+t$
            5. 计算误差
            $$\xi =MSE(dist(P_f^d,Q_n^d))$$

         7. 产生 registered nD 模型，包含$x,y,z,nX,nY,nZ$等其他特征，RGB如果可靠也可以使用

      4. 进行边缘（曲率大的点会被提取）和边界检测，边界点在3D上可能找不到，然而如果将其映射到2D空间上，可以很容易的识别出边界，因为对于边界点来说，向量与邻域点之间形成的最大角度值，会明显大于边界点内部的值，可以将邻域点投影到其法平面内求解，计算方法与移除离群点一样。

  • 点云处理和重采样
    测量时包含误差、离群点、空洞等，会影响多边形重构

    • 离群点通过去除均方误差最大的点
    • 其余通过滑动最小二乘处理（MLS：Moving Least Squares）
      通过重采样（上采样或下采样）和丢弃不需要的数据来重现它所代表的平滑曲面。
      1. 使用包含整个长方体的下对绞线最为最大值，归一化点云坐标，使用
      $u:$平局值
      $h:$权重
      $\sigma :$两点之间平均距离的标准差
      $$h=u+k\ast \sigma$$
      2. 通过位于原始点$Q^3$，的邻域点集来近似点云并初始化迭代值，其坐标为重采样大小$s$（step size）的整数倍：
      - 有孔洞的地方已经被填充
      - 类似于边界检测，检测一个物体是否在内部，其最大角度应小于指定角度，确保正确填充
      Q 3 = { q k ∈ R 3 ∣ q k x y z = s ∗ ∣ p i x y z s ∣ , p j ∈ P d } Q^3=\{q_k \in R^3 | q_k^{xyz}=s*|\frac{p_i^{xyz}}{s}|,p_j \in P^d\} Q3={qk​∈R3∣qkxyz​=s∗∣spixyz​​∣,pj​∈Pd}
      3. $q_k\in {\overline{Q}}^3$获取邻域值（半径内或固定数量的最近值），采用距离的权重（减少远距离的影响），等式如下：$$w_{(p_k)}^{q_k}=exp(-\frac{dist(p_j^{xyz},q_k{)}^2}{h^2}),q_k\in {\overline{Q}}^3,p_j\in P_k^d$$
      4. 选择局部平面进行坐标转换：
      - 使用$P_k^d$拟合一个局部平面，并使距离误差（加权距离之和），为了减少离群点影响（虽然加权减少了离群点影响），使用RANSAC法：
      - 如何使用RANSAC：定义一组内点，选择与内点临域点并拟合，为了衡量准确性，使其加权和为（0-1）
      - 曲率和平面方程：通过内点的协防差矩阵求得曲率、平面拟合分解协防法矩阵获取
      - 颜色：内点的加权平均获取，如下：
      $$P_g=1-(\sum _{p_j\in P_k^d}{w}_{P_j}^{q_k}{)}^{-1}\ast \sum _{P_j\in P_k^d;D_{p_j}\le dT}{w}_{p_j}^{q_k}\ast D_{P_j}$$
      -
      5. 最小二乘滑动二乘（MLS）：多项式拟合后，将原始x,y,z坐标转换为拟合平面（切平面）的u,v,n，并重新计算点高度值（位于平面上方的所有点），得到新的高度（相对于拟合平面）：方法如下（没看懂，可以自己去搜滑动最小二乘）：效果图如下
      - 对于$t=1,2^2,3^2,..,$
      $a,b\in Nanda+b<t$
      $c^{(q_k)}=(X{W}_{diag}{X}^T{)}^{-1}\ast X\ast W_{diag}\ast p_l^N,W_{diag(l,l)}^{q_k}=w_{p_l}^{q_k}$
      $$q_k\in {\overline{Q}}^3,q_l\in P_{in}^d，存在X(t,l)=uniq((p_l^U{)}^a\ast (p_l^U){)}^b$$
      

4 .环境地图
经过上述步骤处理后，可以得到地图上边界和边缘片段（检测目标区域的第一步），见下图，
- 上边橱柜没开
- 下边橱柜已开

并且对齐了点云,下图表示了对齐过程，
- 左上角的图片：（灰色点：目标模型 蓝色点：数据源 红色点：表示两个点云之间的用距离得到匹配点），
- 右上角的图片：对齐后的结果
- 下边两个：互相减去的结果

• 橱柜的识别
  1. 提取出边缘后，需要执行WRANSAC（？？），对点云进行聚类和线段搜索
  2. 通过边界聚簇(cluster boundaries)，将线段增增为完全的直线, 同时在直线方向的空间上聚簇，将会有4个直线形成满足下列条件的矩形,图8为结果
  - 不相邻两条边方向相同且共面
  - 相邻两边角度接近90度
  - 矩形边的长度必须介于给定的最小值和最大值之间：论文中选择两个边分别为0.3和1.5m
  - 如果存在重叠的矩形，将其分割为更小的矩形
• 桌子的识别
  需要识别桌子和桌子上的物品
  - 桌子
  1. 平面区域（数据的30%至少为平面），高度位于$h_{max}（一般取100cm）$与$h_{min}(一般取60cm)$之间
  2. 需要机器人设置地平面，如果未设置，使用包含做多点的平面作为地面。使用基于WRANSAC 的方法检测平面，结果见下图
  

5. 目标模型
   日常对象不同于测绘，目标对象通常是非静止的，文中提出了假设：
   • 能够知道物体的正面：这里正面的形状是多种多样的，因为对象核能有多个几何模块拼成，所以需要知道模型的所有参数
   • 对模型的要求：旋装不变性，闭塞、许多特征点构成

• 点云重构
  进行重采样并填补孔洞（RMLS算法），之后使用三角剖析（如果对象是watertight(密集)的使用PowerCrust方法）,见下图
  
  1. 最后得到的物体会用于配准(RnDICP)，通过马氏距离确定拟合的几何模型与源数据的是否一致。
  2. 使用SVM对数据进行划分。通过将nD 点减少到1维特征数组：计算4维的归一化曲率（得到自身切平面法线与其他切平面的关系）
     • 对于每一个点对$p_1$,$p_2$,定义第一个点$p_1$作为原点可以得到：
       $n_1,n_2:$两个点的法线
       $u=n_1,v=(p_2-p_1)\times u,w=u\times v$
     • 维特征$F_c$具有旋转和位置不变性,该种方法无法识别相同形状不同颜色的东西。
       $$F_c=\sum _{i=1}^{i\le 4}bin(f_i)\ast n{r}_{bin}^{4-i}=\{\begin{array}{l}{f}_i=||p_2-p_1||\\ f_2=v\ast n_2\\ f_3=atan(w\ast n_2,u\ast n_2)\\ f_4=u\ast (p_2-p_1)/a\end{array}$$
     • 使用曲率和颜色构建直方图对原模型进行拓展，如下图，只需要很小的区域就可以区分对象：
       
     3. 学习模型
        步骤：
        1. 获取目标模型，并保存为的多边形格式
        2. 将多边形模型转换为三角形格式
        3. 给定指定步长，拟合三角点
        4. 将具有不同标准差的零均值高斯噪声添加到每个生成的点云中
           以下是三种方法拟合结果：