[发明专利]一种面向RGBD数据流的相机位姿估计方法

摘要

本发明涉及一种面向RGBD数据流的相机位姿估计方法，包括构建三维点云映射图、三维点云映射图的配准两个部分。首先将深度数据进行滤波等预处理操作，然后根据相机内参将预处理后的深度图像构建三维点云映射图；根据上一帧的相机位姿，采用投影映射算法对两帧三维点云映射图进行对应点匹配，然后对点到平面的距离误差函数作能量优化，估计两帧点云间变换矩阵的六个参数；根据上一帧的相机位姿估计当前帧的相机位姿，迭代上述过程优化得到最终的相机位姿。本发明能够在现有的移动设备上实时运行，并能够在输入数据分辨率较低时也能得到良好的结果。

主权项

一种面向RGBD数据流的相机位姿估计方法，其特征在于包括以下步骤：步骤(1)、对原始RGBD数据流中的深度数据作预处理操作，得到平滑的深度数据；同时对原始RGBD数据流中的RGB图像去模糊；步骤(2)、对平滑的深度数据，根据相机内部参数，将所述深度数据中的每一个像素点的深度转化为相机坐标系下的三维坐标；利用相邻像素的三维坐标值计算每一个像素对应的三维点云的法向量；由所述三维坐标和法向量构成三维点云映射图；步骤(3)、对于当前帧和上一帧的两组三维点云映射图，利用当前估计的相机位姿和上一帧的相机位姿，将当前帧三维点云映射图中每一个像素的三维点云坐标P1转化到上一帧相机坐标系下的坐标，再利用相机内部参数投影到像素坐标系下，获得上一帧三维点云映射图中对应像素坐标的点云数据P2；将P1与P2分别计算在世界坐标系下的三维坐标和法向量以及颜色值，其中三维坐标的距离、法向量的夹角及颜色的差值均在设定阈值范围内的对应点对视为匹配；步骤(4)、将两帧三维点云映射图之间的相机变换矩阵表示为六个参数，根据点到平面的距离误差函数，利用步骤(3)所得的对应点对构造系数矩阵，求解所述六个参数的线性方程组，将所述六个参数转化为当前估计的两帧三维点云映射图之间的相机变换矩阵；步骤(5)、根据步骤(4)所得的两帧三维点云映射图之间的相机变换矩阵，及上一帧的相机位姿，估计当前帧的相机位姿，再将估计的当前相机位姿代回到步骤(3)中，迭代步骤(3)～(4)直到设定次数或误差在设定范围内。