一、LocalParameterization

简单来说，LocalParameterization 的作用是定义参数空间在优化过程中的“局部变化规则”。它主要解决两类核心问题：

1. 处理过参数化（Over-parameterization）：当你的参数变量数量多于其实际自由度时。

2. 施加参数约束（Constraints）：当你的参数必须满足某些特定规则（如位于流形上）时。

在最新的 Ceres 版本中（约自 2.0.0 起），LocalParameterization 接口已被标记为“弃用”，取而代之的是更数学化的 Manifold（流形）接口。但它们的核心思想是完全一致的，并且 LocalParameterization 在现有代码中仍然非常常见。理解其中一个，就能理解另一个。

1.1 为什么需要 LocalParameterization？

Ceres 等优化库的核心是基于梯度的非线性优化（如高斯-牛顿法、LM算法）。这些算法通过在当前参数点附近进行局部线性化来寻找下降方向。这个“局部”通常意味着在参数空间的欧几里得切线空间中进行操作。

对于大多数参数（比如一个点的 x, y, z 坐标），这个“局部”就是常规的欧几里得空间，直接做加减法（x + Δx）是完全有意义的。Ceres 默认也是这么处理的。

但是，有些参数不能简单地用欧几里得空间中的加减法来处理。最经典的两个例子是：

1.1.1. 旋转的表示（例如四元数）

一个单位四元数 [q0, q1, q2, q3] 有 4 个参数，但它的自由度是 3（旋转可以用滚转、俯仰、偏航 3 个角度表示）。此外，它还必须满足单位长度约束：q0² + q1² + q2² + q3² = 1。

如果你在优化过程中直接对四元数做普通的加法更新 q = q + Δq，会带来两个问题：

• 破坏约束：更新后的 q 几乎肯定不再是单位四元数。

• 过参数化：更新量 Δq 有 4 个维度，但实际需要的搜索空间只有 3 维。这会导致求解的雅可比矩阵和信息矩阵出现奇异（零空间），使得优化数值不稳定。

1.1.2. 其他流形上的参数

例如，一个位于球面上的点，其参数有 3 个坐标 (x, y, z)，但它的自由度是 2（例如可以用经纬度表示），并且需要满足 x² + y² + z² = 1。

---

1.2 LocalParameterization 如何解决这个问题？

LocalParameterization 通过两个核心函数将问题分解：

1. Plus(parameters, delta, parameters_plus_delta)

   • 作用：定义如何用一个小量的局部扰动 delta（位于切线空间）来更新参数 parameters，并保证更新后的结果 parameters_plus_delta 仍然位于原始的流形上。

   • 物理意义：parameters_plus_delta = parameters ⊕ delta

   • 对于四元数：delta 是一个 3 维向量（通常是一个轴角表示的旋转向量）。Plus 操作会将这个小的旋转向量转换为一个微小的四元数，然后通过四元数乘法（⊗）作用到当前四元数上。这样得到的新的四元数自动满足单位约束。

   • 对于普通向量：默认就是简单的加法 parameters + delta。

2. ComputeJacobian(parameters, jacobian) (可选，但通常需要)

   • 作用：计算从切线空间 delta 到参数空间 parameters 的局部雅可比矩阵。即，计算 ⊕ 操作关于 delta 的导数。

   • 为什么重要：Ceres 在求解时需要知道如何在切线空间（delta 所在的空间）中计算梯度，这个雅可比矩阵就是将梯度从参数空间映射到切线空间的关键。

通过这种方式，Ceres 的优化器始终在一个最小、无约束的切线空间（欧几里得空间）中进行运算（例如对于四元数，是在 3 维空间中进行 Δx, Δy, Δz 的优化），而在每次迭代更新参数时，再通过 Plus 操作安全地将切线空间的更新量映射回原始的、带约束的参数流形上。

常见用法示例

1. 自动微分（AutoDiff）中的使用
   在定义残差块时，你可以为其关联一个 LocalParameterization：

   Problem problem;
   ceres::LocalParameterization* quaternion_parameterization =
       new ceres::EigenQuaternionParameterization; // Ceres 内置的四元数参数化
   
   double rotation[4]; // 四元数
   double translation[3];
   
   // 添加残差项...
   problem.AddParameterBlock(rotation, 4); // 先告诉 Ceres 参数块的大小是 4
   problem.SetParameterization(rotation, quaternion_parameterization); // 再设置其参数化规则

2. Ceres 内置的 LocalParameterization：IdentityParameterization：默认的欧几里得空间参数化。EigenQuaternionParameterization：处理单位四元数。SubsetParameterization：用于固定一部分参数（将其自由度设为 0）。ProductParameterization：将多个参数化组合起来

3. 自定义 LocalParameterization：

   如果内置的不满足需求，你可以继承 LocalParameterization 类并实现自己的 Plus 和 ComputeJacobian 函数。 VINS中也是这样的做法：

   #include "pose_local_parameterization.h"
   
   class PoseLocalParameterization : public ceres::LocalParameterization
   {
       virtual bool Plus(const double *x, const double *delta, double *x_plus_delta) const;
       virtual bool ComputeJacobian(const double *x, double *jacobian) const;
       virtual int GlobalSize() const { return 7; };
       virtual int LocalSize() const { return 6; };
   };
   
   bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const{
       Eigen::Map<const Eigen::Vector3d> _p(x);
       Eigen::Map<const Eigen::Quaterniond> _q(x + 3);
       Eigen::Map<const Eigen::Vector3d> dp(delta);
       Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));
       Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
       Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);
   
       p = _p + dp;
       q = (_q * dq).normalized();
   
       return true;
   }
   
   bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const{
       Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
       j.topRows<6>().setIdentity();
       j.bottomRows<1>().setZero();
       return true;
   }

1.3 Manifold 

Manifold 是 LocalParameterization 的现代替代品，用于处理过参数化问题和流形约束。在 Ceres 中，求解器核心工作在欧几里得空间（使用 + 和 - 进行变量更新）。Manifold （以及之前的 LocalParameterization）的职责就是定义如何在这个流形空间上进行移动和更新。

它提供了两个核心操作：

1. Plus(x, delta, x_plus_delta): 在流形上，从点 x 出发，沿着切空间（Tangent Space，一个局部欧几里得空间）的方向 delta 移动，得到新的流形上的点 x_plus_delta。

2. Minus(x, y, delta): 计算两个流形上的点 x 和 y 之间在切空间中的差值 delta，即 y = Plus(x, delta)。

Manifold 相比已弃用的 LocalParameterization 的优势：

• 更清晰的抽象：Manifold 的命名和数学概念（切空间、流形）更贴合其背后的几何原理。

• 更高效的自动微分：Manifold 接口的设计与 Ceres 的自动微分框架结合得更好，有时能带来性能提升。

• 更丰富的内置实现：新版提供了更多常用的、经过优化的 Manifold 实现。

1.3.1 Manifold 的核心用法

        a) 为问题参数块设置 Manifold

当你调用 Problem.AddParameterBlock() 添加一个参数块后，你可以为其关联一个 Manifold。

// 假设有一个四元数参数块，double[4]
double rotation_quaternion[4] = {1.0, 0.0, 0.0, 0.0}; // 初始值：单位四元数

// 添加参数块
problem.AddParameterBlock(rotation_quaternion, 4);

// 创建并设置 Manifold
// 使用内置的 EigenQuaternionManifold（它假定四元数的存储顺序是 [x, y, z, w]）
ceres::Manifold* quaternion_manifold = new ceres::EigenQuaternionManifold;
problem.SetManifold(rotation_quaternion, quaternion_manifold);

// 注意：也可以使用 SetParameterization（为了兼容性），但推荐 SetManifold
// problem.SetParameterization(rotation_quaternion, new ceres::QuaternionParameterization); // 旧版方式

关键点：

• SetManifold 告诉 Ceres：rotation_quaternion 这个参数块存在于一个流形上，其更新规则由 quaternion_manifold 定义。

• 求解器在内部迭代时，会使用 Manifold::Plus 和 Manifold::Minus 来更新和比较这个参数块，确保其始终满足流形约束（如单位长度）。

b) 如果参数是欧几里得的？

对于普通的欧几里得空间参数（如 3D 点坐标 double[3]），你不需要设置任何 Manifold。Ceres 默认对其使用恒等映射（Identity Manifold），即普通的向量加法和减法。

double point_3d[3] = {1.0, 2.0, 3.0};
problem.AddParameterBlock(point_3d, 3);
// 不需要调用 SetManifold，默认为欧几里得空间

1.3.2. 常用的内置 Manifold

Ceres 提供了许多常用的 Manifold 实现，你应该优先使用它们而不是自己实现。

流形类型	类名	说明	参数块大小 / 切空间维度
欧几里得空间	EuclideanManifold	默认。用于普通的向量空间。通常不需要显式设置。	size / size
四元数 (单位长度)	EigenQuaternionManifold	推荐。用于表示旋转的单位四元数。存储顺序为 [x, y, z, w]。	4 / 3
四元数 (旧版)	QuaternionManifold	旧版的四元数流形实现。	4 / 3
特殊正交群 SO(3)	SphereManifold<3>	表示 3D 旋转矩阵，实质上和单位四元数类似。	9 / 3 *
单位球面点	SphereManifold<2>	表示一个 2D 球面上的点（如归一化的 3D 向量）。	3 / 2
子集流形	SubsetManifold	用于参数块中只有某几个维度需要被优化，其他维度保持固定。	size / (size - num_fixed)

示例：使用 SubsetManifold

假设你有一个 7 维的参数块 [tx, ty, tz, qx, qy, qz, qw]，其中前 3 个平移量 [tx, ty, tz] 需要被优化，而后 4 个四元数 [qx, qy, qz, qw] 你希望固定住。

double pose[7] = {0, 0, 0, 0, 0, 0, 1}; // [tx, ty, tz, qx, qy, qz, qw]

// 指定要固定的维度索引（从0开始）：这里是索引 3,4,5,6
std::vector<int> constant_parameters = {3, 4, 5, 6};
ceres::Manifold* subset_manifold = new ceres::SubsetManifold(7, constant_parameters);

problem.AddParameterBlock(pose, 7);
problem.SetManifold(pose, subset_manifold);

二、AddParameterBlock

AddParameterBlock 的主要目的是向优化问题（Problem）注册一个参数块，并告知 Ceres 关于这个参数块的一些信息。虽然 Ceres 通常能在你添加残差块时自动推断出参数块的存在，但显式调用 AddParameterBlock 让你能对其进行更精细的控制。

2.1 函数签名与重载

在 ceres::Problem 类中，AddParameterBlock 主要有以下两种重载形式：

2.1.1 基本形式（指定大小）

​​​​​​​void Problem::AddParameterBlock(double* values, int size);

• values: 指向参数块数据的指针。这个数组的长度必须至少为 size。

• size: 参数块的维度（例如，3D点的 size=3，四元数的 size=4）。

2.1.2 高级形式（指定大小和Manifold）

void Problem::AddParameterBlock(double* values,
                               int size,
                               Manifold* manifold);

• 前两个参数同上。

• manifold: 一个指向 Manifold 对象的指针。它定义了该参数块所在的流形空间（例如，用于四元数的 EigenQuaternionManifold）。Ceres 将接管该 manifold 对象的所有权，你无需手动删除它。 如果参数块存在于欧几里得空间（如普通的3D点），可以传入 nullptr，这是默认行为。

在旧版 Ceres 中，第三个参数是 LocalParameterization*，但在 2.0.0 及以上版本中，它已被 Manifold 取代。

2.2. 何时需要显式调用 AddParameterBlock？

在以下三种主要场景下，你需要显式调用这个函数：

2.2.1. 设置参数的流形（Manifold）

这是最常见的原因。如果你想为一个参数块设置 Manifold（如处理旋转的四元数），你必须先通过 AddParameterBlock 添加该参数块，然后使用 SetManifold() 方法，或者更简单的方式是直接在 AddParameterBlock 调用中传入 Manifold。

示例：为四元数设置 Manifold

#include <ceres/ceres.h>
#include <ceres/manifold.h>

// ... 假设有一些残差函数 ...

double rotation_quaternion[4] = {1.0, 0.0, 0.0, 0.0}; // [x, y, z, w]
double translation[3] = {0.0, 0.0, 0.0};

ceres::Problem problem;

// 正确做法：显式添加参数块并设置 Manifold
problem.AddParameterBlock(rotation_quaternion, 4);
// 方法一：分开设置
ceres::Manifold* quat_manifold = new ceres::EigenQuaternionManifold;
problem.SetManifold(rotation_quaternion, quat_manifold);

// 方法二：在AddParameterBlock时直接设置 (更简洁)
// problem.AddParameterBlock(rotation_quaternion, 4, new ceres::EigenQuaternionManifold);

// 对于欧几里得空间的平移向量，不需要Manifold
problem.AddParameterBlock(translation, 3); // manifold 默认为 nullptr

// ... 添加残差块 ...
// problem.AddResidualBlock(...);

2.2.2. 固定参数（设置参数块恒定）

如果你想在优化过程中固定某个参数块不变（不优化它），你需要先显式添加它，然后调用 SetParameterBlockConstant()。

示例：固定相机内参

double camera_intrinsics[5] = {focal_length, cx, cy, k1, k2}; // 径向畸变模型

// 1. 显式添加参数块
problem.AddParameterBlock(camera_intrinsics, 5);

// 2. 将其设置为恒定（不优化）
problem.SetParameterBlockConstant(camera_intrinsics);

// 现在，即使添加了使用 camera_intrinsics 的残差块，它们也不会被优化。

与之对应的 SetParameterBlockVariable() 可以重新启用优化。

2.2.3. 覆盖参数块的大小

在某些复杂情况下，同一个指针地址可能被用于不同大小的参数块。显式调用 AddParameterBlock 可以确保 Ceres 对你意图使用的参数块大小有清晰的认知，避免歧义。

2.3 何时可以省略 AddParameterBlock？

当你添加残差块（AddResidualBlock）时，Ceres 会自动遍历该残差函数依赖的所有参数块。如果这些参数块之前没有被添加到问题中，Ceres 会自动为你隐式地执行 AddParameterBlock(parameter, size)，其中 size 由残差函数的参数类型决定。

这意味着，对于简单的、存在于欧几里得空间且不需要被固定的参数，你完全可以省略 AddParameterBlock，直接 AddResidualBlock 即可。

示例：省略 AddParameterBlock

double point_3d[3] = {1.0, 2.0, 3.0};
// 假设 my_cost_function 依赖于 point_3d

// 这是完全有效的，Ceres 会自动添加 point_3d 参数块，大小为3。
problem.AddResidualBlock(my_cost_function, nullptr, point_3d);

推荐做法：
为了代码的清晰性和可维护性，即使对于不需要 Manifold 的简单参数，也建议显式调用 AddParameterBlock。这清晰地表明了你的意图：“我将要优化这个数组”，使得代码的读者一目了然。