Ceres(四) LocalParameterization

Ceres(四) LocalParameterization

四元数表示的是一个SO3，四元数有四维即四个自由度，但它表示的含义其实只有三个自由度，这显然是不合理的，所以产生了单位四元数这么一个东西，单位四元数指四元数的四个量的二范数是1，这其实是一个约束，约束了四元数的一个自由度，这样四元数就只剩下三个自由度了，正好符合一个SO3的维数。

这段话没有完全理解： 这里其实是一个 Manifold 变量 over parameterized的问题，即 Manifold 上变量的维度大于其自由度。这会导致 Manifold 上变量各个量之间存在约束，如果直接对这些量求导、优化，那么这就是一个有约束的优化，实现困难。为了解决这个问题，在数学上是对 Manifold 在当前变量值处形成的切空间（Tangent Space）求导，在切空间上优化，最后投影回 Manifold。

在ceres里面，如果使用的是自动求导，然后再结合爬山法，那么每步迭代中都会产生一个四维的迭代的增量delta，这样就仅仅需要将原四元数 “加上” 这个迭代产生的delta就能够得到新的四元数了。这里问题就来了，直接加上以后这个四元数就不再是一个单位四元数了，不符合二范数之和为1。如果每次迭代过后，都将这个四元数进行归一化处理，就会显然很麻烦。

同理，旋转矩阵也是如此。二者都是使用过参数化表示旋转的方式，它们是不支持广义的加法(一般使用符号 ⊞⊞ 表示)。也就是说使用普通的加法就会打破约束：旋转矩阵加旋转矩阵得到的就不再是旋转矩阵，四元数加四元数得到的不是单位四元数。所以我在使用ceres对其进行迭代更新的时候就需要自定义其更新方式了，具体的做法是实现一个参数本地化的子类，需要继承LocalParameterization，它是纯虚类，所以继承的时候要把所有的纯虚函数都实现。

Ceres存储四元数的顺序是(w, x, y, z)，Eigen::Quaternion内部内存分布为(x, y, z, w)

class CERES_EXPORT QuaternionParameterization : public LocalParameterization
{
 public:
  virtual ~QuaternionParameterization() {}

virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const;
  virtual bool ComputeJacobian(const double* x,
                               double* jacobian) const;

// 表示参数的自由度，也就是 Manifold 空间（四元数）是 4 维的
  // 即四元数本身的实际维数，如果是旋转矩阵，则为 9
  virtual int GlobalSize() const { return 4; }
  // 表示迭代增量所在的切空间(tangent space)是 3 维的。即旋转矢量的实际维数
  virtual int LocalSize() const { return 3; }
};

对两个Size，我的理解是：四元数本身的维度是 4，但其自由度只有 3，因此迭代量的维度也应该是 3

• Plus()函数

原型： bool Plus(const double* x, const double* delta, double* x_plus_delta)

四元数的更新方法，参数分别为优化前的四元数 x，用旋转矢量表示的增量delta（ 而且本来就是半轴角，所以不需要除以2 ），更新后的四元数 x_plus_delta。函数首先将增量由旋转矢量转换为四元数，转换过程就是《视觉SLAM十四讲》的公式(3.19)，然后用优化前的x左乘增量，获得x_plus_delta

bool QuaternionParameterization::Plus(const double* x,
                                      const double* delta,
                                      double* x_plus_delta) const
{
	/* -------- 将旋转矢量转换为四元数形式-------- */
  const double norm_delta =
      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0)
  {
  	// 这里的 除以norm_delta 相当于归一化
  	const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
  	double q_delta[4];
  	q_delta[0] = cos(norm_delta);
  	q_delta[1] = sin_delta_by_delta * delta[0];
  	q_delta[2] = sin_delta_by_delta * delta[1];
  	q_delta[3] = sin_delta_by_delta * delta[2];

/* --------2.采用四元数乘法更新-------- */
  	// 注意这个乘法的顺序关乎求雅克比矩阵，具体说明参照 ComputeJacobian
  	x_plus_delta = q_delta * x;
  }
  else
  {
    for (int i = 0; i < 4; ++i)
        x_plus_delta[i] = x[i];
  }
  return true;
}

• ComputeJacobian函数

原型：bool ComputeJacobian(const double* x, double* jacobian)

四元数相对于旋转矢量的雅克比矩阵，即 x_plus_delta( 也就是 Plus(x, delta) ) 关于 delta(接近0)的雅克比矩阵。计算该参数加上更新量的结果对更新量的雅可比矩阵

四元数维度为 4，迭代量维度为 3，因此雅可比维度应该是 4x3。也就是 GlobalSize×LocalSizeGlobalSize×LocalSize

如果直接在求观测误差对四元数求雅可比，得到的结果维度和该变量的维度是一致的，也就是说 雅格比同样具有冗余的自由度 ，还是开始就提出的问题，因此还需要对其迭代量进行雅可比求解。数学表示如下

(2)式为四元数上的例子，q为四元数，维度为 4，ΔqΔq为作用在四元数上的更新量，维度为 3

上面两个公式的第一部分是对原始过参数化的优化变量（四元数）的导数，这个很容易求得，直接借助ceres的AutoDiffCostFunction() 计算即可，或者自己计算雅可比矩阵，实现一个costfunction。关键是第二部分。

先补充一下四元数知识，来自十四讲

继续推导如下

// x[0] 对应 w
bool QuaternionParameterization::ComputeJacobian(const double* x,
                                                 double* jacobian) const
{
  jacobian[0] = -x[1]; jacobian[1]  = -x[2]; jacobian[2]  = -x[3];  // NOLINT
  jacobian[3] =  x[0]; jacobian[4]  =  x[3]; jacobian[5]  = -x[2];  // NOLINT
  jacobian[6] = -x[3]; jacobian[7]  =  x[0]; jacobian[8]  =  x[1];  // NOLINT
  jacobian[9] =  x[2]; jacobian[10] = -x[1]; jacobian[11] =  x[0];  // NOLINT
  return true;
}

从 Ceres version 2.2.0开始，LocalParameterization interface and associated classes are deprecated. Please use Manifold instead.

参考：LocalParameterization子类说明
Ceres 学习记录（二）

通过Problem::AddResidualBlock()函数，记录输入parameters和residuals的尺寸

AddParameterBlock添加的优化变量并不一定是ceres内部运算时采用的优化变量，例如我们通常会采用四元数+平移也就是SE3作为外部维护的状态，在ceres优化中也被成为global parameter，但通常会对 se3(local parameter)求解jacobian，那么此时我们必须要告诉ceres，求解出se3的优化增量后如何对SE3进行更新，这个就是通过指定参数化方式完成的，即传入LocalParameterization的子类对象，

参数化类中需要实现什么？

1. 定义该参数优化过程中，求解出来的Local parameter对Global parameter进行更新的方法，virtual bool Plus(const double* x, const double* delta, double* x_plus_delta) const

2. 定义Global parameter对Local parameter的jacobian, virtual bool ComputeJacobian(const double* x, double* jacobian) const，因为ceres优化时，只能设置残差关于李群的jacobian(通过实现ceres::SizedCostFunction子类完成)，但我们需要的是残差关于李代数的jacobian，因此通过这个函数传入李群与对应李代数的jacobian，从而实现转换。