深度图像中的自动地面检测及摄像机相对位姿估计方法
【专利摘要】本发明公开了一种深度图像中的自动地面检测及摄像机相对位姿估计方法，该方法步骤包括：标定深度图像；逐像素计算深度图像depth中各点在深度摄像机坐标系下的点云坐标；基于随机采样一致性提取地面；提取地面法向矢量；计算世界坐标系下的点云坐标。本发明无需人工干预，可全自动进行地面的检测，且由于该方法是基于三维深度信息的，相比基于图像颜色信息的方法，对地面的检测更加准确，而且还可以恢复出场景中各像素的高度。另外本发明的方法仅需要一幅图像即可完成当前深度摄像机位姿以及场景高度的求解，不依赖于历史信息。
【专利说明】深度图像中的自动地面检测及摄像机相对位姿估计方法
【技术领域】
[0001] 本发明涉及一种深度图像中的自动地面检测及摄像机相对位姿估计方法。
【背景技术】
[0002] 地面检测已经成为机器人、自动驾驶、个人娱乐、视频监控等领域中至为关键的技 术。基于图像的传统地面检测方法大多数根据地面的先验颜色信息、颜色信息的一致性以 及强烈的边缘来进行地面的检测，比如利用公路的深色路面以及路边的白色标线信息等。 其工作方式只能够适用于一些比较简单的场景，对于复杂的环境不具有可靠的性能。另外 在这些应用系统中，很多时候需要随意变更深度摄像机的位置，或者深度摄像机的位置会 受到系统的影响而发生位置变化，因而无法随时估计深度摄像机相对于地面的高度、姿态 以及场景中其他物体相对于地面的高度。
[0003] 由于能够直接获取在一般透视成像变换中丢失的深度信息，深度摄像机相比于普 通可见光深度摄像机有着更加明显的技术优势。这些深度摄像机包括但不限于基于双目视 觉法，飞行时间法以及光斑编码法三种制式的深度摄像机。在这些应用中，深度摄像机大多 通过水平安装或者斜下视的方式对场景进行观测，因而地面的自动化检测对于系统的自动 化程度以及稳定性提升都是至关重要的。
[0004] 现有技术需要知道许多有关地面的先验信息，不具有一般普适性。因而只能够适 用于一些比较简单的场景，如对于复杂的环境不具有可靠的性能。另外现有技术一般难于 得到场景中各点的高度信息，限制了系统的应用范围。
【发明内容】
[0005] 本发明所要解决的技术问题是，针对现有技术不足，提供一种深度图像中的自动 地面检测及摄像机相对位姿估计方法，自动检测深度图像中的地面，同时求解出深度摄像 机相对于地面的位置和姿态以及场景中各点相对于地面的位置。
[0006] 为解决上述技术问题，本发明所采用的技术方案是：一种深度图像中的自动地面 检测及摄像机相对位姿估计方法，包括以下步骤：
[0007] 1)将深度图像cbpth的像素坐标（X，y)转换为弧度坐标（u，V):
【权利要求】
1. 一种深度图像中的自动地面检测及摄像机相对位姿估计方法，其特征在于，包括以 下步骤： 1) 将深度图像Cbpth的像素坐标（x，y)转换为弧度坐标（u，V):
其中，A为深度摄像机内参数矩阵； 2) 逐像素计算深度图像depth中各点在深度摄像机坐标系下的点云坐标：求取任一像 素点（u，V)在以深度摄像机位置为原点，由深度摄像机横轴X。，深度图像纵轴Y。，深度方向 Z。构成的右手深度摄像机坐标系下的点云坐标（x。(u, V)，y。(u, V)，z。(u, V)),即： xc (u, v) = V X depth (u, v)； yc (u, v) = u X depth (u, v)； zc (u, v) = depth (u, v)； 其中cbpth(u，v)为坐标（u，v)处的深度图像的深度值； 3) 每次采样过程中，随机在深度图像中选择三个点PpP2^3，然后求取由此三点 P1, P2, P3构成的平面n的法向量：
其中，P1P2表示由点P1指向P2的向量，?表示向量的叉积；选取第四点P 4进行如下检 验： Iiormit X P1P4 < Th ； 其中，阈值Th = 2.56, S为深度图像中深度值的测量噪声标准差；若第四点匕满足 iion^x P,P4| <77?.，那么第四点P4为平面上的点；依照此步骤遍历整幅深度图像，如果 一幅深度图像中满足.IMrn^xP1PilIcr/!的点的比例超过阈值Rati 〇,则认为平面即为待 检测地面； 4) 提取出满足式|n〇ri< X PlPilIcTZi测试的所有点在深度摄像机坐标系下的坐标，记 为：(AXX)，......(<,乂，<)，求取出这些点的坐标平均值（1；^〇，；^，2；^)， 并按照下式构造矩阵M : VI -M1 M1 ；
5) 计算待检测地面的法向量在深度摄像机坐标系下的坐标如果 norm；；,.-的第二项为正，则令= 调整深度摄像机的位置，使 得|riorm_,,,,⑴I < 0.01，然后计算深度摄像机相对于待检测地面的高度heighteMeM， 办四?挪=-[尤Lg，乂叹，C x normcg_rf;其中，norm:- (1)表示 6) 计算世界坐标系的Z轴方向Zw在深度摄像机坐标系下的坐标&: Z:,.=Horm^mrf7 并确定深度摄像机坐标系下深度图像的点（\，7。，2。）到世界坐标系下深度图像的点 (xw, yw, zw)的关系：
坐标系是指以待检测地面为水平面XY,高度为垂直于Z轴方向，坐标原点为深度摄像机在 地面的垂足确定的坐标系。
2. 根据权利要求1所述的深度图像中的自动地面检测及摄像机相对位姿估计方法，其 特征在于，所述步骤1)中，深度摄像机内参数矩阵记为A通过直接线性法、两步法、平面法 中的一种计算得到。
3. 根据权利要求1或2所述的深度图像中的自动地面检测及摄像机相对位姿估计方 法，其特征在于，所述步骤3)中，其中比值Ratio依据图像中的地面部分占整幅图像中的比 值而定，如果地面占整幅图像中的比例为Ratiogramd，那么Ratio = 0? 6?0? 8Ratiogramd。
4. 根据权利要求3所述的深度图像中的自动地面检测及摄像机相对位姿估计方法， 其特征在于，所述步骤5)中，的计算过程如下：对矩阵M做标准奇异值分解： (U，D，V) = svd(M);使得M = UDV，其中矩阵U，V均为单位酉矩阵，矩阵D为矩阵M的特征 值所构成的对角阵；V矩阵的最后一列即为待检测地面的法向量在深度摄像机坐标系下的 坐标Horm^nrf。
【文档编号】G06T7/00GK104361575SQ201410556886
【公开日】2015年2月18日 申请日期:2014年10月20日 优先权日:2014年10月20日
【发明者】不公告发明人 申请人:湖南戍融智能科技有限公司