Ceres-Solver学习笔记(6)

溯洄从之，道阻且长
建模最小化二乘问题
Ceres有两个组成部分，一个是建模API，它提供了一组丰富的工具，可以在一段时间内构造一个优化问题，另一个是求解程序API，控制最小化算法。这里我们只讨论建模。

class CostFunction

class CostFunction { public:  virtual bool Evaluate(double const* const* parameters,                        double* residuals,                        double** jacobians) = 0;  const vector<int32>& parameter_block_sizes();  int num_residuals() const; protected:  vector<int32>* mutable_parameter_block_sizes();  void set_num_residuals(int num_residuals);};

从这个类继承的用户代码将使用相应的访问器来设置CostFunction::parameter_block_sizes_ 和CostFunction::num_residuals_ 。

bool CostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians)

计算残差和Jacobain矩阵
parameters 参数是指向数组的指针数组，其中包含各种参数块。参数的元素数量与CostFunction::parameter_block_sizes_相同，参数块的顺序与s CostFunction::parameter_block_sizes_相同。
residuals 是num_residuals_大小的数组。
residuals 是ostFunction::parameter_block_sizes_大小的数组，包含指向每个参数块对应的Jacobain矩阵的指针。Jacobain矩阵与CostFunction::parameter_block_sizes_的顺序是相同的。jacobians[i]是一个数组包含CostFunction::num_residuals_ x CostFunction::parameter_block_sizes_ [i]元素，每个jacobians矩阵都以行优先顺序存储，例如：
jacobians[i][r * parameter_block_size_[i] + c]=∂residual[r]∂parameters[i][c]
如果jacobians 是 NULL，就不返回导数，当只计算cost时才会这样。如果jacobians[i]是空的，那么jacobians矩阵对应的第i个参数块就不能返回，这就是当一个参数块被标记为常量时的情况。
注意，返回值表示残差 和/或 jacobians 的计算是否成功。
例如 这可以用来传递 雅可比矩阵中数值计算失败。

SizedCostFunction
如果参数块的大小和惨差响亮的大小在编译时是已知的，在这些值可以指定为模板参数时可以使用SizeCostFunction，用户只需要实现 CostFunction::Evaluate().

template<int kNumResiduals,         int N0 = 0, int N1 = 0, int N2 = 0, int N3 = 0, int N4 = 0,         int N5 = 0, int N6 = 0, int N7 = 0, int N8 = 0, int N9 = 0>class SizedCostFunction : public CostFunction { public:  virtual bool Evaluate(double const* const* parameters,                        double* residuals,                        double** jacobians) const = 0;};

AutoDiffCostFunction

定义一个CostFunction 或 SizedCostFunction可能是一个繁琐且容易出错的过程，尤其是在计算导数的时候。为此，Ceres提供了 automatic differentiation.。

template <typename CostFunctor,       int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.       int N0,       // Number of parameters in block 0.       int N1 = 0,   // Number of parameters in block 1.       int N2 = 0,   // Number of parameters in block 2.       int N3 = 0,   // Number of parameters in block 3.       int N4 = 0,   // Number of parameters in block 4.       int N5 = 0,   // Number of parameters in block 5.       int N6 = 0,   // Number of parameters in block 6.       int N7 = 0,   // Number of parameters in block 7.       int N8 = 0,   // Number of parameters in block 8.       int N9 = 0>   // Number of parameters in block 9.class AutoDiffCostFunction : publicSizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> { public:  explicit AutoDiffCostFunction(CostFunctor* functor);  // Ignore the template parameter kNumResiduals and use  // num_residuals instead.  AutoDiffCostFunction(CostFunctor* functor, int num_residuals);};

要获得一个自动微分的成本函数，您必须定义一个带有模板操作符()(函数)的类，该函数以模板参数T的形式计算成本函数。

注意,在声明 operator() 输入参数x和y是第一位的,并作为T类型常量数组指针传递 .如果有三个输入参数,然后第三个输入跟在y后面。输出总是最后一个参数,和也是一个指向数组的指针。
AutoDiffCostFunction支持在运行时决定残差数量。

DynamicAutoDiffCostFunction

AutoDiffCostFunction 需要在编译时知道参数块的数量和它们的大小。它也有10个参数块的上限。在许多应用程序中，这是不够的。如贝塞尔曲线拟合，神经网络训练等。

template <typename CostFunctor, int Stride = 4>class DynamicAutoDiffCostFunction : public CostFunction {};

在这种情况下可以使用DynamicAutoDiffCostFunction ，像 AutoDiffCostFunction一样，用户必须定义模板函数，但是函数的特点稍微不同，我们希望 cost functors 的接口是：

struct MyCostFunctor {  template<typename T>  bool operator()(T const* const* parameters, T* residuals) const {  }}

由于参数的大小是在运行时确定的，所以在创建dynamic autodiff cost function之后，您还必须指定大小。例如:

DynamicAutoDiffCostFunction<MyCostFunctor, 4>* cost_function =  new DynamicAutoDiffCostFunction<MyCostFunctor, 4>(    new MyCostFunctor());cost_function->AddParameterBlock(5);cost_function->AddParameterBlock(10);cost_function->SetNumResiduals(21);

在底层，实现对成本函数进行多次评估，每次计算一小组的导数(默认情况下4个，由 Stride 模板参数控制)。传递的大小有一个性能权衡;较小的尺寸更高效，但会导致更多的传递，而更大的跨步长度可以破坏缓存的位置，同时减少成本函数的传递次数。最优值取决于各种参数块的数量和大小。
作为一个经验法则,试着用AutoDiffCostFunction 在使用DynamicAutoDiffCostFunction之前。

DynamicAutoDiffCostFunction
在不能使用模板定义的时候使用。

template <typename CostFunctor,          NumericDiffMethodType method = CENTRAL,          int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.          int N0,       // Number of parameters in block 0.          int N1 = 0,   // Number of parameters in block 1.          int N2 = 0,   // Number of parameters in block 2.          int N3 = 0,   // Number of parameters in block 3.          int N4 = 0,   // Number of parameters in block 4.          int N5 = 0,   // Number of parameters in block 5.          int N6 = 0,   // Number of parameters in block 6.          int N7 = 0,   // Number of parameters in block 7.          int N8 = 0,   // Number of parameters in block 8.          int N9 = 0>   // Number of parameters in block 9.class NumericDiffCostFunction : publicSizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> {};

NumericDiffCostFunction也支持运行时决定残差数量。
Numeric Differentiation & LocalParameterization
如果您的cost function 依赖于一个参数块，它必须位于一个manifold上，并且不能对函数的值进行求值，因为参数块的值不是在manifold上，那么您可能会有问题数值微分这些函数。
这是因为Ceres的数值微分是通过扰动一个cost functor所依赖的参数块的个别坐标来实现的。这个时候，我们假设参数块位于欧氏空间中，忽略它们所处的manifold 的结构，因此有些扰动可能不会位于与参数块对应的manifold中。
例如，考虑一个4维的参数块，它被解释为一个单元四元数。扰乱这个参数块的坐标将会违反参数块的单位范数属性。
要解决这个问题，NumericDiffCostFunction 需要计算出与每个参数块相关联的LocalParameterization ，并且只在每个参数块的局部切线空间中产生扰动。
就目前而言，这并不是一个足够严重的问题，必须改变数字扩散函数的API。而且，在大多数情况下，在函数中使用它之前，将一个点投射到多个manifold上是相对简单的。例如，在四元数的情况下，在使用它之前对4个向量进行规范化会奏效。

由于各种原因，包括与遗留代码的兼容性， NumericDiffCostFunction 也可以将 CostFunction对象作为输入。

DynamicNumericDiffCostFunction

就像 AutoDiffCostFunction 一样，NumericDiffCostFunction需要在编译时知道参数块的数量和它们的大小。它也有10个参数块的上限。在许多应用程序中，这是不够的。

template <typename CostFunctor, NumericDiffMethodType method = CENTRAL>class DynamicNumericDiffCostFunction : public CostFunction {};

就像NumericDiffCostFunction一样，用户必须定义一个functor。 cost functors的预期接口是:

struct MyCostFunctor {  bool operator()(double const* const* parameters, double* residuals) const {  }}

由于参数的大小是在运行时完成的，所以您还必须在创建dynamic numeric diff 成本函数之后指定大小。例如:

DynamicNumericDiffCostFunction<MyCostFunctor>* cost_function =  new DynamicNumericDiffCostFunction<MyCostFunctor>(new MyCostFunctor);cost_function->AddParameterBlock(5);cost_function->AddParameterBlock(10);cost_function->SetNumResiduals(21);

CostFunctionToFunctor

CostFunctionToFunctor是一个适配器类，它允许用户在模板函数中使用CostFunction对象，用于自动微分。这允许用户无缝地混合analytic, numeric and automatic differentiation。

class IntrinsicProjection : public SizedCostFunction<2, 5, 3> {  public:    IntrinsicProjection(const double* observation);    virtual bool Evaluate(double const* const* parameters,                          double* residuals,                          double** jacobians) const;};

这是一个CostFunction，它实现了局部坐标系中的点的投影到它的图像平面上，并将观察到的点减去投影。它可以计算它的残值，通过 analytic 或numerical differentiation可以计算出它的雅克比。

现在，我们想用相机外参例如旋转和平移来构成这个 CostFunction的作用。假设我们有一个模板函数

template<typename T>void RotateAndTranslatePoint(const T* rotation,                             const T* translation,                             const T* point,                             T* result);

然后如下：

struct CameraProjection {  CameraProjection(double* observation)  : intrinsic_projection_(new IntrinsicProjection(observation)) {  }  template <typename T>  bool operator()(const T* rotation,                  const T* translation,                  const T* intrinsics,                  const T* point,                  T* residual) const {    T transformed_point[3];    RotateAndTranslatePoint(rotation, translation, point, transformed_point);    // Note that we call intrinsic_projection_, just like it was    // any other templated functor.    return intrinsic_projection_(intrinsics, transformed_point, residual);  } private:  CostFunctionToFunctor<2,5,3> intrinsic_projection_;};

注意， CostFunctionToFunctor接受传递给构造函数的 CostFunction的所有权。

在上面的例子中，我们假设 IntrinsicProjection是一个能够评估值和导数的CostFunction 。假设，如果不是这样，而IntrinsicProjection的定义是这样的:

struct IntrinsicProjection  IntrinsicProjection(const double* observation) {    observation_[0] = observation[0];    observation_[1] = observation[1];  }  bool operator()(const double* calibration,                  const double* point,                  double* residuals) {    double projection[2];    ThirdPartyProjectionFunction(calibration, point, projection);    residuals[0] = observation_[0] - projection[0];    residuals[1] = observation_[1] - projection[1];    return true;  } double observation_[2];};

这里ThirdPartyProjectionFunction是一些第三方库函数,我们无法控制。这个函数可以计算它的值，我们想用数值微分来计算它的导数。在这种情况下，我们可以NumericDiffCostFunction 和 CostFunctionToFunctor的组合来完成任务。

struct CameraProjection {  CameraProjection(double* observation)    intrinsic_projection_(      new NumericDiffCostFunction<IntrinsicProjection, CENTRAL, 2, 5, 3>(        new IntrinsicProjection(observation)) {  }  template <typename T>  bool operator()(const T* rotation,                  const T* translation,                  const T* intrinsics,                  const T* point,                  T* residuals) const {    T transformed_point[3];    RotateAndTranslatePoint(rotation, translation, point, transformed_point);    return intrinsic_projection_(intrinsics, transformed_point, residual);  } private:  CostFunctionToFunctor<2,5,3> intrinsic_projection_;};

DynamicCostFunctionToFunctor

DynamicCostFunctionToFunctor提供相同的功能作为CostFunctionToFunctor情况参数向量的数量和大小和残差是在编译时不知道。DynamicCostFunctionToFunctor匹配提供的API与DynamicAutoDiffCostFunction相同,即它提供了一个模板化functor有如下的形式:

template<typename T>bool operator()(T const* const* parameters, T* residuals) const;

与 CostFunctionToFunctor的例子类似，让我们假设

class IntrinsicProjection : public CostFunction {  public:    IntrinsicProjection(const double* observation);    virtual bool Evaluate(double const* const* parameters,                          double* residuals,                          double** jacobians) const;};

是一个CostFunction ，它将其局部坐标系中的一个点投射到它的图像平面上，并将它从观察到的点减去投影。
用模板化函数使用这个 CostFunction将会是这样的:

struct CameraProjection {  CameraProjection(double* observation)      : intrinsic_projection_(new IntrinsicProjection(observation)) {  }  template <typename T>  bool operator()(T const* const* parameters,                  T* residual) const {    const T* rotation = parameters[0];    const T* translation = parameters[1];    const T* intrinsics = parameters[2];    const T* point = parameters[3];    T transformed_point[3];    RotateAndTranslatePoint(rotation, translation, point, transformed_point);    const T* projection_parameters[2];    projection_parameters[0] = intrinsics;    projection_parameters[1] = transformed_point;    return intrinsic_projection_(projection_parameters, residual);  } private:  DynamicCostFunctionToFunctor intrinsic_projection_;};

ConditionedCostFunction

这个类允许您对打包的cost function的残差值应用不同的条件。一个例子,您有一个现有的成本函数产生N个值,但你想要的总成本不是这些值的平方和,也许你想要应用不同的缩放值,改变他们对成本的贡献。

//  my_cost_function produces N residualsCostFunction* my_cost_function = ...CHECK_EQ(N, my_cost_function->num_residuals());vector<CostFunction*> conditioners;//  Make N 1x1 cost functions (1 parameter, 1 residual)CostFunction* f_1 = ...conditioners.push_back(f_1);CostFunction* f_N = ...conditioners.push_back(f_N);ConditionedCostFunction* ccf =  new ConditionedCostFunction(my_cost_function, conditioners);

现在 ccf 的 residual[i] (i=0..N-1) 将通过第i个条件.

ccf_residual[i] = f_i(my_cost_function_residual[i])

Jacobain也会被适当的改变。

GradientChecker

这个类将 cost function与使用有限微分估计的导数进行比较。它是用于单元测试的工具，它比solver选项中的check_gradients选项提供了更细粒度的控制。

强制执行的条件是：

∀i,j:Jij−J′ijmaxij(Jij−J′ij)<r

Jij是由提供的成本函数(由用户)计算的雅可比矩阵乘以局部参数化Jacobian矩阵，J′ij是由有限差分计算的jacobian矩阵乘以局部参数化雅可比矩阵，r是相对精度。

用法：

//  my_cost_function takes two parameter blocks. The first has a local//  parameterization associated with it.CostFunction* my_cost_function = ...LocalParameterization* my_parameterization = ...NumericDiffOptions numeric_diff_options;std::vector<LocalParameterization*> local_parameterizations;local_parameterizations.push_back(my_parameterization);local_parameterizations.push_back(NULL);std::vector parameter1;std::vector parameter2;// Fill parameter 1 & 2 with test data...std::vector<double*> parameter_blocks;parameter_blocks.push_back(parameter1.data());parameter_blocks.push_back(parameter2.data());GradientChecker gradient_checker(my_cost_function,    local_parameterizations, numeric_diff_options);GradientCheckResults results;if (!gradient_checker.Probe(parameter_blocks.data(), 1e-9, &results) {  LOG(ERROR) << "An error has occurred:\n" << results.error_log;}

GradientChecker

class NormalPrior: public CostFunction { public:  // Check that the number of rows in the vector b are the same as the  // number of columns in the matrix A, crash otherwise.  NormalPrior(const Matrix& A, const Vector& b);  virtual bool Evaluate(double const* const* parameters,                        double* residuals,                        double** jacobians) const; };

实现 cost function形如：

cost(x)=||A(x−b)||2

矩阵A和向量b是固定的，x是变量。如果用户对实现如下形式的成本函数感兴趣

cost(x)=(x−μ)TS−1(x−μ)

μ是一个向量，S是协方差矩阵，然后，A=S−1/2,A是协方差矩阵的平方根，也就是所谓的stiffness矩阵。然而对A的形状没有任何限制。如果协方差矩阵是秩不足的情况就会是矩形。

LossFunction

对于最小的平方问题，最小化可能会遇到包含异常值的输入项，也就是，完全虚假的测量，使用损失函数降低它们的影响是很重要的。

class LossFunction { public:  virtual void Evaluate(double s, double out[3]) const = 0;};

他的关键方法是 LossFunction::Evaluate()，它给出一个非负的标量，计算

out=[ρ(s),ρ′(s),ρ″(s)]

这里的约定是，一项对成本函数的贡献是12ρ(s)给出的，s=|fi|2。s是负值是一个错误，并且不需要实现这种情况。

最明智的选择:

ρ(0)ρ′(0)ρ′(s)ρ″(s)=0=1<1 in the outlier region<0 in the outlier region

实例
Ceres包含许多预定义的损失函数。为了简单起见，我们描述了它们的未缩放版本。下图演示了图形的形状。更多的细节可以在include/ceres/loss_function.h中找到。

class TrivialLoss

ρ(s)=s

class HuberLoss

ρ(s)={s2s√−1s≤1s>1

class SoftLOneLoss

ρ(s)=2(1+s‾‾‾‾‾√−1)

class ArctanLoss

ρ(s)=arctan(s)

class TolerantLoss

ρ(s,a,b)=blog(1+e(s−a)/b)−blog(1+e−a/b)

class ComposedLoss
给定两个损失函数f和g，实现损失函数h(s)=f(g(s))。

class ComposedLoss : public LossFunction { public:  explicit ComposedLoss(const LossFunction* f,                        Ownership ownership_f,                        const LossFunction* g,                        Ownership ownership_g);};

class LossFunctionWrapper
有时，在优化问题出现后，我们希望改变损失函数的scale。这个模板化类允许用户实现一个损失函数，在构造优化问题之后，它的scale可以发生变化，

Problem problem;// Add parameter blocksCostFunction* cost_function =    new AutoDiffCostFunction < UW_Camera_Mapper, 2, 9, 3>(        new UW_Camera_Mapper(feature_x, feature_y));LossFunctionWrapper* loss_function(new HuberLoss(1.0), TAKE_OWNERSHIP);problem.AddResidualBlock(cost_function, loss_function, parameters);Solver::Options options;Solver::Summary summary;Solve(options, &problem, &summary);loss_function->Reset(new HuberLoss(1.0), TAKE_OWNERSHIP);Solve(options, &problem, &summary);

理论
让我们考虑单个问题和单个参数块的问题。

minx12ρ(f2(x))

然后，梯度和高斯-牛顿的Hessian是

g(x)H(x)=ρ′J⊤(x)f(x)=J⊤(x)(ρ′+2ρ″f(x)f⊤(x))J(x)

其中涉及f(x)的二阶导数的项被忽略了。注意，H(x)是不确定的，如果ρ″f(x)⊤f(x)+12ρ′<0。如果不是这样的话，那么它就有可能重新加权残差和Jacobian矩阵，这样就可以将相应的线性最小二乘问题解决到Gauss-Newton的步骤中。
让 α 是下式的根

12α2−α−ρ″ρ′∥f(x)∥2=0.

定义残差和Jacobian

f̃ (x)J̃ (x)=ρ′‾‾√1−αf(x)=ρ′‾‾√(1−αf(x)f⊤(x)∥∥f(x)∥∥2)J(x)

在这个例子中，2ρ″∥∥f(x)∥∥2+ρ′≲0，我们限制了α≤1−ϵ对一个很小的ϵ。
利用这种简单的重新计算，可以利用雅可比矩阵的非线性最小二乘算法对非线性最小二乘问题进行求解。

LocalParameterization

class LocalParameterization { public:  virtual ~LocalParameterization() {}  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;};

有时，参数x可能对问题进行过参数化。在这种情况下，最好选择参数化来移除cost的零方向。更一般地说，如果x位于一个比它所嵌入的环境空间小的多的维度上，可以用每个点的切空间参数化来在数字上和计算上得到更有效的优化。
例如，三维空间中的球体是一个二维的manifold，嵌入在三维空间中。在球面上的每一点上，与它相切的平面定义了一个二维的切线空间。对于这个球面上定义的成本函数，给定一个点x，在这个点向法线方向移动是没有用的。因此，在球面上对点进行参数化的一个更好的方法是在球面点上的切线空间中对二维向量x进行优化，，然后“移动”到点x+Δx,其中移动操作涉及到投影到球面上。这样做可以消除优化中的冗余维度，使其在数字上更加鲁棒和高效。

一般来说，我们可以定义一个函数

x′=⊞(x,Δx),

x′和x具有相同的大小，Δx小于或等于x的大小，函数 ⊞概括了向量加法的定义。因此它满足了这个等式

⊞(x,0)=x,∀x.

LocalParameterization的实例实现了 ⊞操作和它相对于Δx的微分（Δx=0）.

LocalParameterization::GlobalSize()

参数块x存在的环境空间的维数。

LocalParameterization::LocalSize()

Δx 存在的切空间的维数。

LocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const

LocalParameterization::Plus() 实现了⊞(x,Δx).

LocalParameterization::ComputeJacobian(const double *x, double *jacobian) const

计算Jacobian矩阵

J=∂∂Δx⊞(x,Δx)∣∣∣Δx=0

以行优先的形式。

MultiplyByJacobian(const double *x, const int num_rows, const double *global_matrix, double *local_matrix) const

local_matrix = global_matrix * jacobian

实例

IdentityParameterizationconst

一个不重要的⊞版本是Δx和x有相同的大小

⊞(x,Δx)=x+Δx

SubsetParameterization

一个有去的情况是x是一个两维向量，用户想要保持地一个坐标不变，

⊞(x,Δx)=x+[01]Δx

SubsetParameterization 通常构造来保持任意部分参数块不变。

QuaternionParameterization

另一个例子发生在SfM问题，当相机旋转用一个四元数参数化，

⊞(x,Δx)=x+[01]Δx

SubsetParameterization 通常构造来保持任意部分参数块不变。只有使更新量正交于定义四元数的向量才会有用。一个方式是让 Δx是一个3维向量，定义⊞

⊞(x,Δx)=[cos(|Δx|),sin(|Δx|)|Δx|Δx]∗x

右边这两个4向量的乘法是标准的四元数乘积

EigenQuaternionParameterization

Eigen使用了一种相对于常用四元数不同的内部内存布局，具体地说Eigen存储的形式是 [x, y, z, w] ，实部存在最后，一般来说存在最前。注意，当通过构造函数创建一个Eigen的四元数时，元素接收顺序为w、x、y、z。因为Ceres是在参数块上运行的，它是原始的double指针，这个差别很重要，需要一个不同的参数化，EigenQuaternionParameterization 和QuaternionParameterization 使用相同的更新，但是使用Eigen内部存储元素顺序。

HomogeneousVectorParameterization

在计算机视觉中，homogeneous向量通常被用来表示投影几何中的实体，例如投影空间中的点。其中一个例子是，使用这种过参数化是指那些三角测量不符合条件的点。在这里，使用齐次向量是有利的，而不是一个欧氏向量，因为它可以表示无穷远处的点。

当使用homogeneous向量时，使更新正交于定义homogeneous的n向量是有利的——定义齐次向量哈列兹瑟曼。一种方法是让x等于n -1维向量，然后定义⊞为

⊞(x,Δx)=[sin(0.5|Δx|)|Δx|Δx,cos(0.5|Δx|)]∗x

右边的两个向量的乘法定义了一个操作，将更新正交于x来保持在球面上。注意，假设x的最后一个元素是homogeneous向量的标量分量。

ProductParameterization

考虑一个关于刚性转换SE(3)的优化问题，它是SO(3)和R3的笛卡尔积。假设您使用四元数来表示旋转，Ceres对它带有一个局部的参数化，而R3则不需要，或IdentityParameterization 参数化。那么我们如何对一个刚体变换构造一个局部参数化？

在一些情况下，一个参数块是很多manifolds的笛卡尔积，然后你有分离的manifolds的本地参数化，ProductParameterizatio可以用来对笛卡尔积构造一个本地参数化。对于刚性变换的情况，比如你有一个大小为7的参数块，当前四个项表示旋转的四元数时，可以构造局部的参数化

ProductParameterization se3_param(new QuaternionParameterization(),                                  new IdentityTransformation(3));

AutoDiffLocalParameterization

AutoDiffLocalParameterization对于LocalParameterization就像t AutoDiffCostFunction 对于 CostFunction一样，他允许用户定义一个模板functor，实现LocalParameterization::Plus() 操作并用自动微分去实现计算Jacobian。

要获得自动微分本地参数化，必须定义一个类，该类具有一个模板化操作符()用来计算

x′=⊞(x,Δx),

例如，Quaternions有一个三维的局部参数化。它的+操作可以被实现为(来自internal/ceres/autodiff_local_parameterization_test.cc)

struct QuaternionPlus {  template<typename T>  bool operator()(const T* x, const T* delta, T* x_plus_delta) const {    const T squared_norm_delta =        delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];    T q_delta[4];    if (squared_norm_delta > 0.0) {      T norm_delta = sqrt(squared_norm_delta);      const T sin_delta_by_delta = sin(norm_delta) / norm_delta;      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];    } else {      // We do not just use q_delta = [1,0,0,0] here because that is a      // constant and when used for automatic differentiation will      // lead to a zero derivative. Instead we take a first order      // approximation and evaluate it at zero.      q_delta[0] = T(1.0);      q_delta[1] = delta[0];      q_delta[2] = delta[1];      q_delta[3] = delta[2];    }    Quaternionproduct(q_delta, x, x_plus_delta);    return true;  }};

给定这个结构，现在可以构造自动微分局部参数化

Problem

Problem持有robustified边界约束的非线性最小二乘问题。使用Problem::AddResidualBlock()和Problem::AddParameterBlock() 方法来创建一个最小二乘问题。

例如，一个问题包含3个参数块，分别为3、4和5，以及2和6的两个残差块:

double x1[] = { 1.0, 2.0, 3.0 };
double x2[] = { 1.0, 2.0, 3.0, 5.0 };
double x3[] = { 1.0, 2.0, 3.0, 6.0, 7.0 };

Problem problem;
problem.AddResidualBlock(new MyUnaryCostFunction(…), x1);
problem.AddResidualBlock(new MyBinaryCostFunction(…), x2, x3);

Problem::AddResidualBlock() 就像他的名字一样，向problem添加残差块。它添加了3个CostFunction，1个可选的LossFunction ，并把CostFunction连接到一组参数块。

cost function 包含它所期望的参数块大小的信息。函数检查这些参数块与parameter_blocks中列出的参数块的大小匹配。如果检测到不匹配，程序将中止。损失函数可以是空的，在这种情况下，这一项就是残差的平方和。

用户能够显式地添加参数块使用Problem::AddParameterBlock()。这就导致了额外的正确性检查;Problem::AddResidualBlock() 隐式添加参数块，所以调用 Problem::AddParameterBlock()显式地不是必需的。

roblem::AddParameterBlock() 还允许用户将LocalParameterization对象与参数块关联起来。
重复调用相同的参数会被忽略。重复调用相同的double 指针，但是不同的大小会导致未定义的行为。

你可以用 Problem::SetParameterBlockConstant()设置任何参数块变成一个常量，不要用SetParameterBlockVariable()来做这个。

实际上，您可以将任意数量的参数块设置为常量，而Ceres非常聪明，可以解决您所构建的依赖于自由更改的参数块的问题，并且只花费时间来解决它。例如，如果你构造了一个有100万个参数块和200万个残差块的问题，然后只留下一个参数块不是常量，只有10个剩余块依赖于这个非常量参数块。那么在解决这个问题时，Ceres花费的计算精力将是和1个参数块，10个残差块的问题相同的。

所有权

Problem 默认持有 cost_function, loss_function 和 local_parameterization 指针的所有权.这些对象在Problem的生存期存活，如果用户想要保持摧毁权，可以设置相应的枚举Problem::Options结构。

注意,即使Problem拥有cost_function和loss_function的所有权,它并不排除用户在另一个残差块重用他们。析构函数会在每一个代价函数或损失函数指针上调用一次删除操作，不管有多少剩余块引用它们。

Problem::AddResidualBlock

向总成本函数添加一个残差块。成本函数包含它所期望的参数块大小的信息。函数检查这些参数块与参数块中列出的参数块的大小匹配。如果检测到不匹配，程序将中止。损失函数可以是空的，在这种情况下，这一项的代价就是残差的平方和。

Problem::AddParameterBlock

有两个重载，有一个会进行参数化。
添加一个适当的大小参数块，并对问题进行参数化。重复调用相同的参数会被忽略。重复调用相同的double指针，但是不同的大小会导致未定义的行为。

Problem::RemoveResidualBlock

从问题中删除一个残差块。残差块所依赖的任何参数都不会被删除。残差块的成本和损失函数不会立即被删除，在问题本身被删除之前不会发生。如果Problem::Options::enable_fast_removal设为true，删除会很快，基本是常量时间，否则，删除一个残差块将会对整个问题对象进行扫描，以验证残差块是否代表问题中的有效残差。

警告:移除残差或参数块将破坏隐式排序，导致解析器无法解释的雅可比矩阵或剩余值返回。如果您依赖于求值的雅可比矩阵，请不要使用remove！这可能会在未来的版本中发生变化。在优化期间保持指定的参数块不变。

Problem::RemoveParameterBlock

从问题中删除一个参数块。参数块的参数化，如果存在，将一直存在直到删除problem。任何依赖于参数的残差块也会被删除，如RemoveResidualBlock()中所述。如果Problem::Options::enable_fast_remova是true，那么删除是快速的(几乎是不变的时间)。否则，删除一个参数块将导致对整个问题对象的扫描。

Problem::SetParameterBlockConstant

在优化期间保持指定的参数块不变。

Problem::SetParameterBlockVariable

允许指定参数在优化期间变化。

Problem::SetParameterization

为其中一个参数块设置局部参数化。local_parameterization在默认情况下是由问题所拥有的。为多个参数设置相同的参数化是可以接受的;析构函数只会小心地删除 local parameterizations 1次。局部参数化只能在每个参数上设置一次，并且不能在设置后改变。

Problem::GetParameterization

获取与此参数块关联的局部参数化对象。如果没有参数化对象，则返回NULL

Problem::SetParameterLowerBound

设置values相对应的参数块中index位置参数的下界值。在默认情况下，下界是−∞。

Problem::SetParameterUpperBound

设置values相对应的参数块中index位置参数的上界值。在默认情况下，下界是∞。

Problem::NumParameterBlocks

问题中的参数块数量。总是等于parameter_blocks().size() 和parameter_block_sizes().size()。

Problem::NumParameters

通过对所有参数块的大小求和得到参数向量的大小。

Problem::NumResidualBlocks

问题中的残差块数量。总是等于residual_blocks().size()。

Problem::NumResiduals

通过对所有残差块的大小求和得到的残差向量的大小。

Problem::ParameterBlockSize

参数块大小。

Problem::ParameterBlockLocalSize

参数块的局部参数化的大小。如果没有与此参数块相关联的局部参数化，那么ParameterBlockLocalSize=ParameterBlockSize。

Problem::HasParameterBlock

problem中是否存在给定的参数快？

Problem::GetParameterBlocks

用指向当前problem中的参数块的指针填充传递的参数块向量，在这个调用之后，parameter_block.size() == NumParameterBlocks.

Problem::GetResidualBlocks

用指向当前problem中的残差块的指针填充传递的残差块向量，在这个调用之后，residual_blocks.size() == NumResidualBlocks.

Problem::GetParameterBlocksForResidualBlock

获得所有的依赖于给定残差块的参数块。

Problem::GetResidualBlocksForParameterBlock

如果Problem::Options::enable_fast_removal是true，那么获取残差块的速度很快，只依赖于残差块的数量。否则，获取参数块的剩余块将会对整个 Problem 对象进行扫描。

Problem::GetCostFunctionForResidualBlock

获得给定残差块的CostFunction。

Problem::GetLossFunctionForResidualBlock

获得给定残差块的 LossFunction 。

Problem::Evaluate

求解一个 Problem，输出指针都可以为NULL，使用哪一个残差块和参数块由 Problem::EvaluateOptions选项控制。

求值将使用在问题构造时使用的参数块指针指向的内存位置中的值，例如在以下代码中:

Problem problem;
double x = 1;
problem.Add(new MyCostFunction, NULL, &x);
double cost = 0.0;
problem.Evaluate(Problem::EvaluateOptions(), &cost, NULL, NULL, NULL);

请注意
如果不使用局部参数化，那么梯度向量的大小就是所有参数块的大小之和。如果一个参数块有一个局部的参数化，那么它就会将“LocalSize”贡献给梯度向量。

在解决问题时，这个函数不能被调用，例如，不能从IterationCallback 调用他，在solve的迭代结束时。

Problem::EvaluateOptions

用于控制 Problem::Evaluate()。

Problem::EvaluateOptions::parameter_blocks

一组参数块，用于进行求值。这个向量决定了在梯度向量和雅可比矩阵列中的参数块的顺序。如果parameter_blocks是空的，则假定它等于一个包含所有参数块的向量。一般来说，在这种情况下，参数块的顺序取决于将它们添加到问题的顺序以及用户是否删除了任何参数块。

注意这个向量应该包含与用于向问题添加参数块的指针相同的指针。这些参数块不应该指向新的内存位置。如果你做了，坏事就会发生。

Problem::EvaluateOptions::residual_blocks

用于执行计算的残差块的集合。这个向量决定了残差的顺序，以及如何排序雅可比矩阵的行。如果residual_blocks是空的，则假设它等于包含所有参数块的向量。

Problem::EvaluateOptions::apply_loss_function

尽管问题中的残差块可能包含损失函数，将apply_loss_function设置为false将会将损失函数的应用程序关闭。例如，如果用户希望分析解决方案的效果，通过研究前后的残差分布，就可以使用这种方法。

Problem::EvaluateOptions::num_threads

使用的线程数量。

rotation.h

Ceres的许多应用程序都涉及到一些优化问题，其中一些变量对应于旋转。为了减轻工作的痛苦，使用各种不同的旋转表示(角轴、四元数和矩阵)，我们提供了一组简便的模板函数。这些函数是模板化的，这样用户就可以在Ceres求解器的自动微分框架中使用它们。

void AngleAxisToQuaternion(T const *angle_axis, T *quaternion)

将轴角表示的值转换为四元数。

angle_axis是三个一组的，它的角度用弧度表示，它的方向与旋转轴是一致的，quaternion是四个一组，它将包含生成的四元数。

void QuaternionToAngleAxis(T const *quaternion, T *angle_axis)

将四元数转换成等效的轴角表示。

quaternion必须是单位四元数，angle_axis将被填充 用弧度表示的旋转 ，方向是旋转轴方向。

RotationMatrixToAngleAxis
AngleAxisToRotationMatrix
RotationMatrixToAngleAxis
AngleAxisToRotationMatrix

在3x3旋转矩阵与轴角表示之间的转换。

EulerAnglesToRotationMatrix
EulerAnglesToRotationMatrix

在3x3旋转矩阵与欧拉角表示之间的转换。
{pitch,roll,yaw} 欧拉角是分别沿着 {x,y,z} 轴.

QuaternionToScaledRotation
QuaternionToScaledRotation

将4元向量转化为3×3缩放的旋转矩阵。

他选择的旋转是这样的，对于四元数 [1 0 0 0],得到一个单位矩阵，对于小的 a,b,c ，四元数[1 a b c]得到矩阵

I+2⎡⎣⎢⎢0c−b−c0ab−a0⎤⎦⎥⎥+O(q2)

对应着Rodrigues近似，最后一个矩阵表示[a b c ]的叉乘矩阵。结合R(q1∗q2)=R(q1)∗R(q2)的特性，这一特性定义了从q到R的映射。

QuaternionToRotation

旋转矩阵用 Frobenius 归一化。RR′=I (det(R)=1)

UnitQuaternionRotatePoint

通过四元数旋转一个点，会对四元数进行归一化。

QuaternionRotatePoint

要求四元数模长不为0。

QuaternionProduct

四元数乘法

CrossProduct

四元数叉乘

AngleAxisRotatePoint

轴角旋转点

Cubic Interpolation

优化问题通常涉及以一个值表形式给出的函数，例如一个图像。计算这些函数及其导数需要对这些值进行插值，插值表函数是一个很大的研究领域，有很多库实现了各种插值方案。然而，在Ceres的自动微分框架中使用它们是相当痛苦的。为此，Ceres提供了对一维和二维表格函数进行插值的能力。

一维插值是基于 Cubic Hermite Spline，也被称为Catmull-Rom Spline。这就产生了一阶可微的内插函数。二维插值方案是一维方案的推广，插值函数被认为在两个维度是可分离的。

class CubicInterpolator

作为输入，一个无限的一维网格，它提供了下面的接口。

struct Grid1D {
enum { DATA_DIMENSION = 2; };
void GetValue(int n, double* f) const;
};

GetValue给出了一个函数f(可能是向量值)对于任何整数n的值，而枚举DATA_DIMENSION则表示插值函数的维数。例如，如果你是用轴角的方式插值相对时间的旋转，那么DATA_DIMENSION = 3.

CubicInterpolator 使用Cubic Hermite splines 来生成一个平滑的近似，可以用来计算在实数线上的任意点f(x)和f’(x)。例如，下面的代码插入一个4个数字的数组。

const double data[] = {1.0, 2.0, 5.0, 6.0};Grid1D<double, 1> array(x, 0, 4);CubicInterpolator interpolator(array);double f, dfdx;interpolator.Evaluate(1.5, &f, &dfdx);

在上面的代码中，我们使用了Grid1D一个模板化的助手类，它允许在C++数组和 CubicInterpolator之间进行简单的接口。

Grid1D支持向量的值函数，在这些函数中，函数的各种坐标可以交错或叠加。它还允许使用任何数值类型作为输入，只要可以安全地将其转换为double。

class BiCubicInterpolator

作为输入，一个无限的二维网格，它提供了下面的接口

struct Grid2D {
enum { DATA_DIMENSION = 2 };
void GetValue(int row, int col, double* f) const;
};

GetValue给出了一个函数f(可能是向量值)对于任何一对整数row和col的值，而枚举DATA_DIMENSION则表示插值函数的维数。例如，如果你插值一个3通道的图像(Red, Green & Blue)，DATA_DIMENSION = 3.

BiCubicInterpolator 使用R. Keys 的三次卷积插值算法，可以产生一个平滑的近似，在任意实数平面计算f(r,c),∂f(r,c)∂r 和∂f(r,c)∂c。

例如下面的代码插值两位数组

const double data[] = {1.0, 3.0, -1.0, 4.0,                       3.6, 2.1,  4.2, 2.0,                       2.0, 1.0,  3.1, 5.2};Grid2D<double, 1>  array(data, 0, 3, 0, 4);BiCubicInterpolator interpolator(array);double f, dfdr, dfdc;interpolator.Evaluate(1.2, 2.5, &f, &dfdr, &dfdc);

上面的代码，模板化的助手类Grid2D用于使一个C++数组对BiCubicInterpolator看起来像一个二维表。

Grid2D支持行优先或列优先的存储。它还支持向量值函数，在这个函数中，函数的各个坐标可能是交叉的或堆叠的。它还允许使用任何数值类型作为输入，只要可以安全地将其转换为double。