Ceres Solver 入门稍微多一点

其实ceres solver用了挺多的，可能是入门不精，有时候感觉感觉不理解代码上是怎么实现的，这次就通过ceres的官网仔细看了一些介绍，感觉对cpp了解更好了一些。

跟g2o的比较的话，感觉ceres solver是一个更通用的非线性优化器，g2o是更加针对SLAM的开发。比如g2o对一个outlier有函数借口，我了解的ceres里就只能在计算error搞一搞了。

本来以为只有ceres提供了autodiff，后来被告之g2o也有了，那感觉ceres也没这么有优势了。不过真的要落地的肯定都要自己写的，前期开发的话，大家哪个熟用哪个呗。

Ceres

Ceres solver consists of two parts:

1. a modeling API construct an optimization problem one term at a time.
2. a solver API that controls the minimization algorithm.

Cost Function

which is responsible for computing a vector of residuals and Jacobian matrices.

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 and CostFunction::num_residuals_ would be set by Problem when using Problem::AddResidualBlock().

bool CostFunction::Evaluate

parameters is an array of arrays of size x and parameters[i] is an array of size xx.

parameters is never NULL.

residuals is an array of size xxx.

residuals is never NULL.

jacobian is an array of arrays of size xxxx.

if jacobian is NULL, the user is only expected to compute the residuals.

jacobian is a row-major array. of residuam_num * parameter_block_size

If jacobian[i] is NULL, then this computation can be skipped. This is the case when the corresponding parameter block is markded constant.

SizedCostFunction

If the size of the parameter block and the size of the residual vector is known at compile time(常规场景).

The user only needs to implement 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 Evalute(double const* const* parameters,
                         double* residuals,
                         double** jacobians) const = 0;
};

AutoDiffCostFunction

template <typename CostFunctor,
        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 AutoDiffCostFunction : public SizedCostFunction<kNumResiduals, N0, N1, N2, N3, N4, N5, N6, N7, N8, N9> {
public:
    explicit AutoDiffCostFunction(CostFunctor* functor);
    AutoDiffCostFunction(CostFunctor* functor, int num_residuals);
};

To get an auto differentiated cost function, you must define a class(functor) with a templated operator() (a functor) that computes the cost function in terms of the template parameter T.

The function must write the computed value in the last argument( the only non-const one) and return true to indicate success. (咋搞的？？)

For example $ e=k - x^Ty $, the actual cost added to the total problem is e^2 while the squaring is implicitly done by the optimization framework.

class MyScalarCostFunctor {
public:
    MyScalarCostFunctor(double k) : k_(k) {}
    template <typename T>
    bool operator()(const T* const x, const T* const y, T* e) const {
        e[0] = k_ - x[0] * y[0] - x[1] * y[1];
        return true;
    }
private:
    double k_;
};

class definition is shown as below:

CostFunction* cost_function = new AutoDiffCostFunction<MyScalarCostFunctor, 1, 2, 2>(new MyScalarCostFUnctor(1.0));
// the sequence is: Dimension of residuals, of x and of y.

AutoDiffCostFunction also supports cost functions with a runtime-determined number of residuals. 这里用了第二个AutoDiffCostFunction的第二个构造函数AutoDiffCostFunction(CostFunctor*, int).

e.g.

CostFunction* cost_function
    = new AutoDiffCostFunction<MyScalarCostFunctor, DYNAMIC, 2, 2>(
        new CostFunctiorWithDynamicNumResiduals(1.0),
        runtime_number_of_residuals);

The framework can currently accommodate cost functions of up to 10 independent variables, and there is no limit on the dimensionality of each of them.

DynamicAutoDiffCostFunction

It requires that the number of parameter blocks and their sizes be known at compile time. It also has an upper limit of 10 parameter blocks.

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

The functor signature is a bit different.

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

    }
}

这玩意反正不太好用。

NumericDiffCostFunction

有时候不能定义一个cost functor, 用这玩意。

template <typename CostFunctor, NumericDiffMethodType method = CENTRAL, int kNumResiduals, int N0, int N1 = 0> // 当然后还有8个
class NumericDIffCostFunction : public SizedCostFunction<kNumResiduals, N0, N1> {}

在ceres中有三种numeric differentiation的方式，FORWARD， CENTRAL， RIDDERS.

如果你的parameter block是在一个流行上的，numerical differentiation可能会有问题，因为这个方式只是扰动parameter block中单个coordinate，这也就表示我们认为parameter block在欧式空间里而忽视了流行的structure。

e.g. Pertubing the coordiantes of a unit quaternion will vilate the unit norm property of the parameter block.

解决这个问题需要让NumericDiffCostFunction意识到LocalParameterization.

一般来说，我们推荐使用AutoDiffCostFunction而非NumericDiffCostFunction,因为在cpp模板类使得自动微分非常有效，而采用数值微分却划不来，由于其数值误差会使得收敛变慢。(反正auto就是用了很吊的扫操作牛逼就行了)

DynamicNumericDiffCostFunction

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

};

NormalPrior

class NormalPrior : public CostFunction {
public:
// 保证b中的行数和A的列数一样
NormalPrior(const Matrix& A, const Vector& b);

virtual bool Evalute(double const* const parameters, double* residuals, double** jacobians) const;
};

LossFunction

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

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;
    //...
}

class IdentityParameterization

加法定义和一般的一样 Plus(x, delta x) = x + delta x

class QuaternionParameterization

class EigenQuaternionParameterization

Eigen存储quaternion的方式不同，是[x, y, z, w].

ProductParameterization可以构建一个Cartesian product of localparameterization.

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

AutoDiffLocalParameterization

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 {
            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;
    }
};

Given this struct, the auto differentiated lcoal parameterization can now be constructed as

LocalParameterization* local_parameterization =
    new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>; // global size and local size

Problem

Use Problem::AddResidualBlock() and Problem::AddParameterBlock() methods.

Problem::AddResidualBlock()加入一个CostFunction，一个可选项的LossFunction，然后链接CostFunction和一系列的parameter block。

AddResidualBlock

ResidualBlockId Problem::AddResidualBlock(CostFunction*, LossFunction*, const vector<double*> parameter_blocks);
ResidualBlockId Problem::AddResidualBlock(CostFunction*, LossFunction*, double* x0, double* x1...);
CostFunction有parameter block的信息，这个函数会检查这些信息是否匹配。
LossFunction可以是NULL。

用户可以选择explicitly加入parameter block用``AddParameterBlock``，这会加入一些额外的检查。但是``AddResidualBlock``会implicitly加入不存在的parameter block。
Problem对象会默认拥有cost_function和loss_function的指针，他们会在problem对象存在的受活着。如果用户想自己销毁的话，可以在option里面设置。

尽管problem拥有cost_function和loss_function,它不会妨碍用户在其他residual block中重复使用。销毁器只会销毁cost_function和loss_function一次，不管多少residual block引用了他们。
？？？如果两个problem用了同一个parameter block，会有double free的问题么？

### AddParameterBlock
```cpp
void Problem::AddParameterBlock(double8 values, int size, LocalParameterization *);

加入合适的size的parameter block给problem。

重复的参数被加入会被ignore。

重复加入一个参数块但是定义不一样的size的结果是undefined。

Problem::RemoveResidualBlock

void Problem::RemoveResidualBlock(ResidualBlockId);

移除一个residual block，但是任何依赖的parameter不会被移除。

对应的cost function和loss function不会被立马删除，会在problem被销毁后。

如果Problem::Options::enable_fast_removal为真，会移除会快（constant time），不然会扫描整个problem来看是否是valid residual。

WARNING: 会改变内部的顺序，导致雅克比和参差没法解析，如果是用evaluated jacobian的话，最好别这么做。

Problem::RemoveParameterBlock

void Problem::RemoveParameterBlock(double *values)

以来这个parameter block的residual block也会被移除。

WARNING和上述一样。

Others

void Problem::SetParameterBlockConstant(double* values);
void Problem::SetParameterBlockVaribale(double* values);
void Problem::SetParameterization(double* values, LocalParameterization*);
LocalParameterization* Problem::GetParameterization(double* values);
void Problem::SetParameterLowerBound(double* value, int index, double lower_bound);
void Problem::SetParameterUpperBound(double* value, int index, double upper_bound);

rotation.h

template <typename T>
void AngleAxisToQuaternion(T const *angle_axis, T* quaternion);

template <typename T>
void QuaternionToAngleAxis(T const *quaternion, T* angle_axis);

template <typename T, int row_stride, int col_stride>
void RotationMatrixToAngleAxis(const MatrixAdapter<const T, row_stride, col_stride>&R, T* angle_axis);

template <typename T, int row_stride, int col_stride>
void AngleAxisToRotationMatrix(T const* angle_axis, const MatrixAdapter<T, row_stride, col_stride> &R)

template <typename T>
void RotationMatrixToAngleAxis(T const* R, T* angle_axis);

tempalte <typename T>
void AngleAxisToRotationMatrix(T const* angle_axis, T* R);