其實ceres solver用了挺多的，可能是入門不精，有時候感覺感覺不理解代碼上是怎么實現的，這次就通過ceres的官網仔細看了一些介紹，感覺對cpp了解更好了一些。
跟g2o的比較的話，感覺ceres solver是一個更通用的非線性優化器，g2o是更加針對SLAM的開發。比如g2o對一個outlier有函數借口，我了解的ceres里就只能在計算error搞一搞了。
本來以為只有ceres提供了autodiff，后來被告之g2o也有了，那感覺ceres也沒這么有優勢了。不過真的要落地的肯定都要自己寫的，前期開發的話，大家哪個熟用哪個唄。
Ceres
Ceres solver consists of two parts:
- a modeling API construct an optimization problem one term at a time.
- 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);