ceres使用总结

杂七杂八的

已于 2023-11-23 10:55:00 修改

阅读量386

点赞数

文章标签：算法

于 2022-07-19 12:20:38 首次发布

本文链接：https://blog.csdn.net/weixin_43662239/article/details/125868465

版权

自动微分Jet原理

CostFunction

CostFunction是有用来计算残差e和雅阁比J的，给出状态值向量[x1,x2,...xn]后按道理就可以根据方程f(x1,x2,...xn)求解出方程的结果vector和雅阁比矩阵J

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_
每个状态值的维度，比如x1是pose的话维度就是7，x2是速度和bias的话维度就是9
CostFunction::num_residuals_
残差的维度，比如视觉就是2，相对pose就是6
继承函数Evaluate，必须继承用于计算出残差和雅各比的,在vins_mono的边缘化中就是使用的Evaluate函数来计算出原始的residuals和raw jacobians

//输入：parameters 输出：residuals jacobians
bool CostFunction::Evaluate(double const *const *parameters, double *residuals, double **jacobians)

parameters：状态值向量,维度不一定为1
$x=\left[ \begin{array}{l} x_1 \\ x_2 \\ \vdots \\x_n\end{array} \right]$
parameter_block_sizes_：每个状态值对应的参数维度
$s=\left[ \begin{array}{l} s_1 \\ s_2\\ \vdots \\s_n\end{array} \right]$
residuals：残差，维度为m的残差对应的向量为
$e=\left[ \begin{array}{l} e_1 \\ e_2 \\ \vdots \\e_m\end{array} \right]$
jacobians:雅阁比矩阵的维度为n
$J=\left[ \begin{array}{l} \frac{\partial e_1}{\partial x_1}_{(1\times s_1)} & \frac{\partial e_2}{\partial x_1}_{(1\times s_1)} \cdots & \frac{\partial e_m}{\partial x_1}_{(1\times s_1)} \\ \frac{\partial e_1}{\partial x_2}_{(1\times s_2)} & \frac{\partial e_2}{\partial x_2}_{(1\times s_2)} \cdots & \frac{\partial e_m}{\partial x_2}_{(1\times s_2)} \\ \vdots &\vdots &\vdots \\ \frac{\partial e_1}{\partial x_n}_{(1\times s_n)} & \frac{\partial e_2}{\partial x_n}_{(1\times s_n)} \cdots & \frac{\partial e_m}{\partial x_n}_{(1\times s_n)} \\\end{array} \right]$
,第i行的维度为表示所有的残差对状态值xi的雅各比
维度为：
$m\times s_i$
雅各比为：
$J_i=\left[ \begin{array}{l} \frac{\partial e_1}{\partial x_i}_{(1\times s_i)} & \frac{\partial e_2}{\partial x_i}_{(1\times s_i)} \cdots & \frac{\partial e_m}{\partial x_i}_{(1\times s_i)} \end{array} \right]$
雅各比是按地址来表示array的，Ji就是表示的第i个雅各比的地址：
$J_i=>jacobians[i]$

LocalParameterization

状态值本身维度和在流体表面维度不一致的情况下使用

class LocalParameterization {
 public:
  virtual ~LocalParameterization() = default;
  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;
  virtual bool MultiplyByJacobian(const double* x,
                                  const int num_rows,
                                  const double* global_matrix,
                                  double* local_matrix) const;
  virtual int GlobalSize() const = 0; //原始空间对应的状态值维度
  virtual int LocalSize() const = 0; //切平面上对应维度
};

只要是非线性的参数块都是定义的LocalParameterization,比如vins mono中的航向角是控制在[-180,180]之间就是一个非线性的
1.航向角相关的local parameter

定义非线性函数

template <typename T>
T NormalizeAngle(const T& angle_degrees) {
  if (angle_degrees > T(180.0))
  	return angle_degrees - T(360.0);
  else if (angle_degrees < T(-180.0))
  	return angle_degrees + T(360.0);
  else
  	return angle_degrees;
};

定义LocalParameterization的方法

class AngleLocalParameterization {
 public:

  template <typename T>
  bool operator()(const T* theta_radians, const T* delta_theta_radians,
                  T* theta_radians_plus_delta) const {
    *theta_radians_plus_delta =
        NormalizeAngle(*theta_radians + *delta_theta_radians);

    return true;
  }

  static ceres::LocalParameterization* Create() {
    return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
                                                     1, 1>);
  }
};

2.姿态相关的local parameter

1.声明local parameter的类型
ceres::LocalParameterization *local_parameter = new ceres::EigenQuaternionParameterization
2.添加local parameter数据
problem.AddParameterBlock(frame.q.coeffs().data(),4,local_parameter);
3.如果要参数为const的话
problem.SetParameterBlockConstant(frame.q.coeffs().data());

q.coeffs() //数据存储方式为: [x,y,z,w]

3.pose定义的local parameter

local parameter类型定义

#include <eigen3/Eigen/Dense>
#include <ceres/ceres.h>
class PoseLocalParameterization : public ceres::LocalParameterization
{
    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 7; };
    virtual int LocalSize() const { return 6; };
};

重定义广义加法

#include "pose_local_parameterization.h"
bool PoseLocalParameterization::Plus(const double *x, const double *delta, double *x_plus_delta) const
{
    Eigen::Map<const Eigen::Vector3d> _p(x);
    Eigen::Map<const Eigen::Quaterniond> _q(x + 3);

    Eigen::Map<const Eigen::Vector3d> dp(delta);

    Eigen::Quaterniond dq = Utility::deltaQ(Eigen::Map<const Eigen::Vector3d>(delta + 3));

    Eigen::Map<Eigen::Vector3d> p(x_plus_delta);
    Eigen::Map<Eigen::Quaterniond> q(x_plus_delta + 3);

    p = _p + dp;
    q = (_q * dq).normalized();

    return true;
}

重定义雅各比矩阵

bool PoseLocalParameterization::ComputeJacobian(const double *x, double *jacobian) const
{
    Eigen::Map<Eigen::Matrix<double, 7, 6, Eigen::RowMajor>> j(jacobian);
    j.topRows<6>().setIdentity();
    j.bottomRows<1>().setZero();

    return true;
}

数据内部存储顺序

ceres::QuaternionParameterization：内部存储顺序为(w,x,y,z)
ceres::EigenQuaternionParameterization：内部存储顺序为(x,y,z,w)
Eigen::Quaternion(w,x,y,z)：内部存储顺序为(x,y,z,w)（与构造函数没有保持一致）

SizedCostFunction

#include <iostream>
#include <ceres/ceres.h>
#include <glog/logging.h>

class QuadraticCostFunction : public ceres::SizedCostFunction<1 /* number of residuals */, 3 /* size of parameter block */> {
public:
    QuadraticCostFunction(double x, double y) : x_(x), y_(y) {}

    virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override {
        const double a = parameters[0][0]; // Parameter a
        const double b = parameters[0][1]; // Parameter b
        const double c = parameters[0][2]; // Parameter c

        residuals[0] = y_ - (a * x_ * x_ + b * x_ + c);

        if (jacobians != nullptr && jacobians[0] != nullptr) {
            // Compute Jacobian if requested
            double* jacobian = jacobians[0];
            jacobian[0] = -x_ * x_; // Derivative of residual w.r.t parameter a
            jacobian[1] = -x_;      // Derivative of residual w.r.t parameter b
            jacobian[2] = -1.0;     // Derivative of residual w.r.t parameter c
        }

        return true;
    }

private:
    const double x_;
    const double y_;
};

int main() {
    double a = 1.0;
    double b = 2.0;
    double c = 3.0;

    double x_data[] = {1.0, 2.0, 3.0, 4.0, 5.0};
    double y_data[] = {5.1, 12.4, 26.1, 44.2, 69.5};

    ceres::Problem problem;

    for (int i = 0; i < 5; ++i) {
        ceres::CostFunction* cost_function =
            new QuadraticCostFunction(x_data[i], y_data[i]);

        problem.AddResidualBlock(cost_function, nullptr, &a, &b, &c);
    }

    ceres::Solver::Options options;
    options.linear_solver_type = ceres::DENSE_QR;
    options.minimizer_progress_to_stdout = true;

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

    std::cout << summary.FullReport() << "\n";
    std::cout << "Final solution: a = " << a << ", b = " << b << ", c = " << c << "\n";

    return 0;
}