Ceres优化库使用

自动求导 ：

struct CostFunctor {
   template <typename T>
   bool operator()(const T* const x, T* residual) const {    //拟函数
     residual[0] = T(10.0) - x[0];
     return true;
   }
};

int main(int argc, char** argv) {
  google::InitGoogleLogging(argv[0]);

  // The variable to solve for with its initial value.
  double initial_x = 5.0;
  double x = initial_x;

  // Build the problem.
  Problem problem;    //构建最小二乘问题

  // Set up the only cost function (also known as residual). This uses
  // auto-differentiation to obtain the derivative (jacobian).
  CostFunction* cost_function =
      new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);    //指定模板参数：误差类型、误差维度、优化变量维度
  problem.AddResidualBlock(cost_function, NULL, &x);    //向问题中添加误差项（核函数、待估计参数）

  // Run the solver!
  Solver::Options options;    //优化选项
#ifdef OPENMVG_USE_OPENMP
  // options.num_threads = omp_get_max_threads();
  // options.num_linear_solver_threads = omp_get_max_threads();
#endif // OPENMVG_USE_OPENMP
  // options.logging_type = ceres::SILENT;
  // options.max_num_iterations = max_iterations;
  // options.parameter_tolerance = parameter_tolerance;
  options.linear_solver_type = ceres::DENSE_QR;    //增量方程如何求解，SPARSE_NORMAL_CHOLESKY, DENSE_SCHUR
  options.minimizer_progress_to_stdout = true;    //输出到cout
  options.gradient_tolerance = 1e-16;    //梯度的阈值
  options.function_tolerance = 1e-16;    //相邻两次迭代之间目标函数之差的阈值



  Solver::Summary summary;    //优化信息
  Solve(options, &problem, &summary);    //开始优化

  std::cout << summary.BriefReport() << "\n";
  // std::cout << summary.FullReport() << "\n";

  if (summary.IsSolutionUsable()) {
    std::cout << "x : " << initial_x
              << " -> " << x << "\n";
  }
  return summary.IsSolutionUsable();
}

  // Since the problem is sparse, use a sparse solver if available
  if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::SUITE_SPARSE))
  {
    options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  }
  else if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::CX_SPARSE))
  {
    options.sparse_linear_algebra_library_type = ceres::CX_SPARSE;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  }
  else if (ceres::IsSparseLinearAlgebraLibraryTypeAvailable(ceres::EIGEN_SPARSE))
  {
    options.sparse_linear_algebra_library_type = ceres::EIGEN_SPARSE;
    options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
  }
  else
  {
    options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;
  }

 数值求导：

struct NumericDiffCostFunctor {
  bool operator()(const double* const x, double* residual) const {
    residual[0] = 10.0 - x[0];
    return true;
  }
};

CostFunction* cost_function =
  new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(
      new NumericDiffCostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);
// 中心差分ceres::CENTRAL, 前向差分ceres::FORWARD, RIDDERS方法ceres::RIDDERS
// 通常情况下有先尝试使用中心差分法，然后再根据中心差分法的求算结果，选择前向差分法提高速度，或者使用Ridders法提高精度。

解析的导数：

class QuadraticCostFunction : public ceres::SizedCostFunction<1, 1> {
 public:
  virtual ~QuadraticCostFunction() {}
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const {
    const double x = parameters[0][0];
    residuals[0] = 10 - x;

    // Compute the Jacobian if asked for.
    if (jacobians != NULL && jacobians[0] != NULL) {
      jacobians[0][0] = -1;
    }
    return true;
  }
};

官方建议： 优先选用自动微分算法，某些情况可能需要用到解析微分算法，尽量避免数值微分算法。 

---

鲁棒核函数（loss function）

https://github.com/ceres-solver/ceres-solver/blob/master/include/ceres/loss_function.h

https://github.com/ceres-solver/ceres-solver/blob/master/internal/ceres/loss_function.cc

ceres::LossFunction* loss_function = new ceres::CauchyLoss(1.0);

ceres::LossFunction* loss_function = new ceres::HuberLoss(1.0);

ceres::LossFunction* loss_function = new ceres::SoftLOneLoss(1.0);

ceres::LossFunction* loss_function = new ceres::ArctanLoss(1.0);

 

---

 曲线拟合

struct ExponentialResidual {
  ExponentialResidual(double x, double y)
      : x_(x), y_(y) {}
  //结构体的构造函数。把x赋值给x_，把y赋值给y_。也就是说在建立一个对象的时候x_和y_是赋完值的。
  template <typename T>
  bool operator()(const T* const m, const T* const c, T* residual) const {
  //计算残差，观测值-理论值。
    residual[0] = T(y_) - exp(m[0] * T(x_) + c[0]);
    return true;
  }
 private:
  // 一个样本数据的观测值。
  const double x_;
  const double y_;
};

double m = 0.0;
double c = 0.0; //初始值

Problem problem;
for (int i = 0; i < kNumObservations; ++i) {
  CostFunction* cost_function =
       new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>
           new ExponentialResidual(data[2 * i], data[2 * i + 1]));
  problem.AddResidualBlock(cost_function, NULL, &m, &c);
  // problem.AddResidualBlock(cost_function, new CauchyLoss(0.5) , &m, &c); // 应用鲁棒核函数来对异常数据进行过滤
}

---

 处理无法直接加减的流形空间(Manifold)的ceres结构

参考：Modeling Non-linear Least Squares — Ceres Solver

我们知道优化问题最重要的就是要计算雅克比矩阵和残差，因此如何执行加减法运算对于优化求解至关重要。由于四元数并不是通过简单的加减乘除就可以进行运算，而是有一套自己的计算方法，因此这种情况必须手动定义加运算，在ceres中要增加ceres::LocalParameterization来定义运算。

1、Ceres预定义的LocalParameterization

使用ceres自带的计算四元数的方法ceres::EigenQuaternionParameterization()

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;
  // local_matrix = global_matrix * jacobian
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;
  virtual int GlobalSize() const = 0; // 参数块 x 所在的环境空间的维度。
  virtual int LocalSize() const = 0; // Δ 所在的切线空间的维度
};

class CERES_EXPORT EigenQuaternionParameterization
    : public ceres::LocalParameterization {
 public:
  virtual ~EigenQuaternionParameterization() {}
  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 4; }
  virtual int LocalSize() const { return 3; }
};

bool EigenQuaternionParameterization::Plus(const double* x_ptr,
                                           const double* delta,
                                           double* x_plus_delta_ptr) const {
  Eigen::Map<Eigen::Quaterniond> x_plus_delta(x_plus_delta_ptr);
  Eigen::Map<const Eigen::Quaterniond> x(x_ptr);
  const double norm_delta =
      sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
  if (norm_delta > 0.0) {
    const double sin_delta_by_delta = sin(norm_delta) / norm_delta;
    // Note, in the constructor w is first.
    Eigen::Quaterniond delta_q(cos(norm_delta),
                               sin_delta_by_delta * delta[0],
                               sin_delta_by_delta * delta[1],
                               sin_delta_by_delta * delta[2]);
    x_plus_delta = delta_q * x;
  } else {
    x_plus_delta = x;
  }
  return true;
}
bool EigenQuaternionParameterization::ComputeJacobian(const double* x,
                                                      double* jacobian) const {
  jacobian[0] =  x[3]; jacobian[1]  =  x[2]; jacobian[2]  = -x[1];  // NOLINT
  jacobian[3] = -x[2]; jacobian[4]  =  x[3]; jacobian[5]  =  x[0];  // NOLINT
  jacobian[6] =  x[1]; jacobian[7]  = -x[0]; jacobian[8]  =  x[3];  // NOLINT
  jacobian[9] = -x[0]; jacobian[10] = -x[1]; jacobian[11] = -x[2];  // NOLINT
  return true;
}

ceres::LocalParameterization* q_parameterization = new ceres::EigenQuaternionParameterization();
problem.AddParameterBlock(q.coeffs().data(), 4, q_parameterization);
problem.AddParameterBlock(t.data(), 3);

assert(problem.NumParameterBlocks() == 2);
assert(problem.NumParameters() == 7);

2、自定义LocalParameterization

参考：https://zhuanlan.zhihu.com/p/545458473

class PoseSO3Parameterization : public ceres::LocalParameterization {
public:       
    virtual ~PoseSO3Parameterization() {}

    virtual bool Plus(const double* x,
                    const double* delta,
                    double* x_plus_delta) const override { 
            const Eigen::Map<const Eigen::Quaterniond> q(x);
            const Eigen::Map<const Eigen::Vector3d> delta_v(delta);

            Eigen::Quaterniond delta_q = Sophus::SO3d::exp(delta_so3).unit_quaternion();
            Eigen::Map<Eigen::Quaterniond>  q_plus_delta_q(x_plus_delta);

            q_plus_delta_q= (delta_q * q).normalized();
            return true;
    }

    virtual bool ComputeJacobian(const double* x, double* jacobian) const override {
            Eigen::Map<Eigen::Matrix<double, 4, 3, Eigen::RowMajor>> j(jacobian);
            (j.topRows(3)).setIdentity();
            (j.bottomRows(1)).setZero();
            return true;
    }

    virtual int GlobalSize() const {
      return  4;
    }
    virtual int LocalSize() const {
      return  3;
    }
};

---

限制优化变量的上下界

double x[3] = {1,2,3}；
problem.SetParameterLowerBound(x, 1, 0);
problem.SetParameterUpperBound(x, 1, 5);

 在优化过程中保持指定的参数快不变

problem.SetParameterBlockConstant(t.data());

---

优化pose

#pragma once

#include "Eigen/Core"

struct MyCost {
  MyCost() = delete;
  MyCost(const Eigen::Vector3d& observe_point)
      : observe_point_(observe_point) {}
  ~MyCost() {}

  template <typename T>
  bool operator()(const T* const q_ptr,
                  const T* const t_ptr,
                  T* residual) const {
    const Eigen::Map<const Eigen::Quaternion<T>> q(q_ptr);
    const Eigen::Map<const Eigen::Matrix<T, 3, 1>> t(t_ptr);

    Eigen::Matrix<T, 3, 1> p0_ =
        q * observe_point_.template cast<T>() + t;

    residual[0] = diff_p0(0, 0); // exp, abs, eigen(dot, cross, slerp)
    residual[1] = diff_p0(1, 0);
    residual[2] = diff_p0(2, 0);

    return true;
  }

  static ceres::CostFunction* Create(const Eigen::Vector3d& observe_point) {
    return (new ceres::AutoDiffCostFunction<MyCost, 3, 3>(
        new MyCost(observe_point)));
  }

 private:
  Eigen::Vector3d observe_point_;
};

参考：

https://github.com/ceres-solver/ceres-solver

Ceres Solver — A Large Scale Non-linear Optimization Library

Ceres Solver 官方教程学习笔记

ceres-solver中的自动求导 - 知乎