之前用过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. 求导方式
ceres的求导方式有3种：自动求导、数值求导和解析求导，详细介绍可以参考博客，这里仅介绍解析求导和自动求导
3.1 解析求导（需自定义Jacobian）
求导方式有自行求导和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_;
};
其中Eigen::Map是对数组jacobians[i]的引用，简要介绍如下：
Eigen中的Map类用于通过C++中普通的连续指针或者数组 （raw C/C++ arrays）来构造Eigen里的Matrix类，这就好比Eigen里的Matrix类的数据和raw C++array 共享了一片地址，也就是引用。
比如有个API只接受普通的C++数组，但又要对普通数组进行线性代数操作，那么用它构造为Map类，直接操作Map就等于操作了原始普通数组，省时省力。
3.2 自动求导Autodiff
在定义costfunction时选择ceres::AutoDiffCostFunction使用自动求导，求数值导数，需要重载operator()定义residual的计算方法。
**注意：**这里重载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_;
};
4. 相关函数
4.1 ceres::QuaternionParameterization
4.1.1 自己求Jacobian
VINS-MONO中，当四元数为优化的对象时，需要调用ceres::QuaternionParameterization来消除自由度冗余
double para_qic[4] = {1, 0, 0, 0};
problem.AddParameterBlock(para_qic, 4, new ceres::QuaternionParameterization);
如果是自己定义的数据结构，则可以继承ceres::LocalParameterization自己定义
比如优化对象是一个7维的pose(Eigen::Quaterniond, Eigen::Vector3d)，7维参数，自由度为6，如下VINS-MONO的例子：
调用：
void Estimator::optimization(){
ceres::Problem problem;
ceres::LossFunction *loss_function;
//loss_function = new ceres::HuberLoss(1.0);//Huber损失函数
loss_function = new ceres::CauchyLoss(1.0);//柯西损失函数
for (int i = 0; i < WINDOW_SIZE + 1; i++)
{
ceres::LocalParameterization *local_parameterization = new PoseLocalParameterization();
problem.AddParameterBlock(para_Pose[i], 7, local_parameterization);
problem.AddParameterBlock(para_SpeedBias[i], 9);
}
//2.添加IMU残差
for (int i = 0; i < WINDOW_SIZE; i++)
{
int j = i + 1;
//两帧KF之间IMU积分时间过长的不进行优化
if (pre_integrations[j]->sum_dt > 10.0)
continue;
IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]);
problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]);
}
}
具体实现：
继承ceres::LocalParameterization类，该类是含有纯虚函数的抽象类，需要把所有虚函数都实现才能实例化出对象，具体可看ceres源码，这里不再赘述。
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; };//参数个数7
virtual int LocalSize() const { return 6; };//自由度6
};
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;
}
4.1.2 使用auto_diff
上面那样是自己定义Jacobian，如果使用auto_diff，则可以不继承ceres::LocalParameterization，而是类似继承ceres::AutoDiffLocalParameterization，定义一个新的local_parameterization的类(名字可以自己定)，在类中需要完成：
- 重载
bool operator()返回true - 定义
static ceres::LocalParameterization* Create()函数 (注意这一步不是必须的，也可以像下面给出的ceres源码注释中的做法，直接new AutoDiffLocalParameterization(...)定义一个LocalParameterization*，只不过我们通常都在类中实现这样一个Create()函数)
在VINS-MONO的pose_graph模块中，优化4自由度pose_graph的yaw角时就定义了AngleLocalParameterization类，代码如下：
//是ceres::AutoDiffLocalParameterization的Functor参数，需要重载operator()
class AngleLocalParameterization {
public:
template <typename T>
//重载()，进行弧度/角度运算，并归一化到[-360, 360]
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;
}
//定义yaw角的更新方式 AutoDiffLocalParameterization的初始化传参为：<typename Functor, int kGlobalSize, int kLocalSize>
static ceres::LocalParameterization* Create() {
return (new ceres::AutoDiffLocalParameterization<AngleLocalParameterization,
1, 1>);
}
};
调用为：
//定义yaw角的local_parameterization
ceres::LocalParameterization* angle_local_parameterization =
AngleLocalParameterization::Create();
//将yaw角添加为待优化参数，维度为1，并设置去冗余方式(angle_local_parameterization)
problem.AddParameterBlock(euler_array[i], 1, angle_local_parameterization);
与4.1.1的自己定义Jacobian不同，这里使用了autodiff自动求导，我们无需自己定义Jacobian，但是需要告诉系统变量是如何更新的，即给你一个
x
x
x和增量
Δ
x
\Delta x
Δx，你如何求出
x
′
x^\prime
x′，正常的加法就是
x
′
=
x
+
Δ
x
x^\prime=x+\Delta x
x′=x+Δx，但是比如四元数就不是这么运算，所以需要我们告诉系统如何去加，意义是系统可以使用告知的“加法”去自己求导，所以其实重载的operator()就是告诉系统如何做“加法”，如下是ceres源码中的注释：
// For example, Quaternions have a three dimensional local
// parameterization. It's plus operation can be implemented as (taken
// from internal/ceres/auto_diff_local_parameterization_test.cc)
//
// struct QuaternionPlus {
// template<typename T>
// bool operator()(const T* x, const T* delta, T* x_plus_delta) const {
// const T squared_norm_delta =
// delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2];
//
// T q_delta[4];
// if (squared_norm_delta > T(0.0)) {
// T norm_delta = sqrt(squared_norm_delta);
// const T sin_delta_by_delta = sin(norm_delta) / norm_delta;
// q_delta[0] = cos(norm_delta);
// q_delta[1] = sin_delta_by_delta * delta[0];
// q_delta[2] = sin_delta_by_delta * delta[1];
// q_delta[3] = sin_delta_by_delta * delta[2];
// } else {
// // We do not just use q_delta = [1,0,0,0] here because that is a
// // constant and when used for automatic differentiation will
// // lead to a zero derivative. Instead we take a first order
// // approximation and evaluate it at zero.
// q_delta[0] = T(1.0);
// q_delta[1] = delta[0];
// q_delta[2] = delta[1];
// q_delta[3] = delta[2];
// }
//
// QuaternionProduct(q_delta, x, x_plus_delta);
// return true;
// }
// };
//
// Then given this struct, the auto differentiated local
// parameterization can now be constructed as
//
// LocalParameterization* local_parameterization =
// new AutoDiffLocalParameterization<QuaternionPlus, 4, 3>;
// | |
// Global Size ---------------+ |
// Local Size -------------------+
//
4.2 problem.AddResidualBlock()
上面指定优化参数维度时调用了AddParameterBlock函数，简单来说就是指定待优化参数的维度，如四元数自由度为3，当然也可以不调用，待优化参数从AddResidualBlock函数传入。
简单介绍一下AddResidualBlock，两个作用：
- 作用一：用户可以选择使用Problem::AddParameterBlock显式添加参数块；
在显式添加的时候ceres会内部进行额外的正确性检查。
当Problem::AddParameterBlock()不存在时，Problem::AddResidualBlock()会隐式添加参数块，因此不需要显式调用Problem::AddParameterBlock()函数。
ps：类似构造函数，显式构造函数。
- 作用二：将LocalParameterization对象与参数块相关联。
建议优先显式添加。
5. 拓展
关于cost_function有适应不同使用需求的实现方式，如需要动态block_size的cost_function，这部分可参考以下博客，有时间再进行拓展。
5万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
