李群累积B样条的高效导数计算

5101520111480李群上累积B样条的高效导数计算0慕尼黑工业大学 Christiane Sommer � Vladyslav Usenko � David Schubert Nikolaus DemmelDaniel Cremers0摘要0连续时间轨迹表示最近在需要高帧率传感器和多个非同步设备融合的任务中越来越受欢迎。李群累积B样条是一种流行的连续轨迹表示方法，没有奇异点。它们已经在几乎实时的SLAM和里程计系统中使用了IMU、LiDAR、常规、RGB-D和事件相机，以及离线校准。这些应用需要高效计算时间导数（速度、加速度），但所有先前的工作都依赖于计算效率低下的公式。在这项工作中，我们提出了一种基于递归关系的时间导数的替代推导方法，它只需要O(k)而不是O(k^2)的矩阵运算（对于k阶样条），并且得到了简单而优雅的表达式。虽然产生相同的结果，所提出的方法显著加快了轨迹优化，并允许计算相对于样条节点的简单解析导数。本文提出的结果为将连续时间轨迹表示纳入更多需要实时性能的应用铺平了道路。01. 引言0估计轨迹是计算机视觉研究中的一个重要主题：在里程计和SLAM应用中，需要估计传感器的运动，在对象跟踪和机器人抓取任务中，我们希望计算随时间变化的6DoF姿态，并且在自主探索、路径规划和避障中，我们需要预测良好的轨迹。在过去的几年里，研究人员越来越多地转向连续时间轨迹：轨迹不再是离散时间点上的简单姿态列表，而是通过时间连续函数在可能姿态空间中优雅地表示。B样条是参数化这种函数的自然选择。它们已经在几个关于连续时间轨迹的著名作品中使用。0� 这些作者同等贡献。本工作得到ERC Consolidator Grant“3DReloaded”的支持。010 a x a y a z0图1.使用李群累积B样条表示IMU轨迹的相机-IMU校准（灰色线与用于可视化旋转的轴）。将校准模式的观测结果与加速度计和陀螺仪测量相结合，通过联合优化来估计轨迹和校准参数。该图可视化了以m/s2为单位的加速度计测量值（点）与经过优化后的样条轨迹生成的连续估计值（线）的重叠情况。如实验部分所示，所提出的公式能够显著减少此类优化的计算工作量。0轨迹估计是计算机视觉研究中的一个重要主题：在里程计和SLAM应用中，需要估计传感器的运动，在对象跟踪和机器人抓取任务中，我们希望计算随时间变化的6DoF姿态，并且在自主探索、路径规划和避障中，我们需要预测良好的轨迹。在过去的几年里，研究人员越来越多地转向连续时间轨迹：轨迹不再是离散时间点上的简单姿态列表，而是通过时间连续函数在可能姿态空间中优雅地表示。B样条是参数化这种函数的自然选择。它们已经在几个关于连续时间轨迹的著名作品中使用。Exp: Rd → L .(1)Its inverse is denotedLog: L → Rd ,(2)which is a composition of the matrix logarithm log: L → Afollowed be the inverse of the hat transform ·∨ : A → Rd.Definition 3.1. For an element X ∈ L, the adjoint AdjX isthe linear map defined byAdjX v = (Xv∧X−1)∨∀ v ∈ Rd .(3)It follows readily from the definition in (3) thatX Exp(v) = Exp(AdjX v)X∀ v ∈ Rd ,(4)Exp(v)X = X Exp(AdjX−1 v)∀ v ∈ Rd .(5)111490相对于控制点的变化。这不仅在理论上有兴趣：通过显著加快时间导数和雅可比计算，我们迈出了向连续时间轨迹表示及其应用（如相机跟踪、运动规划、物体跟踪或滚动快门建模）的实时能力迈出了一大步。总之，我们的贡献如下：0•李群累积B样条的时间导数的简单公式，需要一些与样条阶数k成线性关系的矩阵运算。0•SO(3)样条关于其节点的值和时间导数的简单（与k成线性关系）解析雅可比。0•与当前可用的实现相比，优化时间更快，因为复杂度明显更低。这在模拟实验和实际应用（如相机-IMU标定）中得到了证明。02. 相关工作0本文由两个主要部分组成：首先，我们详细研究了B样条的理论，特别是在李群上。在第二部分中，我们研究了我们的高效导数计算在计算机视觉中的可能应用。接下来，我们将分别回顾这两个部分的相关工作。0李群中的B样条自从20世纪50年代以来，B样条已经成为逼近和插值一维函数的流行工具。最著名的是，de Casteljau [6]，Cox[4]和De Boor[5]引入了一种从一组理想属性（如局部性和平滑性）推导样条系数的原则方法。Qin[16]发现，由于其局部性质，B样条可以方便地使用矩阵表示。通过使用所谓的累积B样条，该概念可以从Rd值函数转移到更一般的李群值函数集合上。这首先是针对3D旋转群SO(3) [11]完成的，后来推广到任意李群L[20]。后者还包含计算L值B样条导数的公式，但它们的表述方式对于实现来说并不实用。关于李群和李代数的计算的一般概述，我们参考[2, 19]。0计算机视觉中的应用由于其在表示函数方面的灵活性，B样条在计算机视觉和机器人领域中被广泛用于轨迹表示。应用范围从标定[8, 12]到0使用不同传感器进行里程计估计[12, 10, 13]，3D重建[14,15]和轨迹规划[21,7]。所有这些工作在某个时候都需要B样条的时间导数，但据我们所知，没有工作明确研究这些的高效计算和复杂性。有几个工作讨论了将轨迹表示为SE(3)中的一个样条，还是使用R3和SO(3)中的两个样条的分割表示[9, 14,15]。虽然这个问题不能没有具体的用例来明确回答，但所有的作者都得出结论，平均而言，使用分割表示在轨迹表示和计算时间方面都更好。03. 李群基础03.1. 符号表示0李群L是一个既是群又是可微流形的集合，群乘法和逆运算都是可微的操作。相应的李代数A是L在单位元素1处的切空间。李群的著名例子是平凡的向量空间李群Rd，其中L=A=Rd，群乘法是简单的向量加法，以及矩阵李群，如变换群SO(n)和SE(n)，群乘法是矩阵乘法。在计算机视觉应用中特别感兴趣的是3D旋转群SO(3)和刚体运动群SE(3)。本文中的连续时间轨迹是时间t的函数，其值在李群L中。如果d表示L的自由度数目，则帽变换∙∧:Rd→A用于将切向量映射到李代数A中的元素。可以使用矩阵指数exp:A→L将李代数元素映射到其李群元素，对于SO(3)和SE(3)，它具有封闭形式的表达式。帽变换后跟矩阵指数的组合由下式给出For a rotation R ∈ SO(3), the adjoint is simply AdjR = R.In this paper, we also use the commutator of two Lie algebraelements:Definition 3.2. The commutator is defined as[·, ·] : A × A → A,[V, W] = V W − V W .(6)For V = v∧, W = w∧ ∈ so(3), the commutator has theproperty[V, W]∨ = v × w .(7)3.2. DifferentiationTo differentiate the trajectories with respect to their pa-rameters, the following definitions and conventions will beused.Definition 3.3. The right Jacobian Jr is defined byJr(v)w = limǫ→0Log(Exp(v)−1 Exp(v + ǫw))ǫ(8)for all vectors w ∈ Rd.Intuitively, the right Jacobian measures how the differ-ence of Exp(v) and Exp(v + w), mapped back to Rd,changes with w. It has the following properties:Log(Exp(v) Exp(δ)) = v + Jr(v)−1δ + O(δ2) ,(9)Exp(v + δ) = Exp(v) Exp(Jr(v)δ) + O(δ2) .(10)If L = SO(3), the right Jacobian and its inverse can befound in [3, p. 40].Whenever an expression f(X) is differentiated w.r.t. toa Lie group element X, the derivative is defined as∂f(X)∂X= ∂f(Exp(δ)X)∂δ����δ=0.(11)Consequently, an update step for the variable X during op-timization is performed as X ← Exp(δ)X, where δ is thethe increment determined by the optimization algorithm.4. B-Spline Foundations4.1. BasicsB-splines define a continuous function using a set of con-trol points (knots), see also Fig. 2. They have a numberof desirable properties for continuous trajectory representa-tion, in particular locality and Ck−1 smoothness for a splineof order k (degree k − 1). We will focus on uniform B-splines of order k in this work.Definition 4.1. A uniform B-spline of order k is defined byits control points pi (0 ≤ i ≤ N), times ti = t0 + i∆t anda set of spline coefficients Bi,k(t):p(t) =N�i=0Bi,k(t) pi ,(12)k=2 (linear)k=4 (cubic)k=6 (degree 5)Figure 2. A set of control points (black) in R2, and the resultingB-splines for different orders k. For the linear spline (k = 2), thespline curve actually hits the control points, while for higher ordersplines, this is not true in general. The lighter lines show howthe splines change if one control point changes: the curves onlychange locally, i.e. in the vicinity of the modified control point.where the coefficients are given by the De Boor–Cox recur-rence relation [4, 5]Bi,0(t) =�1,for ti ≤ t < ti+10,otherwise(13)Bi,j(t) = t − tij∆t Bi,j−1(t) + ti+j+1 − tj∆tBi+1,j−1(t) .(14)It is possible to transform (12) into a cumulative repre-sentation:p(t) = �B0,k(t) p0 +N�i=1�Bi,k(t)(pi − pi−1) ,(15)�Bi,k(t) =N�s=iBs,k(t) .(16)p(u)����d×1=�pi,pi+1,· · · ,pi+k−1�����d×kM (k)� �� �k×ku����k×1,(17)1115004.2. 矩阵表示0B样条具有局部支持，这意味着对于阶为k的样条，在给定的t处只有k个控制点对样条的值有贡献。正如[16]所示，可以使用矩阵表示来表示均匀B样条的样条系数。在时间t∈[ti,ti+1)时，p(t)的值仅取决于控制点pi，pi+1，...，pi+k−1。为了简化计算，我们将时间转换为统一表示s(t):=(t−t0)/∆t，使得控制点转换为{0，..，k−1}。我们定义u(t):=s(t)−i作为从段[t i,ti+1)开始经过的标准化时间，并且从现在开始使用u作为时间变量。可以使用矩阵表示来评估p(u)，如下所示[16]：m(k)s,n =Cnk−1(k − 1)!k−1�l=s(−1)l−sCl−sk(k − 1 − l)k−1−n ,s, n ∈ {0, . . . , k − 1} ,(18)p(u) = pi ,di1 ,· · · ,dik−1 M (k)u ,(19)λ(u) = M (k)u ,(21)p(u) = pi +k−1�j=1λj(u) · dij .(22)X(u) = Xi ·k−1�j=1Exp�λj(u) · dij�,(23)dij = Log�X−1i+j−1Xi+j�∈ Rd .(24)0)111510其中un=u−n，M(k)是具有以下条目的混合矩阵：0s!(k−s)!是二项式系数。也可以使用累积样条的矩阵表示：0具有累积矩阵元素�m(k)j,n=�k−1s=jm(k)s,n和差分向量dij=pi+j−pi+j−1。我们在附录中证明了矩阵�M(k)的第一行始终等于单位向量e0∈Rk：0其中Kronecker delta δn,0。特别地，如果我们定义0这意味着λ0(u)≡1。将λ与λ0=1插入累积矩阵表示（19）中，我们可以方便地将p(u)写为如下形式：0定理4.2. 在位置u处，阶为k的B样条可以写为：05. 李群中的累积B样条0累积B样条（22）可以推广到李群[20]，特别是对于平滑旋转生成的SO(3)[11]。首先，（22）中的简单Rd加法对应于一般李群中的群乘法（矩阵李群中的矩阵乘法）。其次，我们可以使用Rd中的标量乘法将向量d∈Rd按因子λ∈R进行缩放，但是对于李群元素X来说，缩放的概念是不存在的。因此，我们首先必须将X从L映射到李代数A，即一个向量空间，然后对结果进行缩放，最后将其映射回L：Exp(λ∙Log(X))。这两个观察结果共同导致了李群中累积B样条的以下定义：0定义5.1.在具有控制点X0，...，XN∈L的李群L中，阶为k的累积B样条具有以下形式：0其中广义差分向量dij为0我们应该提到，与 R^d 中的 B样条不同，这里的乘法顺序(在(22)中的加法)是有关系的，不同的 Lie群的推广原则上是可能的。在实践中，我们使用最常用的约定，这是相关工作中最常用的约定[12, 10, 13, 14,15]。我们省略 i 以简化表示，并定义0Aj(u) = Exp(λj(u) ∙ dj) (25)0以得到更简洁的表达式0X(u) = X_i ∙0k−0j=1 Aj(u). (26)0注意这可以重新表述为一个递归关系：0X(u) = X(k)(u), (27)0X(j)(u) = X(j−1)(u)Aj−1(u), (28)0X(1)(u) = X_i. (29)05.1. 时间导数0本文的主要贡献是对时间导数的简化表示，与相关工作相比需要更少的操作。我们首先回顾当前标准的导数，然后引入我们的导数。我们用点表示对 u的微分，并应用乘积法则得到0˙X(u) = X_i ∙0k−1 �0j=10� j−1 �0l=1 Al(u)0� ˙Aj(u)0� k−1 �0l=j+1 Al(u)0�0�0with0˙Aj(u) = ˙λj(u)Aj(u)Dj = ˙λj(u)DjAj(u), (31)0并且 Dj = (dj)∧。注意 Aj(u) 和 Dj是可交换的。对于三次样条(k = 4)，这可以简化为0˙X = Xi � ˙A1A2A3 + A1˙A2A3 + A1A2˙A3 �, (32)0这是来自[12, 13, 14,15]等文献中的众所周知的公式。按照这个公式进行实现需要进行(k−1)2+1次矩阵-矩阵乘法，因此在样条度数上是二次的。我们提出了一种递归定义时间导数的方法，这需要更少的操作：0定理5.2. 时间导数˙X由以下递归关系给出：0˙X = Xω(k)∧, (33)0ω(j) = AdjA−1j−1ω(j−1) + ˙λj−1dj−1 ∈ Rd, (34)0ω(1) = 0 ∈ Rd. (35)0ω(k)通常被称为速度。对于 L =SO(n)，我们也称之为角速度。.(37)(45)111520证明。我们使用 X(u) 的递归定义(28)，并通过对 j进行归纳证明0˙X(j) = X(j)ω(j)∧, (36)0这等价于 j = k 的情况。首先，我们注意到对于 j =1，X(j)(u) = X_i 是关于 u 的常数，因此˙X(1) = 0 =X(1)ˆω(1)∧。现在，假设对于某个 j ∈{1,...,k−1}，(36)成立，那么我们有0˙X(j+1) = ∂u �X(j)Aj� = ˙X(j)Aj + X(j)˙Aj0= X(j)Aj�Aj−1ω(j)∧Aj +0= X(j)Aj�Aj−1ω(j)∧Aj + ˙λj(dj)∧�0= X(j)Aj�Aj−1ω(j)∧Aj +˙λj(dj)∧�0∧ + ˙λj(dj)∧�0这等价于对于 j = k 的情况。首先，我们注意到对于 j= 1，X(j)(u) = X_i 是关于 u 的常数，因此˙X(1) = 0= X(1)ˆω(1)∧。现在，假设对于某个 j ∈{1,...,k−1}，(36)成立，那么我们有0注意我们对˙X的递归定义使得很容易看出X^(-1)˙X = ω(k)∧∈A对于任何Lie群都成立，这是许多工作中隐含使用的性质[12,15]，但对于任意L从未明确显示。定理5.2中提出的方案只需要进行k−1次矩阵-向量乘法和向量加法，以及一次矩阵-矩阵乘法。由于在SO(3)中的旋转是Lie群中B样条的常见且重要的用例，我们明确给出了L =SO(3)的角速度递归关系(34)：0ω(j) = A�j-1ω(j-1) + ˙λj-1dj-1. (38)0对于二阶时间导数，很容易看出相关工作中提出的计算需要0k(k-1) + kC2k-1 = 102k2(k-1) (39)0矩阵-矩阵乘法，因此它们与样条的阶数成立方关系。我们提出了一种不同的计算¨X的方法：0定理5.3. X关于u的二阶导数可以通过以下递归关系计算：0¨X = X(ω(k))2∧ + ˙ω(k)∧, (40)0其中(角)加速度˙ω(k)通过递归定义为0˙ω(j) = ˙λj-1ω(j)∧, Dj-10∨ + AdjA-1j-1˙ω(j-1) + ¨λj-1dj-1, (41)0˙ω(1) = 0 ∈ Rd. (42)0证明。(40)是根据我们对˙X(33)的表达式和乘积法则得出的。对于(41)，最后一项是平凡的，因此我们关注速度递归(34)中第一项的导数：首先，我们注意到0AdjA-1j-1ω(j-1) = A-1j-1ω(j-1)∧Aj-10∨ =: ¯ω, (43)0所以该项的时间导数是三个项的和，遵循求导的乘积法则。中间项是A-1j-1˙ω(j-1)∧Aj-10∨ = AdjA-1j-1˙ω(j-1), (44)0这正是(41)中的第二个求和项。剩下的两个项是˙A-1j-1ω(j-1)∧Aj-1 + A-1j-1ω(j-1)∧˙Aj-10∨ (31) = −˙λj-1Dj-1¯ω∧ +0∨ (6) = ˙λj-1[¯ω∧, Dj-1]∨ (34) =˙λj-1ω(j)∧ - ˙λj-1Dj-1, Dj-10∨0= ˙λj-1ω(j)∧, Dj-10∨ .0这个提出的方案在任意L中只需进行2k个矩阵-矩阵乘法、k-1个矩阵-向量乘法、3(k-1)个向量加法和一个矩阵加法就可以计算出二阶时间导数。对于L =SO(3)，加速度˙ω(j)在(41)中简化为0˙ω(j) = ˙λj-1ω(j)×dj-1 + A�j-1˙ω(j-1) + ¨λj-1dj-1. (46)0这意味着对于SO(3)，二阶时间导数只需要3(k-1)个矩阵-向量乘法和向量加法以及两个矩阵-矩阵乘法，进一步减少计算时间。计算时间导数的迭代方案可以扩展到更高阶导数。作为示例，我们在附录中提供了X的三阶时间导数以及jerk ¨ω(k)。计算这个所需的矩阵运算数量仍然与样条的阶数成线性关系。我们还在附录中提供了不同方法所需的矩阵运算的全面概述。05.2. 对SO(3)中控制点的雅可比矩阵0为了强调这段文字关注的是SO(3)，我们用R代替X来表示元素。样条函数本身及其速度和加速度的值取决于控制点的选择。对于B样条的控制点的导数，我们首先注意到控制点∂dj∂Ri+j= J−1r(dj)R⊤i+j ,∂dj+1∂Ri+j= − ∂dj+1∂Ri+j+1. (47)dfdRi+j= ∂f∂dj·∂dj∂Ri+j+∂f∂dj+1· ∂dj+1∂Ri+j.(48)dfdRi= ∂f∂Ri+ ∂f∂d1· ∂d1∂Ri.(49)ρ(u) = Log R(u) .(50)∂∂d Exp(−λd)ω = λ Exp(−λd)ω∧Jr(−λd)(51)∂ω(j+1)∂dj= λjA⊤j ω(j)∧ Jr(−λjdj) + ˙λj1 .(52)∂ ˙ω(j+1)∂dj= ˙λj�ω(j+1)∧− Dj∂ω(j+1)∂dj�+ λjA⊤j ˙ω(j)∧ Jr(−λjdj) + ¨λj1 .(53)ω × d = ω∧d = −Dω(54)Pk−1 = 1 ,(55)sk−1 = 0 ,(56)∂ρ∂dj= λjJ−1r(ρ)PjJr(λjdj) ,(57)∂ω∂dj= Pj∂ω(j+1)∂dj,(58)∂ ˙ω∂dj= Pj∂ ˙ω(j+1)∂dj− (sj)∧∂ω∂dj,(59)Pj−1 = PjA⊤j ,(60)sj−1 = sj + ˙λjPjdj .(61)R = RiApre Exp(λjdj)Apost ,(62)RiApre Exp(λj(dj + δ))Apost= R Exp(λjA⊤postJr(λjdj)δ) + O(δ2)(63)Log (RiApre Exp(λj(dj + δ))Apost)= ρ + λjJ−1r(ρ)A⊤postJr(λjdj)δ + O(δ2) .(64)∂ω∂dj=∂∂dj�A⊤k−1ω(k−1) + ˙λk−1dk−1�= A⊤k−1∂ω(k−1)∂dj.(65)111530Ri+j在dj和dj+1中隐含地出现，并且对于j =0，我们还有R(u)对Ri的显式依赖。因此，我们计算关于dj的导数，然后应用链式法则。我们将重点放在L =SO(3)上，因为它是计算机视觉应用中最相关的群。为了应用链式法则，我们需要计算dj对Ri+j的导数，这可以从右雅可比矩阵和SO(3)的伴随的定义中轻松得出：0现在考虑一个映射到 R d 的曲线 f，例如样条值、速度或加速度。 f 依赖于控制点集合 R i + j，并具有导数0对于 j >  0 ，对于 j  = 0 ，我们得到0因此，为了计算相对于控制点的雅可比矩阵，我们需要相对于 d j 和 R i的偏导数。接下来，我们将首先推导出一些有用的性质，然后提出一个递归方案来计算 ρ ,  ω  =  ω ( k ) 和  ˙ ω  = ˙ ω( k ) 的雅可比矩阵，其中我们通过 Log 映射将向量  ρ  ∈R d 定义为 R 到 R d 的映射:0ω ( j ) 和  ˙ ω ( j ) 相对于 d j 的雅可比矩阵为零:从定理5.2和5.3中 ω  和  ˙ ω的递归方案中，我们发现第一个索引 j  + 1 是 d j显式或隐式出现的位置（以 A j的形式）。此外，在我们的推导中，我们使用了附录中证明的以下重要关系:0对于 λ  ∈  R  和  d ,  ω  ∈  R 3。这些发现有两个重要的含义:0定理5.4.  ω ( j +1) 相对于 d j 的雅可比矩阵是0证明. 我们将 (51) 应用于角速度  ω ( j +1) ，如 (38) 中对 L =  SO (3) 的推导。0定理5.5.  ˙ ω ( j +1) 相对于 d j 的雅可比矩阵是0证明. 我们将 (51) 应用于  ˙ ω ( j +1) ，如 (46) 中所定义，并使用0对于 d ,  ω  ∈  R 3 和  D  =  d ∧ 。0这些是 ω ( j +1) 和  ˙ ω ( j +1) 的雅可比矩阵结果0相对于 d j 的雅可比矩阵可以通过递归来推导 ω  和  ˙ ω的雅可比矩阵:0定理5.6. 从 j=k-1 到 j=1的以下递归关系允许在线性（相对于k）数量的乘法和加法中计算 ρ ,  ω 和  ˙ ω 的雅可比矩阵:0P j  和  s j  分别是累加器的乘积和和。0证明 (57) .  我们将 R 写成0其中 A pre  和  A post  通过与累积李群 B 样条的通用形式(23) 进行比较而隐式定义。它们不依赖于 d j。右雅可比矩阵性质 (10) 结合伴随性质得到0现在，我们应用右雅可比矩阵性质（9）得到0在  δ  = 0 时进行微分，并插入  A � post =  P j，得到（57）。0证明（58）。由于  j  ≤  k  −  2 ，A k − 1 不依赖于 d j，因此0迭代应用这个等式得到了该命题。∂ ˙ω∂dj=∂∂dj�− ˙λk−1Dk−1ω + A⊤k−1 ˙ω(k−1) + ¨λk−1dk−1�= − ˙λk−1Dk−1∂ω∂dj+ A⊤k−1∂ ˙ω(k−1)∂dj.https://gitlab.com/tum-vision/lie-spline-experimentsSO(3)4acc.0.0570.1472.57SO(3)4vel.0.0580.0881.52SO(3)5acc.0.0810.2803.45SO(3)5vel.0.0820.1411.73SO(3)6acc.0.1170.5204.43SO(3)6vel.0.1110.2171.95SE(3)4acc.0.2770.5872.12SE(3)4vel.0.2530.3341.32SE(3)5acc.0.4451.1962.69SE(3)5vel.0.4050.5811.43SE(3)6acc.0.6442.3323.62SE(3)6vel.0.5900.9361.59111540证明（59）。首先，由于  j  =  k  −  1的情况是平凡的，我们可以集中于  j  ≤  k − 2：对于这些情况，我们发现（通过插入到（46）中）0（66）我们在附录中通过归纳证明了这个等式与（59）的等价性。06. 实验0为了评估基于B样条时间导数和我们的SO(3)雅可比矩阵的提出的公式，我们进行了两个实验。在第一个实验中，我们使用模拟的速度和加速度测量来估计由样条表示的轨迹。这可以突出提出的公式的计算优势。在第二个实验中，我们展示了一个真实世界应用的例子，特别是多相机和IMU标定。在这种情况下，我们估计IMU的连续轨迹，相机到IMU的变换，加速度计和陀螺仪的偏差以及世界坐标系中的重力。在这两种情况下，我们实现了基线方法，该方法计算了先前工作[12,13]中的时间导数。除非另有说明，我们使用Ceres[1]进行优化，使用自动微分选项。该选项使用双重数来计算雅可比矩阵。在所有情况下，我们使用Levenberg-Marquardt算法进行优化，使用稀疏Cholesky分解来解线性系统。实验在Ubuntu 18.04上使用Intel Xeon E5-1620CPU进行，我们使用clang-9作为编译器，使用-O3-march=native-DNDEBUG标志。尽管残差和雅可比矩阵的计算很容易并行化，但在本文中我们集中于不同公式之间的差异，并在单线程配置下运行所有实验。我们已经将实验开源，可在以下网址找到：06.1. 模拟序列0B样条在李群上的一个典型应用是从一组传感器测量中估计轨迹。在我们的第一个实验中，我们假设我们有姿态、速度和加速度测量，可以为SO(3)或SE(3)制定一个优化问题，该问题应该估计表示真实轨迹的样条节点的值。在这种情况下，我们最小化平方和0L k Con�g. 我们的 基线 加速比0表1.使用速度（vel.）和加速度（acc.）测量的提出的和基线公式的优化时间（以秒为单位），以及我们的公式所实现的加速比。在所有实验中，两种公式都收敛到相同的结果，迭代次数也相同。0残差，其中残差是测量值与计算值之间的差异。我们使用一个具有100+k个节点的样条曲线，节点之间间隔2秒，有25个值测量和2020个速度或加速度测量，这些测量均匀分布在样条曲线上。这些测量值是从地面真值样条曲线生成的。我们将要优化的样条曲线的节点值初始化为扰动的地面真值，这样可以进行5次优化迭代直到收敛。表1总结了结果。如预期的那样，提出的公式在所有情况下都优于基准公式。由于基准公式在样条曲线的阶数上是三次的，因此加速度测量的时间差异更大。06.2. 相机-IMU标定0在第二个实验中，我们旨在展示我们的方法在相机-IMU标定等实际应用中的适用性。我们使用阶数为5、节点间隔为10毫秒的两种类型的样条曲线来表示IMU坐标系的连续运动：SO(3) × R^3。0（分割表示）和SE(3)。对于这两种情况，我们实现了计算时间导数的提出方法和基准方法。在这个实验中，我们使用了[18]的calib-cam1序列。它包含了51.8秒的数据，包括10336个加速度计和相同数量的陀螺仪测量值，以及观测到标定图案的291324个角点的1036张图像。我们假设相机的内部参数已经预先标定，并且从角速度计算得到的相机和IMU旋转之间有一个良好的初始猜测。我们实验中的所有优化都具有相同的初始条件和噪声设置。∈ SE(3),(68)ra(u) = Rwi(u)−1(¨twi(u) + g) − ˜a + ba,(70)E =r⊤ω Wωrω +r⊤a Wara +r⊤p Wprp,(71)SO(3) × R3SE(3)OursOursBaselineOursBaselineAnalyticCeresCeresCeresCeres5.8214.1815.0923.5637.14111550估计的变量 g  [m/s^2] b a  [m/s^2] b g  [rad/s] t ic 0  [m] t ic 1  [m] R ic 0  [rad] R ic 1  [rad]0最大偏差 6 . 07  ∙  10 − 5 6 . 32  ∙  10 − 5 2 . 14  ∙  10 − 9 6 . 34  ∙  10 − 6 6 . 33  ∙  10 − 6 3 . 51  ∙  10 − 8 3 . 34  ∙  10 − 80表2. 平均估计值与所有标定方法的估计值之间的最大差异。对于向量（g，b a，b g，t ic 0，t ic 1），使用L2范数。对于旋转值（R ic 0，Ric 1），使用弧度的角度范数。结果表明，无论底层的样条曲线表示是SO(3) × R^3还是SE(3)，标定结果都收敛到相同的值。0优化后的序列轨迹的一个片段如图1所示。投影残差定义如下：0r p ( u ) =  π ( T -1 ic T wi ( u ) - 1 x )  -  ˆ p , (67)0T wi  = [R wi t wi;0 1]0其中 T wi ( u )是从样条曲线计算得到的IMU在坐标系中的姿态，可以直接表示为一个姿态（SE(3)），也可以分别表示为旋转和平移的两个值（SO(3) × R^3）。T ic是从观测到角点的相机到IMU的变换，π是一个固定的投影函数，x 是标定角点的三维坐标，ˆp表示在图像中检测到的角点的二维坐标。陀螺仪和加速度计的残差定义如下：0r ω ( u ) =  ω ( u )  -  ˜ ω  +  b g , (69)0其中 ˜ ω 和 ˜ a 是测量值，b g 和 b a 是静态偏差，g是世界坐标系中的重力矢量。R wi ( u )是从IMU到世界坐标系的旋转。ω ( u ) 和 ¨ t wi ( u )的定义取决于我们使用的样条曲线表示。对于SO(3) ×R^3表示，ω ( u )是在(38)中计算的身体坐标系中的角速度，¨ t wi ( u )是表示IMU在世界坐标系中的平移的R^3样条曲线的二阶导数。对于SE(3)，ω ( u )是在(34)中计算的速度的旋转部分，¨ t wi ( u )是在(40)中计算的姿态的二阶时间导数的平移部分。这些残差的SE(3)表示与[12]中使用的表示相同。标定是通过最小化将所有测量的残差组合在一起的函数来完成的：0其中，W ω ，W a ，W p是使用传感器噪声特性计算的权重矩阵。在所有进行的实验中，校准在12次迭代后收敛到相同的值（见表2），因此切换到我们的公式不会影响解的准确性。我们的结果证实了之前的研究[9, 15]，即 SE (3)样条表示与分裂表示相比没有引入任何优势，但需要更多的计算。0表3.完成相机-IMU校准所需的时间（以秒为单位）。解析法使用自定义求解器，对所有残差使用解析雅可比矩阵。其他方法使用Ceres求解器，使用双数进行雅可比矩阵计算。0时间结果如表3所示。在所有情况下，我们都可以看到所提出的公式在时间导数方面的优势。对于分裂表示，只有陀螺仪残差受到影响，因此差异相对较小，如果使用Ceres雅可比矩阵（时间减少6%）。对于 SE (3)表示，陀螺仪和加速度计残差都受到影响，因为我们需要计算线性加速度的二阶时间导数。在这种情况下，我们的公式导致计算时间减少36.7%。我们还展示了使用分裂表示和我们在第5.2节中介绍的 SO (3)的解析雅可比矩阵的自定义求解器的结果。它导致计算时间进一步减少，并能够在不到6秒的时间内进行校准（比使用分裂表示的基线方法快2.6倍）。结果表明，我们的时间导数公式需要更少的计算量，特别是如果需要计算二阶时间导数。这对于多个IMU的校准[17]可能会产生更大的影响，因为即使对于分裂表示，也需要评估旋转加速度。07. 结论0在这项工作中，我们展示了如何在李群上常用的B样条以非常高效的方式进行微分（关于时间和控制点）。我们的时间导数和雅可比矩阵都可以通过 O ( k )的矩阵运算来计算，而传统的计算方案最多是 O ( k^3)。我们在数学上证明了我们的陈述的正确性。虽然我们的贡献明确关注理论，但我们还展示了如何选择一个良好的表示以及我们的导数计算方法，可以在实际的计算机视觉应用中加速高达4.4倍。这使得我们提出的方法对于实时连续时间轨迹表示的应用非常重要。[1] Sameer Agarwal, Keir Mierle, and Others.Ceres solver.http://ceres-solver.org.[2] Timothy D Barfoot. State Estimation f