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

权利要求书

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

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

uv1=A-1xy1;]]>

其中，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₃构成的平面π的法向量：

normπ′c=P1P2⊗P1P3;]]>

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

normπc=normπ′c||normπ′c||;]]>

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

|normπc×P1P4|<Th;]]>

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

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

M=M1TM1;]]>

其中，M1=x1cy1cz1cx2cy2cz2c···xncyncznc-xavgcYavgczavgcxavgcYavgczavgc···xavgcyavgczavgc;]]>

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

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

xwywzw=XwcYwcZwcxcyczc+00heightcamera;]]>

其中，Xwc=Xwc′||Xwc′||,Xwc′=Xwc-(Xwc·Zwc)·Zwc,Xwc=[1,0,0];Ywc=Zwc⊗Xwc;]]>所述世界坐标系是指以待检测地面为水平面XY，高度为垂直于Z轴方向，坐标原点为深度摄像机在地面的垂足确定的坐标系。