ceres::Problem求解最小二乘总结

最新推荐文章于 2024-08-18 13:27:56 发布

leeayu

最新推荐文章于 2024-08-18 13:27:56 发布

阅读量649

点赞数 11

分类专栏： ceres tutorial 文章标签： ceres 最小二乘问题 Manifold 解析求导自动求导

本文链接：https://blog.csdn.net/qq_23225073/article/details/140985975

版权

ceres tutorial 专栏收录该内容

7 篇文章 3 订阅

订阅专栏

文章目录

1 ceres::Problem组合拳
2 关于ceres中的QuaternionManifold

1 ceres::Problem组合拳

1.1 定义`ceres::Problem problem;`

专门解决最小二乘问题
$\begin{array}{ll} \min _{\mathbf{x}} & \frac{1}{2} \sum_i \rho_i\left(\left\|f_i\left(x_{i_1}, \ldots, x_{i_k}\right)\right\|^2\right) \\ \text { s.t. } & l_j \leq x_j \leq u_j \end{array}$

1.2 定义CostFunction

【重要】解析求导写法1：残差、参数块dim确定，使用SizedCostFunction


template <int kNumResiduals, int... Ns>
class SizedCostFunction : public CostFunction {
 public:
  SizedCostFunction() {
    set_num_residuals(kNumResiduals);
    *mutable_parameter_block_sizes() = std::vector<int32_t>{Ns...};
  }

  // Subclasses must implement Evaluate().
};
  // 在继承的类中实现Evaluate函数，填充残差和jabocian
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const = 0;

注意jacobian是残差对状态的导数，不一定是观测方程对状态的导数。
如果残差定义为 $r = h (x) - z$ ，那么jacobian正好对应观测方程对状态的导数，否则少了一个负号。

解析求导写法2：残差、参数块dim不确定，使用DynamicCostFunction

class CERES_EXPORT DynamicCostFunction : public CostFunction {
 public:
  virtual void AddParameterBlock(int size) {
    mutable_parameter_block_sizes()->push_back(size);
  }

  virtual void SetNumResiduals(int num_residuals) {
    set_num_residuals(num_residuals);
  }
};


  // 在继承的类中实现Evaluate函数，填充残差和jabocian
  virtual bool Evaluate(double const* const* parameters,
                        double* residuals,
                        double** jacobians) const = 0;

具体实现和使用方法可以参考 DynamicAutoDiffCostFunction

//   DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function(
//       new MyCostFunctor());
//   cost_function.AddParameterBlock(5);
//   cost_function.AddParameterBlock(10);
//   cost_function.SetNumResiduals(21);

解析求导写法3: NormalPrior

参看 https://blog.csdn.net/qq_23225073/article/details/140917949

【重要】自动求导写法1: 残差、参数块dim确定，AutoDiffCostFunction

template <typename CostFunctor,
          int kNumResiduals,  // Number of residuals, or ceres::DYNAMIC.
          int... Ns>          // Number of parameters in each parameter block.
class AutoDiffCostFunction final
    : public SizedCostFunction<kNumResiduals, Ns...> {
 public:
  // Takes ownership of functor by default. Uses the template-provided
  // value for the number of residuals ("kNumResiduals").
  explicit AutoDiffCostFunction(CostFunctor* functor,
                                Ownership ownership = TAKE_OWNERSHIP)
      : functor_(functor), ownership_(ownership) {
    static_assert(kNumResiduals != DYNAMIC,
                  "Can't run the fixed-size constructor if the number of "
                  "residuals is set to ceres::DYNAMIC.");
  }

functor写法

struct AutoDiffCostFunctor{
  
  // parameters的dim和数量由AutoDiffCostFunction的template实例化参数决定
  template <typename T>
  bool operator()(const T* parameters1, const T* parameters2, T* residual) const {}

  ceres::CostFunction* auto_diff_cost_function = new ceres::AutoDiffCostFunction<AutoDiffBiCubicCost, 1, 2， 2>(
  		new AutoDiffBiCubicCost(interpolator, point, value));
};

自动求导写法2: 残差、参数块dim不确定，DynamicAutoDiffCostFunction

template <typename CostFunctor, int Stride = 4>
class DynamicAutoDiffCostFunction final : public DynamicCostFunction {
 public:
  // Takes ownership by default.
  explicit DynamicAutoDiffCostFunction(CostFunctor* functor,
                                       Ownership ownership = TAKE_OWNERSHIP)
      : functor_(functor), ownership_(ownership) {}

使用方法

   struct MyCostFunctor {
    // 参数dim和参数块数量由AddParameterBlock和SetNumResiduals下面的动态决定。
    template<typename T>
    bool operator()(T const* const* parameters, T* residuals) const {
      // Use parameters[i] to access the i'th parameter block.
    }
  };
 
   DynamicAutoDiffCostFunction<MyCostFunctor, 3> cost_function(
      new MyCostFunctor());
  cost_function.AddParameterBlock(5);
  cost_function.AddParameterBlock(10);
  cost_function.SetNumResiduals(21);

自动求导和解析求导混用：CostFunctionToFunctor

1.3 求解问题

problem->AddResidualBlock
problem->SetManifold
problem->SetParameterBlockConstant
求解设置

bool SolveOptimizationProblem(ceres::Problem* problem) {
  CHECK(problem != nullptr);

  ceres::Solver::Options options;
  options.max_num_iterations = 200;
  // 看问题是稀疏的还是稠密的
  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;

  ceres::Solver::Summary summary;
  ceres::Solve(options, problem, &summary);

  std::cout << summary.FullReport() << '\n';

  return summary.IsSolutionUsable();
}

2 关于ceres中的QuaternionManifold

https://www.cnblogs.com/JingeTU/p/11707557.html

这里只谈 QuaternionManifold，EigenQuaternionManifold，前者是wxyz四元数顺序，后者是xyzw顺序。
这里Manifold中的jacobian有一个接口是

  virtual bool PlusJacobian(const double* x, double* jacobian) const = 0;

求的是 $\left[\frac{\partial (\delta_{\theta}q)}{\partial \delta_{\theta}} \right]_{4 \times 3}$
这个在ceres中被称为 local to tangent jacobian
使用自动求导得到的 $\left[\frac{\partial f(q)}{\partial q} \right]_{N \times 4}$
得到的是 global to local jacobian ，二者相乘得到的才是 global to tangent space jacobian