郑州网站建设预订,自助网站建设用什么好,快速网站建设服务,高端包装设计最大后验概率估计问题
我们常将状态估计问题建模为最大后验估计#xff08;MAP#xff09;。之所以命名为最大后验估计#xff0c;是因为它在给定了观测 Z \bm Z Z的情况下#xff0c;最大化状态 X \bm X X的后验概率密度 p ( X ∣ Z ) p(\bm X|\bm Z) p(X∣Z) X M A P …最大后验概率估计问题
我们常将状态估计问题建模为最大后验估计MAP。之所以命名为最大后验估计是因为它在给定了观测 Z \bm Z Z的情况下最大化状态 X \bm X X的后验概率密度 p ( X ∣ Z ) p(\bm X|\bm Z) p(X∣Z) X M A P arg max X p ( X ∣ Z ) arg max X p ( Z ∣ X ) p ( X ) p ( Z ) \begin{aligned} \bm X^{\rm MAP}\arg\max_{\bm X}p(\bm X|\bm Z)\\ \arg\max_{\bm X}\dfrac{p(\bm Z|\bm X)p(\bm X)}{p(\bm Z)} \end{aligned} XMAPargXmaxp(X∣Z)argXmaxp(Z)p(Z∣X)p(X) 上式利用了贝叶斯法则将后验概率密度表示为观测概率密度 p ( Z ∣ X ) p(\bm Z|\bm X) p(Z∣X)与状态的先验概率密度 p ( X ) p(\bm X) p(X)的乘积再利用因子 p ( Z ) p(\bm Z) p(Z)进行归一化。当给定观测 Z \bm Z Z时归一化项就与最大后验概率无关可以被忽略。将最大后验估计表述为另一种形式 X M A P arg max X l ( X ; Z ) p ( X ) \bm X^{\rm MAP}\arg\max_{\bm X}l(\bm X;\bm Z)p(\bm X) XMAPargXmaxl(X;Z)p(X) 这里是给定观测 Z \bm Z Z的情况下状态量 X \bm X X的似然估计被定义为任意一个与 p ( Z ∣ X ) p(\bm Z|\bm X) p(Z∣X)成正比的函数 l ( X ; Z ) ∝ p ( Z ∣ X ) l(\bm X;\bm Z)\propto p(\bm Z|\bm X) l(X;Z)∝p(Z∣X) 符号 l ( X ; Z ) l(\bm X;\bm Z) l(X;Z)也强调了似然估计是关于 X \bm X X的函数而不是关于 Z \bm Z Z的函数。并非状态 X X X中的每一部分都和所有观测相关因此根据变量块的独立性可以将 p ( X ∣ Z ) p(\bm X|\bm Z) p(X∣Z)分解为 p ( X ∣ Z ) ∏ i p ( x i ∣ z i ) p(\bm X|\bm Z)\prod_i p(\bm x_i|\bm z_i) p(X∣Z)∏ip(xi∣zi)等同于 l ( X ; Z ) ∏ i l ( x i ; z i ) l(\bm X;\bm Z)\prod_i l(\bm x_i;\bm z_i) l(X;Z)∏il(xi;zi)
设观测模型为 z i h i ( x i ) η i \bm z_i h_i(\bm x_i)\bm \eta_i zihi(xi)ηi
在多数情况下认为观测值受到均值为零的高斯噪声干扰。其中 h i ( ⋅ ) h_i(\cdot) hi(⋅)是观测函数噪声 η i \bm \eta_i ηi由协方差矩阵为 R i \bm R_i Ri的零均值高斯分布描述。对于观测量 z i \bm z_i zi条件概率 p ( z i ∣ x i ) p(\bm z_i|\bm x_i) p(zi∣xi) 如下 p ( z i ∣ x i ) N ( z i ; h ( x i ) , Σ i ) 1 ∣ 2 π Σ i ∣ exp { − 1 2 ∥ h i ( x i ) − z i ∥ Σ i 2 } p(\bm z_i|\bm x_i)\mathcal{N}(\bm z_i;h(\bm x_i),\bm \Sigma_i) \dfrac{1}{\sqrt{|2\pi \bm \Sigma_i|}}\exp\left\{-\dfrac{1}{2}\left\|h_i(\bm x_i)-\bm z_i\right\|_{\bm \Sigma_i}^2\right\} p(zi∣xi)N(zi;h(xi),Σi)∣2πΣi∣ 1exp{−21∥hi(xi)−zi∥Σi2} 其中 ∣ ⋅ ∣ |\cdot| ∣⋅∣表示求行列式 ∥ x − μ ∥ Σ 2 d e f ( x − μ ) T Σ − 1 ( x − μ ) \| \bm x -\bm \mu\|_{\bm \Sigma}^2\overset{\rm def}{}(\bm x - \bm \mu)^T\bm \Sigma^{-1}(\bm x - \bm \mu) ∥x−μ∥Σ2def(x−μ)TΣ−1(x−μ)表示马氏距离的平方。
概率问题的图表示
贝叶斯网络
贝叶斯网络是一个有向图模型图中的节点代表变量 θ j \bm \theta_j θj。我们将全部感兴趣的随机变量集合表示为 Θ { θ 1 , ⋯ , θ n } \varTheta\{\bm \theta_1,\cdots,\bm\theta_n\} Θ{θ1,⋯,θn}。一个贝叶斯网络在所有的变量 Θ \varTheta Θ上的联合概率密度函数 p ( Θ ) p(\varTheta) p(Θ)被定义为与每个节点相关联的条件概率密度的乘积 p ( Θ ) ≜ ∏ j p ( θ j ∣ π j ) p\left(\varTheta\right)\triangleq\prod_{j}p\left(\bm\theta_{j}|\bm\pi_{j}\right) p(Θ)≜j∏p(θj∣πj) 式中 p ( θ j ∣ π j ) p\left(\bm\theta_{j}|\bm\pi_{j}\right) p(θj∣πj)是节点的条件概率密度函数 π j \bm\pi_j πj是 θ j \bm\theta_j θj的父节点。在一个贝叶斯网络中联合概率密度的分解由其图结构特别是它与父节点之间的关系所决定。
以一个小型SLAM问题为例一个机器人在3个连续位姿 x 1 , x 2 , x 3 \bm x_1,\bm x_2,\bm x_3 x1,x2,x3对2个路标点 l 1 , l 2 \bm l_1,\bm l_2 l1,l2进行观测假设在第一个位姿 x 1 \bm x_1 x1处有队绝对位姿的观测量 z 1 \bm z_1 z1其余观测量为对路标的相对观测 z 2 , z 3 , z 4 \bm z_2,\bm z_3,\bm z_4 z2,z3,z4。按照通常的贝叶斯网络定义联合概率密度 p ( X , Z ) p ( x 1 , x 2 , x 3 , l 1 , l 2 , z 1 , z 2 , z 3 , z 4 ) p(\bm X,\bm Z)p(\bm x_1,\bm x_2,\bm x_3,\bm l_1,\bm l_2,\bm z_1,\bm z_2,\bm z_3,\bm z_4) p(X,Z)p(x1,x2,x3,l1,l2,z1,z2,z3,z4)可由一系列概率密度函数的乘积得到 p ( X , Z ) p ( x 1 ) p ( x 2 ∣ x 1 ) p ( x 3 ∣ x 2 ) p ( l 1 ) p ( l 2 ) × p ( z 1 ∣ x 1 ) p ( z 2 ∣ x 1 , l 1 ) p ( z 3 ∣ x 2 , l 1 ) p ( z 4 ∣ x 3 , l 2 ) \begin{aligned} p\left(\bm X,\bm Z\right) p\left(\bm x_{1}\right)p\left(\bm x_{2}|\bm x_{1}\right)p\left(\bm x_{3}|\bm x_{2}\right) p\left(\bm l_{1}\right)p\left(\bm l_{2}\right) \\ \times p\left(\bm z_{1}|\bm x_{1}\right) p\left(\bm z_{2}|x_{1},\bm l_{1}\right)p\left(\bm z_{3}|\bm x_{2},\bm l_{1}\right)p\left(\bm z_{4}|\bm x_{3},\bm l_{2}\right) \end{aligned} p(X,Z)p(x1)p(x2∣x1)p(x3∣x2)p(l1)p(l2)×p(z1∣x1)p(z2∣x1,l1)p(z3∣x2,l1)p(z4∣x3,l2)
因子图
因子图也是一种刻画概率问题的方式。贝叶斯网络是一种非常适合用于建模的语言相比而言因子图则更适合用于推断。像贝叶斯网络一样因子图允许我们将一个联合概率密度表示为一系列因子的乘积。然而因子图比贝叶斯网络更加通用因为它不仅可以指定概率密度还可以指定在变量 X \bm X X的集合上的任意一个因子函数 ϕ ( X ) \phi(\bm X ) ϕ(X)。
我们考虑在小型 SLAM 例子上进行最大后验概率推断。考虑到未知量依赖于观测量 Z \bm Z Z 可以利用贝叶斯法则将后验概率 p ( X ∣ Z ) p(\bm X|\bm Z) p(X∣Z)重写为如下形式 p ( X ∣ Z ) ∝ p ( x 1 ) p ( x 2 ∣ x 1 ) p ( x 3 ∣ x 2 ) p ( l 1 ) p ( l 2 ) × l ( x 1 ; z 1 ) l ( x 1 , l 1 ; z 2 ) l ( x 2 , l 1 ; z 3 ) l ( x 3 , l 2 ; z 4 ) \begin{aligned} p\left(\bm X|\bm Z\right) \propto p\left(\bm x_{1}\right)p\left(\bm x_{2}|\bm x_{1}\right)p\left(\bm x_{3}|\bm x_{2}\right) p\left(\bm l_{1}\right)p\left(\bm l_{2}\right) \\ \times l\left(\bm x_{1};\bm z_{1}\right)l\left(\bm x_{1},\bm l_{1};\bm z_{2}\right)l\left(\bm x_{2},\bm l_{1};\bm z_{3}\right)l\left(\bm x_{3},\bm l_{2};\bm z_{4}\right) \end{aligned} p(X∣Z)∝p(x1)p(x2∣x1)p(x3∣x2)p(l1)p(l2)×l(x1;z1)l(x1,l1;z2)l(x2,l1;z3)l(x3,l2;z4) 显然上式表示在未知变量上的未归一化的因子的概率密度。为了使这个因式分解更加明确我们使用了因子图factor graph。与贝叶斯网络一样所有的未知状态量 X \bm X X包括机器人位姿和路标点都用一个节点表示。然而与贝叶斯网络不同的是观测量不会被显式地表示出来。 在形式上因子图是有两种类型节点的二分图 F ( U , V , E ) \bm F (\mathcal{U},\mathcal{V},\mathcal{E}) F(U,V,E)因子节点factor ϕ i \bm \phi_i ϕi和变量节点variable x j ∈ V \bm x_j\in \mathcal{V} xj∈V边总是存在于因子节点和变量节点之间。与因子 ϕ i \bm \phi_i ϕi相邻的变量节点几何被写作 N ( ϕ i ) \mathcal{N}(\bm \phi_i) N(ϕi)并且我们将对这个变量集合的赋值写作 X i \bm X_i Xi。然后就可以将因子图 F \bm F F定义为对作用于全体变量的函数 ϕ ( X ) \bm \phi(\bm X) ϕ(X) ϕ ( X ) ∏ i ϕ i ( X i ) \phi(\bm X)\prod_i \phi_i(\bm X_i) ϕ(X)i∏ϕi(Xi)
每一个贝叶斯网络都可以很容易的转换为因子图。因为贝叶斯网络中的每一个节点都指示了相应的变量及其父节点的一个条件概率密度所以转换过程非常简单每一个贝叶斯网络节点都分解为相应因子图中的一个变量节点和一个因子节点。因子节点与变量节点相连接和贝叶斯网络中的父节点与对应的变量节点相连接的含义是一样的。如果贝叶斯网络中的一些节点是证据节点evidence node即它们所包含的变量是已知的。我们就可以忽略相应的变量节点已知变量变成了响应因子中的固定参数。
将一个贝叶斯网络 p ( X ∣ Z ) p(\bm X|\bm Z) p(X∣Z)转换为一个因子图通过在证据观测 Z \bm Z Z上进行条件化会生成一个后验概率的表示 ϕ ( X ) ∝ p ( X ∣ Z ) \phi (\bm X)\propto p(\bm X|\bm Z) ϕ(X)∝p(X∣Z)。
下面我们将说明对一个符合高斯噪声模型的SLAM问题进行最大后验概率估计等价于求解一个非线性最小二乘问题。实际上对于任何一个因子图最大后验概率估计等价于最大化所有因子的乘积 X M A P arg max X ϕ ( X ) arg max X ∏ i ϕ i ( X i ) (1) \begin{aligned} \bm X^{\rm MAP}\arg\max_{\bm X} \phi(\bm X) \\\arg\max_{\bm X}\prod_i \phi_i(\bm X_i) \end{aligned}\tag{1} XMAPargXmaxϕ(X)argXmaxi∏ϕi(Xi)(1) 让我们假设所有因子都具有如下形式 ϕ i ( X i ) ∝ exp { − 1 2 ∥ h i ( X i ) − z i ∥ Σ i 2 } \phi_i(\bm X_i)\propto\exp\left\{-\dfrac{1}{2}\left\|h_i(\bm X_i)-\bm z_i\right\|_{\bm \Sigma_i}^2\right\} ϕi(Xi)∝exp{−21∥hi(Xi)−zi∥Σi2} 其中既包括简单的高斯先验也包括由受到零均值高斯噪声干扰的观测量推导出来的似然因子。 对(1)式取负对数并丢掉 1 2 \frac{1}{2} 21问题就转化为非线性最小二乘的和每一个最小二乘项对应着一个概率 X M A P arg max X ∑ i ∥ h i ( X i ) − z i ∥ Σ i 2 \bm X^{\rm MAP} \arg\max_{\bm X}\sum_i \|h_i(\bm X_i)-\bm z_i\|^2_{\bm \Sigma_i} XMAPargXmaxi∑∥hi(Xi)−zi∥Σi2
线性化
使用泰勒展开公式将非线性最小二乘目标函数中的所有观测函数 h i ( ⋅ ) h_i(\cdot) hi(⋅)线性化 h j ( X i ) h i ( X i 0 Δ i ) ≈ h i ( X i 0 ) H i Δ i h_j(\bm X_i)h_i(\bm X^0_i\bm \Delta_i)\approx h_i(\bm X^0_i)\bm H_i\bm \Delta_i hj(Xi)hi(Xi0Δi)≈hi(Xi0)HiΔi
其中 Δ i ≜ X i − X i 0 \bm \Delta _i\triangleq \bm X_i - \bm X_i^0 Δi≜Xi−Xi0是状态更新向量 H i \bm H_i Hi是偏微分矩阵 H i ≜ ∂ h i ( X i ) ∂ X i ∣ X i 0 \bm H_i\triangleq \left.\dfrac{\partial h_i(\bm X_i)}{\partial \bm X_i}\right|_{\bm X_i^0} Hi≜∂Xi∂hi(Xi) Xi0
将泰勒展开式带入非线性最小二乘问题可以得到线型最小二乘问题其状态更新向量如下 Δ ∗ arg min Δ ∑ i ∥ h i ( X i 0 ) H i Δ i − z i ∥ Σ i 2 \begin{aligned} \bm\Delta^\ast \arg\min_{\bm \Delta}\sum_i\left\| h_i(\bm X_i^0)\bm H_i\bm\Delta_i-\bm z_i\right\|^2_{\bm \Sigma_i} \end{aligned} Δ∗argΔmini∑ hi(Xi0)HiΔi−zi Σi2 通过简单的变量变换可以将马氏范数转换为2-范数 ∥ e ∥ Σ 2 e T Σ − 1 e ( Σ − 1 2 e ) T ( Σ − 1 2 e ) ∥ Σ − 1 2 e ∥ 2 2 \left\|\bm e\right\|^2_{\bm \Sigma}\bm e^T\bm \Sigma^{-1}\bm e(\bm \Sigma^{-\frac{1}{2}}\bm e)^T(\bm \Sigma^{-\frac{1}{2}}\bm e)\left\|\bm \Sigma^{-\frac{1}{2}}\bm e\right\|^2_2 ∥e∥Σ2eTΣ−1e(Σ−21e)T(Σ−21e) Σ−21e 22 令 A i Σ i 1 2 H i b i Σ i 1 2 ( z i − h i ( X i 0 ) ) \begin{aligned} \bm A_i \bm\Sigma_i^{\frac{1}{2}}\bm H_i\\ b_i \bm\Sigma_i^{\frac{1}{2}}\left(\bm z_i-h_i(\bm X_i^0)\right) \end{aligned} AibiΣi21HiΣi21(zi−hi(Xi0)) 最终得到如下标准最小二乘问题 Δ ∗ arg min Δ ∑ i ∥ A i Δ i − b i ∥ 2 2 arg min Δ ∥ A Δ − b ∥ 2 2 \begin{aligned} \bm \Delta^\ast \arg\min_{\bm \Delta}\sum_i\left\|\bm A_i\bm\Delta_i-b_i\right\|^2_2 \\\arg\min_{\bm \Delta}\left\|\bm A\bm\Delta-\bm b\right\|^2_2 \end{aligned} Δ∗argΔmini∑∥AiΔi−bi∥22argΔmin∥AΔ−b∥22 因此这样就将求解最大后验概率问题转化为了矩阵求解问题概率的条件化操作对应着矩阵的QR分解。
变量消元
变量消元算法是一种将因子图转换回贝叶斯网络的方法 将未归一化的后验概率 ϕ ( X ) ∝ p ( X ∣ Z ) \phi(\bm X)\propto p(\bm X|\bm Z) ϕ(X)∝p(X∣Z)表示为一系列因子的乘积等价于概率的条件分解。变量消元算法将因子图 ϕ ( X ) ϕ ( x 1 , ⋯ , x n ) \phi(\bm X)\phi(\bm x_1,\cdots,\bm x_n) ϕ(X)ϕ(x1,⋯,xn)分解为如下形式的因子化贝叶斯网络概率密度。 p ( X ) p ( x 1 ∣ S 1 ) p ( x 2 ∣ S 2 ) ⋯ p ( x n ) ∏ j p ( x j ∣ S j ) p(\bm X)p(\bm x_1 | \bm S_1)p(\bm x_2|\bm S_2)\cdots p(\bm x_n) \prod_j p(\bm x_j|\bm S_j) p(X)p(x1∣S1)p(x2∣S2)⋯p(xn)j∏p(xj∣Sj) 选定的变量消元顺序为 x 1 , ⋯ , x n \bm x_1,\cdots,\bm x_n x1,⋯,xn与变量 x j \bm x_j xj相关的分离器 S ( x j ) \mathcal{S}(\bm x_j) S(xj)记为 S j \bm S_j Sj。分离器是这样一组变量的集合在 x j \bm x_j xj被消去后 x j \bm x_j xj 在这组变量上会被条件化。
线性最小二乘项的和可以表示为 A [ x ; S ] − b \bm A\begin{bmatrix}\bm x;\bm S\end{bmatrix}-\bm b A[x;S]−b 通过对增广矩阵 [ A ∣ b ] \begin{bmatrix}\bm A|\bm b\end{bmatrix} [A∣b]进行部分QR分解可以转化为 [ A ∣ b ] Q [ R T d A ˉ b ˉ ] \begin{bmatrix}\bm A|\bm b\end{bmatrix}\bm Q \begin{bmatrix} \bm R\bm T\bm d\\ \bar{\bm A}\bar{\bm b} \end{bmatrix} [A∣b]Q[RTAˉdbˉ] 因此 ∥ A [ x ; S ] − b ∥ 2 2 ∥ R x T S − d ∥ 2 2 ∥ A ˉ S − b ˉ ∥ 2 2 \left\|\bm A\begin{bmatrix}\bm x;\bm S\end{bmatrix}-\bm b\right\|_2^2\left\|\bm R\bm x\bm T \bm S-\bm d\right\|_2^2 \left\|\bar{\bm A}\bm S- \bar{\bm b}\right\|_2^2 A[x;S]−b 22∥RxTS−d∥22 AˉS−bˉ 22
对应高斯概率的条件化 p ( x ; S ) ∝ exp { − 1 2 ∥ A [ x ; S ] − b ∥ 2 2 } p ( x ∣ S ) p ( S ) ∝ exp { − 1 2 ∥ R x T S − d ∥ 2 2 } exp { − 1 2 ∥ A ˉ S − b ˉ ∥ 2 2 } \begin{aligned} p(\bm x;\bm S)\propto\exp\left\{-\dfrac{1}{2}\left\|\bm A\begin{bmatrix}\bm x;\bm S\end{bmatrix}-\bm b\right\|_2^2\right\} \\ p(\bm x|\bm S)p(\bm S)\propto\exp\left\{-\dfrac{1}{2}\left\|\bm R\bm x\bm T \bm S-\bm d\right\|_2^2\right\}\exp\left\{-\dfrac{1}{2}\left\|\bar{\bm A}\bm S- \bar{\bm b}\right\|_2^2\right\} \end{aligned} p(x;S)∝exp{−21 A[x;S]−b 22}p(x∣S)p(S)∝exp{−21∥RxTS−d∥22}exp{−21 AˉS−bˉ 22} 通过上述的变量消元法按照特定的变量消元顺序可以将一个因子图转化为一个贝叶斯网络并且消元得到的贝叶斯网络有一个特殊的性质他是一个弦图chordal graph即任何一个长度大于3的无向环都有一个弦。弦定义为链接环中两个非连续节点的边。通过识别这个弦图中的团一组全连接的变量贝叶斯网络可以被重写为贝叶斯树Bayes tree。
利用贝叶斯树存储和计算信息矩阵即协方差矩阵的的树结构在进行增量推断时每增加一个新的观测时。对应增加一个新的因子这种情况下贝叶斯树中只有包含因子的变量节点的团到根团之间的路径会受影响。包含因子的变量节点的团以下的子树以及任何其他不包含 因子的变量节点的子树则并未受影响。因此为了更新贝叶斯树树中受影响的部分被转换回一个因子图将与新的测量相关联的新的因子添加进去。通过对这个临时因子图使用任意一种方便的消元顺序进行重新消元可以生成一棵新的贝叶斯子树并且之前没有受影响的子树也可以重新添加回去。
之所以能够实现这样的效果是由于贝叶斯树的两个重要的性质。两个性质都来自贝叶斯树包含了消元过程的信息流第一贝叶斯树是由反向消元顺序的弦贝叶斯网络生成的。在这种方式下每个团中的变量通过对它们的子团进行消元来收集信息。因此在任何团中的信息只会向上传播直到根节点上。第二只有与某个因子连接的第一个变量被消元这个因子的信息才进入消元过程。结合这两个性质我们看到一个新的因子并不能影响任何没有与该因子相连的变量的后继变量。然而变量上的因子到根节点有不同的即独立的路径这意味着必须重新对这些路径进行消元来表达它们之间新的依赖。
因子图优化 iSAM2 利用上述方法实现了增量平滑建图可以高效的增量求解非线性优化问题。选择性地对那些距离线性化点的偏差大于阈值的变量因子进行重新线性化。相比于由增加新的因子引起的对树的修改我们现在对包含受影响变量不仅仅是前端变量还有分离器变量的所有团进行重新处理。
这会影响到树的大部分但是在绝大多数情况下这也比重新计算整棵树的代价要小得多。我们也需要回到原始的完整因子图上计算而不是将团转换成局部因子图。在消元的过程中同时需要进行一定量的缓存。在文献中有对整个增量非线性算法iSAM2详细的描述。
GTSAM
流型运算 a , b a,b a,b是李群上的元素 δ \delta δ是李代数空间的向量 a . compose ( b ) a ∗ b a . between ( b ) a . inverse ( ) ∗ b a . localaordinates ( b ) b ⊟ a log ( a − 1 b ) a . retract ( δ ) a ⊞ δ a exp ( δ ) \begin{aligned} a.\text{compose}(b) a*b\\ a.\text{between}(b)a.\text{inverse}()*b\\ a.\text{localaordinates}\left(b\right) b\boxminus a\log \left( a^{-1}b\right) \\ a.\text{retract}\left( \delta \right) a\boxplus \deltaa\exp\left( \delta \right) \end{aligned} a.compose(b)a.between(b)a.localaordinates(b)a.retract(δ)a∗ba.inverse()∗bb⊟alog(a−1b)a⊞δaexp(δ) 对于gtsam::Pose2的构造函数是平移在前旋转在后gtsam::Pose2(x, y, yaw)其噪声协方差模型也是平移在前(x,y)旋转在后(yaw)。而这与gtsam::Pose3的定义恰恰相反 对于gtsam::Pose3的构造函数是旋转在前平移在后gtsam::Pose3(gtsam::Rot3, gtsam::Point3)其噪声协方差模型也是旋转在前(roll,pitch,yaw)平移在后(x,y,z) // Add a prior on pose x0. This indirectly specifies where the origin is.// 0.1 rad std on roll, pitch, yaw, 30cm std on x,y,z.auto noise noiseModel::Diagonal::Sigmas((Vector(6) Vector3::Constant(0.1), Vector3::Constant(0.3)).finished());newgraph.push_back(PriorFactorPose3(X(0), pose_0, noise));Jacobian矩阵
GTSAM中对Jacobian矩阵 H H H的定义如下 h ( q exp ξ ^ ) ≈ h ( q ) H ξ h(q\exp\hat{\xi})\approx h(q)H\xi h(qexpξ^)≈h(q)Hξ 其中 exp ξ ^ \exp\hat\xi expξ^是变量的指数映射
