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);