CN104361575A - 深度图像中的自动地面检测及摄像机相对位姿估计方法 - Google Patents

Abstract

本发明公开了一种深度图像中的自动地面检测及摄像机相对位姿估计方法，该方法步骤包括：标定深度图像；逐像素计算深度图像depth中各点在深度摄像机坐标系下的点云坐标；基于随机采样一致性提取地面；提取地面法向矢量；计算世界坐标系下的点云坐标。本发明无需人工干预，可全自动进行地面的检测，且由于该方法是基于三维深度信息的，相比基于图像颜色信息的方法，对地面的检测更加准确，而且还可以恢复出场景中各像素的高度。另外本发明的方法仅需要一幅图像即可完成当前深度摄像机位姿以及场景高度的求解，不依赖于历史信息。

Description

深度图像中的自动地面检测及摄像机相对位姿估计方法

技术领域

本发明涉及一种深度图像中的自动地面检测及摄像机相对位姿估计方法。

背景技术

地面检测已经成为机器人、自动驾驶、个人娱乐、视频监控等领域中至为关键的技术。基于图像的传统地面检测方法大多数根据地面的先验颜色信息、颜色信息的一致性以及强烈的边缘来进行地面的检测，比如利用公路的深色路面以及路边的白色标线信息等。其工作方式只能够适用于一些比较简单的场景，对于复杂的环境不具有可靠的性能。另外在这些应用系统中，很多时候需要随意变更深度摄像机的位置，或者深度摄像机的位置会受到系统的影响而发生位置变化，因而无法随时估计深度摄像机相对于地面的高度、姿态以及场景中其他物体相对于地面的高度。

由于能够直接获取在一般透视成像变换中丢失的深度信息，深度摄像机相比于普通可见光深度摄像机有着更加明显的技术优势。这些深度摄像机包括但不限于基于双目视觉法，飞行时间法以及光斑编码法三种制式的深度摄像机。在这些应用中，深度摄像机大多通过水平安装或者斜下视的方式对场景进行观测，因而地面的自动化检测对于系统的自动化程度以及稳定性提升都是至关重要的。

现有技术需要知道许多有关地面的先验信息，不具有一般普适性。因而只能够适用于一些比较简单的场景，如对于复杂的环境不具有可靠的性能。另外现有技术一般难于得到场景中各点的高度信息，限制了系统的应用范围。

发明内容

本发明所要解决的技术问题是，针对现有技术不足，提供一种深度图像中的自动地面检测及摄像机相对位姿估计方法，自动检测深度图像中的地面，同时求解出深度摄像机相对于地面的位置和姿态以及场景中各点相对于地面的位置。

为解决上述技术问题，本发明所采用的技术方案是：一种深度图像中的自动地面检测及摄像机相对位姿估计方法，包括以下步骤：

1)将深度图像depth的像素坐标(x,y)转换为弧度坐标(u,v)：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=A^{-1}\left[\begin{array}{c}x\\ y\\ 1\end{array}\right];$

其中，A为深度摄像机内参数矩阵；

2)逐像素计算深度图像depth中各点在深度摄像机坐标系下的点云坐标：求取任一像素点(u,v)在以深度摄像机位置为原点，由深度摄像机横轴X_c，深度图像纵轴Y_c，深度方向Z_c构成的右手深度摄像机坐标系下的点云坐标(x_c(u,v),y_c(u,v),z_c(u,v))，即：

x_c(u,v)＝v×depth(u,v)；

y_c(u,v)＝u×depth(u,v)

z_c(u,v)＝depth(u,v)；

其中depth(u,v)为坐标(u,v)处的深度图像的深度值；

3)每次采样过程中，随机在深度图像中选择三个点P₁,P₂,P₃，然后求取由此三点P₁,P₂,P₃构成的平面π的法向量：

${\mathrm{norm}}_{\mathrm{\π}}^{\′c}=P_1{P}_2\⊗P_1{P}_3$

将所述法向量单位化，即：

${\mathrm{norm}}_{\mathrm{\π}}^c=\frac{{\mathrm{norm}}_{\mathrm{\π}}^{\′c}}{\left|\right|{\mathrm{norm}}_{\mathrm{\π}}^{\′c}\left|\right|}$

式错误！未找到引用源。中P₁P₂表示由点P₁指向P₂的向量，表示向量的叉积；然后再选取第四点P₄进行如下检验：

$|{\mathrm{norm}}_{\mathrm{\&pi;}}^c\&times;P_1{P}_4|<\mathrm{Th}$

式(7)中阈值Th＝2.5δ，δ为深度图像中深度值的测量噪声标准差；如果式(7)得到满足，那么认为第四点P₄为平面π上的点；依照此步骤遍历整幅深度图像，如果一幅深度图像中满足测试条件(7)点的比例超过阈值Ratio，则认为平面π即为待检测地面，其中比值Ratio依据图像中的地面部分占整幅图像中的比值而定，如果地面占整幅图像中的比例为Ratio_ground，那么最佳取值范围为Ratio＝0.6～0.8Ratio_ground；

4)提取出满足式(7)测试的所有点在深度摄像机坐标系下的坐标，记为： 然后求取出这些点的坐标平均值并按照下式构造矩阵M：

$M=M_1^T{M}_1$

其中， $M_1=\left[\begin{array}{ccc}{x}_1^c& y_1^c& z_1^c\\ x_2^c& y_2^c& z_2^2\\ & ...& \\ x_n^c& y_n^c& z_n^c\end{array}\right]-\left[\begin{array}{ccc}{x}_{\mathrm{avg}}^c& Y_{\mathrm{avg}}^c& z_{\mathrm{avg}}^c\\ x_{\mathrm{avg}}^c& Y_{\mathrm{avg}}^c& z_{\mathrm{avg}}^c\\ & ...& \\ x_{\mathrm{avg}}^c& y_{\mathrm{avg}}^c& z_{\mathrm{avg}}^c\end{array}\right];$

5)计算待检测地面的法向量在深度摄像机坐标系下的坐标如果的第二项为正，则令 ${\mathrm{norm}}_{\mathrm{ground}}^c=-{\mathrm{norm}}_{\mathrm{ground}}^c;$ 调整摄像机的位置，使得 $\left|{\mathrm{norm}}_{\mathrm{ground}}^c\left(1\right)\right|<0.01,$ 然后计算深度摄像机相对于待检测地面的高度height_camera： ${\mathrm{height}}_{\mathrm{camera}}=-[x_{\mathrm{avg}}^c,y_{\mathrm{avg}}^c,z_{\mathrm{avg}}^c]\&times;{\mathrm{norm}}_{\mathrm{ground}}^c;$

6)计算世界坐标系的Z轴方向Z_w在深度摄像机坐标系下的坐标并确定深度摄像机坐标系下深度图像的点(x_c,y_c,z_c)到世界坐标系下深度图像的点(x_w,y_w,z_w)的关系：

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=\left[\begin{array}{c}{X}_w^c\\ Y_w^c\\ Z_w^c\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ {\mathrm{height}}_{\mathrm{camera}}\end{array}\right]$

其中， $X_w^c=\frac{X_w^{c^{\&prime;}}}{\left|\right|X_w^{c^{\&prime;}}\left|\right|},$ $X_w^{c\&prime;}=X_w^c-(X_w^c\&CenterDot;Z_w^c)\&CenterDot;Z_w^c,$ $X_w^c=\left[\mathrm{1,0,0}\right];$ $Y_w^c=Z_w^c\&CircleTimes;X_w^c;$ 所述世界坐标系是指以待检测地面为水平面XY，高度为垂直于Z轴方向，坐标原点为深度摄像机在地面的垂足确定的坐标系。

所述步骤5)中，的计算过程如下：对矩阵M做标准奇异值分解：(U,D,V)＝svd(M)；使得M＝UDV，其中矩阵U,V均为单位酉矩阵，矩阵D为矩阵M的特征值所构成的对角阵。V矩阵的最后一列即为待检测地面的法向量在深度摄像机坐标系下的坐标

与现有技术相比，本发明所具有的有益效果为：本发明无需人工干预，可全自动进行地面的检测，且由于该方法是基于三维深度信息的，相比基于图像颜色信息的方法，对地面的检测更加准确，而且还可以恢复出场景中各像素的高度。另外本发明的方法仅需要一幅图像即可完成当前深度摄像机位姿以及场景高度的求解，不依赖于历史信息。

具体实施方式

说明：在描述本发明的公式中，用正体加粗表示多维向量以及矩阵，如A，depth，等，而斜体不加粗表示一维的变量，如x，y，z，u，v等。

本发明的实现步骤如下：

第一步、标定图像：

假设已知的深度摄像机内参数矩阵记为A，内参数矩阵可离线通过经典的标定方法如Tsai的两步法^[1]以及张正友的平面法^[2]等计算出。在获取了内参数矩阵以后，将深度图像的像素坐标转换为弧度坐标，如下式错误！未找到引用源。所示：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=A^{-1}\left[\begin{array}{c}x\\ y\\ 1\end{array}\right]---\left(1\right)$

上式中u,v表示以弧度为单位的坐标，x,y表示深度图像的像素坐标。

第二步、逐像素计算深度图像depth中各点在深度摄像机坐标系下的点云坐标；

求取各个像素点在以深度摄像机位置为原点，由深度摄像机横轴X_c(U方向，图像左至右)，图像纵轴Y_c(V方向，图像上至下)，深度方向Z_c(D方向)构成的右手深度摄像机坐标系下的点云坐标。即：

x_c(u,v)＝v×depth(u,v)        (2)

y_c(u,v)＝u×depth(u,v)        (3)

z_c(u,v)＝depth(u,v)         (4)

第三步、基于随机采样一致性(RANSAC)的地面提取；

每次采样过程中，随机在深度图像中选择三个点P₁,P₂,P₃，然后求取由此三点P₁,P₂,P₃构成的平面π的法向量：

${\mathrm{norm}}_{\mathrm{\&pi;}}^{\&prime;c}=P_1{P}_2\&CircleTimes;P_1{P}_3---\left(5\right)$

将所述法向量单位化，即：

${\mathrm{norm}}_{\mathrm{\&pi;}}^c=\frac{{\mathrm{norm}}_{\mathrm{\&pi;}}^{\&prime;c}}{\left|\right|{\mathrm{norm}}_{\mathrm{\&pi;}}^{\&prime;c}\left|\right|}---\left(6\right)$

式(6)中P₁P₂表示由点P₁指向P₂的向量，表示向量的叉积；然后再选取第四点P₄进行如下检验：

$|{\mathrm{norm}}_{\mathrm{\&pi;}}^c\&times;P_1{P}_4|<\mathrm{Th}$

式(7)中阈值Th＝2.5δ，δ为深度图像中深度值的测量噪声标准差；如果式错误！未找到引用源。得到满足，那么认为第四点P₄为平面π上的点；依照此步骤遍历整幅深度图像，如果一幅深度图像中满足测试条件错误！未找到引用源。点的比例超过阈值Ratio，则认为平面π即为待检测地面，其中比值Ratio依据图像中的地面部分占整幅图像中的比值而定，如果地面占整幅图像中的比例为Ratio_ground，那么最佳取值范围为Ratio＝0.6～0.8Ratio_ground；

第四步：基于奇异值分解的地面法向矢量提取；

首先提取出平面上所有点在深度摄像机坐标系下的坐标，记为： 然后求取出这些点的坐标平均值并按照如下式(8)以及式(9)构造矩阵M。

$M_1=\left[\begin{array}{ccc}{x}_1^c& y_1^c& z_1^c\\ x_2^c& y_2^c& z_2^2\\ & ...& \\ x_n^c& y_n^c& z_n^c\end{array}\right]-\left[\begin{array}{ccc}{x}_{\mathrm{avg}}^c& Y_{\mathrm{avg}}^c& z_{\mathrm{avg}}^c\\ x_{\mathrm{avg}}^c& Y_{\mathrm{avg}}^c& z_{\mathrm{avg}}^c\\ & ...& \\ x_{\mathrm{avg}}^c& y_{\mathrm{avg}}^c& z_{\mathrm{avg}}^c\end{array}\right];$

$M=M_1^T{M}_1$

接着对矩阵M做标准奇异值分解(Singular Value Decomposition)：

(U,D,V)＝svd(M)       (10)

其中矩阵U,V均为单位酉矩阵，矩阵D为矩阵M的特征值所构成的对角阵，且满足M＝UDV。V矩阵的最后一列即为待检测地面的法向量在深度摄像机坐标系下的坐标

如果的第二项为正，需要利用 ${\mathrm{norm}}_{\mathrm{ground}}^c=-{\mathrm{norm}}_{\mathrm{ground}}^c$ 对进行替代。如果要求深度摄像机水平放置，那么矢量的第一项即描述了此特性。如果此项不接近于0，表示深度摄像机没有水平放置，需要重新调整，如果第一项为负，摄像头右侧应往上调整，如果第一项为正，摄像头右侧应往下调整，调整到为佳。同时可以得到深度摄像机相对于底面的高度如下：

${\mathrm{height}}_{\mathrm{camera}}=-[x_{\mathrm{avg}}^c,y_{\mathrm{avg}}^c,z_{\mathrm{avg}}^c]\&times;{\mathrm{norm}}_{\mathrm{ground}}^c;$

第五步：计算世界坐标系下的点云坐标

这里的世界坐标系设定为以地面为水平面XY，以高度为垂直Z轴方向，坐标原点为深度摄像机在地面的垂足。

设表示世界坐标系的Z轴方向Z_w在深度摄像机坐标系下的坐标，则有：

$Z_w^c={\mathrm{norm}}_{\mathrm{ground}}^c---\left(12\right)$

同理再预设然后去相关：

$X_w^{c\&prime;}=X_w^c-(X_w^c\&CenterDot;Z_w^c)\&CenterDot;Z_w^c---\left(13\right)$

再归一化得到接着计算出：

$Y_w^c=Z_w^c\&CircleTimes;X_w^c---\left(14\right)$

这样我们可以描述深度摄像机坐标系下到世界坐标系的关系：

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=\left[\begin{array}{c}{X}_w^c\\ Y_w^c\\ Z_w^c\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ {\mathrm{height}}_{\mathrm{camera}}\end{array}\right]$

其中 $\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]=\left[\begin{array}{c}v\\ u\\ 1\end{array}\right]\&CenterDot;\mathrm{depth}(u,v),$ depth(u,v)为坐标(u,v)处的深度值。

至此已经完成了深度摄像机高度height_camera以及世界坐标系与深度摄像机坐标系之间的表示姿态的旋转矩阵 $\left[\begin{array}{c}{X}_w^c\\ Y_w^c\\ Z_w^c\end{array}\right]$ 和表示位置的平移向量 $\left[\begin{array}{c}0\\ 0\\ {\mathrm{height}}_{\mathrm{camera}}\end{array}\right],$ 也就可以通过式(15)计算出深度图像中各点与世界坐标系之间的一一对应关系。

本发明所指的深度摄像机均假设其内参数已知，并可以经内参数矩阵的校正转化为理想的透视投影深度摄像机模型。

Claims (4)

1.一种深度图像中的自动地面检测及摄像机相对位姿估计方法，其特征在于，包括以下步骤：

1)将深度图像depth的像素坐标(x,y)转换为弧度坐标(u,v)：

$\left[\begin{array}{c}u\\ v\\ 1\end{array}\right]=A^{-1}\left[\begin{array}{c}x\\ y\\ 1\end{array}\right];$

其中，A为深度摄像机内参数矩阵；

2)逐像素计算深度图像depth中各点在深度摄像机坐标系下的点云坐标：求取任一像素点(u,v)在以深度摄像机位置为原点，由深度摄像机横轴X_c，深度图像纵轴Y_c，深度方向Z_c构成的右手深度摄像机坐标系下的点云坐标(x_c(u,v),y_c(u,v),z_c(u,v))，即：

x_c(u,v)＝v×depth(u,v)；

y_c(u,v)＝u×depth(u,v)；

z_c(u,v)＝depth(u,v)；

其中depth(u,v)为坐标(u,v)处的深度图像的深度值；

3)每次采样过程中，随机在深度图像中选择三个点P₁,P₂,P₃，然后求取由此三点P₁,P₂,P₃构成的平面π的法向量：

${\mathrm{norm}}_{\mathrm{\&pi;}}^{\&prime;c}=P_1{P}_2\&CircleTimes;P_1{P}_3;$

将所述法向量单位化，即：

$\mathrm{nor}{m}_{\mathrm{\&pi;}}^c=\frac{{\mathrm{norm}}_{\mathrm{\&pi;}}^{\&prime;c}}{\left|\right|{\mathrm{norm}}_{\mathrm{\&pi;}}^{\&prime;c}\left|\right|};$

其中，P₁P₂表示由点P₁指向P₂的向量，表示向量的叉积；选取第四点P₄进行如下检验：

$|{\mathrm{norm}}_{\mathrm{\&pi;}}^c\&times;P_1{P}_4|<\mathrm{Th};$

其中，阈值Th＝2.5δ，δ为深度图像中深度值的测量噪声标准差；若第四点P₄满足那么第四点P₄为平面π上的点；依照此步骤遍历整幅深度图像，如果一幅深度图像中满足的点的比例超过阈值Ratio，则认为平面π即为待检测地面；

4)提取出满足式测试的所有点在深度摄像机坐标系下的坐标，记为：求取出这些点的坐标平均值并按照下式构造矩阵M：

$M=M_1^T{M}_1;$

其中， $M_1=\left[\begin{array}{ccc}{x}_1^c& y_1^c& z_1^c\\ x_2^c& y_2^c& z_2^c\\ & \&CenterDot;\&CenterDot;\&CenterDot;& \\ x_n^c& y_n^c& z_n^c\end{array}\right]-\left[\begin{array}{ccc}{x}_{\mathrm{avg}}^c& Y_{\mathrm{avg}}^c& z_{\mathrm{avg}}^c\\ x_{\mathrm{avg}}^c& Y_{\mathrm{avg}}^c& z_{\mathrm{avg}}^c\\ & \&CenterDot;\&CenterDot;\&CenterDot;& \\ x_{\mathrm{avg}}^c& y_{\mathrm{avg}}^c& z_{\mathrm{avg}}^c\end{array}\right];$

5)计算待检测地面的法向量在深度摄像机坐标系下的坐标如果的第二项为正，则令调整深度摄像机的位置，使得然后计算深度摄像机相对于待检测地面的高度height_camera，其中，表示的第一项；

6)计算世界坐标系的Z轴方向Z_w在深度摄像机坐标系下的坐标并确定深度摄像机坐标系下深度图像的点(x_c,y_c,z_c)到世界坐标系下深度图像的点(x_w,y_w,z_w)的关系：

$\left[\begin{array}{c}{x}_w\\ y_w\\ z_w\end{array}\right]=\left[\begin{array}{c}{X}_w^c\\ Y_w^c\\ Z_w^c\end{array}\right]\left[\begin{array}{c}{x}_c\\ y_c\\ z_c\end{array}\right]+\left[\begin{array}{c}0\\ 0\\ {\mathrm{height}}_{\mathrm{camera}}\end{array}\right];$

其中， $X_w^c=\frac{X_w^{c^{\&prime;}}}{\left|\right|X_w^{c^{\&prime;}}\left|\right|},X_w^{c^{\&prime;}}=X_w^c-(X_w^c\&CenterDot;Z_w^c)\&CenterDot;Z_w^c,X_w^c=\left[\mathrm{1,0,0}\right];Y_w^c=Z_w^c\&CircleTimes;X_w^c;$ 所述世界坐标系是指以待检测地面为水平面XY，高度为垂直于Z轴方向，坐标原点为深度摄像机在地面的垂足确定的坐标系。

2.根据权利要求1所述的深度图像中的自动地面检测及摄像机相对位姿估计方法，其特征在于，所述步骤1)中，深度摄像机内参数矩阵记为A通过直接线性法、两步法、平面法中的一种计算得到。

3.根据权利要求1或2所述的深度图像中的自动地面检测及摄像机相对位姿估计方法，其特征在于，所述步骤3)中，其中比值Ratio依据图像中的地面部分占整幅图像中的比值而定，如果地面占整幅图像中的比例为Ratio_ground，那么Ratio＝0.6～0.8Ratio_ground。

4.根据权利要求3所述的深度图像中的自动地面检测及摄像机相对位姿估计方法，其特征在于，所述步骤5)中，的计算过程如下：对矩阵M做标准奇异值分解：(U,D,V)＝svd(M)；使得M＝UDV，其中矩阵U,V均为单位酉矩阵，矩阵D为矩阵M的特征值所构成的对角阵；V矩阵的最后一列即为待检测地面的法向量在深度摄像机坐标系下的坐标