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_yawRl_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_lc2Map<Vector2d> residual(residuals);residual = Rl_lc.inverse() * thc1_hc2 - z_;  //(hc1)thc1_hc2' - (hc1)thc1_hc2if (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);