一种处理数据的方法和装置与流程
本申请涉及人工智能领域，并且更具体地，涉及一种处理数据的方法和装置。
背景技术：
同时定位与地图构建(simultaneouslocalizationandmapping，slam)技术是机器人能够探索未知环境所依赖的关键能力。机器人在没有环境先验信息的情况下，利用自身搭载的传感器(摄像头、激光等)对周围环境的特征进行观测，并构建基于相关观测约束的优化目标，进而通过对优化目标的求解完成系统状态向量，即环境特征位置与机器人位姿的估计，同时完成地图构建与自定位。
在当前slam技术领域，主要使用激光雷达和/或摄像头对周围环境的路标进行观测。在基于激光雷达的slam中，一种机器人运动姿态估计的方法为：依据惯性测量单元(inertialmeasurementunit，imu)对imu和激光雷达采集的点云数据进行注册，从注册好的每一帧点云数据中提取所有的线特征点和所有的面特征点；然后将当前帧中所有的线特征点和所有的面特征点依据上一次估计出的位姿变换投影到前一帧的坐标系下，并匹配各自距离最近的点；再根据上述线特征点到对应线的距离和上述面特征点到对应面的距离建立约束，并通过列文伯格(levenberg-marquardt，lm)算法迭代优化，求解最优的运动位姿。该方法中，需要对大量的特征点构建的约束迭代优化求解，运算量较大，浪费计算资源和时间开销。
因此，如何降低slam运动姿态估计过程中的计算资源和计算时间开销，提升整个slam过程的运行效率成为了一个亟待解决的问题。
技术实现要素：
本申请提供一种处理数据的方法和装置，能够降低计算资源及计算时间的开销，提升整个slam过程的运行效率。
第一方面，本申请提供一种处理数据的方法，该方法包括：获取第一三维点集合中每个三维点的数据，所述每个三维点的数据是智能设备在第一位置时采集得到的，所述第一三维点集合包括位于第一坐标系的多个三维点；从所述第一三维点集合中确定第一子三维点集合，所述第一子三维点集合形成第一平面，所述第一平面为所述第一坐标系内的任意一个平面；计算多个第一距离，所述第一距离为第一三维点与所述第一位置之间的距离，所述第一距离与所述第一三维点一一对应，所述第一三维点是所述第一子三维点集合中的任意一个三维点；根据所述多个第一距离，从所述第一子三维点集合中确定第一目标三维点；将所述第一目标三维点作为约束条件，对所述智能设备进行定位，获取所述第一位置的位姿。
根据本申请实施例中的方法，从所述第一子三维点集合中确定第一目标三维点，第一子三维点集合中包括多个三维点，将第一子三维点集合中的第一目标三维点作为约束条件，可以降低后续步骤的计算量及计算时间的开销，提升整个slam过程的运行效率。
在一种可能的实现方式中，所述将所述第一目标三维点作为约束条件，对所述智能设备进行定位，包括：根据所述第一目标三维点确定约束条件，所述约束条件用于影响所述智能设备的定位过程。
在一种可能的实现方式中，所述方法还包括：将所述第一目标三维点作为约束条件对所述智能设备途经区域进行地图构建。
在一种可能的实现方式中，所述第一目标三维点是所述第一子三维点集合中与所述第一位置之间的距离最小的三维点。
根据本申请实施例中的方法，第一目标三维点是与第一坐标系的原点之间的距离最小的三维点，即与第一坐标系的原点距离最近的点，便于后续以第一目标三维点作为参数表示第一平面的面特征。
在一种可能的实现方式中，所述第一位置为所述第一坐标系的原点，所述第一距离为所述第一距离对应的三维点与所述第一坐标系的原点之间的距离。
根据本申请实施例中的方法，第一坐标系为以第一位置为原点的相对坐标系，所述第一平面位于第一坐标系中，能够消除第一平面的面特征的奇异性，从而可以提高slam方法的鲁棒性。
在一种可能的实现方式中，所述方法还包括：获取第二三维点集合中每个三维点的数据，所述第二三维点集合中每个三维点的数据是所述智能设备在第二位置时采集得到的，所述第二三维点集合包括位于第二坐标系的多个三维点；从所述第二三维点集合中确定第二子三维点集合，所述第二子三维点集合形成第二平面，所述第二平面为所述第二坐标系内的任意一个平面；计算多个第二距离，所述第二距离为第二三维点与所述第二位置之间的距离，所述第二距离与所述第二三维点一一对应，所述第二三维点是所述第二子三维点集合中的任意一个三维点；根据所述多个第二距离，从所述第二子三维点集合中确定第二目标三维点。
在一种可能的实现方式中，所述将所述第一目标三维点作为约束条件，对所述智能设备进行定位，包括：将第一向量与第二向量之间的残差作为约束条件，对所述智能设备进行定位，其中，所述残差包括所述第一向量与所述第二向量之间的距离，所述第一向量以所述第一位置为起点，所述第一向量以所述第一目标三维点为终点，所述第一向量用于在所述第一坐标系中确定所述第一平面，所述第二向量以所述第二位置为起点，所述第二向量以所述第二目标三维点为终点，所述第二向量用于在所述第二坐标系中确定所述第二平面。
在一种可能的实现方式中，所述第二目标三维点是所述第二子三维点集合中与所述第二位置之间的距离最小的三维点。
在一种可能的实现方式中，所述第二位置为所述第二坐标系的原点，所述第二距离为所述第二距离对应的三维点与所述第二坐标系的原点之间的距离。
在一种可能的实现方式中，在所述将第一向量与第二向量之间的残差作为约束条件之前，所述方法还包括：确定所述第二向量在所述第一坐标系中的投影，并在所述第一向量与所述第二向量在所述第一坐标系中的投影之间的距离小于或等于预设距离时，将所述第一向量和所述第二向量在所述第一坐标系中的投影之间的残差作为约束条件。
在一种可能的实现方式中，在所述将第一向量与第二向量之间的残差作为约束条件之前，所述方法还包括：确定所述第一向量在所述第二坐标系中的投影，并在所述第一向量在所述第二坐标系中的投影与所述第二向量之间的距离小于或等于预设距离时，将所述第一向量在所述第二坐标系中的投影和所述第二向量之间的残差作为约束条件。
根据本申请实施例中的方法，将第一向量和第二向量投影到同一相对坐标系中，能够计算第一向量和第二向量的残差，从而可以提高slam方法的鲁棒性。
在一种可能的实现方式中，所述将第一向量与第二向量之间的残差作为约束条件，对所述智能设备进行定位，包括：获取惯性测量单元采集的运动状态信息，所述运动状态信息包括所述智能设备在所述第一位置采集所述第一三维点的数据时的运动状态信息与所述智能设备在所述第二位置采集所述第二三维点的数据时的运动状态信息；将所述运动状态信息和所述第一向量与所述第二向量之间的残差作为约束条件，对所述智能设备进行定位。
根据本申请实施例中的方法，将所述运动状态信息和所述第一向量与所述第二向量之间的残差作为约束条件，对所述智能设备进行定位，融合了面特征因子和预积分因子，可以提高slam系统的精度。
第二方面，本申请提供了一种处理数据的装置，该装置包括：获取模块，用于获取第一三维点集合中每个三维点的数据，所述每个三维点的数据是智能设备在第一位置时采集得到的，所述第一三维点集合包括位于第一坐标系的多个三维点；第一确定模块，用于从所述第一三维点集合中确定第一子三维点集合，所述第一子三维点集合形成第一平面，所述第一平面为所述第一坐标系内的任意一个平面；计算模块，用于计算多个第一距离，所述第一距离为第一三维点与所述第一位置之间的距离，所述第一距离与所述第一三维点一一对应，所述第一三维点是所述第一子三维点集合中的任意一个三维点；第二确定模块，根据所述多个第一距离，从所述第一子三维点集合中确定第一目标三维点；定位模块，用于将所述第一目标三维点作为约束条件，对所述智能设备进行定位，获取所述第一位置的位姿。
根据本申请实施例中的装置，从所述第一子三维点集合中确定第一目标三维点，第一子三维点集合中包括多个三维点，将第一子三维点集合中的第一目标三维点作为约束条件，可以降低后续步骤的计算量及计算时间的开销，提升整个slam过程的运行效率。
在一种可能的实现方式中，所述定位模块还用于：根据所述第一目标三维点确定约束条件，所述约束条件用于影响所述智能设备的定位过程。
在一种可能的实现方式中，所述装置还包括：地图构建模块，用于将所述第一目标三维点作为约束条件对所述智能设备途经区域进行地图构建。
在一种可能的实现方式中，所述第一目标三维点是所述第一子三维点集合中与所述第一位置之间的距离最小的三维点。
根据本申请实施例中的装置，第一目标三维点是与第一坐标系的原点之间的距离最小的三维点，即与第一坐标系的原点距离最近的点，便于后续以第一目标三维点作为参数表示第一平面的面特征。
在一种可能的实现方式中，所述第一位置为所述第一坐标系的原点，所述第一距离为所述第一距离对应的三维点与所述第一坐标系的原点之间的距离。
根据本申请实施例中的装置，第一坐标系为以第一位置为原点的相对坐标系，所述第一平面位于第一坐标系中，能够消除第一平面的面特征的奇异性，从而可以提高slam方法的鲁棒性。
在一种可能的实现方式中，所述获取模块还用于：获取第二三维点集合中每个三维点的数据，所述第二三维点集合中每个三维点的数据是所述智能设备在第二位置时采集得到的，所述第二三维点集合包括位于第二坐标系的多个三维点；所述第一确定模块还用于：从所述第二三维点集合中确定第二子三维点集合，所述第二子三维点集合形成第二平面，所述第二平面为所述第二坐标系内的任意一个平面；所述计算模块还用于：计算多个第二距离，所述第二距离为第二三维点与所述第二位置之间的距离，所述第二距离与所述第二三维点一一对应，所述第二三维点是所述第二子三维点集合中的任意一个三维点；所述第二确定模块还用于：根据所述多个第二距离，从所述第二子三维点集合中确定第二目标三维点。
在一种可能的实现方式中，所述定位模块具体用于：将第一向量与第二向量之间的残差作为约束条件，对所述智能设备进行定位，其中，所述残差包括第一向量与第二向量之间的距离，第一向量以所述第一位置为起点，所述第一向量以所述第一目标三维点为终点，所述第一向量用于在所述第一坐标系中确定所述第一平面，所述第二向量以所述第二位置为起点，所述第二向量以所述第二目标三维点为终点，所述第二向量用于在所述第二坐标系中确定所述第二平面。
在一种可能的实现方式中，所述第二目标三维点是所述第二子三维点集合中与所述第二位置之间的距离最小的三维点。
在一种可能的实现方式中，所述第二位置为所述第二坐标系的原点，所述第二距离为所述第二距离对应的三维点与所述第二坐标系的原点之间的距离。
在一种可能的实现方式中，所述定位模块具体用于：确定所述第二向量在所述第一坐标系中的投影，并在所述第一向量与所述第二向量在所述第一坐标系中的投影之间的距离小于或等于预设距离时，将所述第一向量和所述第二向量在所述第一坐标系中的投影之间的残差作为约束条件。
在一种可能的实现方式中，所述定位模块具体用于：确定所述第一向量在所述第二坐标系中的投影，并在所述第一向量在所述第二坐标系中的投影与所述第二向量之间的距离小于或等于预设距离时，将所述第一向量在所述第二坐标系中的投影和所述第二向量之间的残差作为约束条件。
根据本申请实施例中的装置，将第一向量和第二向量投影到同一相对坐标系中，能够计算第一向量和第二向量的残差，从而可以提高slam方法的鲁棒性。
在一种可能的实现方式中，所述定位模块具体用于：获取惯性测量单元采集的运动状态信息，所述运动状态信息包括所述智能设备在所述第一位置采集所述第一三维点的数据时的运动状态信息与所述智能设备在所述第二位置采集所述第二三维点的数据时的运动状态信息；将所述运动状态信息和所述第一向量与所述第二向量之间的残差作为约束条件，对所述智能设备进行定位。
根据本申请实施例中的装置，将所述运动状态信息和所述第一向量与所述第二向量之间的残差作为约束条件，对所述智能设备进行定位，由于融合了面特征因子和预积分因子，可以提高slam系统的精度。
第二方面中的装置包括的各个模块可以通过软件和/或硬件方式实现。
例如，第二方面中的装置包括的各个模块可以通过处理器实现，即第二方面中的装置可以包括处理器，该处理器用于执行程序指令，以实现该装置包括的各个模块能够实现的各个功能。
可选地，第二方面中的装置开可以包括存储器，用于存储处理器执行的程序指令，甚至用于存储各种数据。
可选地，第二方面中的装置可以是智能设备，例如智能机器人、无人机等。其中，该装置还可以包括接收器或发送器。
可选地，第二方面中的装置可以是能够集成在智能设备中的芯片，此时，该装置还可以包括通信接口。
第三方面，本申请提供了一种计算机可读存储介质。该计算机可读存储介质中存储用于处理数据的装置执行的程序代码。该程序代码包括用于执行第一方面或其中任意一种可能的实现方式中的方法的指令。
第四方面，本申请提供了一种包含指令的计算机程序产品。当该计算机程序产品在处理数据的装置上运行时，使得该装置执行第一方面或其中任意一种可能的实现方式中的方法。
根据本申请实施例中的方法，从所述第一子三维点集合中确定第一目标三维点，第一子三维点集合中包括多个三维点，将第一子三维点集合中的第一目标三维点作为约束条件，可以降低后续步骤的计算量及计算时间的开销，提升整个slam过程的运行效率。
附图说明
图1是本申请实施例的技术方案的一种应用场景的示意图。
图2是本申请一个实施例的处理数据的方法的示意性流程图。
图3是本申请另一个实施例的处理数据的方法的示意性流程图。
图4是本申请一个实施例的处理数据的装置的示意性框图。
具体实施方式
下面将结合附图，对本申请中的技术方案进行描述。
本申请实施例适用于同时定位与地图构建(simultaneouslocalizationandmapping，slam)领域以及任何用于非线性估计问题中的状态边缘化处理的智能设备(intelligentequipment)中。
应理解，智能设备是指任何一种具有计算处理能力的设备、器械或者机器，本申请实施例中的智能设备可以是机器人(robot)、自动驾驶汽车(autonomousvehicles)、无人机(unmannedaerialvehicle)、智能家居(smarthome)和手机终端等，本申请对该智能设备并不做任何限定。
图1示出了本申请实施例的技术方案的一种应用场景的示意图，如图1所示，本申请实施例的技术方案适用于智能设备的slam系统100中，该slam系统包括前端(front-end)110和后端(back-end)120，其中，前端110主要完成传感器的数据采集和数据关联，而后端120主要功能是对前端产生的模型进行参数估计以及优化。下面以智能设备为例对本申请实施例的技术方案进行描述。
本申请实施例以将imu和激光雷达传感器作为观测输入传感器的智能设备为例。其中，imu采集智能设备的运动状态信息，例如，运动状态信息可以包括slam装置的角速度和线加速度；激光雷达采集智能设备周围环境的三维(threedimensions，3d)点云(下文简称“3d点云”)，该3d点云包括该智能设备周围环境中的多个三维点(下文也称“3d点”)，所述三维点可以理解为具有三维坐标值的点。
可选地，可以使用三维坐标系来表示3d点的三维坐标值。
例如，可以用空间直角坐标系中的(x，y，z)三轴坐标位置表示3d点的三维坐标值，其中，x代表横轴坐标，y代表纵轴坐标，z代表竖轴坐标。
图2为本申请一个实施例的处理数据的方法的示意性流程图。应理解，图2示出了该方法的步骤或操作，但这些步骤或操作仅是示例，本申请实施例还可以执行其他操作或者图2中的各个操作的变形，或者，并不是所有步骤都需要执行，或者，这些步骤可以按照其他顺序执行。
图2中的方法可以由图1中的slam系统100执行，该方法包括：
s210、获取第一3d点集合中每个3d点的数据，所述每个3d点的数据是智能设备在第一位置时采集得到的，所述第一3d点集合包括位于第一坐标系的多个3d点。
可选地，s210可以由图1中的slam系统的前端110执行。
获取智能设备中的激光雷达传感器采集的当前帧的3d点云，其中，3d点云可以是包括多个3d点的3d点集合，因此，当前帧采集的3d点云也可以称为第一3d点集合。
在本申请实施例中，智能设备位于第一位置时对周围环境进行观测，可以得到包括多个3d点的第一3d点集合。
可选地，上述第一位置可以是所述智能设备的中心点所处的位置，也可以是所述智能设备上的传感器的中心点所处的位置(这里的传感器可以是激光雷达或惯性测量单元)，或者也可以是所述智能设备的着地点所处的位置，或者也可以是其他与所述智能设备之间的相对位置固定不变的点或位置，本申请实施例对此不作限定。
在本申请实施例中，可以获取智能设备中的imu采集的当前帧的运动状态信息。可选地，可以对采集到的第一3d点集合和运动状态信息进行时间同步。
其中，时间同步也可以理解为，对第一3d点集合及运动状态信息进行时间对准，或者时间配准。
对采集到的第一3d点集合和运动状态信息进行时间同步，可以是指确定第一3d点集合及其对应的同一时刻的运动状态信息。应理解，这里的运动状态信息可以是指采集第一3d点集合时，该智能设备的运动状态信息。
在本申请实施例中，第一坐标系可以是绝对坐标系(绝对坐标系也可称为全局坐标系或世界坐标系)。其中，绝对坐标系的坐标轴和原点的位置是固定不变，不随智能设备的移动而改变。
可选地，第一坐标系可以是相对坐标系。其中，相对坐标系的坐标轴和原点与智能设备的相对位置固定不变，不随智能设备的移动而改变。
可选地，当相对坐标系的原点是所述智能设备的中心点时，该相对坐标系也可称为局部坐标系。
s220、从所述第一3d点集合中确定第一子3d点集合，所述第一子3d点集合形成第一平面，所述第一平面为所述第一坐标系内的任意一个平面。
其中，第一3d点集合可以是激光雷达采集的当前帧的3d点云。
可选地，s220可以由图1中的slam系统的前端110执行。
在本申请实施例中，第一3d点集合可以划分为一个或多个3d点子集，同一个3d点子集中的多个3d点可以属于同一个平面，第一3d点集合划分得到的一个或多个3d子集可以称为第一子3d点集合。也可以说是，对所述第一3d点集合中包括的多个3d点进行平面分割。
例如，可以通过随机抽样一致(randomsampleconsensus，ransac)算法对第一3d点集合中3d点的进行平面分割。
可选地，第一子3d点集合可以包括多个3d点。其中，第一子3d点集合中的多个3d点可以位于同一平面。
可选地，第一子3d点集合中的多个3d点可以位于第一平面，所述第一平面可以是所述第一3d点集合中的多个3d点对应的多个平面中的一个平面。
也就是说，所述第一平面位于所述第一坐标系中。
作本申请实施例一种可能的实现方式，可以去除第一子3d点集合中的噪点。例如，可以通过huber算法去除第一子3d点集合中的噪点。
在本申请实施例中，去除第一子3d点集合中的噪点，可以降低去除平面分割产生的噪点影响，提升平面分割的鲁棒性。
s230、计算多个第一距离，所述第一距离为第一3d点与所述第一位置之间的距离，所述第一距离与所述第一三维点一一对应，所述第一3d点是所述第一子3d点集合中的任意一个3d点。
其中，所述多个第一距离可以与所述第一子3d点集合中的多个3d点一一对应，所述第一距离可以为所述第一距离对应的3d点与所述第一位置之间的距离。
可选地，s230可以由图1中的slam系统的前端110执行。
在本申请实施例中，第一位置可以为第一坐标系的原点，因此，第一距离可以为第一距离对应的3d点与第一坐标系的原点之间的距离。
例如，第一坐标系可以是局部坐标系。此时，当前帧采集到的第一3d点集合中的多个3d点的坐标，可以是在局部坐标系中的坐标。应理解，局部坐标系属于相对坐标系中的一种。
在本申请实施例中，当第一坐标系为局部坐标系时，能够消除第一平面的面特征的奇异性，从而可以提高slam方法的鲁棒性。
s240、根据所述多个第一距离，从所述第一子3d点集合中确定第一目标3d点。
可选地，s240可以由图1中的slam系统的前端110执行。
作为本申请实施例一种可能的实现方式，所述第一目标3d点可以是所述第一子3d点集合中与所述第一位置之间的距离最小的3d点。
可选地，第一目标3d点可以是所述第一子3d点集合中与第一坐标系的原点之间的距离最小的3d点。
在本申请中，可以用第一目标3d点作为参数，表示第一平面的面特征，并使用第一目标3d点的坐标构建第一平面的面特征模型。
第一子3d点集合中的多个3d点所属的平面的面特征可以称为该第一子3d点集合对应的第一平面的面特征。
可选地，第一目标3d点也可以是第一子3d点集合中满足预设条件的其他3d点，例如，第一目标3d点可以是第一子3d点集合中与第一坐标系的原点之间的距离等于预设距离的3d点。
可选地，当第一目标3d点是所述第一子3d点集合中与第一坐标系的原点之间的距离最小的3d点时，构建的第一平面的面特征模型可以称为第一平面的最近点(closestpoint，cp)模型。
在本申请实施例中，第一目标3d点是与第一坐标系的原点之间的距离最小的3d点(即最近距离)，便于后续利用第一目标3d点将第一平面表示成向量的形式。
例如，使用第一目标3d点的坐标构建第一平面的cp模型的一种方式如公式(1)所示：
其中，g∏(x)表示第一目标3d点在全局坐标系g中的x轴坐标，g∏(y)表示第一目标3d点在全局坐标系g中的y轴坐标，g∏(z)表示第一目标3d点在全局坐标系g中的z轴坐标，g∏为第一平面在全局坐标系g中的cp模型。
其中，g∏可以通过公式(2)和(3)表示成向量的形式。
g∏＝gngd(2)
其中，gn表示第一平面在全局坐标系g中的法向量，gd表示第一目标3d点与全局坐标系g的原点的距离。
本申请实施例中，用第一平面中的一个3d点的坐标表示该平面的面特征，可以减少后续步骤的计算量以及节省时间开销，提升整个slam过程的运行效率。
s250、将所述第一目标3d点作为约束条件，对所述智能设备进行定位，获取所述第一位置的位姿。
在本申请实施例中，可以获取第二3d点集合中每个3d点的数据，所述第二3d点集合中每个3d点的数据可以是所述智能设备在第二位置时采集得到的，所述第二3d点集合包括位于第二坐标系的多个3d点。
可选地，从所述第二3d点集合中可以确定第二子3d点集合，其中，所述第二子3d点集合可以形成第二平面，所述第二平面可以为所述第二坐标系内的任意一个平面。
可选地，计算多个第二距离，所述第二距离可以为第二3d点与所述第二位置之间的距离，所述第二距离与所述第二3d点一一对应，所述第二3d点可以是所述第二子3d点集合中的任意一个3d点。
可选地，根据所述多个第二距离，从所述第二子3d点集合中可以确定第二目标3d点。
在本申请实施例中，可以用第二目标3d点作为参数，表示第二平面的面特征，即使用第二目标3d点构建第二平面的面特征模型。
第二子3d点集合中的多个3d点所属的平面的面特征可以称为该第二子3d点集合对应的第二平面的面特征。
可选地，当第二目标3d点是所述第二子3d点集合中与第二坐标系的原点之间的距离最小的3d点时，构建的第二平面的面特征模型可以称为第二平面的cp模型。
在本申请实施例中，第二平面可以为第一次被观测到的平面，即第二平面对应的第二子3d点集合中的多个3d点都是第一次被观测到。
此时，第二平面可以称为基准面，第二平面的面特征可以称为基准面特征，获取第二3d点集合的帧可以称为第二平面对应的平面的基准帧。
应理解，在基准帧之前采集到的3d点云中，不存在与第二子3d点集合中的多个3d点相同的点。
在本申请实施例中，所述第二位置可以为所述第二坐标系的原点。可选地，所述第二距离可以为所述第二距离对应的3d点与所述第二坐标系的原点之间的距离。
作为本申请实施例一种可能的实现方式，可以根据第一向量与第二向量之间的残差确定约束条件，对所述智能设备进行定位。
其中，所述残差可以包括第一向量与第二向量之间的距离，所述第一向量可以以第一位置为起点，第一向量可以以第一目标3d点为终点，第一向量可以用于在第一坐标系中确定第一平面，所述第二向量可以以第二位置为起点，第二向量可以以第二目标3d点为终点，第二向量可以用于在第二坐标系中确定第二平面。
应理解，第一向量为第一平面内的一个向量，当第一平面的法向量为已知时，第一向量可以在第一坐标系中唯一地确定第一平面，其中，第一平面的法向量可以通过第一平面内的多个3d点计算得到。第二向量与第一向量类似，这里不再赘述。
可选地，第一位置可以为第一坐标系的原点，也就是说，第一向量以第一坐标系的原点为起点，第一向量以第一目标3d点为终点。
可选地，第二位置可以为第二坐标系的原点，也就是说，第二向量以第二坐标系的原点为起点，第二向量以第二目标3d点为终点。
在本申请实施例中，可以确定所述第二向量在所述第一坐标系中的投影，并在所述第一向量与所述第二向量在所述第一坐标系中的投影之间的距离小于或等于预设距离时，根据所述第一向量和所述第二向量在所述第一坐标系中的投影之间的残差确定约束条件。
或者，可以确定所述第一向量在所述第二坐标系中的投影，并在所述第一向量在所述第二坐标系中的投影与所述第二向量之间的距离小于或等于预设距离时，根据所述第一向量在所述第二坐标系中的投影和所述第二向量之间的残差确定约束条件。
或者，可以确定所述第一向量和所述第二向量在除第一坐标系和第二坐标系外的任一坐标系中的投影，并在所述第一向量在所述任一坐标系中的投影与所述第二向量在所述任一坐标系中的投影之间的距离小于或等于预设距离时，根据所述第一向量的投影和所述第二向量的投影之间的残差确定约束条件。
作为本申请实施例一种可能的实现方式，可以根据第一位置对应的运动状态信息和第二位置对应的运动状态信息，确定第一坐标系相对第二坐标系的位移变化及姿态变化。
可选地，根据第一坐标系相对第二坐标系的位移变化及姿态变化，可以得到第一坐标系和第二坐标系之间的转换关系。
例如，根据第一坐标系相对第二坐标系的位移变化及姿态变化，可以得到第一坐标系和第二坐标系之间的变换矩阵。
此时，可以根据第一坐标系和第二坐标系之间的转换关系，将第二平面的面特征投影到第一坐标系，或者，将第一平面的面特征投影到第二坐标系。
将第二平面的面特征投影到第一坐标系的一种实现方式如公式(4)、(5)和(6)所示。
其中，g表示全局坐标系，a表示第二坐标系，l表示第一坐标系，an表示第二平面在第二坐标系中的法向量，ln表示第二平面在第一坐标系中的法向量，ad第一目标3d点与第二坐标系的原点的距离，ld第一目标3d点与第一坐标系的原点的距离，
在本申请实施例中，可以根据投影到同一坐标系的第一目标3d点和第二目标3d点，构建面特征因子。
可选地，上述投影到同一坐标系的所述第一向量(也可以说是第一平面的面特征)和所述第二向量(也可以说是第二平面的面特征)的距离可以为马哈拉诺比斯(mahalanobisdistance)距离。在本申请中，马哈拉诺比斯距离也可以称为马氏距离。
可选地，当第二平面为基准面时，若投影后的所述第一向量(即第一平面的面特征)和第二向量(即所述第二平面的面特征)的马氏距离大于一定的阈值，则可以将第一平面记作新的基准面，且将当前帧标记为第一平面对应的平面的基准帧；若该马氏距离小于或等于该阈值，则将第二平面与第一平面记为同一平面。
将第一平面记作新的基准面，可以理解为，第一平面在当前帧是第一次观测到，即第一平面对应的第一子3d点集合中的多个3d点都是第一次被观测到。
也就是说，在当前帧之前采集到的3d点云中，不存在与第一子3d点集合中的多个3d点相同的点。
第二平面与第一平面为同一平面，可以理解为，第一平面不是第一次被观测到。
在本申请实施例中，当第二平面与第一平面为同一平面时，可以根据投影到同一坐标系的所述第一向量和所述第二向量，构建面特征因子。
可选地，可以将投影到同一坐标系的所述第一向量和所述第二向量的残差，构建面特征因子。其中，所述残差可以为所述第一向量和所述第二向量之间的距离。
作为本申请实施例一种可能的实现方式，面特征因子可以为投影到同一坐标系的所述第一向量和所述第二向量的残差。例如，公式(7)为根据投影到同一坐标系的所述第一向量和所述第二向量的残差确定面特征因子的一种实现方式：
其中，rp(x)表示第二向量(即所述第二平面的面特征)在第一坐标系中的投影与第一平面(即所述第一平面的面特征)的残差，即面特征因子，l∏(x)表示第二向量在第一坐标系中的投影，
本申请实施例中，由于第一平面所在的第一坐标系以及第二平面所在的第二坐标系为相对坐标系，因此可以消除第一平面的面特征和第二平面的面特征的奇异性，从而可以提高slam方法的鲁棒性。
在本申请实施例中，可以获取惯性测量单元采集的运动状态信息，将所述运动状态信息和所述第一向量与所述第二向量之间的残差作为约束条件，对所述智能设备进行定位。
可选地，所述运动状态信息可以包括所述智能设备在所述第一位置采集所述第一三维点的数据时的运动状态信息与所述智能设备在所述第二位置采集所述第二三维点的数据时的运动状态信息。
可选地、可以对所述运动状态信息进行处理，构建预积分因子。例如，可以对所述运动状态信息进行预积分，根据所述预积分结果构建全局状态观测模型，并根据所述全局状态观测模型构建预积分因子。本申请实施例的一种构建预积分因子的方法如图3所示。
图3为本申请一个实施例的处理数据的方法300的示意性流程图。该方法300可以用于对运动状态信息进行处理，构建预积分因子。
图3中的方法300可以由图1中的slam系统的前端110执行，该方法300包括：
s310，根据运动状态信息构建状态观测模型。
例如，根据imu采集的运动状态信息构建imu的状态观测模型的一种实现方式为：根据公式(8)和(9)构建imu的状态观测模型。
ωm＝ω+bω+nω(8)
其中，gg表示全局坐标g下的重力加速度，ωm表示测量的角速度，αm表示测量的线加速度，ω表示角速度，α表示线加速度，bω表示角速度的偏置误差，ba表示线加速度的偏置误差，nω表示角速度的噪声，na表示线加速度的噪声。
s320，对相邻两帧的运动状态进行预积分。
可选地，可以根据imu的状态观测模型，对相邻两帧的3d点的运动状态进行预积分。
例如，可以根据公式(10)和(11)进行积分。
其中，
s330，构建全局状态观测模型。
可选地，根据s420中的预积分结果，构建全局状态观测模型。
例如，可以根据公式(12)、(13)和(14)构建全局坐标系下的全局状态观测模型。
其中，gg表示全局坐标g下的重力加速度，
s340，根据全局状态观测模型，构建预积分因子。
例如，可以根据公式(15)构建预积分因子。
其中，ri(x)表示基于imu预积分构建的全局状态观测模型的残差，
可选地，可以根据所述面特征因子和所述预积分因子，构建目标函数。
根据面特征因子和预积分因子构建目标函数可以包括：根据面特征因子和预积分因子构建状态变量，以及，根据状态变量x，构建目标函数。
例如，可以根据公式(16)构建状态变量。
x＝[x∏xi](16)
其中，x∏表示面特征状态，可以通过公式(17)表示，xi表示imu状态，可以通过式(18)表示。
x∏＝g∏(17)
公式(18)中，
例如，可以根据公式(19)构建目标函数。
其中，rpi(x)如公式(7)所示，rij(x)如公式(15)所示。
本申请实施例中，融合面特征因子与预积分因子，可以提高slam方法输出的位姿以及地图的精度。
可选地，可以根据目标函数进行位姿估计，输出该智能设备的位姿。
在本申请实施例中，可以根据目标函数，构建因子图，并通过因子图求解位姿估计。
s260、将所述第一目标3d点作为约束条件对所述智能设备途经区域进行地图构建。
可选地，s260可以由图1中的slam系统的后端120执行。
在本申请实施例中，可以根据位姿估计可以完成地图构建，同时输出位姿和地图。
此时，需要判断slam过程是否结束，如果slam未结束，循环迭代执行s210到s260。
应理解，本申请对方法200中各步骤的执行顺序和方式不作限定。
图4是本申请一个实施例的处理数据的装置的示意性框图。应理解，图4示出的处理数据的装置400仅是示例，本申请实施例的装置还可包括其他模块或单元。
装置400可以包括获取模块410、第一确定模块420、计算模块430、第二确定模块440、定位模块450和地图构建模块460。其中，装置400包括的各个模块可以通过软件和/或硬件方式实现。
例如，获取模块410可以是接收器，第一确定模块420、计算模块430、第二确定模块440、定位模块450和地图构建模块460可以是处理器。
又如，获取模块410可以是通信接口、第一确定模块420、计算模块430、第二确定模块440、定位模块450和地图构建模块460可以是处理器。这种情况下，该装置400可以是芯片。
可选地，第一确定模块420和第二确定模块440可以是同一模块。
可选地，定位模块450和地图构建模块460可以是同一模块。
可选地，装置400还可以包括存储器，用于存储处理器执行的程序代码，甚至可以用于存储数据。
装置400可以用于执行图2或图3中任意一个所描述的方法中的步骤。
例如，获取模块410，用于获取第一3d点集合中每个3d点的数据，所述第一3d点集合中每个3d点的数据是智能设备在第一位置时采集得到的，所述第一3d点集合包括位于第一坐标系的多个3d点；
第一确定模块420，用于从所述第一3d点集合中确定第一子3d点集合，所述第一子3d点集合形成第一平面，所述第一平面为所述第一坐标系内的任意一个平面；
计算模块430，用于计算多个第一距离，所述第一距离为第一3d点与所述第一位置之间的距离，所述第一距离与所述第一3d点一一对应，所述第一3d点是所述第一子3d点集合中的任意一个3d点；
第二确定模块440，根据所述多个第一距离，从所述第一子3d点集合中确定第一目标3d点；
定位模块450，用于将所述第一目标3d点作为约束条件，对所述智能设备进行定位，获取所述第一位置的位姿。
在一种可能的实现方式中，所述装置还包括：
地图构建模块460，用于将所述第一目标3d点作为约束条件对所述智能设备途经区域进行地图构建。
在一种可能的实现方式中，所述第一目标3d点是所述第一子3d点集合中与所述第一位置之间的距离最小的3d点。
在一种可能的实现方式中，所述第一位置为所述第一坐标系的原点，所述第一距离为所述第一距离对应的3d点与所述第一坐标系的原点之间的距离。
在一种可能的实现方式中，所述获取模块410还用于：获取第二3d点集合中每个3d点的数据，所述第二3d点集合中每个3d点的数据是所述智能设备在第二位置时采集得到的，所述第二3d点集合包括位于第二坐标系的多个3d点；
所述第一确定模块420还用于：从所述第二3d点集合中确定第二子3d点集合，所述第二子3d点集合形成第二平面，所述第二平面为所述第二坐标系内的任意一个平面；
所述计算模块430还用于：计算多个第二距离，所述第二距离为第二3d点与所述第二位置之间的距离，所述第二距离与所述第二3d点一一对应，所述第二3d点是所述第二子3d点集合中的任意一个3d点；
所述第二确定模块440还用于：根据所述多个第二距离，从所述第二子3d点集合中确定第二目标3d点；
在一种可能的实现方式中，所述定位模块450具体用于：
将第一向量与第二向量之间的残差作为约束条件，对所述智能设备进行定位，其中，所述残差包括第一向量与第二向量之间的距离，第一向量以所述第一位置为起点，所述第一向量以所述第一目标三维点为终点，所述第一向量用于在所述第一坐标系中确定第一平面，所述第二向量以所述第二位置为起点，所述第二向量以所述第二目标三维点为终点，所述第二向量用于在所述第二坐标系中确定第二平面。
在一种可能的实现方式中，所述第二目标3d点是所述第二子3d点集合中与所述第二位置之间的距离最小的3d点。
在一种可能的实现方式中，所述第二位置为所述第二坐标系的原点，所述第二距离为所述第二距离对应的3d点与所述第二坐标系的原点之间的距离。
在一种可能的实现方式中，所述定位模块450具体用于：确定所述第二向量在所述第一坐标系中的投影，并在所述第一向量与所述第二向量在所述第一坐标系中的投影之间的距离小于或等于预设距离时，将所述第一向量和所述第二向量在所述第一坐标系中的投影之间的残差作为约束条件。
在一种可能的实现方式中，所述定位模块450具体用于：确定所述第一向量在所述第二坐标系中的投影，并在所述第一向量在所述第二坐标系中的投影与所述第二向量之间的距离小于或等于预设距离时，将所述第一向量在所述第二坐标系中的投影和所述第二向量之间的残差作为约束条件。
在一种可能的实现方式中，所述定位模块450具体用于：获取惯性测量单元采集的运动状态信息，所述运动状态信息包括所述智能设备在所述第一位置采集所述第一三维点的数据时的运动状态信息与所述智能设备在所述第二位置采集所述第二三维点的数据时的运动状态信息；
将所述运动状态信息和所述第一向量与所述第二向量之间的残差作为约束条件，对所述智能设备进行定位。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、或者计算机软件和电子硬件的结合来实现。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本申请的范围。
所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
在本申请所提供的几个实施例中，应该理解到，所揭露的系统、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。
另外，在本申请各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。
应理解，本申请实施例中的处理器可以为中央处理单元(centralprocessingunit，cpu)，该处理器还可以是其他通用处理器、数字信号处理器(digitalsignalprocessor，dsp)、专用集成电路(applicationspecificintegratedcircuit，asic)、现场可编程门阵列(fieldprogrammablegatearray，fpga)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
所述功能如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本申请各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器(read-onlymemory，rom)、随机存取存储器(randomaccessmemory，ram)、磁碟或者光盘等各种可以存储程序代码的介质。
应理解，本申请中的“/”表示“或”的意思。其中“和/或”可以包括三种并列的方案。例如“a和/或b”可以包括：a，b，a和b。应理解，本申请中的“a或b”可以包括：a，b，a和b。
以上所述，仅为本申请的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。
- 还没有人留言评论。精彩留言会获得点赞！