Ceres库相关知识点

Ceres库相关整理与总结

1. 数值微分-（Numeric Differential）

Ceres Solver 官方教程学习笔记（八）——数值微分法Numeric derivatives

利用极限的形式近似求解 Jacob的值
使用外部库函数时

2. 解析微分（Analytic Differential）

手动推导Jacob

struct NewCost : public SizedCostFunction<m, n> {

       // 重载计算函数
        virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
        }
}

3. 自动求导（Automatic Differential）

Ceres Solver 官方教程学习笔记（Ⅹ）——自动微分法接口Interfacing with Automatic Differentiation

4. 协方差估计（Covariance Estimation）

Ceres-Solver学习笔记(9)

// result cov
Eigen::Matrix<double, 6, 6, Eigen::RowMajor> cov_pose = Eigen::Matrix<double, 6, 6, Eigen::RowMajor>::Zero();


// 然后正常solve你的problem
...

// solve完成后，请打代码
ceres::Covariance::Options cov_options;
ceres::Covariance covariance(cov_options);

std::vector<std::pair<const double*, const double*> > covariance_blocks; //这个是你要算的两个玩意儿的协方差，地球人都知道协方差是要有两个变量的，当然自己对自己就是方差了
covariance_blocks.push_back(std::make_pair(pose.data(), pose.data())); //你要算pose的协方差，请把要算的pose的数据指针这样排好
//要算什么就加入什么，当然也可以无脑都搞，或许就有点慢而已
covariance.Compute(covariance_blocks, &problem);
covariance.GetCovarianceBlockInTangentSpace(pose.data(),pose.data(), cov_pose.data()); //这里得调用切空间那个接口，因为pose的协方差定义在切空间上

5. 计算残差

／／ 1
std::vector<ceres::ResidualBlockId> psrIDs;

／／ 2
auto ID = problem.AddResidualBlock(ps_function, loss_function, state_array[m]);
psrIDs.push_back(ID);

／／ 3
/* Calculate residual */
ceres::Problem::EvaluateOptions EvalOpts;
EvalOpts.num_threads = 8;
EvalOpts.apply_loss_function = false;
EvalOpts.residual_blocks = psrIDs;

std::vector<double> Residuals;
problem.Evaluate(EvalOpts, nullptr, &Residuals, nullptr, nullptr);
FILE* psr_residual  =fopen(".../src/result/residual.csv", "w+");
for(int i = 0;  i < Residuals.size(); i++)
{
    fprintf(psr_residual, "%7.9f \n", Residuals.at(i));
    fflush(psr_residual);  
}

6. ceres使用总结（转）

ceres-solver学习

7. ConditionedCostFunction

这个类的作用是给隐藏代价函数（cost function）的残差值赋上不同的权值。举个简单的例子：你有一个产生了N个残差值的代价函数，但是你想要残差的总体值不是简单的等于每个参加的平方和，你想给每一个残差一个特定的权值（尺度），来改变每个残差对总体参加的贡献。

//  my_cost_function produces N residuals 
CostFunction* 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个conditioner中传出

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

8. DynamicAutoDiffCostFunction / DynamicNumericDiffFunction

参数块的数量可以自由设定，类似传入参数块的动态数组

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

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

9. NormalPrior

／／ 求解马氏距离？ 类似于gtsam中的Priorfactor
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;
 };

10. 四元数存取顺序差异——参考

// Eigen in ceres
ceres::LocalParameterization *q_parameterization = new ceres::EigenQuaternionParameterization();  // [x, y, z, w]
double* quat = new double[4]{0, 0, 0, 1};
// default quaternion in ceres
ceres::LocalParameterization *q_parameterization = new ceres::QuaternionParameterization();  // [w, x, y, z]
double* quat = new double[4]{1, 0, 0, 0};
ceres::QuaternionProduct();
ceres::QuaternionRotatePoint();

// Eigen
Eigen::Quaternion<T> q {qw, qx, qy, qz}

11. SetParameterBlockConstant()

可以设置某个参数块为固定，不改变值

12. Mahalanobis Distance

Eigen::Matrix3<double, 3, 3> sqrt_info = Eigen::LLT<Eigen::Matrix3<double, 3,3> >(covariance.inverse()).matrixL().transpose();
residual = sqrt_info * residual;

13. LOG conflict

ceres中使用glog定义了很多宏（LOG），在同一代码中使用其他库可能会因此冲突（比如Super4PCS）
ceres_solver ISSUE

14. LocalParameterization

简单地说。$LocalParameterization$ 是在优化 Manifold 上的变量时需要考虑的，Manifold 上变量是 over parameterized，即 Manifold 上变量的维度大于其自由度。这会导致 Manifold 上变量各个量之间存在约束，如果直接对这些量求导、优化，那么这就是一个有约束的优化，实现困难。为了解决这个问题，在数学上是对 Manifold 在当前变量值处形成的切空间（Tangent Space）求导，在切空间上优化，最后投影回 Manifold。
$QuaternionParameterization$ 的方法 $boolComputeJacobian(constdouble\ast x,double\ast jacobian)$ 计算得到一个 4x3 的矩阵。这些由 ComputeJacobian 计算得到的矩阵在 ceres 代码中被称作 “global_to_local”，含义是 Manifold 上变量对 Tangent Space 上变量的导数。在 ceres::CostFunction 处提供 residuals 对 Manifold 上变量的导数$\frac{\partial e}{\partial X(G)}$，乘以这个矩阵$\frac{\partial X(G)}{\partial X(L)}$之后变成对 Tangent Space 上变量的导数$\frac{\partial e}{\partial X(L)}$
$QuaternionParameterization$ 的方法 $boolPlus(constdouble\ast x,constdouble\ast delta,double\ast x_{p}lu{s}_{d}elta)$ 将优化得到的增量（delta，在 Tangent Space）加到待优化变量（x，在 Manifold）中形成优化结果（x_plus_delta，在 Manifold），完成一次优化迭代。
当然以上这段文字描述的操作可以实现是有前提条件的，“所有用户对 Manifold 变量的导数在进入 ceres 之后会先经过$LocalParameterization::ComputeJacobian$ 转换为对 Tangent Space 变量的导数，再于其他处使用”。在当前对 ceres 的使用范围内，以上假设没有被打破。$\xi \in [\upsilon ,\omega ]$
注意：1. 在 ceres 源码中没有明确说明之处都认为矩阵 raw memory 存储方式是 Row Major 的，这与 Eigen 默认的 Col Major 是相反的。2. ceres 默认的 Quaternion raw memory 存储方式是 w, x, y, z，而 Eigen Quaternion 的存储方式是 x, y, z, w，这就导致在 ceres 代码中除 ceres::QuaternionParameterization 之外还有 ceres::EigenQuaternionParameterization 。3. ceres 中 Quaternion 是 Hamilton Quaternion，遵循 Hamilton 乘法法则。

class  NewParameterization : 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;
  virtual int GlobalSize() const { return param_num; }
  virtual int LocalSize() const { return dof; }
};

Ceres（二）LocalParameterization参数化

[ceres-solver] From QuaternionParameterization to LocalParameterization