相对位姿的估计方法、装置、电子设备及介质

技术领域

本申请涉及计算机视觉技术领域，特别是涉及一种相对位姿的估计方法、装置、电子设备及计算机存储介质。

背景技术

利用深度摄像头获取深度图像有非常广泛的应用，基于深度图像实现位姿估计、地图构建和三维重建等是当下计算机视觉的研究热点。

迭代最近点算法(Iterative Closed Point,ICP)算法作为经典的位姿估计算法，只使用了深度信息，导致位姿估计精度较低。

发明内容

本申请提供一种相对位姿的估计方法、装置、电子设备及介质，以解决相关技术中基于深度图像的位姿估计误差大的问题。

为解决上述技术问题，本申请提供一种相对位姿的估计方法。该方法包括：获取传感器采集的测量数据以及第一图像和第二图像之间的预估相对位姿，测量数据包括第一图像的第一颜色信息和第一深度信息，第二图像的第二颜色信息和第二深度信息；基于第一深度信息和预估相对位姿，确定第一图像在第二图像上的投影点；第一颜色信息、第二颜色信息和投影点，计算第一图像和第二图像之间的颜色差异；基于第一深度信息、第二深度信息以及投影点，计算第一图像和第二图像之间的深度差异；基于颜色差异、深度差异优化预估相对位姿，得到目标相对位姿。

在一种可能的实现方式中，测量数据还包括与第一图像同一时刻采集的第一惯性信息，和与第二图像同一时刻采集的第二惯性信息，该方法还包括：根据第一惯性信息、第二惯性信息以及预估相对位姿，计算第一图像和第二图像之间的惯性差异；基于颜色差异、深度差异优化预估相对位姿，得到目标相对位姿，包括：基于颜色差异、深度差异和惯性差异优化预估相对位姿，得到目标相对位姿。

在一种可能的实现方式中，基于颜色差异、深度差异和惯性差异优化预估相对位姿，得到目标相对位姿，包括：获取颜色差异、深度差异和惯性差异的权重；根据权重对颜色差异、深度差异和惯性差异进行加权求和，得到总差异度；响应于总差异度未满足预设条件，调整预估相对位姿，直至使总差异度满足预设条件；将使总差异度满足预设条件的调整后的预估相对位姿作为目标相对位姿。

在一种可能的实现方式中，基于颜色差异、深度差异和惯性差异优化预估相对位姿，得到目标相对位姿，包括：调整预估相对位姿，得到使颜色差异最小的第一目标相对位姿；调整预估相对位姿，得到使深度差异最小的第二目标相对位姿；调整预估相对位姿，得到使惯性差异最小的第三目标相对位姿；将第一目标相对位姿、第二目标相对位姿和第三目标相对位姿进行融合，得到目标相对位姿。

在一种可能的实现方式中，预估相对位姿包括预估旋转矩阵，第一惯性信息包括第一重力信息，第二惯性信息包括第二重力信息，根据第一惯性信息、第二惯性信息以及预估相对位姿，计算预估相对位姿下的第一图像和第二图像之间的惯性差异，包括：根据预估旋转矩阵对第一重力信息进行旋转；计算第二重力信息与旋转后的第一重力信息的差值的平方，得到惯性差异。

在一种可能的实现方式中，还包括：预估相对位姿包括预估旋转矩阵，第一惯性信息包括第一角速度信息，第二惯性信息包括第二角速度信息，根据第一惯性信息、第二惯性信息以及预估相对位姿，计算预估相对位姿下的第一图像和第二图像之间的惯性差异，包括：确定第一角速度信息和第二角速度信息的时间戳，并计算第一角速度信息和第二角速度信息之间的积分时间；根据积分时间对第一角速度信息进行积分，得到旋转量；根据预估旋转矩阵与旋转量的差值，计算得到惯性差异。

在一种可能的实现方式中，第一图像包括多个第一像素点，基于第一深度信息和预估相对位姿，确定第一图像在第二图像上的投影点，包括：根据每一第一像素点的像素坐标，从第一深度信息中获取每一第一像素对应的第一深度值；根据第一深度值，将每一第一像素点反投影到第一相机坐标系下得到第一三维点；根据预估相对位姿将每一第一三维点转换到第二相机坐标系下，得到第二三维点；将每一第二三维点投影至第二图像的第二像素坐标系下，得到第一像素点在第二图像中对应的投影点。

在一种可能的实现方式中，第一颜色信息、第二颜色信息和投影点，计算第一图像和第二图像之间的颜色差异，包括：将每一第一像素点和与第一像素点对应的投影点配对，得到多个匹配点对；根据第一颜色信息获得每一第一像素点的第一颜色值，根据第二颜色信息获得每一投影点的第二颜色值；分别计算每一匹配点对的第一颜色值与第二颜色值的颜色差值；对多个匹配点对的颜色差值进行平方后求和，得到颜色差异。

在一种可能的实现方式中，第一深度信息包括第一图像中多个第一像素点的第一深度值，基于第一深度信息、第二深度信息以及预估相对位姿，计算预估相对位姿下的第一图像和第二图像之间的深度差异，包括：将每一第一像素点和与第一像素点对应的投影点配对，得到多个匹配点对；计算每一第二三维点的第二深度值；根据第二深度信息获取投影点的第三深度值；分别计算每一匹配点对的第二深度值与第三深度值的深度差值；对多个匹配点对的深度差值进行平方后求和，得到深度差异。

在一种可能的实现方式中，确定第一图像和第二图像的目标相对位姿之前，还包括：对第一图像和第二图像进行同样的图像金字塔降采样，得到目标数量层的第一图像金字塔和第二图像金字塔，将第一图像金字塔和第二图像金字塔的最顶层记为目标层；确定第一图像和第二图像的目标相对位姿包括：确定目标层的第一图像和第二图像的目标相对位姿；判断目标层是否为第一图像金字塔和第二图像金字塔的最底层；响应于目标层为最底层的情况，将目标层的目标相对位姿确定为最终目标相对位姿；响应于目标层不为最底层的情况，将目标层的下一层作为新的目标层，将目标层的目标相对位姿记为新的目标层的预估相对位姿，并返回执行“确定目标层的第一图像和第二图像的目标相对位姿”。

为解决上述技术问题，本申请提供一种相对位姿估计装置。该装置包括获取模块、重投影模块和优化模块；获取模块用于获取传感器采集的测量数据，测量数据包括第一图像的第一颜色信息和第一深度信息，第二图像的第二颜色信息和第二深度信息，以及第一图像和第二图像之间的预估相对位姿；重投影模块用于基于第一深度信息和预估相对位姿，确定第一图像在第二图像上的投影点；位姿优化模块用于根据第一颜色信息、第二颜色信息和投影点，计算第一图像和第二图像之间的颜色差异；基于第一深度信息、第二深度信息以及投影点，计算第一图像和第二图像之间的深度差异；基于颜色差异、深度差异优化预估相对位姿，得到目标相对位姿。

为解决上述技术问题，本申请提供一种电子设备。电子设备包括处理器和存储器；处理器耦接存储器，在工作时执行指令，以配合存储器实现上述的相对位姿的估计方法。

为解决上述技术问题，本申请提供一种计算机存储介质。计算机存储介质存储有计算机程序，计算机程序被执行以实现上述相对位姿的估计方法的步骤。

本申请分别计算第一图像和第二图像的颜色差异和深度差异，并基于颜色差异、深度差异对预估相对位姿进行迭代更新，充分利用了图像中像素的颜色信息和深度信息，使得得到的目标相对位姿更加精确。进一步地，本申请还结合惯性信息优化预估相对位姿，能够进一步降低深度图像中弱纹理特征造成目标位姿估计的误差。采用图像金字塔的方式逐层优化第一图像和第二图像的相对位姿，降低了对初始的预估相对位姿的要求，并且能够进一步提高目标相对位姿的精度。

附图说明

图1是本申请提供的相对位姿的估计方法第一实施例的流程示意图；

图2是本申请提供的基于第一深度信息和预估相对位姿确定所述第一图像在所述第二图像上的投影点一实施方式的流程示意图；

图3是本申请提供的相对位姿的估计方法的第二实施例的流程示意图；

图4是本申请提供的相对位姿的估计方法第三实施例的流程示意图；

图5是本申请提供的相对位姿估计装置一实施例的结构示意图；

图6是本申请提供的一种电子设备一实施例的结构示意图；

图7是本申请提供的计算机存储介质一实施例的结构示意图。

具体实施方式

为使本领域的技术人员更好地理解本发明的技术方案，下面结合附图和具体实施方式对本申请所提供的相对位姿的估计方法、装置、电子设备及介质做进一步详细描述。

请参阅图1，图1是本申请提供的相对位姿的估计方法第一实施例的流程示意图。本实施例包括如下步骤：

S110：获取传感器采集的测量数据，以及第一图像和第二图像之间的预估相对位姿。

其中，传感器例如包括图像传感器，测量数据例如包括第一图像的第一颜色信息和第一深度信息，第二图像的第二颜色信息和第二深度信息。

本实施例中，以第一图像是当前时刻深度相机采集的深度图像，第二图像是前一时刻深度相机获取的深度图像为例进行说明。当然，第一图像也可以指前一时刻深度相机获取的深度图像，且第二图像指当前时刻深度相机获取的深度图像，本申请对此不作限制。

其中，深度相机相比较传统的相机，在功能上添加了一个深度测量功能，能够获取深度相机与环境中物体表面的距离，即深度数据。本实施例中，深度相机具体可以是RGB-D相机，在RGB-D相机输出深度图像时，深度图像中每个像素的RGB数据与深度数据已经对齐，即RGB数据和深度数据中相同的像素坐标对应的数据，是对三维空间中的同一点采集得到的。

第一颜色信息和第二颜色信息可以是指第一图像和第二图像中每一像素的灰度值。灰度值是对像素的RGB数据进行灰度转化得到的，灰度转化的方法可以是浮点算法、整数方法、平均值法或移位方法等。

预估相对位姿表示初始估计的第一图像和第二图像之间的位姿变化量。预估相对位姿可以是对第一图像和第二图像进行粗匹配得到的，例如通过稀疏直接法计算得到。具体而言，分别从第一图像和第二图像中提取少量(数十或数百个)特征点，通过最小化第一图像和第二图像中特征点之间的亮度误差，得到预估相对位姿。稀疏直接法使用的特征点少，且不必计算特征点的描述子，使得能够快速地计算出预估相对位姿，提高位姿估计效率。

对于配置有里程计、磁力计或加速度计的电子设备而言，可以基于里程计数据、磁力计数据或加速度数据计算预估相对位姿。

当然，在采集第一图像和第二图像的间隔时间较短的情况下，还可以使用采集第二图像的时刻的相对位姿作为当前时刻的预估相对位姿。

S120：基于第一深度信息和预估相对位姿，确定所述第一图像在所述第二图像上的投影点。

根据第一深度信息，可以将第一图像反投影到三维空间，得到第一图像的第一三维点云，并且根据预估相对位姿，可以将第一三维点云进行位姿转换、投影至第二像素坐标系下，得到重投影的第一图像。如此可以进一步使用第一颜色信息和第二颜色信息计算重投影的第一图像和第二图像中相同像素坐标的颜色差异，使用第一深度信息和第二深度信息计算重投影的第一图像和第二图像中相同像素坐标的深度差异，以充分利用第一图像和第二图像中的像素信息，使得位姿估计更精确。

其中，第一图像包括多个第一像素点。第一像素点可以是第一图像中所有的像素点，也可以是其中具有代表性的部分像素点(特征点)。当第一像素点是特征点时，可以是使用FAST(Features from Accelerated Segments Test)算法或ORB(Oriented Fast andRotated Brief)算法等算法提取的。第一像素点的数量可以是几百个，也可以是几千个，还可以是上万个甚至数十万个。第一像素点的数量小，则计算速度更快且对处理器的要求较低；第一像素点的数量越大，则对预估位姿的评价越准确，但处理速度会下降且对处理器的要求更高。第一像素点的数量可根据实际需求设置，本申请对此不作限制。

请参阅图2，图2是本申请提供的基于第一深度信息和预估相对位姿确定所述第一图像在所述第二图像上的投影点一实施方式的流程示意图。具体步骤如下：

S121：根据每一第一像素点的像素坐标，从第一深度信息中获取每一第一像素对应的第一深度值。

本实施例中，假设三维空间中特征点的位置是固定的，且同一特征点在第一图像和第二图像中的亮度不变。

第一像素点包括特征点在第一图像中的像素坐标。根据第一像素点的像素坐标可以从第一深度信息读取第一像素点的第一深度值。

S122：根据第一深度值，将第一像素点反投影到第一相机坐标系下得到第一三维点。

根据深度相机的内部参数和第一像素点的第一深度值，可以使用反投影函数分别将每一个第一像素点反投影至三维空间，得到第一相机坐标系下的多个第一三维点。第一相机坐标系采集第一图像时相机位姿下的三维坐标系。

S123：根据预估相对位姿将第一三维点转换到第二相机坐标系下，得到第二三维点。

使用预估相对位姿，将每一第一三维点转换到第二相机坐标系(即第二相机坐标系)下，得到多个第二三维点。

S124：将所述第二三维点投影至第二图像的第二像素坐标系，得到第一像素点在第二图像中对应的投影点。

本实施例以第一图像所在的像素坐标系为第一像素坐标系，以第二图像所在的像素坐标系为第二像素坐标系。

根据深度相机的内部参数，使用投影函数将每一第二三维点投影到第二像素坐标系下，得到每一第一像素点在第二图像上对应的投影点。深度相机的内部参数包括焦距和像素大小等。

将第一像素点和与第一像素点对应的投影点配对，得到多个匹配点对。使用重投影方法在第二图像上确定与第一像素点配对的投影点，能够快速地得到匹配点对，能够提高位姿估算的效率。

若预估相对位姿与实际相对位姿一致，则投影点的坐标与该投影点对应的特征点在第二图像中的坐标一致。但一般情况下，预估相对位姿与实际相对位姿存在一定的偏差，导致投影点与该投影点对应的特征点在第二图像中的坐标不一致，因此需要优化预估相对位姿，使投影点的坐标与该投影点对应的特征点在第二图像中的坐标一致或接近一致。

S130：基于第一颜色信息、第二颜色信息以及投影点，计算第一图像和第二图像之间的颜色差异。

根据每一第一像素点的像素坐标，读取第一颜色信息中每一第一像素点的灰度值作为第一颜色值，读取第二颜色信息中每一投影点的灰度值作为第二颜色值。

计算每一匹配点对的第一颜色值和第二颜色值的颜色差值，所有匹配点对的颜色差值进行平方后求和得到第一图像和第二图像的颜色差异。

计算颜色差异的公式如下：

P＝π

其中，E

S140：基于第一深度信息、第二深度信息以及投影点，计算第一图像和第二图像之间的深度差异。

具体来说，本实施例中第一图像和第二图像的深度差异，是指经过反投影和位姿变换后的第一图像的深度值与投影点的深度值之间的差值。即深度差异是将第二三维点的第二深度值与投影点的第三深度值进行比较得到的。

根据投影点的像素坐标，可以从第二深度信息中读取每一投影点的第二深度值。

计算每一匹配点对的深度差值，并对每一深度差值进行平方后求和，得到深度差异。深度差异公式表示如下：

其中，E

S150：基于颜色差异、深度差异优化预估相对位姿，得到目标相对位姿。

根据颜色差异和深度差异求解目标相对位姿的方法有多种。

例如，可以是对颜色差异和深度差异进行加权求和，得到总差异度，通过对预估相对位姿进行迭代更新，求解使总差异度的值最小的调整后的预估相对位姿作为目标相对位姿。

总差异度公式表示如下：

E＝argmin

其中，E表示总差异度，w

求解当前迭代中使总差异度最小的李代数，使用李代数更新预估相对位姿，得到更新后的预估相对位姿。其中，求解总差异度最小的优化算法可以是L-M(Levenberg-Marquardt)算法或G-N(Gauss-Newton)算法，或二者结合。李代数到李群满足指数映射关系，李群到李代数满足对数映射关系，李群中包括上述的六维优化变量。通过最小化总差异度，得到当前迭代使总差异度的值最小的优化变量，从而根据李群与李代数的映射关系可以得到当前迭代的李代数。

李代数用于更新预估相对位姿，公式如下：

判断迭代更新次数或总差异度是否满足预设条件。若满足预设条件，则停止迭代，输出更新后的预估相对位姿作为目标相对位姿。若不满足预设条件，将更新后的预估相对位姿作为下一次迭代的预估相对位姿，并执行S120的步骤，继续进行迭代以优化预估相对位姿。

其中，迭代更新次数满足预设条件例如是指迭代更新次数大于迭代阈值。迭代阈值可以是4次、5次、6次或6次以上等。迭代阈值可以根据深度图像的分辨率而定，深度图像的分辨率越高，迭代阈值越大，例如6次、8次或10次等；深度图像的分辨率越高，迭代阈值可以越小，例如3次或4次等，本申请对此不作限制。

总差异度满足预设条件可以指总差异度的值小于误差阈值，或当前迭代的总差异度的值与上次迭代的总差异度的值之间的变化量小于变化阈值等。

根据颜色差异和深度差异求解目标相对位姿，还可以是使用颜色差异和深度差异分别计算目标相对位姿，最后对两个目标相对位姿进行融合，得到最终输出的目标相对位姿。

具体而言，对预估相对位姿进行迭代更新，求解使颜色差异最小的相对位姿作为第一预估相对位姿。对预估相对位姿进行迭代更新，求解使深度差异最小的相对位姿作为第二预估相对位姿。

使用融合算法对第一预估相对位姿和第二预估相对位姿进行融合计算，输出相对位姿。融合算法例如是卡尔曼滤波算法。

本实施例中，除了利用第一图像和第二图像的颜色信息，进一步结合深度信息进行约束，能够提高相对位姿估计的精度和鲁棒性。此外，采用投影的方式确定第一图像和第二图像的匹配点，能够减少特征点匹配的计算量，使得优化预估相对位姿的过程更加迅速。

位姿估计的精确度，往往依赖相邻两帧图像中的特征点的数量，而面对球体或圆柱体等弱纹理回转体时，从图像中提取到的特征点较少，会降低位姿估计的准确度。为了能够有效处理弱纹理的回转体，本申请还加入惯性数据作为位姿估算中的约束，以减少位姿估计的误差。

请参阅图3，图3是本申请提供的相对位姿的估计方法的第二实施例的流程示意图。本实施例与相对位姿的估计方法的第一实施例的区别在于，本实施例进一步使用惯性信息进行约束。本实施例包括如下步骤：

S210：获取传感器采集的测量数据，以及第一图像和第二图像之间的预估相对位姿。

测量数据还可以包括与第一图像同一时刻采集的第一惯性信息，和与第二图像同一时刻采集的第二惯性信息。

惯性测量单元是测量物体三轴姿态角(或角速率)以及加速度的装置，本实施例中通过惯性测量单元测量深度相机的惯性信息，可用于估算深度相机的位姿。惯性测量单元具体可以包括陀螺仪、加速度传感器或重力感应器等中的一种或多种。

第一惯性信息可以包括陀螺仪采集的第一角速度信息，第二惯性信息可以包括陀螺仪采集的第二角速度信息。

第一惯性信息进一步可以包括第一重力信息，第二惯性信息进一步可以包括第二重力信息。重力信息是由重力传感器测量由于重力引起的加速度，可以计算出设备相对于水平面的倾斜角度。

S220：基于第一深度信息和预估相对位姿，确定所述第一图像在所述第二图像上的投影点。

S230：基于第一颜色信息、第二颜色信息以及投影点，计算第一图像和第二图像之间的颜色差异。

S240：基于第一深度信息、第二深度信息以及投影点，计算第一图像和第二图像之间的深度差异。

S250：根据第一惯性信息、第二惯性信息以及预估相对位姿，计算第一图像和第二图像之间的惯性差异。

惯性差异包括旋转差异。

确定第一角速度信息和第二角速度信息的时间戳，可以计算出第一角速度信息和第二角速度信息之间的积分时间。使用积分时间对第一角速度信息进行积分，得到第一图像与第二图像之间的旋转量。

计算预估相对位姿中的预估旋转矩阵与旋转量的差值，即预估旋转矩阵与旋转量的比值求对数得到的值，对差值进行平方运算，得到旋转差异。旋转差异公式表示如下：

其中，E

惯性差异还包括重力差异。

根据预估相对位姿中的预估旋转矩阵对第一重力信息进行旋转，得到旋转后的第一重力信息。计算第二重力信息与旋转后的第一重力信息的差值的平方，得到重力差异。重力差异公式表示如下：

其中，E

通过旋转差异与重力差异的约束，能够保证第一图像和第二图像之间的旋转矩阵得到准确的估计，降低预估相对位姿的误差，使得相对位姿的估计更加鲁棒。

S260：基于颜色差异、深度差异和惯性差异优化预估相对位姿，得到目标相对位姿。

根据颜色差异、深度差异和惯性差异求解目标相对位姿的方法有多种。

例如，可以是对颜色差异、深度差异和惯性差异进行加权求和，得到总差异度，通过对预估相对位姿进行迭代更新，求解使总差异度的值最小的调整后的预估相对位姿作为目标相对位姿。

总差异度公式表示如下：

E＝argmin

其中，E表示总差异度，w

在一些实施方式中，若惯性信息仅包括重力信息，则w

在另一些实施方式中，若惯性信息仅包括角速度信息，则w

位姿的迭代过程与S150相同，在此不再赘述。

当然，根据颜色差异、深度差异和惯性差异求解目标相对位姿，还可以是使用颜色差异、深度差异和惯性差异分别计算目标相对位姿，最后对目标相对位姿进行两两融合，得到最终输出的目标相对位姿。例如，先将基于颜色差异和深度差异计算得到的目标相对位姿进行融合，得到融合的目标相对位姿，然年将融合的目标相对位姿与基于惯性差异得到的目标相对位姿进行融合，得到最终输出的目标相对位姿。

使用融合算法对第一预估相对位姿和第二预估相对位姿进行融合计算，输出相对位姿。融合算法例如是卡尔曼滤波算法。

本实施例中，除了利用第一图像和第二图像的深度信息和颜色信息，进一步惯性信息进行约束，能够降低图像中回转体等弱纹理物体对位姿估计精度的影响，从而提高相对位姿估计的精度和鲁棒性。在位姿估计的过程中，往往对初始位姿，也即预估相对位姿的精度要求较高，在预估相对位姿的精度较低的情况下，会增加位姿估计的迭代次数，导致位姿估计较慢，并且低精度的预估相对位姿也会导致最终输出的目标相对位姿精度低。因此，请参阅图4，图4是本申请提供的相对位姿的估计方法第三实施例的流程示意图，本实施例在相对位姿的估计方法第一实施例和第二实施例的基础上，还对第一图像和第二图像进行金字塔降采样，从低分辨率的最顶层开始优化预估相对位姿，降低预估相对位姿精度低对目标相对位姿精度的影响。本实施例包括如下步骤：

S310：对第一图像和第二图像进行同样的图像金字塔降采样，得到目标数量层的第一图像金字塔和第二图像金字塔，将第一图像金字塔和第二图像金字塔的最顶层记为目标层。

一个图像金字塔是一系列图像的组合，最底层图像尺寸最大，最顶层图像尺寸最小，由最底层图像到最顶层图像，图像尺寸依次减小，从空间上往下看就像一个金字塔。利用图像金字塔降采样产生一系列不同分辨率图像，最底层图像分辨率最高，最顶层图像分辨率最低，由最底层图像到最顶层图像，图像分辨率依次降低，然后在不同尺度空间去寻找图像对应的特征，图像金字塔可以保证图像特征存在。

图像金字塔降采样使用可以是均值降采样，即第一图像和第二图像上四个相邻像素的颜色值和深度值被平均成一个值。

将第一图像金字塔和第二图像金字塔的最顶层记为目标层，逐层向最底层过渡。构建图像金字塔的目的是为了从粗到细地计算相机位置姿态，可以加速计算目标相对位姿，并且能够降低对预估相对位姿初始值的要求。

对原始分辨率的第一图像和第二图像进行同样的图像金字塔降采样，得到目标数量层的第一图像和第二图像。

进一步地，降采样过程中为了保证数据平滑，每层深度图像降采样之前可以先对第一图像和第二图像进行滤波。滤波算法可以是高斯滤波、双边滤波或中值滤波等。

进一步地，由于对下层第一图像和第二图像进行金字塔降采样，导致上层第一图像和第二图像的图像分辨率下降、尺寸变小，使得上层第一图像和第二图像的深度相机内部参数(焦距和像素中心)发生变化。因此可以根据上层第一图像和第二图像与下层第一图像和第二图像的尺寸比值，等比例缩小内部参数。每层第一图像和第二图像的分辨率与内部参数一一对应，在后续将第一图像的特征点反投影到三维空间，以及将三维空间的特征点投影到第二像素坐标系时，能够得到准确的像素坐标。本实施例所述的上层、下层是相对于相邻两层第一图像和第二图像而言。

对每层第一图像和第二图像之间的预估相对位姿进行迭代时，第一惯性信息和第二惯性信息不变。

S320：获取目标层第一图像的第一颜色信息、第一深度信息和与第一图像同一时刻采集的第一惯性信息，第二图像的第二颜色信息、第二深度信息和与第二图像同一时刻采集的第二惯性信息，以及第一图像和第二图像的预估相对位姿。

本实施例中，是从第一图像金字塔和第二图像金字塔的最顶层图像开始优化预估相对位姿，将最顶层图像记为目标层，依次将目标层的目标相对位姿作为下一层的预估相对位姿，直到目标层变为第一图像金字塔和第二图像金字塔的最底层，计算基于最底层计算得到的目标相对位姿作为最终输出的目标相对位姿。

本步骤与S210类似，故在此不再赘述。

S330：基于第一深度信息和预估相对位姿，确定所述第一图像在所述第二图像上的投影点。

S340：基于第一颜色信息、第二颜色信息以及投影点，计算第一图像和第二图像之间的颜色差异。

S350：基于第一深度信息、第二深度信息以及投影点，计算第一图像和第二图像之间的深度差异。

S360：根据第一惯性信息、第二惯性信息以及预估相对位姿，计算第一图像和第二图像之间的惯性差异。

S370：基于颜色差异、深度差异和惯性差异优化预估相对位姿，得到目标相对位姿。

S380：判断目标层是否为第一图像金字塔和第二图像金字塔的最底层。

响应于目标层为最底层的情况，则执行S490。

响应于目标层不为最底层的情况，将目标层的下一层作为新的目标层，将目标层的目标相对位姿记为新的目标层的预估相对位姿，并返回执行“确定目标层的第一图像和第二图像的目标相对位姿”，即执行S320的步骤。

S390：停止迭代，并输出目标层的目标相对位姿作为第一图像和第二图像的最终目标相对位姿。

本实施例中，除了利用第一图像和第二图像的深度信息外，进一步结合颜色信息和惯性信息进行约束，提高了相对位姿估计的精度和鲁棒性。此外，采用投影的方式确定第一图像和第二图像的匹配点，能够减少特征点匹配的计算量，使得优化预估相对位姿的过程更加迅速。进一步地，采用图像金字塔的方式逐层优化第一图像和第二图像的相对位姿，降低了对初始的预估相对位姿的要求，并且能够进一步提高目标相对位姿的精度。

请参阅图5，图5是本申请提供的相对位姿估计装置一实施例的结构示意图。该相对位姿估计装置500包括：

获取模块501，用于获取传感器采集的测量数据，测量数据包括第一图像的第一颜色信息和第一深度信息，第二图像的第二颜色信息和第二深度信息，以及第一图像和第二图像之间的预估相对位姿。

重投影模块502，用于基于第一深度信息和预估相对位姿，确定第一图像在第二图像上的投影点。

位姿优化模块503，用于基于第一颜色信息、第二颜色信息和投影点，计算第一图像和第二图像之间的颜色差异；基于第一深度信息、第二深度信息以及投影点，计算第一图像和第二图像之间的深度差异；基于颜色差异、深度差异优化预估相对位姿，得到目标相对位姿。

在一些实施例中，相对位姿估计装置可以用于执行上述实施例中所描述的相对位姿的估计方法，当然可以包括用于执行上述实施例所描述的相对位姿的估计方法中的任意流程和/或步骤的单元或模块，为了简洁，不再赘述。

以上装置实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本发明装置实施例中未披露的技术细节，请参照本发明方法实施例的描述而理解。

请参阅图6，图6是本申请提供的一种电子设备一实施例的结构示意图。该电子设备600包括：

处理器601和存储器602。处理器601耦接存储器602，在工作时执行指令，以配合存储器602实现上述的相对位姿的估计方法。

可选地，电子设备为手机、相机、平板电脑或移动机器人。

其中，存储器602用于存储测量数据，测量数据包括第一图像的第一颜色信息和第一深度信息，第二图像的第二颜色信息和第二深度信息，以及第一图像和第二图像之间的预估相对位姿。

处理器601用于使用深度图像的颜色信息和深度信息，对第一图像和第二图像的预估相对位姿进行优化。具体地，处理器601基于第一深度信息和预估相对位姿，确定第一图像在第二图像上的投影点；基于第一颜色信息、第二颜色信息和投影点，计算第一图像和第二图像之间的颜色差异；基于第一深度信息、第二深度信息以及投影点，计算第一图像和第二图像之间的深度差异；基于颜色差异、深度差异优化预估相对位姿，得到目标相对位姿。

其中，处理器601可以是一种集成电路芯片，具有信号的处理能力。处理器601还可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现场可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

对于上述实施例的方法，其可以计算机程序的形式存在，因而本申请提出一种计算机存储介质，请参阅图7，图7是本申请提供的计算机存储介质一实施例的结构示意图。本实施例计算机存储介质700中存储有计算机程序701，其可被执行以实现上述实施例中的方法。

本实施例计算机存储介质700可以是U盘、移动硬盘、只读存储器(ROM，Read-OnlyMemory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等可以存储程序指令的介质，或者也可以为存储有该程序指令的服务器，该服务器可将存储的程序指令发送给其他设备运行，或者也可以自运行该存储的程序指令。

在本申请所提供的几个实施例中，应该理解到，所揭露的方法和装置，可以通过其它的方式实现。例如，以上所描述的装置实施方式仅仅是示意性的，例如，模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。

另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本申请各个实施方式方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本申请的实施方式，并非因此限制本申请的专利范围，凡是利用本申请说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本申请的专利保护范围内。