On-Manifold Optimization: Local Parameterization

Overview

Manifold Space vs Tangent Space

Jacobian w.r.t Error State

Jacobian w.r.t Error State vs True State

According ^[1] 2.4,

The idea is that for a $x \in N$ the function $g(\delta) := f (x \boxplus \delta)$ behaves locally in $0$ like $f$ does in $x$. In particular $|f(x)|^2$ has a minimum in $x$ if and only if $|g(\delta)|^2$ has a minimum in $0$. Therefore finding a local optimum of $g$, $\delta = \arg \min{\delta} |g(\delta)|^2$ implies $x \boxplus \delta = \arg \min{\xi} |f(\xi)|^2$.

$$f(x \boxplus \delta)=f(x)+J_x \delta+\mathcal{O}\left(\|\delta\|^2\right)$$

where

$$J = \left. \frac{\partial f(x \boxplus \delta)}{\partial \delta} \right|_{\delta=0} \quad \longleftrightarrow \quad J = \left. \frac{\partial f(x)}{\partial x} \right|_{x}$$

ESKF ^[2] 6.1.1: Jacobian computation

$$\left.\mathbf{H} \triangleq \frac{\partial h}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\left.\left.\frac{\partial h}{\partial \mathbf{x}_t}\right|_{\mathbf{x}} \frac{\partial \mathbf{x}_t}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\mathbf{H}_{\mathbf{x}} \mathbf{X}_{\delta \mathbf{x}}$$

• $x_t$: true state
• $x$: normal state
• $\delta x$: error state

lifting and retraction:

$$\left.\mathbf{X}_{\delta \mathbf{x}} \triangleq \frac{\partial \mathbf{x}_t}{\partial \delta \mathbf{x}}\right|_{\mathbf{x}}=\left[\begin{array}{ccc} \mathbf{I}_6 & 0 & 0 \\ 0 & \mathbf{Q}_{\delta \boldsymbol{\theta}} & 0 \\ 0 & 0 & \mathbf{I}_9 \end{array}\right]$$

the quaternion term

$$\begin{aligned} \left.\mathbf{Q}_{\delta \boldsymbol{\theta}} \triangleq \frac{\partial(\mathbf{q} \otimes \delta \mathbf{q})}{\partial \delta \boldsymbol{\theta}}\right|_{\mathbf{q}} &=\left.\left.\frac{\partial(\mathbf{q} \otimes \delta \mathbf{q})}{\partial \delta \mathbf{q}}\right|_{\mathbf{q}} \frac{\partial \delta \mathbf{q}}{\partial \delta \boldsymbol{\theta}}\right|_{\delta \hat{\boldsymbol{\theta}}=0} \\ &=\left.\left.\frac{\partial\left([\mathbf{q}]_L \delta \mathbf{q}\right)}{\partial \delta \mathbf{q}}\right|_{\mathbf{q}} \frac{\partial\left[\begin{array}{c} 1 \\ \frac{1}{2} \delta \boldsymbol{\theta} \end{array}\right]}{\partial \delta \boldsymbol{\theta}}\right|_{\hat{\delta}=0} \\ &=[\mathbf{q}]_L \frac{1}{2}\left[\begin{array}{lll} 0 & 0 & 0 \\ 1 & 0 & 0 \\ 0 & 1 & 0 \\ 0 & 0 & 1 \end{array}\right] \end{aligned}$$

Least Squares on a Manifold ^[3]

Local Parameterization in Ceres Solver ^{[4] [5] [6] [7] [8]}

class LocalParameterization {
 public:
  virtual ~LocalParameterization() = default;
  virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const = 0;
  virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0;
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;
  virtual int GlobalSize() const = 0;
  virtual int LocalSize() const = 0;
};

Plus

Retraction

$$\boxplus(x, \Delta)=x \operatorname{Exp}(\Delta)$$

ComputeJacobian

global w.r.t local

$$J_{GL} = \frac{\partial x_G}{\partial x_L} = D_2 \boxplus(x, 0) = \left. \frac{\partial \boxplus(x, \Delta)}{\partial \Delta} \right|_{\Delta = 0}$$

参考 ^[9]

$r$ w.r.t $x_{L}$

在 ceres::CostFunction 处提供 residuals 对 Manifold 上变量的导数

$$J_{rG} = \frac{\partial r}{\partial x_G}$$

则 对 Tangent Space 上变量的导数

$$J_{rL} = \frac{\partial r}{\partial x_L} = \frac{\partial r}{\partial x_G} \cdot J_{GL}$$

Sub Class

• QuaternionParameterization
• EigenQuaternionParameterization

自定义 QuaternionParameterization

参考 ^[7]

Summary

• QuaternionParameterization 的 Plus 与 ComputeJacobian 共同决定使用左扰动或使用右扰动形式

Quaternion in Eigen

Quaterniond q1(1, 2, 3, 4);           // wxyz
Quaterniond q2(Vector4d(1, 2, 3, 4)); // xyzw
Quaterniond q3(tmp_q);                // xyzw, double tmp_q[4];
q.coeffs();                           // xyzw

Quaternion in Ceres Solver

• order: wxyz
• Ceres Solver 中 Quaternion 是 Hamilton Quaternion，遵循 Hamilton 乘法法则
• 矩阵 raw memory 存储方式是 Row Major

Reference

1. A Framework for Sparse, Non-Linear Least Squares Problems on Manifolds ↩
2. Quaternion kinematics for the error-state Kalman filter, Joan Solà ↩
3. A Tutorial on Graph-Based SLAM ↩
4. http://ceres-solver.org/nnls_modeling.html#localparameterization ↩
5. On-Manifold Optimization Demo using Ceres Solver ↩
6. Matrix Manifold Local Parameterizations for Ceres Solver ↩
7. [ceres-solver] From QuaternionParameterization to LocalParameterization 😄 ↩
8. LocalParameterization子类说明：QuaternionParameterization类和EigenQuaternionParameterization类 ↩
9. 优化库——ceres（二）深入探索ceres::Problem ↩