【Ceres】（三）Covariance Estimation

上一篇：Ceres（二）LocalParameterization参数化

---

参考：Ceres Covariance Estimation
One way to assess the quality of the solution returned by a non-linear least squares solver is to analyze the covariance of the solution.

求解非线性问题
$$y=f(x)+N(0,I)$$

观测$y$是一个独立于$x$的随机非线性函数，其均值是$f(x)$，协防差是单位矩阵，为了求解$x$最大似然估计值，转换成如下最小二乘问题：
$$x^{\ast }=\arg \underset{x}{\min }\Vert f(x){\Vert }^2$$

$x^{\ast }$的协防差如下：
$$C(x^{\ast })={\left(J^{\prime }(x^{\ast })J(x^{\ast })\right)}^{-1}$$

$J(x^{\ast })$是$f$关于$x$的雅可比矩阵，上述式子是在假定$J(x^{\ast })$是列满秩时成立的，如果$J(x^{\ast })$不是列满秩的，需要求伪逆
$$C(x^{\ast })={\left(J^{\prime }(x^{\ast })J(x^{\ast })\right)}^{†}$$

如果协防差不是单位矩阵，上述求解过程变成：
$$\begin{array}{rl}y& =f(x)+N(0,S)\\ x^{\ast }& =\arg \underset{x}{\min }{f}^{\prime }(x)S^{-1}f(x)\\ C(x^{\ast })& ={\left(J^{\prime }(x^{\ast })S^{-1}J(x^{\ast })\right)}^{-1}\end{array}$$

其中$S$是positive semi-definite矩阵，在ceres中，用户需要使用$S^{-1/2}f(x)$替换$f(x)$

---

1. Rank of the Jacobian

当雅可比是rank deficient类型的时候，$J^{\prime }J$的逆不存在，下面介绍导致此种问题的原因

1.1 Structural rank deficiency

导致雅可比欠定的可能原因是一些structural-columns，这些列是0或者是常数，这种Structural rank deficiency问题经常出现在：当该problem包含常数参数块时，比如，1) 使用SetParameterBlockConstant()，2) 手动将不优化的变量对应的那一列雅可比置零，3) 手动使用常数代替某个不优化变量，达到不优化该变量（见旷世标定工具箱）

1.2 Numerical rank deficiency

Numerical rank deficiency, where the rank of the matrix cannot be predicted by its sparsity structure and requires looking at its numerical values is more complicated.

存在两种情况：
1）过参数化导致的欠定
比如SO(3)，这种问题需要用户自己实现LocalParameterization来解决
2）More general numerical rank deficiency in the Jacobian requires the computation of the so called Singular Value Decomposition (SVD) of J′J.

• We do not know how to do this for large sparse matrices efficiently.
• For small and moderate sized problems this is done using dense linear algebra.

---

2. Covariance::Options::algorithm_type

Default: SPARSE_QR

2.1 SPARSE_QR

只能用于当Jacobian正定时，速度较SVD快，
$$\begin{array}{rl}QR& =J\\ {\left(J^{\top }J\right)}^{-1}& ={\left(R^{\top }R\right)}^{-1}\end{array}$$

对$R^{\top }R$的求逆采用回代的方式以降低运算量，参考：《numerical mathematics》

ceres中的QR分解有两种实现方式：
1）Eigen’s sparse QR factorization is a moderately fast algorithm suitable for small to medium sized matrices.
2）For best performance we recommend using SuiteSparseQR which is enabled by setting Covaraince::Options::sparse_linear_algebra_library_type to SUITE_SPARSE

2.2 DENSE_SVD

DENSE_SVD uses Eigen’s JacobiSVD to perform the computations
$$US{V}^{\top }=J$$

$$(J^{\prime }J{)}^{†}=V{S}^{†}{V}^{\top }$$

It is an accurate but slow method and should only be used for small to moderate sized problems.
It can handle full-rank as well as rank deficient Jacobians.

---

3. Covariance::Options::min_reciprocal_condition_number

如果Jacobian是接近奇异的，$J^{\prime }J$的逆将很不稳定，比如：
$$\begin{array}{r}J=\left[\begin{array}{cc}1.0& 1.0\\ 1.0& 1.0000001\end{array}\right]\end{array}$$

这个矩阵几乎是欠定的，我们将得到这样的结果，
$$\begin{array}{r}(J^{\prime }J{)}^{-1}=\left[\begin{array}{cc}2.0471e+14& -2.0471e+14\\ -2.0471e+14& 2.0471e+14\end{array}\right]\end{array}$$

因此需要根据结果来判断求解是否成功，也即，$J^{\prime }J$是否是正定的，下面针对上面说到的两种算法类型采用不同策略进行判别：
1）SPARSE_QR
$$\mathrm{rank}(J)<num\_col(J)$$

$\mathrm{rank}(J)$能直接从sparse QR factorization中获取到

2）DENSE_SVD
min_reciprocal_condition_number的默认值是：$10^{-14}$
$$\frac{{\sigma }_{\text{min}}}{{\sigma }_{\text{max}}}<\sqrt{\text{min reciprocal condition number}}$$

where ${\sigma }_{min}$ and ${\sigma }_{max}$ are the minimum and maxiumum singular values of $J$ respectively.

---

4. Covariance::Options::null_space_rank

该选项仅用于当使用DENSE_SVD来求解逆时来使用，$J^{\prime }J$的特征分解是$({\lambda }_i,e_i)$， ${\lambda }_i$ is the $i^{th}$ eigenvalue and $e_i$ is the corresponding eigenvector, then the inverse of $J^{\prime }J$ is:

$$(J^{\prime }J{)}^{-1}=\sum _i\frac{1}{{\lambda }_i}{e}_i{e}_i^{\prime }$$

min_reciprocal_condition_number 和 null_space_rank控制着要丢掉上式中哪些小特征项

1. If null_space_rank is non-negative,
   最小的null_space_rank 个 eigenvalue/eigenvectors将要被dorpped掉，
   If the ratio of the smallest non-zero eigenvalue to the largest eigenvalue in the truncated matrix is still below min_reciprocal_condition_number, then the Covariance::Compute() will fail and return false.

2. 当null_space_rank = -1时，dorpped规则是这样的
   $$\frac{{\lambda }_i}{{\lambda }_{\text{max}}}<\text{min reciprocal condition number}$$

---

5. Covariance::Options::apply_loss_function

Default: true

用于控制核函数对协防差的影响

Even though the residual blocks in the problem may contain loss functions, setting apply_loss_function to false will turn off the application of the loss function to the output of the cost function and in turn its effect on the covariance.

---

6. 在ceres中计算参数块对应的协防差

6.1 bool Covariance::Compute(const vector<pair<const double *, const double *>> &covariance_blocks, Problem *problem)

其中covariance_blocks表示要计算哪些参数块的协防差，

由于协防差是对称性的，所以用户往covariance_blocks中传进去一个<block1, block2>, 那么用户可以通过GetCovarianceBlock得到(block1, block2)和(block2, block1)的协防差

covariance_blocks不能包含重复的参数块，否则将发生bad thing

在计算协防差的，ceres会使用所有的Jacobian来计算一个大H，然后根据covariance_blocks的设置选择计算哪些部分的协防差，即covariance_blocks不会影响要使用哪些Jacobian来参与计算

6.2 bool GetCovarianceBlock(const double *parameter_block1, const double *parameter_block2, double *covariance_block) const

在调用该函数之前，需要先调用上面的compute来添加参数块，然后再获取<parameter_block2, parameter_block1>对应的协防差
covariance_block 必须指向一个parameter_block1_size x parameter_block2_size大小的内存块

6.3 bool GetCovarianceBlockInTangentSpace(const double *parameter_block1, const double *parameter_block2, double *covariance_block) const

Returns cross-covariance in the tangent space if a local parameterization is associated with either parameter block; else returns cross-covariance in the ambient space.

---

@leatherwang

二零二零年十一月八日

---