Ceres使用

之前用过Ceres，但是只是跑例程，现在来着重学习一下使用流程。

1. 解决的问题

主要解决非线性优化问题。Ceres是一个较为通用的库。
参考链接

2. 如何使用

这个是求解的函数，主要关注这三个参数

CERES_EXPORT void Solve(const Solver::Options& options, 
						Problem* problem, 
						Solver::Summary* summary);

1. options

与优化相关的一些参数配置

2. problem

定义problem
重要的函数

  ResidualBlockId AddResidualBlock(
      CostFunction* cost_function,
      LossFunction* loss_function,
      const std::vector<double*>& parameter_blocks);

其中cost_function是需要我们自己定义的代价函数，拿SLAM14讲中的CURVE_FITTING_COST为例

添加残差项：

    ceres::Problem problem;
    for(int i=0; i<N; ++i){  //100个点句添加100个误差项
        //使用自动求导
        problem.AddResidualBlock(
                new ceres::AutoDiffCostFunction<CURVE_FITTING_COST, 1, 3>(new CURVE_FITTING_COST(x_data[i], y_data[i])),
                nullptr,
                abc  //待估计参数，可在此处给初值
        );
    }

其中，AddResidualBlock
@param1ceres::AutoDiffCostFunction是用自动求导的方式，是一个类模板，需要传入参数类型实例化为模板类（类名，输出维度（标量误差，维度1），输入维度（abc三个参数，维度3）），然后传入实际参数来实例化出一个类，（也可以自己求雅克比传给ceres，这里不多说）
@param2 核函数一般不用，传nullptr
@param3 待估计参数（由于非线性优化对初值敏感，所以可以从这里传入待优化变量的初值）

关于残差项的构建：

using namespace std;
struct CURVE_FITTING_COST{
    CURVE_FITTING_COST(double x, double y):_x(x), _y(y){}  //构造函数需要传入的对象

    template<typename T>
    bool operator()(const T *const abc, T *residual) const 
    {
        residual[0] = T(_y) - ceres::exp(abc[0] * T(_x) * T(_x) + abc[1] * T(_x) + abc[2]);
        return true;
    }
    const double _x, _y;
};

重载operator()，
@param1 ：输入参数，三维待估计参数。
@param2 ：输出参数，一维误差。
这个函数就是用输入的参数通过计算，算出残差用于求导。

3. summary

用于保存优化log（日志）

3. 求导方式

3.1 自行求导

求导方式有自行求导和Autodiff
自行求导需要继承ceres::SizedCostFunction，并重载Evaluate()函数自行推导导数计算jacobians，parameters传入的即为待优化参数，
调用：

            CameraLidarFactor *f = new CameraLidarFactor(Rl1_l2, tl1_l2, _z);  //求导方式
            problem.AddResidualBlock(f, new ceres::HuberLoss(1e-6), &_phi, _t);  //第三部分为待优化参数，可赋初值

具体实现：

class CameraLidarFactor : public ceres::SizedCostFunction<2, 1, 2> {  //第一个是输出维度，phi和t一个1维，一个2维
public:
    CameraLidarFactor(Matrix2d &Rl1_l2, Vector2d &tl1_l2, Vector2d &z) :  // 待优化的phi，lidar系下的平移
            Rl1_l2(Rl1_l2), tl1_l2(tl1_l2), z_(z) {}

    virtual bool Evaluate(double const *const *parameters, double *residuals, double **jacobians) const {
        double phi_l_lc = parameters[0][0];
        Matrix2d Rl_lc;  // rot2d_from_yaw
        Rl_lc << cos(phi_l_lc), -sin(phi_l_lc),
                sin(phi_l_lc), cos(phi_l_lc);
        Vector2d tl_lc(parameters[1][0], parameters[1][1]);

        Vector2d thc1_hc2 = (-tl_lc + tl1_l2 + Rl1_l2 * tl_lc);  // -(l1)tl1_lc1 + (l1)tl1_l2 + Rl1_l2 * (l2)tl2_lc2
        Map<Vector2d> residual(residuals);
        residual = Rl_lc.inverse() * thc1_hc2 - z_;  //(hc1)thc1_hc2' - (hc1)thc1_hc2

        if (jacobians) {
            if (jacobians[0]) {
                Matrix2d Rl_hc_inverse_prime;
                Rl_hc_inverse_prime << -sin(phi_l_lc), cos(phi_l_lc),  //逆求导
                        -cos(phi_l_lc), -sin(phi_l_lc);
                Map<Matrix<double, 2, 1>> jacobian_phi(jacobians[0]);
                jacobian_phi = Rl_hc_inverse_prime * thc1_hc2;
            }
            if (jacobians[1]) {
                Map<Matrix<double, 2, 2, RowMajor>> jacobian_t(jacobians[1]);
                jacobian_t = Rl_lc.inverse() * (Rl1_l2 - Matrix2d::Identity());
            }
        }
        return true;
    }
    Matrix2d Rl1_l2;
    Vector2d tl1_l2, z_;
};

3.2 Autodiff自动求导

在定义costfunction时选择ceres::AutoDiffCostFunction使用自动求导，求数值导数，需要重载operator()。

**注意：**这里重载operator需要是函数模板，里面所有的数据都要使用模板的数据类型。

调用：

        ceres::CostFunction *cost_function = NULL;
        cost_function = CamTiltFactor::Create(init_z, image_poses_[i].second.translation());
        problem.AddResidualBlock(cost_function, new ceres::HuberLoss(1e-5), para_qic);

实现：

struct CamTiltFactor {
    CamTiltFactor(const double init_z, const Eigen::Vector3d trans) :
            init_z_(init_z), trans_(trans) {}

    static ceres::CostFunction *Create(const double init_z, Eigen::Vector3d trans) {
        return new ceres::AutoDiffCostFunction<CamTiltFactor, 1, 4>(
                new CamTiltFactor(init_z,  trans));
    }

    template<typename _T2>
    bool operator()(const _T2 *const para_qic, _T2 *residuals) const {
        //计算residualspara_qic[0]
        Eigen::Quaternion<_T2> _quat{para_qic[0], para_qic[1], para_qic[2], para_qic[3]};
        Eigen::Matrix<_T2, 3, 1> tmp_trans_(_T2(trans_.x()), _T2(trans_.y()), _T2(trans_.z()));
        Eigen::Matrix<_T2, 3, 1> _t_rotated = _quat * tmp_trans_;  //使用重载的乘法
        residuals[0] = _T2(_t_rotated.z()) - _T2(init_z_);  //残差
        return true;
    }

    Vector3d trans_;
    double init_z_;
};

另外，当四元数为优化的对象时，需要调用ceres::QuaternionParameterization来消除自由度冗余

    double para_qic[4] = {1, 0, 0, 0};
    problem.AddParameterBlock(para_qic, 4, new ceres::QuaternionParameterization);