• 因子图相关理论汇总


    本文主要是学习《Factor Graphs for Robot Perception》一书是记录的笔记,耗时较长,篇幅较大,后续再做细致的分节以及补充相关知识。

    问题表达

    Measurement predictions and noise models are the core elements of a generative model, which is well matched with the Bayesian network framework.

    转换为贝叶斯网络:

    联合概率密度:

    [egin{aligned} p(X,Y) &= p(x_1)p(x_2|x_1)p(x_3|x_2) \ & imes p(l_1)p(l_2) \ & imes p(z_1|x_1) \ & imes p(z_2|x_1,l_1)p(z_3|x_2,l_1)p(z_4|x_3,l_2) end{aligned} ]

    squared Mahalanobis distance:

    [parallel heta - mu parallel ^{2}_{Sigma} riangleq ( heta - mu)^ opSigma^{-1}( heta - mu) ]

    多维高斯分布:

    [egin{aligned} mathcal{N}( heta;mu,Sigma) &= frac {1}{sqrt{2pi|Sigma|}}operatorname{exp}(-frac{1}{2}( heta - mu)^TSigma^{-1}( heta - mu)) \ &= frac {1}{sqrt{2pi|Sigma|}}operatorname{exp}(-frac{1}{2}parallel heta - mu parallel ^{2}_{Sigma}) end{aligned} ]

    a bearing measurement from a given pose x to a given landmark l would be modeled as

    [z = h(x,l)+eta \ eta sim mathcal{N}( heta;0,Sigma) ]

    [p(z|x,l) = mathcal{N}(z;h(x,l),R) ]

    最大后验概率密度
    maximizes the posterior density p(X|Z) of the states X given the measurements Z

    [egin{aligned} X^{MAP} &= underset{x}{ ext{argmax}}: p(X|Z) \ &= underset{x}{ ext{argmax}}: frac{p(Z|X)p(X)}{p(Z)} \ &= underset{x}{ ext{argmax}}: l(X;Z)p(X) end{aligned} ]

    (l(X; Z)) is the likelihood of the states X given the measurements Z.

    [l(X;Z) propto p(Z|X) ]

    通过条件化传感器数据,贝叶斯网络可以很容易得转换为因子图。

    使用因子图的原因:

    • the distinct division between states X and measurements Z
    • the fact that we are more interested in the non-Gaussian likelihood functions, which are not proper probability densities.

    因此:

    [egin{aligned} p(X|Z) &propto p(x_1)p(x_2|x_1)p(x_3|x_2) \ & imes p(l_1)p(l_2) \ & imes l(x_1;z_1) \ & imes l(x_1,l_1;z_1)l(x_2,l_1;z_3)l(x_3,l_2:z_4) end{aligned} ]

    与贝叶斯网络不同测量值不在明确的显示在图中, 也不再将每个节点与相应的条件概率密度关联起来,而是使用因子(factors)来表示后验概率密度(p(X|Z))。上图中,每个小黑点都是一个因子,只与其所连接的变量有关,例如似然(likelihood)因子(l(x_3,l_2;z_4))只与变量节点(x_3,l_2)有关。

    每一个因子图都是一个二分图:(F=(mathcal{U}, mathcal{V}, mathcal{varepsilon})),有两种类型节点 factors (phi_i in mathcal{U})variables (x_j in mathcal{V}).edges (e_{ij} in mathcal{varepsilon})
    在factor与variable之间。The set of variable nodes (X_i) adjacent to a factor (phi(i)) is written as (mathcal{N}(phi(i))).a factor graph (mathcal{F}) defines the factorization of a global function (phi(X)) as:

    [phi(X) = prod_{i}phi_{i}(X_i) ]

    每个因子代表未归一化的后验概率(phi(X) propto P(X|Z))

    factor graph factorization:

    [egin{aligned} phi(l_1,l_2,x_1,x_2,x_3) &= phi_1(x_1)phi_2(x_2,x_1)phi_3(x_3,x_2) \ & imes phi_4(l_1)phi_5(l_2) \ & imes phi_6(x_1) \ & imes phi_7(x_1,l_1)phi_8(x_2,l_1)phi_9(x_3,l_2) end{aligned} ]

    应该注意到因子与原先概率密度的关系,比如(phi_7(x_1,l_1)=l(x_1,l_1;z_1) propto p(z_1|x_1,l_1))

    对于任意的因子图,MAP问题转换成了最大化因子乘积:

    [egin{aligned} X^{MAP} &= underset{X}{ ext{argmax}} : phi(X) \ &= underset{X}{ ext{argmax}} : prod_i phi_i(X_i) end{aligned} ]

    假设所有的因子服从零均值高斯分布(zero-mean normal distribution),则

    [phi_i(X_i) propto exp(-frac{1}{2}parallel h_i(X_i) - z_i parallel ^{2}_{Sigma_i}) ]

    取负对数,并去除系数(1/2)转为非线性最小二乘问题

    [X^{MAP} = underset{X}{ ext{argmin}} : Sigma_iparallel h_i(X_i) - z_i parallel ^{2}_{Sigma_i} ]

    线性化求解非线性问题:

    (h(X_i)=h_i(X^0_i + Delta_i) approx h_i(X^0_i) + H_iDelta_i)

    其中(H_i)为测量雅可比矩阵

    [H_i = left.egin{matrix}frac{partial h(X_i)}{partial X_i} end{matrix} ight|_{X^0_i} \ Delta_i riangleq X_i - X^0_i ]

    [parallel e parallel ^{2}_{Sigma} riangleq e^ opSigma^{-1}e = (Sigma^{-1/2}e)^ op(Sigma^{-1/2}e) = parallel Sigma^{-1/2}e parallel^2_2 ]

    上面的过程也叫作白化,误差除以测量标准差,消除了量纲,这样可以将不同类型的误差放在一起处理。
    代替最小二乘问题:

    [egin{aligned} Delta^* &= underset{Delta}{ ext{argmin}} : sum_iparallel H_iDelta_i - { z_i - h_i(X^0_i) } parallel ^{2}_{Sigma_i} \ &= underset{Delta}{ ext{argmin}} : sum_iparallel underbrace{A_i}_{Sigma_i^{-1/2}H_i}Delta_i - underbrace{b_i}_{Sigma_i^{-1/2}(z_i - h_i(X^0_i))} parallel ^{2}_{2} \ &= underset{Delta}{ ext{argmin}} : parallel ADelta - b parallel end{aligned} ]

    假设矩阵(A_{m imes n})列满秩,通常情况下(m ge n),那么最小二乘问题有唯一解:

    [A^{ op}A Delta = A^{ op} mathbf{b} ]

    此时(A^{ op}A)正定。

    cholesky法求解

    其中(A^{ op}A)是实对称正定矩阵,Cholesky factorization后信息矩阵 (Lambda riangleq A^{ op}A = R^{ op}R),其中(R)是上三角矩阵。

    [A^{ op}A Delta = A^{ op} mathbf{b} ightarrow \ R^{ op}R Delta = A^{ op} mathbf{b} ightarrow \ R^{ op} mathbf{y} = A^{ op} mathbf{b} ]

    先求解(mathbf{y})再求解(Delta)

    QR法求解

    [H_{n} ldots H_{2} H_{1} A = Q_{m imes m}^{ op} A_{m imes n} = left[egin{array}{l} R_{n imes n} \ 0_{(m -n) imes n} end{array} ight] ]

    QR Decomposition with Householder Reflections

    与Gram-Schmidt正交化操作相比,Householder reflections在QR分解中更常用。Householder reflections是另外一种正交变换(orthogonal transformation),它可以将一个向量(mathbf{x})变换到一个与之平行的向量(mathbf{y})

    [H = I - 2vv^T ]

    [|A Delta - b |_{2}^{2} = left | Q^{ op} A Delta - Q^{ op} b ight |_{2}^{2} = |R Delta - d |_{2}^{2} + | e |_{2}^{2} ]

    其中

    [Q^{ op} b = left[egin{array}{l} d \ e end{array} ight] ]

    (|e|^{2})是QR分解的残差,与(Delta)无关。

    (Lambda)被称作信息矩阵可以和另一种无向图模型(undirected graphical model)马科夫随机场联系起来。(Lambda)(G)的伴随矩阵。后续不采用MRF而是使用因子图,是因为因子图可以表达更细粒度的因子化,更贴近于原始问题方程。

    Elimination Algorithm

    边缘化marginalization
    one solution is to remove older variables without removing information

    消除算法可以将因子图转换回贝叶斯网络(有向无环图),但是只关注未知变量(X)。对于任意形式为(phi(X) = phi(x_1,x_2, dots, x_n))的因子图,变量消除算法可以将这样的因子图转换为因子化的贝叶斯网络,概率密度函数的形式为:

    [p(X) = p(x_1|S_1)p(x_2|S_2) dots p(x_n) ]

    稀疏高斯因子

    [egin{aligned} phi(l_1,l_2,x_1,x_2,x_3) &= phi_1(x_1)phi_2(x_2,x_1)phi_3(x_3,x_2) \ & imes phi_4(l_1)phi_5(l_2) \ & imes phi_6(x_1) \ & imes phi_7(x_1,l_1)phi_8(x_2,l_1)phi_9(x_3,l_2) end{aligned} ]

    [phi_i(X_i) propto operatorname{exp} left{ -frac{1}{2} left| A_iX_i - b_i ight|^2_2 ight} ]

    (X_i)是与因子(phi_i)相关的变量,例如(phi_7)(X_7 = { x_1;l_1}),分号在这里表示列方向级联。

    [phi_7(X_7) = operatorname{exp} left{ -frac{1}{2} left| A_{71}l_1 + A_{73}x_1 - b_7 ight|^2_2 ight} ]

    (A_7 = [A_{71}|A_{73}]),则

    [phi_7(X_7) = operatorname{exp} left{ -frac{1}{2} left| A_7X_7 - b_7 ight|^2_2 ight} ]

    [egin{aligned} psi(x_j,S_j) &= prod_i phi_i(X_i) \ &= operatorname{exp} left{ -frac{1}{2} sum_i left| A_{i}X_i - b_i ight|^2_2 ight} \ &= operatorname{exp} left{ -frac{1}{2} left| ar{A}_{j}[x_j;S_j] - ar{b}_j ight|^2_2 ight} end{aligned} ]

    这里以消除(l_1)(为例,与其相邻的因子有)phi_4,phi_7,phi_8$,因此

    [egin{aligned} psi(l_1,{x_1;x_2}) &= operatorname{exp} left{ -frac{1}{2} left| ar{A}_{1}[l_1;x_1;x_2] - ar{b}_1 ight|^2_2 ight} end{aligned} ]

    其中:

    [ar{A}_{1} = egin{bmatrix} A_{41} & & \ A_{71} & A_{73} & \ A_{81} & & A_{84} end{bmatrix}, ar{b}_1 = egin{bmatrix} b_4 \ b_7 \ b_8 end{bmatrix} ]

    这一步只是将多个求和的因子整合到一个大矩阵中。

    使用Partial QR消除变量

    使用partial QR-factorization将(psi(x_j,S_j))中提取的增广矩阵([ar{A}_{j}|ar{b}_{j}])变换为:

    [[ar{A}_{j}|ar{b}_{j}] = Qegin{bmatrix} R_j & T_j & d_j \ & ilde{A}_ au & ilde{b}_ au end{bmatrix} ]

    其中(Q)为单位正交阵,(R_j)为上三角矩阵。

    [psi(x_j,S_j) = operatorname{exp} left{ -frac{1}{2} left| ar{A}_{j}[x_j;S_j] - ar{b}_j ight|^2_2 ight} ]

    [egin{aligned} left| ar{A}_{j}[x_j;S_j] - ar{b}_j ight|^2_2 &= left| Q(egin{bmatrix} R_j & T_j \ & ilde{A}_ au end{bmatrix}egin{bmatrix}x_j \ S_j end{bmatrix} - egin{bmatrix}d_j \ ilde{b}_ au end{bmatrix}) ight|^2_2 \ &= left| R_jx_j + T_jS_j - d_j ight|^2_2 + left| ilde{A}_ au S_j - ilde{b}_ au ight|^2_2 end{aligned} ]

    因此

    [egin{aligned} psi(x_j,S_j) &= operatorname{exp} left{ -frac{1}{2} left| R_jx_j + T_jS_j - d_j ight|^2_2 ight}operatorname{exp} left{ -frac{1}{2} left| ilde{A}_ au S_j - ilde{b}_ au ight|^2_2 ight} \ &= p(x_j|S_j)p(S_j) end{aligned} ]

    比如消除变量(l_1),此时

    [psi(l_1,{x_1, x_2}) = operatorname{exp} left{ -frac{1}{2} left| R_1 l_1 + [T_{13}|T_{14}][x_1;x_2] - d_1 ight|^2_2 ight}operatorname{exp} left{ -frac{1}{2} left| [ ilde{A}_{13}| ilde{A}_{14}] [x_1;x_2] - ilde{b}_1 ight|^2_2 ight} ]

    [left[{egin{array}{c|c} egin{matrix} & &A_{13} & & \ & &A_{23} &A_{24} & \ & & &A_{34} &A_{35} \ A_{41} \ &A_{52} \ & &A_{63} \ A_{71} & &A_{73} \ A_{81} & & &A_{84} \ &A_{92} & & &A_{95} end{matrix} & egin{matrix} b_1 \ b_2 \ b_3 \ b_4 \ b_5 \ b_5 \ b_6 \b_7 \ b_8 \ b_9 end{matrix} end{array}} ight] ightarrow left[ {egin{array}{c|cccc|c} R_1 & &T_{13} &T_{14} & &d_1\ hline & &A_{13} & & &b_1\ & &A_{23} &A_{24} & &b_2\ & & &A_{34} &A_{35} &b_3\ &A_{52} & & & &b_5 \ & &A_{63} & & &b_6 \ &A_{92} & & &A_{95} &b_9 \ hline & & ilde{A}_{13} & ilde{A}_{14} & & ilde{b}_1 end{array}} ight] ]

    后续消除后的结果:

    a Bayes net is a directed acyclic graph (DAG), and that is exactly the “uppertriangular” property for matrices.
    贝叶斯网络是一个有向无环图,相关的矩阵具有可转为上三角矩阵的特性。

    转换后的贝叶斯网络很明显是个上三角矩阵,使用倒推法即可倒序求解问题。

    Elimination Ordering

    消除的顺序对于性能有很大的影响。假设消除所有变量的开销为:

    [fleft(Phi_{1: n} ight) = sum_{j=1}^{n} gleft(Phi_{j: n}, x_{j} ight) ]

    其中(gleft(Phi_{j: n}, x_{j} ight))为从剩下的图结构(Phi_{j: n})中消除变量(x_j)的计算开销。而每一步变量消除的开销主要在partial QR factorization上面。对于一个(m_k imes n_k)的矩阵来说,QR分解需要(4m_kn_k)次浮点运算,那么总开销可以近似为:

    [gleft(Phi_{j: n}, x_{j} ight) approx sum_{k=1}^{n_{j}} 4 m_{k} n_{k}=sum_{k=1}^{n_{j}} 4left(m_{j}-k ight)left(n_{j}+s_{j}+1-k ight) ]

    其中(n_j)为待消除变量(x_j)的维数,(s_j)(S_j)中变量的个数,(m_j)为增广矩阵([ar{A}_{j}|ar{b}_{j}])的行数。对于一个(m imes n)稠密矩阵矩阵做QR分解所需算力为:

    (fleft(Phi_{1: n} ight)=sum_{k=1}^{n-1} 4(m-k)(n+1-k)=2(m-n / 3) n^{2}+O(m n))

    这里假设路标(l)为3维,位姿为(x,y, heta)三自由度。则第一步消除(假设稠密矩阵):4(39)(35+1) = 1728, 第二步消除:4(38)(34+1) = 1248。这里的计算和书中的有出入。(fleft(Phi_{1: 5} ight)=1752+1304+1256+608+152=5072 ext { flops})
    但实际上对于稀疏矩阵来说:(fleft(Phi_{1: 5} ight)=32+20+488+488+128=1156 ext { flops })。可以看到稀疏矩阵比稠密矩阵的所需的计算少了很多。

    变量消除顺序

    变量消除顺序对于计算量的影响很大,不同的消除顺序会将因子图转为不同结构的贝叶斯网络,但是描述的是等价的MAP估计。

    COLAMD and METIS are best overall performers.The Schur complement trick is an often-used technique in computer vision.

    Updating a Matrix Factorization

    [egin{aligned} Delta^* &= underset{Delta}{ ext{argmin}} : sum_iparallel H_iDelta_i - { z_i - h_i(X^0_i) } parallel ^{2}_{Sigma_i} \ &= underset{Delta}{ ext{argmin}} : sum_iparallel underbrace{A_i}_{Sigma_i^{-1/2}H_i}Delta_i - underbrace{b_i}_{Sigma_i^{-1/2}(z_i - h_i(X^0_i))} parallel ^{2}_{2} \ &= underset{Delta}{ ext{argmin}} : parallel ADelta - b parallel end{aligned} ]

    [|A Delta-b|_{2}^{2}=left|Q^{ op} A Delta-Q^{ op} b ight|_{2}^{2}=|R Delta-d|_{2}^{2} + |e|_{2}^{2} \ Q^{ op} b = left[egin{array}{l} d \ e end{array} ight] ]

    当有新的测量值时,假设所带来的影响为(A' =left[egin{array}{l} A \ a^ op end{array} ight],b' = left[egin{array}{l} b \ eta end{array} ight])。构造新的正交矩阵(Q')

    [Q' = left[egin{array}{l} Q_{m imes m} & 0_{m imes 1} \ 0_{1 imes m} & 1_{1 imes 1} end{array} ight] ]

    此时

    [left| A' Delta - b' ight|_{2}^{2} = left| Q'^{ op} A' Delta - Q'^{ op} b' ight|_{2}^{2} \ left[egin{array}{l} Q^{ op}_{m imes m} & 0_{m imes 1} \ 0_{1 imes m} & 1_{1 imes 1} end{array} ight] left[egin{array}{l} A_{m imes n} \ a^ op_{1 imes n} end{array} ight] = left[egin{array}{l} R_{n imes n} \ 0_{(m -n) imes n} \ a^ op_{1 imes n} end{array} ight] = R_a, quad left[egin{array}{l} Q^{ op}_{m imes m} & 0_{m imes 1} \ 0_{1 imes m} & 1_{1 imes 1} end{array} ight] left[egin{array}{l} b_{m imes 1} \ eta_{1 imes 1} end{array} ight] = left[egin{array}{l} d_{n imes 1} \ e_{(m-n) imes 1} \eta_{1 imes 1} end{array} ight] ]

    实际上还有:

    [left| A' Delta - b' ight|_{2}^{2} = left| left[egin{array}{l} A \ a^ op end{array} ight]Delta - left[egin{array}{l} b \ eta end{array} ight] ight|_{2}^{2} = left| ADelta - b ight|_{2}^{2} + left| a^{ op}Delta - eta ight|_{2}^{2} ]

    我们希望(R_a)满足这样的形式:

    [R_a = left[egin{array}{l} R' \ 0 end{array} ight] ]

    使用旋转的方式来更新矩阵分解在数值上更稳定也更精确,给定的旋转为:

    [G = left[egin{array}{l} ext{cos}phi & ext{sin}phi \ - ext{sin}phi & ext{cos}phi end{array} ight] ]

    挑选参数(phi)可以使(A)中的第((i,j)^{th})个元素为0。

    上图中最左边的矩阵为(G)实际的形式,可以将R中的x位置元素变为0。通过一系列的旋转操作将新添加的行变0,这样以来:

    [G_{j_k} dots G_{j_2}G_{j_1}R_a = left[egin{array}{l} R_{n imes n}' \ 0_{(m - n + 1) imes n} end{array} ight] ]

    显然这些(G)都是正交矩阵,因此

    [egin{aligned} |A' Delta - b' |_{2}^{2} &= left| Q'^{ op} A' Delta - Q'^{ op} b' ight|_{2}^{2} \ &= left| G_{j_k} dots G_{j_2}G_{j_1}[Q'^{ op} A' Delta - Q'^{ op} b'] ight|_{2}^{2} \ &= left| left[egin{array}{l} R_{n imes n}' \ 0_{(m - n + 1) imes n} end{array} ight] Delta -left[egin{array}{l} d_{n imes 1}' \ e_{(m - n + 1) imes 1} end{array} ight] ight|_{2}^{2} \ &= |R_{n imes n}' Delta - d' |_{2}^{2} + | e_{(m - n + 1) imes 1} |_{2}^{2} end{aligned} ]

    The Bayes tree

    clique: 一个无向图中,满足两两之间有边连接的顶点的集合,被称为该无向图的团。
    chord(弦): 环中的一条连接两个非邻顶点的边;
    chordal(弦图): 任何长度超过3的环有一个chord;

    在求解非线性问题的时候,矩阵分解的方法没法很好的处理,因此我们引入新的图模型Bayes tree。在推理方面,树结构的图效率很高。构建树结构图模型的步骤:

    1. 使用变量消除将因子图转换为贝叶斯网络;
    2. 在贝叶斯网络的cliques中找出一个树结构;

    为了检测贝叶斯网络中的团结构(clique),可能需要把贝叶斯网络重写为贝叶斯树。虽然不能明显的看出贝叶斯网络中的团可以形成一个树结构,但是由于弦的属性,事实确实是这样。罗列出无向树中所有的团就是一个团树(clique tree)。

    贝叶斯树是一棵有向树,节点代表潜在的弦图贝叶斯网络中的团(C_k)。定义每个节点的条件概率密度为(p(F_k|S_k)),分隔因子(S_k)是团(C_k)与父团(prod_k)的交集。

    贝叶斯树更新

    增量推导只需要对贝叶斯树做简单的更新。相比于抽象的增量矩阵分解过程,增量更新贝叶斯树可以提供一种更好的解释和理解方式。当一个新的测量值带来一个因子,比如该测量值与有两个变量有关,将会引入一个新的二元因子(f(x_j,x_{j'})),只有包含(x_j,x_{j'})的团之间的边和root节点受影响,这些团下面的子树和其他不包含(x_j,x_{j'})的子树并不会受影响。因此,贝叶斯树更新的过程就是,将树中受影响的部分转换回因子图,然后将新测量相关的新因子添加进去。使用任何便捷的消除方法对这个临时的因子图做消除操作,形成新的贝叶斯树,未受影响的子树再添加上去。

    贝叶斯树的两个重要属性保证了只有树的顶部部分会被影响。贝叶树形成于弦图贝叶斯网络的逆向消除顺序,因此团中的变量是从他们的子团消除过程中收集到信息,这就导致任何团中信息只向其根节点方向传播。其次,只有当与因子相关的第一个变量被消除时,该因子的信息才进入消除状态。

    流型上的优化

    增量旋转

    使用轴角((ar{omega}, heta))来表示三维旋转更加符合向量值增量的表示形式,(ar{omega} in S^2)是球面上的单位向量,表示旋转操作的旋转轴,( heta)是绕该向量旋转的角度。(xi = ar{omega} heta in mathbb{R}^3),对于一个很小的( heta)

    [R(xi) approx left[ egin{array}{} 1 & -xi_{z} & xi_{y} \ xi_{z} & 1 & -xi_{x} \ -xi_{y} & xi_{x} & 1 end{array} ight] = I + hat{xi} \ hat{xi} riangleq left[egin{array}{ccc} 0 & -xi_{z} & xi_{y} \ xi_{z} & 0 & -xi_{x} \ -xi_{y} & xi_{x} & 0 end{array} ight] ]

    其中(hat{xi})(xi)的斜对称矩阵。

    指数映射

    相比于上面的近似,指数映射可以将三维增量(xi)映射为精确的旋转。

    [exp hat{xi} riangleq sum_{k=0}^{infty} frac{1}{k !} hat{xi}^{k}=I+hat{xi}+frac{hat{xi}^{2}}{2 !}+frac{hat{xi}^{3}}{3 !}+ldots ]

    实际上指数映射对于任意大的向量(xi)都有效,这就罗格里德斯公式:

    [exp hat{xi} = I + frac{sin heta}{ heta}hat{xi} + frac{1 - cos heta}{ heta ^2}hat{xi}^2 ]

    局部坐标

    指数映射可以将一个局部坐标(Local Coordinates)向量(xi in mathbb{R}^3)映射到待评估旋转值(R_0 in SO(3))的附近。

    [R_{0} oplus xi riangleq R_{0} cdot exp hat{xi} ]

    指数化局部坐标(xi)将会带来以(R_0)为基础的增量旋转。

    平面旋转

    对于平面旋转来说,(xi in mathbb{R})

    [hat{xi} riangleqleft[egin{array}{cc} 0 & -xi \ xi & 0 end{array} ight] ]

    [exp hat{xi} riangleq sum_{k=0}^{infty} frac{1}{k !} hat{xi}^{k} = left[egin{array}{cc} 1-xi^{2} / 2 ldots & -xi+xi^{3} / 6 ldots \ xi-xi^{3} / 6 ldots & 1-xi^{2} / 2 ldots end{array} ight]=left[egin{array}{cc} cos xi & -sin xi \ sin xi & cos xi end{array} ight] ]

    [egin{aligned} R_{0} oplus xi & riangleq R_{0} cdot exp hat{xi} \ &=left[egin{array}{cc} cos heta_{0} & -sin heta_{0} \ sin heta_{0} & cos heta_{0} end{array} ight]left[egin{array}{cc} cos xi & -sin xi \ sin xi & cos xi end{array} ight] \ &=left[egin{array}{cc} cos left( heta_{0}+xi ight) & -sin left( heta_{0}+xi ight) \ sin left( heta_{0}+xi ight) & cos left( heta_{0}+xi ight) end{array} ight] end{aligned} ]

    局部位置坐标

    假设在有限的时间(Delta au)内角速度为(omega),平移速度为(v),局部位置坐标(xi)为:

    [xi = left[ egin{array}{c} omega \ v end{array} ight] Delta au ]

    hat操作符定义为:

    [hat{ }:left[egin{array}{l} omega \ v end{array} ight] longmapsto left[egin{array}{ll} hat{omega} & v \ 0 & 0 end{array} ight] ]

    (SE(2))的hat操作:

    [hat{} : mathbb{R}^{3} ightarrow mathfrak{s e}(2): xi mapsto left[egin{array}{cc|c} 0 & -omega_{z} & v_{x} \ omega_{z} & 0 & v_{y} \ hline 0 & 0 & 0 end{array} ight] Delta au ]

    (SE(3))的hat操作:

    [hat{}: mathbb{R}^{6} ightarrow mathfrak{s e}(3): xi mapstoleft[egin{array}{ccc|c} 0 & -omega_{z} & omega_{y} & v_{x} \ omega_{z} & 0 & -omega_{x} & v_{y} \ -omega_{y} & omega_{x} & 0 & v_{z} \ hline 0 & 0 & 0 & 0 end{array} ight] Delta au ]

    我们可以使用指数映射将一个局部位置坐标映射到初始评估位姿(x_0)附近:

    [x_0 oplus xi = x_0 cdot exp hat{xi} ]

    矩阵李群

    hat操作符将一个向量(xi)映射到李代数(mathfrak{g})

    [hat{}:mathbb{R}^n ightarrow mathfrak{g},xi mapsto hat{xi} ]

    逆操作vee操作符:

    [{}^vee: mathfrak{g} ightarrow mathbb{R}^n, hat{xi} mapsto xi ]

    指数映射可以将局部坐标(xi)映射到任何初始评估点(a in G)的附近:

    [a oplus xi riangleq a cdot exp hat{xi} ]

    General Manifolds and Retractions

    不满足群结构的流型(mathcal{M})上也可以做上述的映射操作:

    retraction (mathcal{R}_a: mathcal{M} imes mathbb{R}^n ightarrow mathcal{M})

    [a oplus xi riangleq mathcal{R}_a(xi), a in mathcal{M} ]

    retraction定义在一般流型上,hat操作是定义在群上。

  • 相关阅读:
    Java核心API需要掌握的程度
    JAVA开发者最常去的20个英文网站
    Java .class files decompile tools
    让aptget到网络上查找源,避免经常插入光盘
    Google Job : Research Engineer Beijing
    教你亚健康该如何饮食
    tcpdump w 和 r 的使用
    黑客发布iOS 4.1永久越狱程序 狼人:
    微软将迎来迄今最大补丁日 一次修补49个漏洞 狼人:
    百度瞄准客户端 迅雷、360、腾讯皆是目标 狼人:
  • 原文地址:https://www.cnblogs.com/hardjet/p/14207560.html
Copyright © 2020-2023  润新知