1.一种轨迹融合方法,其特征在于,所述方法包括:
获取待融合轨迹和基础轨迹;
确定所述基础轨迹的第一节点的节点数据和所述待融合轨迹的第二节点的节点数据,所述节点数据包括:节点的三维点云数据、节点的GPS数据和节点的位姿数据;
针对每一第二节点,基于该第二节点的GPS数据和各所述第一节点的GPS数据,确定该第二节点的邻近第一节点,对该第二节点和所述邻近第一节点进行三维点云特征匹配,得到匹配节点对;
对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准,得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵,作为所述待融合轨迹和所述基础轨迹之间的关联约束;
基于每一所述匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、所述基础轨迹中各第一节点的位姿以及所述待融合轨迹中各第二节点的位姿,对所述待融合轨迹进行位姿优化,并将位姿优化后的待融合轨迹和所述基础轨迹进行组合,得到融合轨迹。
2.根据权利要求1所述的方法,其特征在于,所述针对每一第二节点,基于该第二节点的GPS数据和各所述第一节点的GPS数据,确定该第二节点的邻近第一节点,包括:针对每一第二节点,基于哈希编码规则将该第二节点的GPS数据转换为GPS哈希码;
基于预先构建的第一节点集合与GPS哈希码的对应关系,从各所述第一节点集合中确定目标第一节点集合,所述目标第一节点集合的GPS哈希码与所述第二节点的GPS哈希码相同;
基于所述第二节点的GPS数据和所述目标第一节点集合中各第一节点的GPS数据,从所述目标第一节点集合中查找与所述第二节点距离最近的第一节点,作为该第二节点的邻近第一节点。
3.根据权利要求1所述的方法,其特征在于,所述对该第二节点和所述邻近第一节点进行三维点云特征匹配得到匹配节点对的步骤,包括:确定所述第二节点的三维点云数据,作为源点云数据;
确定所述邻近第一节点的三维点云数据,作为目标点云数据;
分别对所述源点云数据和所述目标点云数据进行关键点提取,得到源点云关键点和目标点云关键点;
分别对所述源点云关键点和所述目标点云关键点进行局部特征描述,得到源点云关键点的局部特征信息和目标点云关键点的局部特征信息;所述局部特征信息表示关键点与邻近的其他数据点之间的相对位置关系;
基于所述源点云关键点的局部特征信息和所述目标点云关键点的局部特征信息进行同名点匹配,并确定同名点匹配对的数目;
判断所述同名点匹配对的数目是否大于预设阈值,若大于,则所述第二节点和所述邻近第一节点构成匹配节点对。
4.根据权利要求1所述的方法,其特征在于,所述基于每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、所述基础轨迹中各第一节点的位姿以及所述待融合轨迹中各第二节点的位姿,对所述待融合轨迹进行位姿优化的步骤,包括:从所述基础轨迹的第一节点中确定一个标准节点,并将该标准节点的位姿确定为标准位姿;
以所述标准位姿为基准,对所述基础轨迹中其他第一节点的位姿进行转换,得到各其他第一节点相对于所述标准节点的第一相对位姿;
以所述标准位姿为基准,对所述待融合轨迹中各第二节点的位姿进行转换,得到各第二节点相对于所述标准节点的第二相对位姿;
根据所述刚体变换矩阵、所述第一相对位姿和所述第二相对位姿,对所述待融合轨迹中各第二节点的所述第二相对位姿进行优化。
5.根据权利要求4所述的方法,其特征在于,所述根据所述刚体变换矩阵、所述第一相对位姿和所述第二相对位姿,对所述待融合轨迹中各第二节点的所述第二相对位姿进行优化的步骤,包括:以所述待融合轨迹中各第二节点的第二相对位姿作为优化变量,构建最小优化误差函数,所述最小优化误差函数包括第一优化误差、第二优化误差和第三优化误差,所述第一优化误差包含所述待融合轨迹中各个相邻节点间的相对位姿误差,所述第二优化误差包含所述待融合轨迹中预先确定的回环节点间的相对位姿误差,所述第三优化误差包含各匹配节点对中第二节点和邻近第一节点之间的相对位姿误差;
求解所述最小优化误差函数,确定所述待融合轨迹中各节点的优化后位姿。
6.根据权利要求5所述的方法,其特征在于,所述最小优化误差函数为:
其中,E表示最小优化误差函数,ξ表示所述待融合轨迹中各节点的位姿的李代数,i、j和k表示所述待融合轨迹中节点的序号,ε表示相邻节点对、一阶跳边节点对和回环节点对的集合,eij表示第i个节点和第j个节点之间的相对位姿误差, 表示预设的针对eij的第一信息矩阵,ek表示所述匹配节点对中第k个第二节点和该第k个第二节点对应的邻近第一节点之间的相对位姿误差,Ω表示匹配节点对的集合, 表示预设的针对ek的第二信息矩T -1阵,(·) 表示矩阵转置,(·) 表示矩阵求逆;
其中,第i个节点和第j个节点之间的相对位姿误差eij通过如下公式进行计算:其中,ΔTij表示根据第i个节点和第j个节点的优化前的位姿计算出的第i个节点和第j个节点之间的相对位姿,Ti表示第i个节点的待优化的位姿,Tj表示第j个节点的待优化的位姿;ξij表示ΔTij的李代数,ξi表示Ti的李代数,ξj表示Tj的李代数;
所述匹配节点对中第k个第二节点和该第k个第二节点对应的邻近第一节点之间的相对位姿误差ek通过如下公式进行计算:其中
Tk表示匹配节点对中第k个第二节点的待优化的位姿,ξk表示Tk的李代数, 表示第k个第二节点和该第k个第二节点对应的邻近第一节点的三维点云之间的刚体变换矩阵,nbk表示第k个第二节点对应的邻近第一节点, 表示第k个第二节点对应的邻近第一节点的位姿,(·)-1表示矩阵求逆。
7.根据权利要求1所述的方法,其特征在于,在所述将位姿优化后的待融合轨迹和所述基础轨迹进行组合,得到融合轨迹之后,还包括:将所述融合轨迹确定为新的基础轨迹,执行获取待融合轨迹的步骤。
8.一种轨迹融合装置,其特征在于,所述装置包括:
获取模块,用于获取待融合轨迹和基础轨迹;
第一确定模块,用于确定所述基础轨迹的第一节点的节点数据和所述待融合轨迹的第二节点的节点数据,所述节点数据包括:节点的三维点云数据、节点的GPS数据和节点的位姿数据;
第二确定模块,用于针对每一第二节点,基于该第二节点的GPS数据和各所述第一节点的GPS数据,确定该第二节点的邻近第一节点,对该第二节点和所述邻近第一节点进行三维点云特征匹配,得到匹配节点对;
配准模块,用于对所有匹配节点对的第二节点和邻近第一节点进行三维点云配准,得到每一匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵,作为所述待融合轨迹和所述基础轨迹之间的关联约束;
融合模块,用于基于每一所述匹配节点对中第二节点和邻近第一节点的三维点云之间的刚体变换矩阵、所述基础轨迹中各第一节点的位姿以及所述待融合轨迹中各第二节点的位姿,对所述待融合轨迹进行位姿优化,并将位姿优化后的待融合轨迹和所述基础轨迹进行组合,得到融合轨迹。
9.一种电子设备,其特征在于,包括处理器、通信接口、存储器和通信总线,其中,处理器,通信接口,存储器通过通信总线完成相互间的通信;
存储器,用于存放计算机程序;
处理器,用于执行存储器上所存放的程序时,实现权利要求1-8任一所述的方法步骤。
10.一种计算机可读存储介质,其特征在于,所述计算机可读存储介质内存储有计算机程序,所述计算机程序被处理器执行时实现权利要求1-8任一所述的方法步骤。