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

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

摘要

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

简介

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

理论基础

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

pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第1张图片
人眼中的厨房

1 . 室内地图

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

\
2. 系统构成
论文中的地图系统:
pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第2张图片

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

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

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

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

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

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

    • 计算步骤:

      1. 初始化
        1. 移除噪声:处理原始数据 P r a w d P^d_{raw} Prawd,使用统计学手段移除李群点,并减少噪声
        2. 计算相对位姿: P d : P^d: Pd:位于模型点 Q d Q^d Qd的收敛区域内,同时使用传感器位姿,传感器位姿不可用时, P d P^d Pd的位姿使用主成分(特征分解PCA)表示:
          p i : p_i: pi:归一化的姿态 q i ′ : q_i': qi:归一化的位置
          C p : C_p: Cp:位置协防差矩阵
          C q : C_q: Cq:姿态协防差矩阵
          T : T: T:转换矩阵
          { p i ′ = p i − 1 n ∑ i = 0 n p i , q i ′ = q i − 1 m ∑ i = 0 m q i C p = P T ∗ P S V D = U p ∗ S p ∗ V p T C q = Q T ∗ Q S V D = U q ∗ S q ∗ V q T T = Q d − ( U q ∗ U p T ) ∗ P d \begin{cases}p_i' =p_i-\frac{1}{n}\sum_{i=0}^np_i,q'_i=q_i-\frac{1}{m}\sum_{i=0}^mq_i\\ C_p=P^T*P^{SVD}=U_p*S_p*V_p^T\\ C_q=Q^T*Q^{SVD}=U_q*S_q*V_q^T\\ T=Q^d-(U_q*U_p^T)*P^d \end{cases} pi=pin1i=0npi,qi=qim1i=0mqiCp=PTPSVD=UpSpVpTCq=QTQSVD=UqSqVqTT=Qd(UqUpT)Pd
      2. 筛选特征并对齐点云
        使用多种几何属性,提取特征(比如平面和法线特征)
        pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第3张图片
      3. 使用聚类(用质心代替点云簇)以加速搜索
      4. 陪准多个视图形成完整的点云
    • 点云寄存器
      目的:将来自传感器不同位置的数据组合,以便得到目标的有效模型

      • 存在多种方法,均使用刚性变换,找到最小的旋转和平移时距离误差最小(ICP求解),即:
        P ∣ Q : P|Q: PQ:相邻的两个点云
        R : R: R:坐标转换矩阵
        t : t: 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)={RP+tR=[3×3],t=[1×3]}:e(R,t)=N1i=1NθqiRpit2

      • 提出了类似ICP算法(nD-ICP)

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

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

        3. 具体步骤:

          1. 移除噪声和离群点;

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

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

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

          5. 从原始点 P d P^d Pd选择最优的特征点集(子集) 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={pidPdFpi>u+dFσ}

          6. 重复如下步骤直到最大迭代次数或者误差小于预设值:
            1. 寻找 Q n d , P f d Q^d_n,P^d_f Qnd,Pfd中的对应点云:
            { 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)=mini=1dθ(Pfi,Qni)2
            2. 移除不配点 R A N S C RANSC RANSC
            3. 使用SVD计算转换矩阵 T = ( R , T ) T=(R,T) T=(R,T)
            4. 将转换矩阵应用到 P f d P^d_f Pfd,更新点的坐标: P f j d = R ∗ P f j − 1 d + t P^d_{f_j}=R*P_{f^d_{j-1}}+t Pfjd=RPfj1d+t
            5. 计算误差
            ξ = M S E ( d i s t ( P f d , Q n d ) ) \xi=MSE(dist(P_f^d,Q^d_n)) ξ=MSE(dist(Pfd,Qnd))

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

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

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

      • 离群点通过去除均方误差最大的点
      • 其余通过滑动最小二乘处理(MLS:Moving Least Squares)
        通过重采样(上采样或下采样)和丢弃不需要的数据来重现它所代表的平滑曲面。
        1. 使用包含整个长方体的下对绞线最为最大值,归一化点云坐标,使用
        u : u: u:平局值
        h : h: h:权重
        σ : \sigma: σ:两点之间平均距离的标准差
        h = u + k ∗ σ h=u+k*\sigma h=u+kσ
        2. 通过位于原始点 Q 3 Q^3 Q3,的邻域点集来近似点云并初始化迭代值,其坐标为重采样大小 s s 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={qkR3qkxyz=sspixyz,pjPd}
        3. q k ∈ Q ‾ 3 q_k\in \overline Q^3 qkQ3获取邻域值(半径内或固定数量的最近值),采用距离的权重(减少远距离的影响),等式如下: w ( p k ) q k = e x p ( − d i s t ( p j x y z , q k ) 2 h 2 ) , q k ∈ Q ‾ 3 , p j ∈ P k d w^{q_k}_{(p_k)}=exp(-\frac{dist(p_j^{xyz},q_k)^2}{h^2}),q_k \in \overline Q^3,p_j \in P_k^d w(pk)qk=exp(h2dist(pjxyz,qk)2),qkQ3,pjPkd
        4. 选择局部平面进行坐标转换:
        - 使用 P k d P^d_k Pkd拟合一个局部平面,并使距离误差(加权距离之和),为了减少离群点影响(虽然加权减少了离群点影响),使用RANSAC法:
        - 如何使用RANSAC:定义一组内点,选择与内点临域点并拟合,为了衡量准确性,使其加权和为(0-1)
        - 曲率和平面方程:通过内点的协防差矩阵求得曲率、平面拟合分解协防法矩阵获取
        - 颜色:内点的加权平均获取,如下:
        P g = 1 − ( ∑ p j ∈ P k d w P j q k ) − 1 ∗ ∑ P j ∈ P k d ; D p j ≤ d T w p j q k ∗ D P j P_g=1-(\sum_{p_j\in P^d_k}w_{P_j}^{q_k})^{-1}*\sum_{P_j \in P^d_k;D_{p_j}\leq dT}w^{q_k}_{p_j}*D_{P_j} Pg=1(pjPkdwPjqk)1PjPkd;DpjdTwpjqkDPj
        - pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第4张图片
        5. 最小二乘滑动二乘(MLS):多项式拟合后,将原始x,y,z坐标转换为拟合平面(切平面)的u,v,n,并重新计算点高度值(位于平面上方的所有点),得到新的高度(相对于拟合平面):方法如下(没看懂,可以自己去搜滑动最小二乘):效果图如下
        - 对于 t = 1 , 2 2 , 3 2 , . . , t=1,2^2,3^2,.., t=1,22,32,..,
        a , b ∈ N a n d a + b < t a,b \in N and a+ba,bNanda+b<t
        c ( q k ) = ( X W d i a g X T ) − 1 ∗ X ∗ W d i a g ∗ p l N , W d i a g ( l , l ) q k = w p l q k c^{(q_k)}=(XW_{diag}X^T)^{-1}*X*W_{diag}*p_l^N,W^{q_k}_{diag(l,l)}=w^{q_k}_{p_l} c(qk)=(XWdiagXT)1XWdiagplN,Wdiag(l,l)qk=wplqk
        q k ∈ Q ‾ 3 , q l ∈ P i n d , 存 在 X ( t , l ) = u n i q ( ( p l U ) a ∗ ( p l U ) ) b q_k\in \overline Q^3,q_l\in P^d_{in},存在X(t,l)=uniq((p_l^U)^a*(p_l^U))^b qkQ3,qlPindX(t,l)=uniq((plU)a(plU))b
        pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第5张图片

4 .环境地图
经过上述步骤处理后,可以得到地图上边界和边缘片段(检测目标区域的第一步),见下图,
- 上边橱柜没开
- 下边橱柜已开
pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第6张图片
并且对齐了点云,下图表示了对齐过程,
- 左上角的图片:(灰色点:目标模型 蓝色点:数据源 红色点:表示两个点云之间的用距离得到匹配点),
- 右上角的图片:对齐后的结果
- 下边两个:互相减去的结果
pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第7张图片

  • 橱柜的识别
    1. 提取出边缘后,需要执行WRANSAC(??),对点云进行聚类和线段搜索
    2. 通过边界聚簇(cluster boundaries),将线段增增为完全的直线, 同时在直线方向的空间上聚簇,将会有4个直线形成满足下列条件的矩形,图8为结果
    - 不相邻两条边方向相同且共面
    - 相邻两边角度接近90度
    - 矩形边的长度必须介于给定的最小值和最大值之间:论文中选择两个边分别为0.3和1.5m
    - 如果存在重叠的矩形,将其分割为更小的矩形
  • 桌子的识别
    需要识别桌子和桌子上的物品
    - 桌子
    1. 平面区域(数据的30%至少为平面),高度位于 h m a x ( 一 般 取 100 c m ) h_{max}(一般取100cm) hmax100cm h m i n ( 一 般 取 60 c m ) h_{min}(一般取60cm) hmin(60cm)之间
    2. 需要机器人设置地平面,如果未设置,使用包含做多点的平面作为地面。使用基于WRANSAC 的方法检测平面,结果见下图
    pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第8张图片
  1. 目标模型
    日常对象不同于测绘,目标对象通常是非静止的,文中提出了假设:
    • 能够知道物体的正面:这里正面的形状是多种多样的,因为对象核能有多个几何模块拼成,所以需要知道模型的所有参数
    • 对模型的要求:旋装不变性,闭塞、许多特征点构成
  • 点云重构
    进行重采样并填补孔洞(RMLS算法),之后使用三角剖析(如果对象是watertight(密集)的使用PowerCrust方法),见下图
    pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第9张图片
    1. 最后得到的物体会用于配准(RnDICP),通过马氏距离确定拟合的几何模型与源数据的是否一致。
    2. 使用SVM对数据进行划分。通过将nD 点减少到1维特征数组:计算4维的归一化曲率(得到自身切平面法线与其他切平面的关系)
      • 对于每一个点对 p 1 p_1 p1, p 2 p_2 p2,定义第一个点 p 1 p_1 p1作为原点可以得到:
        n 1 , n 2 : n_1,n_2: n1,n2:两个点的法线
        u = n 1 , v = ( p 2 − p 1 ) × u , w = u × v u=n_1,v=(p_2-p_1)×u,w=u×v u=n1,v=(p2p1)×u,w=u×v
      • 维特征 F c F_c Fc具有旋转和位置不变性,该种方法无法识别相同形状不同颜色的东西。
        F c = ∑ i = 1 i ≤ 4 b i n ( f i ) ∗ n r b i n 4 − i = { f i = ∣ ∣ p 2 − p 1 ∣ ∣ f 2 = v ∗ n 2 f 3 = a t a n ( w ∗ n 2 , u ∗ n 2 ) f 4 = u ∗ ( p 2 − p 1 ) / a F_c=\sum_{i=1}^{i\leq4}bin(f_i)*nr^{4-i}_{bin} =\begin{cases}f_i=||p_2-p_1|| \\ f_2=v*n_2\\ f_3=atan(w*n_2,u*n_2)\\ f_4=u*(p_2-p_1)/a\end{cases} Fc=i=1i4bin(fi)nrbin4i=fi=p2p1f2=vn2f3=atan(wn2,un2)f4=u(p2p1)/a
      • 使用曲率和颜色构建直方图对原模型进行拓展,如下图,只需要很小的区域就可以区分对象:
        pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots)_第10张图片
      1. 学习模型
        步骤:
        1. 获取目标模型,并保存为的多边形格式
        2. 将多边形模型转换为三角形格式
        3. 给定指定步长,拟合三角点
        4. 将具有不同标准差的零均值高斯噪声添加到每个生成的点云中
          以下是三种方法拟合结果:
          在这里插入图片描述

你可能感兴趣的:(pcl 学习论文阅读(Towards 3D Object Maps for Autonomous Household Robots))