[发明专利]深度图像中的自动地面检测及摄像机相对位姿估计方法

摘要

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

主权项

一种深度图像中的自动地面检测及摄像机相对位姿估计方法，其特征在于，包括以下步骤：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{nor}{m}_{\mathrm{\π}}^c=\frac{{\mathrm{norm}}_{\mathrm{\π}}^{\′c}}{\left|\right|{\mathrm{norm}}_{\mathrm{\π}}^{\′c}\left|\right|};$其中，P₁P₂表示由点P₁指向P₂的向量，表示向量的叉积；选取第四点P₄进行如下检验：$|{\mathrm{norm}}_{\mathrm{\π}}^c\×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\\ & \·\·\·& \\ 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)计算待检测地面的法向量在深度摄像机坐标系下的坐标如果的第二项为正，则令调整深度摄像机的位置，使得然后计算深度摄像机相对于待检测地面的高度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^{\′}}}{\left|\right|X_w^{c^{\′}}\left|\right|},X_w^{c^{\′}}=X_w^c-(X_w^c\·Z_w^c)\·Z_w^c,X_w^c=\left[\mathrm{1,0,0}\right];Y_w^c=Z_w^c\⊗X_w^c;$所述世界坐标系是指以待检测地面为水平面XY，高度为垂直于Z轴方向，坐标原点为深度摄像机在地面的垂足确定的坐标系。