Category: Digital Industry and Frontier Technologies

  • 具身智能论文和开源系统之-UniAct实现思路介绍

    这篇文章将向大家介绍具身智能通用策略模型UniAct,其主要思想是对异构的具身智能机器人动作空间进行统一表示,然而显示的动作空间在不同的具身智能机器人上面差异较大,这里论文的作者提出了基于隐式的通用动作空间表示方法,认为这些隐式动作元语空间(latent action primitives)中的原子动作(图1中的atomic behaviors)可以通过加权组合成支持多种异构机器人的具体动作。 如图1中的右边部分所示,对于UniAct的结构的解释说明如下:1、多模态大模型Backbone:VLM模型为LlavaOnevisionForConditionalGeneration,支持视觉和自然语言的多模态信息融合(多个模态数据不是进行cross attention计算而是在输入层进行拼接融合);2、universal_actions_codebook为universal action语义信息融合提取层,在这里定义了码本codebook(离散表示空间,如codebook_size=64则表示64个动作元语组成动作元语空间),基于codebook_size=64个latent action primitives的加权和输出universal action embeddings,关于加权和的权重如何通过参数学习可以参考引文[2]中的问题2描述;3、action head,在代码实现中提供了两种head: (1)将universal action embeddings和vision_embedding(vision embedding通过专门的视觉嵌入信息提取模型计算获得)拼接作为三层MLP的输入输出特定的机器人action行为预测(MLP_Decoder);(2)在上面的基础上输入中再多加入本体感知观测数据proprios(ACT_Decoder),具体的描述参考引文[2]中的问题3描述。 论文中进行了较为全面的实验和评估,模型针对真实环境的机器人验证集和模拟环境的机器人数据验证集均做了评估(针对仿真数据做了finetune训练),以及将backbone和codebook的参数进行冻结,来适配新的任务head(如上所述的ACT_Decoder)的模型通用性扩展性验证实验及评估。实验结果说明了模型结构和实验过程的有效性。 References

  • 具身智能论文和开源系统之-CrossFormer模型实现代码介绍

    本篇文章将向大家介绍支持异构机器人跨模态数据的CrossFormer模型如何实现通用策略模型的模仿学习算法。 在CrossFormer的实现过程中,主要将token区分为task tokens,observations tokens以及readout tokens。依此来实现多模态输入的分词和输出语义的提取。 其中task tokens具体实现时被安排在整个token输入序列的前面(wrapper类为PrefixGroup,the beginning of the token sequence),task的描述规范根据任务不同有不同的定义,如基于抓取动作的任务描述规范一般基于自然语言,而基于导航任务的任务描述规范一般为目标图像(goal image)。基于语言的任务指令描述(language instructions)一般通过FiLM层实现和图像信息的融合,可能是借鉴了RT-1的实现思路,具体的实现可以参考ResNet26FILM类[1]。 observations tokens的wrapper类为TimestepGroup,论文中提到,输入序列中的观测历史长度为k,即为k个时间点的观测数据的集合。 readout的tokens的wrapper类也为TimestepGroup,append到all_timestep_groups的list中,放在observeations的token后面。readout token和BERT的[CLS]特殊token类似,用于提取整个输入的token序列对应的总体语义信息,作为具体的任务head模型组件的输入。但实现时和BERT专门定义的词元[CLS]不同,在Cross Former中以positonal_embedding的形式实现,不同任务readout的tokens个数不同,在生成时将从positonal_embedding中按设定的个数截取即可)。BlockTransformer类的assemble_input_tokens函数实现了将prefix tokens和timestamps tokens进行concatenate形成整体输入。split_output_tokens在transformer架构的backbone的输出张量上做类似于assemble_input_tokens的逆操作逻辑后读取readout对应的张量输出后将其作为输入对接到具体的任务head。 问题1:该开源代码实现中语言指令(language instructions)和图像融合的FiLM代码具体的体现在什么地方? 回答:FiLM的实现可能借鉴了RT-1中的模型中的视觉和语言融合实现的思路,其中具体的实现解释参考引文[3]的问题1及解答。 问题2:不同的观测tokens类型(如图像和本体观测数据)如何实现的融合,是通过一个映射层可学习参数实现不同modality的观测token的融合处理的吗? 回答:get_model_config函数里有定义,针对不同的观测类型有不同的tokenizer,如图像(和自然指令融合)的tokenizer为ImageTokenizer,双臂机器人,四组机器人等的本体感知观测数据的tokenizer为LowdimObsTokenizer。不同类型的tokenizer的词汇表独立,通过线性可学习参数层实现映射到统一的token_embedding_size维度,具体的解释参考引文[3]的问题2及解答。 问题3:readouts的token具体有哪些,如何定义的,和BERT模型的[CLS]类似吗? 回答:在该开源系统的实现中,readouts在实现上以生成位置嵌入的形式存在并concatenate到observations的tokens后面。具体的实现可以参考引文[3]的问题3的回答。 问题4:异构的机器人数据集如何在一起训练,不同horizone窗口的数据对应的tokens输入序列的长度大小不同,是通过padding和mask相结合进行统一处理的吗?以及有什么标记来将transformer的output的embeddings输出给对应的任务head吗? 回答:和一般的LLM模型的批处理方法类似,通过padding和mask实现了不同长度序列的数据的统一训练,可以在generate_attention_mask函数中看一下具体的实现。同时在transformer结构输出时可以根据分组的名称完成split(函数split_output_tokens),和输入时的tokens的组装(assemble_input_tokens)可以看成是互逆的过程,在不同的head进行前向的时候根据不同的readout对应的group的名称的不同,就可以取对应的transformer bacbone输出的output对应的输出(可以类别取BERT的[CLS]位置对应输出的张量)作为对应head的输入进行计算。具体的过程可以参考引文[3]的问题4的回答。 References

  • 具身智能论文和开源系统之RLDS及辅助工具库介绍

    本篇文章将向大家介绍RLDS数据集格式定义及其提供的辅助工具库的使用,和COCO(Common Objects in Context,上下文中的常见物体,微软开源的计算机视觉任务数据集)数据集的定义和辅助工具cocotools类似。RLDS(Reinforcement Learning Data Schema)为google团队提出的面向强化学习任务的数据集数据模式的定义,本篇文章将简要介绍该数据模式以及相关辅助数据集处理的工具库及关键函数。 具身智能开源数据集OpenX-Embodiment是通过RLDS的格式构建的,将20多种不同的机器人数据进行了综合,由于原先不同数据集的格式存在着较大差异,如Berkeley Cable Routing采用HDFS格式进行存储,Stanford Hydra Dataset采用多个层级的json格式存储actions和观测数据observations等等,RLDS则定义了统一的存储规则即数据模式,同时在实现上以TFRecord进行二进制序列化,并提供了tensorflow_dataset库将其解析为tf.data.Dataset类方便模型训练和验证。 RLDS以层级字典的形式进行标准化的数据格式定义,强化学习的一次完整采样Rollout一般称为trajectory,其数据形式类似于{“steps”:{“observation”:[obs_t1,obst2,…obstn],”action”:[act_t1,act_t2,…act_tn]}}。以下代码为对OpenX-Embodiment的数据集的一个名为fractal20220817_data的子集采用tensorflow_dataset库进行解析的代码示例。OpenX-Embodiment的数据集在google cloud storage(如gs://gresearch/robotics/fractal20220817_data/0.1.0)上采用TFRecord序列化格式进行存储,关于RLDS的数据集的解释请参考文档[5]。 map() 是python的一个内置函数,但在这里tf.data.Dataset类实现了自定义的map函数可以使用在dataset的对象上。tf.data.Dataset类也实现了数据集采样的函数sample_from_datasets,可以从多个数据集按照权重进行采样,具体更加详细的介绍请参见[5]。 在crossfromer代码实现中,采用了dlimp函数库,其对符合RLDS格式的数据集做了进一步的包装,如在处理时去掉了steps[4]层级,数据的表示更加扁平化,具体的更多介绍可以参考[3][6]。 References

  • 具身智能论文和开源系统之-CrossFormer实现思路介绍

    本篇文章将向大家介绍一篇跨机器人模态的通用策略模型的论文CrossFormer[1]。 在多机器人数据(multi-robot data)上训练单一策略是一个具有挑战的任务,因为机器人的形态,传感器数据,执行器的类别以及相关器件的工作频率等特征多样性使得模型学习到泛化的策略难度增大。这篇文章提出了一个灵活可伸缩的基于Transformer架构的基础策略模型,该模型能够适应接收各种具身机器人(单臂机器人,双臂机器人,轮式机器人,四足机器人,四轴飞行器quadcopters)的异构数据(heterogeneous robot data)作为输入,论文通过充足的实验验证了该模型在性能表现上和针对特定的具身机器人训练的策略模型相当,同时超越了当前最好的跨多种具身机器人学习的模型算法(cross-embodiment learning)。 如图所示,该模型借鉴了之前基于Transformer架构训练多模态数据的一些思路,对观测数据和任务描述规范采用模态专属的分词器进行分词后组合成输入tokens序列。骨干网络为基于Transformer的时序因果解码器架构的自回归模型,输出的嵌入(output embeddings)将作为任务相关的head(如图所示有Quadruped Action Head,Single Arm Action Head,Navigation Action Head,Bimanual Action Head)的输入,通过专属head的计算输出为行为相关特定维度的输出。 观测数据主要有外部环境观测数据(如图像和激光点云)以及机器人本体感知观测数据(本体感知观测,Proprioceptive Observations,指的是机器人对自身状态的感知如关节几何夹爪的姿势和机器人部件的物理状态如加速度,角速度,温度等)将这些数据不同模态的观测数据采用不同的分词器进行分词后通过可学习的线性层映射到统一大小维度的词嵌入空间。一般任务的描述和定义用自然语言或全局图像(如导航任务中)或两者结合,语言和图像多模态数据的融合采用FiLM模块[7](RT-1模型也采用的该组件结构)。 在实验室该模型使用了20种不同的具身机器人的数据,具体详细的信息为:(1)Open Cross-Embodiment数据集的单臂机器人操作数据子集[3],该数据自己在Octo算法中被采用;(2)DROID Franka(DROID Distributed Robot Interaction Dataset)机器人操作数据集[4];(3)来自两个组织的7K个ALOHA机器人的轨迹数据ALOHA-multi-task[5];(4)60小时来自GNM数据集的导航数据[6];(5)25分钟的Go1四组机器人行走数据(通过在模拟环境中训练出的专家模型通过Rollout过程收集收据);(6)在实验过程中通过Franka机器人自行收集的200个轨迹数据。 不同数据集的数据通过转化后如数的tokens的序列长度可能不同,和LLM类似采用padding和mask实现不同的数据集的统一训练,transformer的输出embeddings根据输入的type输入到特定任务的head,实现了多个不同机器人不同任务数据的混合训练,实现了backbone参数共享的统一策略的模型。 关于crossformer的代码实现在后续的文章种将进一步进行较为详细的介绍。 References

  • 具身智能论文和开源系统之-AnyGrasp实现思路简介

    这篇文章将向大家简要介绍机器人夹爪时空位姿感知方面的一篇论文”AnyGrasp”[1]的实现思路,其也是上篇介绍文章GraspNet[2]的后续研究。 空间几何处理模块基本沿用了类似GraspNet的网络结构,但在输出的维度上做了更新:1、depth加了一个等级;2、有一个stable_score的度量(metric),和现有的graspnet的score采用重参化技巧相结合。 其更多的创新之处在于在空间几何处理模块(左边GSNet部分)的基础之上,添加了一个时间相关的模块(Temporal Association Module)来实现抓取位姿的跟踪功能(grasp pose tracking),该跟踪模块主要组成为(具体可以将解释和图1中的结构图进行对照理解):1、特征提取功能,纹理和颜色可以给tracking提供相关的线索信息,对于种子点的的approaching vector和GSNet中的基于cylinder grouping的特征分组类似的方法对点云的颜色信息进行聚合计算并通过MLP和Pooling后形成颜色特征;2、特征组合功能,和GSNet前向过程形成的相关特征拼接后连接MLP形成M个种子点的夹爪位姿的C个通道的feature输出,并和在时间Buffer里存储的上一帧的夹爪姿态的C个通道特征输出计算相关性(如通过计算特征间的余弦夹角来获取特证间的相似度得分similarity score,形成M*M的对应矩阵correspondence matrix)。在计算loss时,根据上一帧的每一个位姿,将当前帧与其距离在一定小范围内的抓取位姿集合一起计算损失函数,损失函数定义思路为增大相似姿势的相似性抑制距离较大姿势间的相关性,具体可以参考[1]中的公式描述。 其中在训练的过程中,首先训练几何处理模块,待几何处理模块收敛之后,则将其参数进行冻结继续训练连接在后面的时间关联模块。在训练时间模块的时候,对数据集有特殊要求,具体实验时的mini-batch的大小设置为4对点云帧数据,其中每对数据为同一场景下采集时间上连续的点云帧和对应的姿态。 关于代码实现,作者开源了其推理部分的代码,可以参考[3]。 References

  • 具身智能论文和开源系统之-GraspNet实现思路介绍

    这篇文章将向大家介绍机器人抓取物体方面的一篇论文GraspNet-1Billion[1]和其开源代码[2]的实现思路,GraspNet仅面向单臂机器人抓取物体任务,和RT-1,RT-2较为通用的任务相比有一些不同,他们的对比具体可以参考[3]: 数据集构建:数据收集和标注的方法可以参考[4],其中从DexNet2.0中收集的13种对抗性物体(adversarial objects),对抗性的含义是指表面较为复杂难以标注。由于抓取姿势和传统的视觉标注任务(如目标检测语义分割)等不同,抓取姿势分布在大的连续搜索空间(如图1所示),因此采用了力学(force-closure metric)等物理模型通过在离散化的搜索空间(其中平面一般基于抓取点的附近点云拟合抓取点的切平面的思路去确定,将搜索空间通过两个维度上的离散化即in-plane rotation sampling和depth sampling对抓取pose的搜索空间进行离散化采样),对每种抓取姿势进行评分。为了使得采样姿势多样,在采集数据时,只需要标注第一帧,后续的视频轨迹可以根据记录下来的位姿变换对抓举姿势进行传播计算。 关于夹爪坐标系有两点需要说明:(1)、平行夹爪的原点一般定义为其两个平面内面最下端的中间点,(2)、z轴指向夹爪当前的朝向(夹持物体的方向)。因为在模型生成时,一般在对应物体的表面的采样选择候选抓取点,在机器人RGB-D点云输入网络模型生成抓取位姿后,机械臂需要通过路径规划和轨迹生成实现从当前位置到目标位置的平滑运动。这里的抓取姿势采用7自由度(7-DoF)表示法。如图2所示,网络会预测如下几个部分的量(夹爪pose):(1)approaching vector v,亦即为物体目标点到夹爪原点形成的方向向量;(2) d为目标点和夹爪原点的距离;(3),R为夹爪转轴的旋转状态。 模型结构:采用了多阶段的多任务混合训练,第一阶段为特征提取层,输入为N个点云点,通过PointNet++骨干网络通过FPS(最远点采样策略[5])等过程采样M个特征[6]并通过下采样和上采样实现编码器解码器架构提取M个候选点的环境语义特征;第二阶段接ApproachNet,即为视点(viewpoint,approaching vector。如图2中的v向量)候选生成;第三,在OperationNet和ToleranceNet之前有一个分组和对齐的功能(CloudCrop里进行的实现),这个功能和之前的二阶段目标检测的ROIPooling或ROIAlign的作用类似,对对照说明,第四:OperationNet的功能时对角度进行分类(通过离散化化细粒度的角度范围通过分类实现角度预测,每个角度下对应着一个夹爪宽度预测,因此为3通道,其中k为离散化的depth数目,如一个bin为0.01m,具体的实现参考OperationNet),ToleranceNet的作用为预测每一个抓取姿势的抗干扰能力,其真值的大体思路为在抓取姿势的邻域空间进行评估,如果其邻近抓取空间的抓取分数都大于一定的阈值,则其抗干扰能力相对就高。 References

  • SLAM系列之FAST-LIVO2代码分析-基于直接法和ESKF相结合的VIO算法介绍

    这篇文章将向大家简要介绍FAST-LIVO2开源系统中的VIO部分实现的思路,具体的较为细节的内容在参考引文的链接里,这里对相关思路做一下汇总说明。VIO的实现采用直接法视觉里程计(Direct Visual Odometry)的核心概念,用光度残差衡量同一3D点在当前帧与参考帧的像素亮度差异。 在该系统的VIO算法中,基于ESKF(error state kalman filter)的误差测量方程的相关公式定义和基于光度残差和状态参数(误差状态,如基于旋转的李代数扰动量,平移噪声等)的雅可比矩阵的计算及推导参见[1],其中链式计算中的图像梯度的具体计算方法参考[2]。ESKF的状态更新公式及相关代码说明参考[3]。其中需要说明的是每一个可视点的测量光度误差的计算时,其参考图像帧可能不属于同一个参考帧(每个点云点根据光度一致性,几何约束或其他等条件选择最优的参考帧),而ESKF的每一次迭代都是基于当前的所有的用来优化位姿的地图点集合的光度误差总和进行的。具体的可以参考代码[4]中的详细实现。 函数retrieveFromVisualSparseMap基于当前处理逻辑点云帧(经时间对齐在原始点云帧基础上处理过的点云集合)中的点云集合信息动态提取当前帧可见的特征点,并构建高质量的局部子地图(visual_submap),其中稀疏地图中的可视点会基于点的多个观测,选择最优的patch参考(不同点可能会选择不同的参考图像帧),并根据姿态变换对图像做出对应的放射变换以便后续的光度残差的计算,ESKF基于子地图的可视点的观测状态(光度残差)和IMU的位姿误差运动方程紧耦合优化位姿; 问题:vio中关于地图的构建思路,和lidar的体素地图的关系? 回答:局部地图visual_submap主要用于优化位姿,在函数retrieveFromVisualSparseMap中构建局部稀疏地图的开始时进行重置,而全局地图feat_map为基于体素索引值和视觉点(VisualPoint)集合的hash表结构(unordered_map)。 References

  • SLAM系列之FAST-LIVO2代码分析-基于ESKF的LIO算法介绍

    这篇文章将向大家介绍LIO部分的基于ESKF(error state kalman filter)的误差状态卡尔曼滤波算法来进行位姿优化的一些细节内容。 LIO的ESKF实现的运动方程参考文献[4],其采用基于误差状态卡尔曼滤波器的原理,将系统的运行状态分解为非线性传播的名义状态和线性传播的误差状态;其中名义状态保持非线性状态的传播特性(IMU预积分的过程的imu pose的积分计算),而误差状态通过一阶泰勒展开将非线性问题局部线性化。通过线性模型将误差状态进行传播(参见预积分过程中state.cov协方差矩阵的传播计算,详细说明见文献[4]中)。 VoxelMapManager类的StateEstimation函数则实现了基于lidar和imu紧耦合的ESKF位姿优化算法,其中误差状态传播方程基于IMU的预积分结果,残差测量方程基于Lidar点云的多种误差因素(如基于李代数的旋转扰动量等),这里比较特殊的地方表现在:1、基于误差状态的传播计算,具体的关于误差状态的传播计算方式参考[1],主要在于基于IMU的误差状态传播协方差F_x的定义及误差传播协方差的计算实现[2];2、基于测量误差方法的特殊性(根据名义状态和误差状态的定义,以及基于李代数的扰动模型等便于求导的方法进行的状态定义),点云的测量残差方程的定义基于点云点到匹配平面的距离残差,其雅可比以及ESKF的计算公式推导请参考[5]。 References

  • LLM技术系列之-DeepSeek V1/V2/V3模型结构简介

    这篇文章将向大家简要介绍DeepSeek-V1/V2/V3的模型结构及其发展演化过程。 在介绍DeepSeek的模型结构之前,这里简要复习一下LLM关键的基于Transformer架构图的模型结构的主要知识和技术点。 Transformer的基本架构,包括多头自注意力机制,FFN等关键组件。其中自注意力机制也分为双向的(Multi-Head Self Attention)以及causal的(Masked Multi-head Self Attention),一般双向自注意力模块的用于编码器结构(如BERT,基于WordPiece分词器),主要用于上下文丰富语义信息提取,为下游任务如文本分类等提供很好的文本特征提取功能;causal自注意模块用于解码器架构(如GPT,基于BPE分词器),主要用于文本生成(自回归语言生成,基于之前生成的内容来生成后续的内容,为基于单向时序因果的计算逻辑,通过掩码机制进行计算),如问答系统等的应用,两种模块都会用到的为编码器解码器组合结构(Auto Encoder,双向自注意力结构和caual+交叉自注意力模块),主要用于机器翻译,序列转换等应用任务。 DeepSeek的文本生成,数学推理,代码补全等任务上基于传统的Transformer的解码器架构框架,但在两个核心组件上进行了调整:1为MSA到MLA(Multi-head Latent Attention)的调整;2为FFN到MoE组件的调整。下面将一一进行简要介绍。 MLA组件和原始Transformer版本的MSA的结构类似,差异仅仅在于对于token embedding的输入维度进行了降维,将其映射到隐空间上进行表示。可以理解为将token的embedding维度进行了压缩后进行的自注意力机制的计算,在计算之后再通过提升维度达到和原始的token embedding的维度一致。而原始版本的MSA中K,Q,V矩阵的第二个维度会保持和token embedding的维度一致(可以理解为一种在token上进行了基于类似扩展的VAE的算法实现了计算性能上的优化)。 MoE(Mixture of Experts)的基本原理是使用混合专家来替代原transformer架构中的前向反馈层(FFN),MOE层由多个FFN组成,每个FFN理论上更关注不同领域知识,即领域专家,每个token会选择其中的top-K(top-K为模型超参)个FFN向前传递。在具体实现时由路由选择组件和前向FFN专家集合组成。 问题1:MoE可以理解为首先用类似于早期的注意力机制选择token embedding的重要信息从而实现降维,然后将所得结果输送给多个领域专家进行计算聚合,可以这么理解对吗? 回答: 这种理解不是很正确,TopKRouter实现动态路,其核心功能为选择部分的token给对应的专家网络模块实现领域专家的计算。具体点的实现思路为: 基于门控网络(简单线性层+Softmax)实现TopK进行稀疏选择,将token输入给前topk个专家进行领域计算(剩下的专家不参与token的计算),这样可以较大效率提升计算效率,同时实现了不同专家的隐式领域划分,采用了负载均衡损失保证专家之间的计算平衡。同时聚合是基于token的,如token的结果由两个专家来进行领域计算,则结果为两个专家计算结果的加权和(topk权重之和归一化到1)。 其中负载均衡损失函数的定义可以参考参考[2]中的说明,其中fi为理想的专家分配概率分布(均匀分布),pi为实际的第i个token对于每个专家的概率分布,loss的计算可以基于KL散度进行计算。负载均衡损失只是一个约束项,在均匀性和有效性之间找到平衡。负载均衡损失的目的是防止专家塌陷,而不是让选择完全随机。 DeepSeek-V1模型在MoE的设计上做了两处的改进,具体如下图所示:(1)、细粒度的专家模型分割,采用了更多的专家模型子模块;(2)、采用了共享专家模型,学习通用知识。 问题2:请分析一下switch transformer和deepseek v1/v2/v3中的moe的实现上的模型结构,负载均衡损失等上的设计差别? 回答:DeepSeek的MoE架构的介绍见参考引文[2]。其中由于MoE的数量较多(v2版本路由专家达到160个),通常也会采用专家并行的策略,将专家分配到多个GPU设备上进行并行计算,但由于如果同一个token分配到的GPU设备如果过多,就会存在着GPU之间的通信开销问题,因此也会在路由中加入了设备数量限制。以达到通信的负载较为均衡的目的。v2中有160个路由专家,6个激活专家,v3的路由专家有256个,8个激活专家。v3在负载均衡的损失设计上也做了较大调整, References

  • SLAM系列之FAST-LIVO2代码分析-总体处理流程简介

    基于ROS1平台,整个流程是while循环,循环体内的流程以VIO和LIO迭代交替循环执行(如图1所示),这也是error state iterated kalman filter (ESIKF)的实现所需要的逻辑。 1、数据同步(sync_packages函数),如图所示,mode将在VIO和LIO间轮流变换,sync_pages会根据当前的mode进行对应的逻辑处理。 在VIO模式下,将以图片帧作为处理的截至时间,对齐imu和点云数据进行处理;如果图片的帧率过快,从buffer里获取的图片比last_lio_update_time时间还要早,则对图片进行丢帧处理,由于lidar传感器的扫描特性,原始帧每一个点云的采样时间不同,需要对原始帧的点云集合按对齐时间进行逻辑划分,比当前图片帧还要晚的点云点数据将会分配到下一个点云数据集合里。获取对齐的imu数据和lidar数据将更新到测量组里(MeasureGroup)。数据对齐完成后切换到LIO mode进行下一步的处理。 LIO模式下的数据同步主要来获取图像数据,图像数据获取后将切换到VIO mode进行下一步的处理。 图1:FAST-LIVO2处理流程图 2、数据预处理(processImu函数)在LIO模式下,主要处理包括:(1)、以预积分的方式计算imu在每一个时间戳下的初始位姿以及计算状态误差状态雅可比矩阵和误差传播协方差矩阵_state.cov,代码实现可以参考引文[1];(2)、通过imu预积分的结果(imu预积分计算在imu采样的时间戳序列下的对应的初始位姿)对lidar点云数据进行运动补偿去除畸变。运动补偿基于点云点时间戳和其对应的最近imu的时间戳的时间差和imu位姿及加速度角速度等数据按照运动方程进行时间对齐后的点云位姿估计,具体代码实现可以参考引文[2]。在VIO模式下,在基于LIO的点云和IMU紧耦合的ESKF优化的基础上,在对点云数据基于图像可视视角范围的过滤后在于图像的光度误差作为误差观测状态(又称为直接法)和前期IMU的姿态优化的基础上采用ESKF算法再次对位姿进行优化。 3、状态优化和建图,函数名为stateEstimationAndMapping,在步骤2中也有说明,其中LIO和VIO的迭代ESKF优化框架ESIKF(error state iterated kalman filter)在LIO和VIO依次都用到了ESKF滤波方法,其误差状态方程都是基于IMU的误差量(如基于李代数的旋转扰动量等19个维度),但误差观测方程不同,基于LIO的ESKF的误差观测基于点云点到匹配平面的距离残差,而VIO的ESKF的误差观测基于光度误差(直接法),具体后续将会在相关文章中详细介绍。 References