模型训练方法、装置及基于拼接图的位姿优化方法、装置与流程

本发明涉及智能驾驶领域，具体而言，涉及一种模型训练方法、装置及基于拼接图的位姿优化方法、装置。

背景技术：

在地下地库的定位方案中，基于拼接图和高精语义地图匹配的定位能够提供全局而精确的位置信息，是定位算法的核心模块。

目前为了计算拼接图和高精语义地图的匹配程度定义了一种计算语义误差的方式：

其中imap，istitch分别为语义地图和拼接语义图对应的灰度图像，p为在拼接语义图中均匀采样的像素坐标，t^wp为p在语义地图中的像素坐标，记当前帧分割图上采样得到的灰度值构成的向量为观测z。

在实现本说明书实施例的过程中，发明人发现，由于地图中的车道线的分布是非常稀疏的，观测z中包含了大量在位姿发生变化时灰度值不会发生变化的分量，导致运算量较大并且结果的精确度较低。图1a为拼接图及均匀采样的像素位置(采样密度仅为示意)灰度图；图1b为图1a对应的语义地图上对应的像素点位置灰度图。定位算法通过最小化语义误差来优化当前的位姿xpose＝[x，y，θ]，但如图1a中正方形框采样点所示，即语义误差所定义的残差中相应的分量对于小范围内的位姿变化的梯度接近0，而长方形框(包括水平方向的和竖直方向的)内的采样点的灰度则对位姿发生的变化更为敏感。

技术实现要素：

本发明提供一种模型训练方法、装置及基于拼接图的位姿优化方法、装置，用以克服现有技术中存在的至少一个问题。

根据本说明书实施例的第一方面，提供了一种基于拼接图的位姿优化方法，包括以下步骤：

根据当前位置，通过位姿变化特征提取模型从所述当前位置对应的语义地图局部区域观测中提取与位姿变化强相关的观测特征，所述局部区域观测为所述语义地图局部区域上采样得到的灰度值构成的向量；

基于所述与位姿变化强相关的观测特征对根据拼接图确定的当前位姿进行优化。

可选的，所述基于所述与位姿变化强相关的观测特征对根据拼接图确定的当前位姿进行优化包括：

设所述与位姿变化强相关的观测特征为f，当前的语义误差e为：

其中，w为观测特征f的维数，fi为观测特征f的第i个特征分量，zmap为语义地图，zstitch为拼接语义图，为从语义地图中提取fi的函数，为从拼接语义图中提取fi的函数；

根据所述语义误差e对当前位姿进行优化。

可选的，所述位姿变化特征提取模型通过以下方式获取：

根据历史位置对应的历史拼接图及历史语义地图，在所述历史位置对应的语义地图局部区域随机采样多个与所述历史拼接图分辨率相同的子地图；

从每个所述子地图中提取观测特征，并由每个所述子图的观测特征形成训练样本集s：

s＝{[x1，y1，z1]，[x2，y2，z2]，....，[xn，yn，zn]}

其中n为所述子图的数量，(xi，yi)为第i个子图中心相对于语义地图局部区域中心(xc，yc)的坐标，i＝1，…，n，记为第i个子图观测特征的第j维分量；

通过分类树从所述训练样本集中提取特征，所述分类树的二分割函数为：

其中m，n为选取的观测维度，a∈(0，1)的常数，从所述分类树的根节点开始，随机选取δ＝(m，n，a，b)作为特征参数，对每个δ固定其中的m，n，a二分查找最优的b使得信息增益i(δ)最大：

其中信息熵h(sd)定义为sd中假设服从2维高斯分布的位置信息(xi，yi)的信息熵：

h(sd)＝0.5*log(2πe)²|cov(sd)|；

对当前节点选取信息增益最大的特征参数i(δ)，将所述训练样本集s中的样本数据相应地分入左右节点，再对左右子节点以同样的方式选取特征，直至达到终止条件，所述终止条件为|sd|＞min_node_samples；

根据所构造的分类树中的所有二分割函数得到位姿变化特征提取模型。

可选的，所述观测特征f通过以下方式获得：

将所述位姿变化特征提取模型的所有二分割函数对应的观测特征合并，得到观测特征f＝{δ1，δ2....δw}，w为最终选取的观测特征的维度。

根据本说明书实施例的第二方面，提供了一种位姿变化特征提取模型的训练方法，包括以下步骤：

根据历史位置对应的历史拼接图及历史语义地图，在所述历史位置对应的语义地图局部区域随机采样多个与所述历史拼接图分辨率相同的子地图；

从每个所述子地图中提取观测特征，并由每个所述子图的观测特征形成训练样本集s：

s＝{[x1，y1，z1]，[x2，y2，z2]，....，[xn，yn，zn]}

其中n为所述子图的数量，(xi，yi)为第i个子图中心相对于语义地图局部区域中心(xc，yc)的坐标，i＝1，…，n，记为第i个子图观测特征的第j维分量；

通过分类树从所述训练样本集中提取特征，所述分类树的二分割函数为：

其中m，n为选取的观测维度，a∈(0，1)的常数，从所述分类树的根节点开始，随机选取δ＝(m，n，a，b)作为特征参数，对每个δ固定其中的m，n，a二分查找最优的b使得信息增益i(δ)最大：

其中信息熵h(sd)定义为sd中假设服从2维高斯分布的位置信息(xi，yi)的信息熵：

h(sd)＝0.5*log(2πe)²|cov(sd)|；

对当前节点选取信息增益最大的特征参数i(δ)，将所述训练样本集s中的样本数据相应地分入左右节点，再对左右子节点以同样的方式选取特征，直至达到终止条件，所述终止条件为|sd|＞min_node-samples；

根据所构造的分类树中的所有二分割函数得到位姿变化特征提取模型。

根据本说明书实施例的第三方面，提供了一种基于拼接图的位姿优化装置，包括：

观测特征提取模块，被配置为根据当前位置，通过位姿变化特征提取模型从所述当前位置对应的语义地图局部区域观测中提取与位姿变化强相关的观测特征，所述局部区域观测为所述语义地图局部区域上采样得到的灰度值构成的向量；

位姿优化模块，被配置为基于所述与位姿变化强相关的观测特征对根据拼接图确定的当前位姿进行优化。

可选的，所述位姿优化模块包括：

误差计算单元，被配置为设所述与位姿变化强相关的观测特征为f，当前的语义误差e为：

其中，w为观测特征f的维数，fi为观测特征f的第i个特征分量，zmap为语义地图，zstitch为拼接语义图，为从语义地图中提取fi的函数，为从拼接语义图中提取fi的函数；

优化单元，被配置为根据所述语义误差e对当前位姿进行优化。

可选的，上述位姿优化装置还包括：

第一子地图提取模块，被配置为根据历史位置对应的历史拼接图及历史语义地图，在所述历史位置对应的语义地图局部区域随机采样多个与所述历史拼接图分辨率相同的子地图；

第一样本集获取模块，被配置为从每个所述子地图中提取观测特征，并由每个所述子图的观测特征形成训练样本集s：

s＝{[x1，y1，z1]，[x2，y2，z2]，....，[xn，yn，zn]}

其中n为所述子图的数量，(xi，yi)为第i个子图中心相对于语义地图局部区域中心(xc，yc)的坐标，i＝1，…，n，记为第i个子图观测特征的第j维分量；

第一分类树模块，被配置为通过分类树从所述训练样本集中提取特征，所述分类树的二分割函数为：

其中m，n为选取的观测维度，a∈(0，1)的常数，从所述分类树的根节点开始，随机选取δ＝(m，n，a，b)作为特征参数，对每个δ固定其中的m，n，a二分查找最优的b使得信息增益i(δ)最大：

其中信息熵h(sd)定义为sd中假设服从2维高斯分布的位置信息(xi，yi)的信息熵：

h(sd)＝0.5*log(2πe)²|cov(sd)|；

第一特征选取模块，被配置为对当前节点选取信息增益最大的特征参数i(δ)，将所述训练样本集s中的样本数据相应地分入左右节点，再对左右子节点以同样的方式选取特征，直至达到终止条件，所述终止条件为|sd|＞min_node_samples：

第一模型获取模块，被配置为根据所构造的分类树中的所有二分割函数得到位姿变化特征提取模型。

可选的，所述观测特征提取模块包括：

观测特征合并单元，被配置为将所述位姿变化特征提取模型的所有二分割函数对应的观测特征合并，得到观测特征f＝{δ1，δ2....δw}，w为最终选取的观测特征的维度。

根据本说明书实施例的第四方面，还提供一种位姿变化特征提取模型的训练装置，包括：

第二子地图提取模块，被配置为根据历史位置对应的历史拼接图及历史语义地图，在所述历史位置对应的语义地图局部区域随机采样多个与所述历史拼接图分辨率相同的子地图；

第二样本集获取模块，被配置为从每个所述子地图中提取观测特征，并由每个所述子图的观测特征形成训练样本集s：

s＝{[x1，y1，z1]，[x2，y2，z2]，....，[xn，yn，zn]}

其中n为所述子图的数量，(xi，yi)为第i个子图中心相对于语义地图局部区域中心(xc，yc)的坐标，i＝1，…，n，记为第i个子图观测特征的第j维分量；

第二分类树模块，被配置为通过分类树从所述训练样本集中提取特征，所述分类树的二分割函数为：

其中m，n为选取的观测维度，a∈(0，1)的常数，从所述分类树的根节点开始，随机选取δ＝(m，n，a，b)作为特征参数，对每个δ固定其中的m，n，a二分查找最优的b使得信息增益i(δ)最大：

其中信息熵h(sd)定义为sd中假设服从2维高斯分布的位置信息(xi，yi)的信息熵：

h(sd)＝0.5*log(2πe)²|cov(sd)|；

第二特征选取模块，被配置为对当前节点选取信息增益最大的特征参数i(δ)，将所述训练样本集s中的样本数据相应地分入左右节点，再对左右子节点以同样的方式选取特征，直至达到终止条件，所述终止条件为|sd|＞min_node_samples；

第二模型获取模块，被配置为根据所构造的分类树中的所有二分割函数得到位姿变化特征提取模型。

本说明书实施例根据位置所在的地图区域选取观测中与位姿变化强相关的观测特征，进而在根据与位姿变化强相关的观测特征计算语义误差时，能够排除掉观测中的冗余信息，降低观测/语义误差的维度，大幅减少非线性优化中与雅可比矩阵相关的运算量，提高运算的效率。同时，所选取的观测特征对位姿的微小变化更为敏感，增加了非线性优化在局部区域的收敛性能，提高位姿优化的精确度。

与现有技术相比，本说明书实施例的发明点至少包括：

1、根据当前位置所在的地图区域自适应地选取观测中与位姿变化强相关的观测特征，进而在根据与位姿变化强相关的观测特征计算语义误差时，能够排除掉观测中的冗余信息，降低观测/语义误差的维度，大幅减少非线性优化中与雅可比矩阵相关的运算量，提高运算的效率，是本发明实施例的发明点之一。

2、利用分类树对选取观测中与位姿变化强相关的观测特征的模型进行训练，使得在线定位时根据当前位置的观测提取观测特征，获取在地图不同区域自适应的语义误差，是本发明实施例的发明点之一。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。

图1a为拼接图及均匀采样的像素位置(采样密度仅为示意)灰度图；

图1b为图1a对应的语义地图上对应的像素点位置灰度图；

图2为本发明一个实施例的位姿变化特征提取模型训练方法流程图；

图3为本发明一个实施例的位姿优化方法流程图；

图4为本发明另一实施例的位姿优化方法流程图；

图5为本发明一个实施例的位姿变化特征提取模型训练装置模块图；

图6为本发明一个实施例的位姿优化装置模块图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有付出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

需要说明的是，本说明书实施例及附图中的术语“包括”和“具有”以及它们任何变形，意图在于覆盖不排他的包含。例如包含了一系列步骤或单元的过程、方法、系统、产品或设备没有限定于已列出的步骤或单元，而是可选地还包括没有列出的步骤或单元，或可选地还包括对于这些过程、方法、产品或设备固有的其它步骤或单元。

本说明书实施例公开了一种模型训练方法、装置及基于拼接图的位姿优化方法、装置。以下分别进行详细说明。

图2为本发明一个实施例的位姿变化特征提取模型训练方法流程图；如图2所示，该模型训练方法包括以下步骤：

s210，根据历史位置对应的历史拼接图及历史语义地图，在所述历史位置对应的语义地图局部区域随机采样多个与所述历史拼接图分辨率相同的子地图；

s220，从每个所述子地图中提取观测特征，并由每个所述子图的观测特征形成训练样本集s：

s＝{[x1，y1，z1]，[x2，y2，z2]，....，[xn，yn，zn]}

其中n为所述子图的数量，(xi，yi)为第i个子图中心相对于语义地图局部区域中心(xc，yc)的坐标，i＝1，…，n，记为第i个子图观测特征的第j维分量；

s230，通过分类树从所述训练样本集中提取特征，所述分类树的二分割函数为：

其中m，n为选取的观测维度，a∈(0，1)的常数，从所述分类树的根节点开始，随机选取δ＝(m，n，a，b)作为特征参数，对每个δ固定其中的m，n，a二分查找最优的b使得信息增益i(δ)最大(这是本发明实施例的发明点之一)：

其中l、r为分类树的左、右子节点，信息熵h(sd)定义为sd中假设服从2维高斯分布的位置信息(xi，yi)的信息熵：

h(sd)＝0.5*log(2πe)²|cov(sd)|；

s240，对当前节点选取信息增益最大的特征参数i(δ)，将所述训练样本集s中的样本数据相应地分入左右节点，再对左右子节点以同样的方式选取特征，直至达到终止条件，所述终止条件为|sd|＞min-node-samples；

s250，根据所构造的分类树中的所有二分割函数得到位姿变化特征提取模型。

上述实施例的模型训练方法根据位置所在的地图区域选取观测中与位姿变化强相关的观测特征，进而在根据与位姿变化强相关的观测特征计算语义误差时，能够排除掉观测中的冗余信息，降低观测/语义误差的维度，大幅减少非线性优化中与雅可比矩阵相关的运算量，提高运算的效率。同时，所选取的观测特征对位姿的微小变化更为敏感，增加了非线性优化在局部区域的收敛性能，提高位姿优化的精确度。

图3为本说明书一实施例的基于拼接图的位姿优化方法流程图；如图3所示，该位姿优化方法包括以下步骤：

s310，根据当前位置，通过位姿变化特征提取模型从所述当前位置对应的语义地图局部区域观测中提取与位姿变化强相关的观测特征，所述局部区域观测为所述语义地图局部区域上采样得到的灰度值构成的向量。

一种实现方式中，所述位姿变化特征提取模型通过以下方式获取：

根据历史位置对应的历史拼接图及历史语义地图，在所述历史位置对应的语义地图局部区域随机采样多个与所述历史拼接图分辨率相同的子地图；

从每个所述子地图中提取观测特征，并由每个所述子图的观测特征形成训练样本集s：

s＝{[x1，y1，z1]，[x2，y2，z2]，....，[xn，yn，zn]}

其中n为所述子图的数量，(xi，yi)为第i个子图中心相对于语义地图局部区域中心(xc，yc)的坐标，i＝1，…，n，记为第i个子图观测特征的第j维分量；

通过分类树从所述训练样本集中提取特征，所述分类树的二分割函数为：

其中m，n为选取的观测维度，a∈(0，1)的常数，从所述分类树的根节点开始，随机选取δ＝(m，n，a，b)作为特征参数，对每个δ固定其中的m，n，a二分查找最优的b使得信息增益i(δ)最大：

其中信息熵h(sd)定义为sd中假设服从2维高斯分布的位置信息(xi，yi)的信息熵：

h(sd)＝0.5*log(2πe)²|cov(sd)|；

对当前节点选取信息增益最大的特征参数i(δ)，将所述训练样本集s中的样本数据相应地分入左右节点，再对左右子节点以同样的方式选取特征，直至达到终止条件，所述终止条件为|sd|＞min_node-samples；

根据所构造的分类树中的所有二分割函数得到位姿变化特征提取模型。

一种实现方式中，所述观测特征f通过以下方式获得：

将所述位姿变化特征提取模型的所有二分割函数对应的观测特征合并，得到观测特征f＝{δ1，δ2....δw}，w为最终选取的观测特征的维度。

s320，基于所述与位姿变化强相关的观测特征对根据拼接图确定的当前位姿进行优化。

一种实现方式中，所述基于所述与位姿变化强相关的观测特征对根据拼接图确定的当前位姿进行优化包括：

设所述与位姿变化强相关的观测特征为f，当前的语义误差e为：

其中，w为观测特征f的维数，fi为观测特征f的第i个特征分量，zmap为语义地图，zstitch为拼接语义图，为从语义地图中提取fi的函数，为从拼接语义图中提取fi的函数；

根据所述语义误差e对当前位姿进行优化。

上述实施例的位姿优化方法根据位置所在的地图区域选取观测中与位姿变化强相关的观测特征，进而在根据与位姿变化强相关的观测特征计算语义误差时，能够排除掉观测中的冗余信息，降低观测/语义误差的维度，大幅减少非线性优化中与雅可比矩阵相关的运算量，提高运算的效率。同时，所选取的观测特征对位姿的微小变化更为敏感，增加了非线性优化在局部区域的收敛性能，提高位姿优化的精确度。

图4示出了根据本说明书另一实施例的位姿优化方法流程图；如图所示，该位姿优化方法包括：

s410，提取自适应的观测特征

在语义地图的一个局部区域{xcenter，yceter，r}，经测试r的取值范围在30～60像素范围训练算法的收敛效果较好，随机采样大量与拼接图分辨率相同的子地图，对每个子地图提取观测构成训练数据：

s＝{[x1，y1，z1]，[x2，y2，z2]，....，[xn，yn，zn]}

其中x1，y1为子图中心相对于xcenter，ycenter的坐标，记为第i个观测的第j维分量。

使用分类树提取特征，定义分类树上的二分割函数(binarysplitfunction)：

其中m，n为选取的观测维度，a∈(0，1)用来控制的相对大小。从分类树的根节点开始，随机选取δ＝(m，n，a，b)作为候选特征，对每个δ固定其中的m，n，a二分查找最优的b使得信息增益(informationgain)最大：

sl，sr为以当前分割函数对s进行分割得到的左右子节点中的样本集。

其中信息熵h(sd)定义为sd中假设服从2维高斯分布的位置信息xi，yi的信息熵：

h(sd)＝0.5*log(2πe)²|cov(sd)|

对当前节点选取信息增益最大的候选特征δ，将数据s相应地分入左右节点，再对左右子节点以同样的方式选取特征，终止条件为|sd|＞min_node_samples(最小节点样本数，可以根据经验设置)，这是本发明实施例的发明点之一。

将构造好的分类树中的所有节点上的二分割函数去二值化后构造成观测特征：

合并起来构成f＝{δ1，δ2....，δw}，w即为最终选取的观测特征的维度，w远小于输入的观测维度。

s420，定位时根据当前位置选取观测特征

在计算定位时，根据当前的初始位姿xpose检索到当前所在的语义地图的局部区域，读取该局部区域对应的观测特征f，定义当前的语义误差为：

根据语义误差优化当前位姿。

由于f对z进行的是线性变换，因此对xpose的偏导数可以用图像梯度的线性变换计算，记第i个观测特征对应两个选取的观测的地图图像的水平和垂直方向的灰度梯度为相对于拼接图图像中心的像素坐标偏移设当前位姿为xpose＝[x0，y0，θ]，则语义误差对应雅可比矩阵为：

其中

j＝1，2

从而使用常用的非线性优化算法即可优化当前的位姿。

与上述方法实施例相适应，图5示出了根据本说明书一实施例的基于拼接图的位姿优化装置模块图；如图5所示，该位姿优化装置500包括：

观测特征提取模块510，被配置为根据当前位置，通过位姿变化特征提取模型从所述当前位置对应的语义地图局部区域观测中提取与位姿变化强相关的观测特征，所述局部区域观测为所述语义地图局部区域上采样得到的灰度值构成的向量；

位姿优化模块520，被配置为基于所述与位姿变化强相关的观测特征对根据拼接图确定的当前位姿进行优化。

可选的，所述位姿优化模块包括：

误差计算单元，被配置为设所述与位姿变化强相关的观测特征为f，当前的语义误差e为：

其中，w为观测特征f的维数，fi为观测特征f的第i个特征分量，zmap为语义地图，zstitch为拼接语义图；

优化单元，被配置为根据所述语义误差e对当前位姿进行优化。

可选的，上述位姿优化装置还包括：

第一子地图提取模块，被配置为根据历史位置对应的历史拼接图及历史语义地图，在所述历史位置对应的语义地图局部区域随机采样多个与所述历史拼接图分辨率相同的子地图；

第一样本集获取模块，被配置为从每个所述子地图中提取观测特征，并由每个所述子图的观测特征形成训练样本集s：

s＝{[x1，y1，z1]，[x2，y2，z2]，....，[xn，yn，zn]}

其中n为所述子图的数量，(xi，yi)为第i个子图中心相对于语义地图局部区域中心(xc，yc)的坐标，i＝1，…，n，记为第i个子图观测特征的第j维分量；

第一分类树模块，被配置为通过分类树从所述训练样本集中提取特征，所述分类树的二分割函数为：

其中m，n为选取的观测维度，a∈(0，1)的常数，从所述分类树的根节点开始，随机选取δ＝(m，n，a，b)作为特征参数，对每个δ固定其中的m，n，a二分查找最优的b使得信息增益i(δ)最大：

其中信息熵h(sd)定义为sd中假设服从2维高斯分布的位置信息(xi，yi)的信息熵：

h(sd)＝0.5*log(2πe)²|cov(sd)|；

第一特征选取模块，被配置为对当前节点选取信息增益最大的特征参数i(δ)，将所述训练样本集s中的样本数据相应地分入左右节点，再对左右子节点以同样的方式选取特征，直至达到终止条件，所述终止条件为|sd|＞min_node_samples：

第一模型获取模块，被配置为根据所构造的分类树中的所有二分割函数得到位姿变化特征提取模型。

可选的，所述观测特征提取模块包括：

观测特征合并单元，被配置为将所述位姿变化特征提取模型的所有二分割函数对应的观测特征合并，得到观测特征f＝{δ1，δ2....δw}，w为最终选取的观测特征的维度。

与上述方法实施例相适应，图6示出了根据本说明书一实施例的位姿变化特征提取模型训练装置模块图；如图6所示，该模型训练装置600包括：

第二子地图提取模块610，被配置为根据历史位置对应的历史拼接图及历史语义地图，在所述历史位置对应的语义地图局部区域随机采样多个与所述历史拼接图分辨率相同的子地图；

第二样本集获取模块620，被配置为从每个所述子地图中提取观测特征，并由每个所述子图的观测特征形成训练样本集s：

s＝{[x1，y1，z1]，[x2，y2，z2]，....，[xn，yn，zn]}

其中n为所述子图的数量，(xi，yi)为第i个子图中心相对于语义地图局部区域中心(xc，yc)的坐标，i＝1，…，n，记为第i个子图观测特征的第j维分量；

第二分类树模块630，被配置为通过分类树从所述训练样本集中提取特征，所述分类树的二分割函数为：

其中m，n为选取的观测维度，a∈(0，1)的常数，从所述分类树的根节点开始，随机选取δ＝(m，n，a，b)作为特征参数，对每个δ固定其中的m，n，a二分查找最优的b使得信息增益i(δ)最大：

其中信息熵h(sd)定义为sd中假设服从2维高斯分布的位置信息(xi，yi)的信息熵：

h(sd)＝0.5*log(2πe)²|cov(sd)|；

第二特征选取模块640，被配置为对当前节点选取信息增益最大的特征参数i(δ)，将所述训练样本集s中的样本数据相应地分入左右节点，再对左右子节点以同样的方式选取特征，直至达到终止条件，所述终止条件为|sd|＞min-node-samples；

第二模型获取模块650，被配置为根据所构造的分类树中的所有二分割函数得到位姿变化特征提取模型。

本说明书实施例根据位置所在的地图区域选取观测中与位姿变化强相关的观测特征，进而在根据与位姿变化强相关的观测特征计算语义误差时，能够排除掉观测中的冗余信息，降低观测/语义误差的维度，大幅减少非线性优化中与雅可比矩阵相关的运算量，提高运算的效率。同时，所选取的观测特征对位姿的微小变化更为敏感，增加了非线性优化在局部区域的收敛性能，提高位姿优化的精确度。

本领域普通技术人员可以理解：附图只是一个实施例的示意图，附图中的模块或流程并不一定是实施本发明所必须的。

本领域普通技术人员可以理解：实施例中的装置中的模块可以按照实施例描述分布于实施例的装置中，也可以进行相应变化位于不同于本实施例的一个或多个装置中。上述实施例的模块可以合并为一个模块，也可以进一步拆分成多个子模块。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明实施例技术方案的精神和范围。