[发明专利]位姿估计方法和装置

权利要求书

1.一种位姿估计方法，其特征在于，所述方法包括：

从深度传感器处获取深度图像视频；

从所述深度图像视频的各帧深度图像中选取第一深度图像和第二深度图像，其中，所述第一深度图像和所述第二深度图像中共有至少一个指示同一物体的像素点；

确定所述第一深度图像中的第一像素点集合以及所述第二深度图像中的第二像素点集合，其中，所述第一像素点集合中的各像素点与所述第二像素点集合中的各像素点一一对应，且对应的两个像素点指示同一物体；

对于所述第一像素点集合中的任一像素点，基于该像素点在所述第一深度图像中的第一二维坐标及与该像素点对应的对应像素点在所述第二深度图像中的第一深度值，确定所述深度传感器的位姿变换参数。

2.根据权利要求1所述的方法，其特征在于，在所述从所述各帧深度图像中选取第一深度图像和第二深度图像之前，所述方法还包括：

对于每帧深度图像，删除该帧深度图像中符合预设条件的像素点；

对删除后的深度图像进行平滑处理。

3.根据权利要求2所述的方法，其特征在于，所述删除该帧深度图像中符合预设条件的像素点，包括：

检测每个像素点的深度值；

将深度值大于第一预设值，且深度值小于第二预设值的像素点删除。

4.根据权利要求2所述的方法，其特征在于，所述删除该帧深度图像中符合预设条件的像素点，包括：

确定该帧深度图像在水平方向的第一偏导数和在竖直方向上的第二偏导数；

根据所述第一偏导数和所述第二偏导数，确定该帧深度图像中的几何边缘像素点；

将所述几何边缘像素点删除。

5.根据权利要求2所述的方法，其特征在于，所述删除该帧深度图像中符合预设条件的像素点，包括：

确定该帧深度图像中深度值不存在的失效像素点；

将所述失效像素点以及与所述失效像素点相邻的像素点删除。

6.根据权利要求1所述的方法，其特征在于，所述基于该像素点在所述第一深度图像中的第一二维坐标及与该像素点对应的对应像素点在所述第二深度图像中的第一深度值，确定所述深度传感器的位姿变换参数，包括：

将该像素点在所述第一深度图像中的第一二维坐标映射为所述第一深度图像所属坐标系的第一三维空间坐标；

将所述第一三维空间坐标变换到所述第二深度图像所属坐标系，得到第二三维空间坐标；

将所述第二三维空间坐标映射到所述第二深度图像中，得到第二二维坐标；

确定所述第二二维坐标在所述第二深度图像中的第二深度值；

基于所述第一深度值以及所述第二深度值，确定所述深度传感器的位姿变换参数。

7.根据权利要求6所述的方法，其特征在于，所述基于所述第一深度值以及所述第二深度值，确定所述深度传感器的位姿变换参数，包括：

根据所述第一深度值以及所述第二深度值，确定所述第一深度图像与所述第二深度图像之间的深度差值；

确定所述深度差值为深度残差，并基于所述深度残差，执行以下迭代步骤：基于所述深度残差，确定位姿估计增量；确定所述深度残差是否小于预设阈值；响应于所述深度残差小于预设阈值，累积所述位姿估计增量与所述第一深度值，确定位姿估计值；根据所述位姿估计值，确定所述深度传感器的位姿变换参数；

响应于所述深度残差大于或等于预设阈值，确定累积的所述位姿估计增量为所述深度残差，继续执行所述迭代步骤。

8.根据权利要求1所述的方法，其特征在于，所述方法还包括：

从与所述深度传感器物理绑定的惯性测量装置处获取角速度和加速度；

根据所述角速度和所述加速度，确定所述惯性测量装置的位姿变换参数；

融合所述深度传感器的位姿变换参数以及所述惯性测量装置的位姿变换参数，确定综合位姿变换参数。