1.Ceres中求解一个优化问题的结构
背景：在SLAM中，很多问题都是在求解Translation（包含旋转和平移量），因此这里以其为代表，来分析使用ceres如何对其近求导。
void Calibrator::Optimize(Eigen::Matrix4d& tf)
{
//待优化参数分别为rotation和t
Eigen::Matrix3d rot = T_.topLeftCorner(3,3);
Eigen::Quaterniond q (rot);
Eigen::Vector3d p = T_.topRightCorner(3,1);
ceres::Problem problem;
ceres::Solver::Summary summary;
ceres::Solver::Options options;
ceres::LossFunction* loss_function_edge (new ceres::SoftLOneLoss(1));
ceres::LocalParameterization* quaternion_local_parameterization =
new ceres::EigenQuaternionParameterization;
for(const auto& ply : polygons_)
{
// add edge 误差来源：每一条3D点云边上的点，到2D平面的直线的距离
for(uint32_t i=0; i<size_; i++)
{
if(ply.pc->edges[i].points.cols() == 0)
{
continue;
}
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<Edge2EdgeError, 1,4,3>(new Edge2EdgeError( K_, ply.pc->edges[i].points, ply.img->edges[ply.ids[i]].coef ));//
problem.AddResidualBlock(cost, loss_function_edge,q.coeffs().data(), p.data());
}
}
problem.SetParameterization(q.coeffs().data(), quaternion_local_parameterization);
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5000;
options.num_threads= boost::thread::hardware_concurrency() - 1;
options.num_linear_solver_threads = options.num_threads;
ceres::Solve(options, &problem, &summary);
std::cout << summary.FullReport() << std::endl;
//打印最后的优化结果
tf = Eigen::Matrix4d::Identity();
tf.topLeftCorner(3,3) = q.matrix();
tf.topRightCorner(3,1) = p;
std::cout << "T: \n" << tf << std::endl;
}1. ceres::Solver::Summary summary
优化中的产生的各种信息。
2. ceres::Solver::Options options;
对优化器进行配置。例如使用多少个核来进行计算，最大迭代次数等。
ceres::Solver::Options options;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.max_num_iterations = 5000;
// or use all cpu cores
options.num_threads= boost::thread::hardware_concurrency() - 1;
options.num_linear_solver_threads = options.num_threads;3. ceres::Solve(options, &problem, &summary)
ceres::Solve(options, &problem, &summary);4. ceres::Problem
最后则是如何构建整个优化问题。这里需要详细分析。
ceres::Problem problem;
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<Edge2EdgeError, 1,4,3>(
new Edge2EdgeError( K_, ply.pc->edges[i].points, ply.img->edges[ply.ids[i]].coef ));
//添加残差块
problem.AddResidualBlock(cost, loss_function_edge,
q.coeffs().data(), p.data() );
// add inlier points 添加内点
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<Point2PolygonError,1,4,3>(
new Point2PolygonError(K_, ply.pc->inliers, ply.img->vertexs) );
problem.AddResidualBlock(cost, NULL, q.coeffs().data(), p.data());
problem.SetParameterization(q.coeffs().data(), quaternion_local_parameterization);4.1 首先是cost function的构建
ceres::CostFunction* cost = new ceres::AutoDiffCostFunction<Edge2EdgeError, 1,4,3>(
new Edge2EdgeError( K_, ply.pc->edges[i].points, ply.img->edges[ply.ids[i]].coef ));那么，cost function的详细定义如下：
/**
* @brief Edge to edge error (point to line error)
* 3D point P_i_j locates at 3D edge i, its index is j. 3D edge i has correspondense in
* image E_i(ax+by+c=0,a^2+b^2+c^2=1), project P_i_j into image with initial T(q,p) and
* get 2D point P'_i_j(u,v). If T(q,p) is correct or precise, P'_i_j should be on E_i.
* Thus, the error is the distance between P'_i_j and E_i
*/
struct Edge2EdgeError
{
const Eigen::Matrix3d& K; // camera matrix
const Eigen::Matrix3Xd& pts; // 3d points
const Eigen::Vector3d& coef; // 2d edge(line) coefficients
/**
* @brief Edge2EdgeError constructor 输入只有内参，3D点，和2D的直线方程。
*
* @param k [in]: camera intrinsic parameter matrix K
* @param ps[in]: 3D edge pints set, P_i_j(j=0,1,...n), it's 3xn matrix
* @param cf[in]: 2D line coefficients (ax+by+c=0,a^2+b^2+c^2=1)
*/
Edge2EdgeError(const Eigen::Matrix3d& k, const Eigen::Matrix3Xd& ps, const Eigen::Vector3d& cf):
K(k), pts(ps), coef(cf) {}
/**
* @brief Ceres error compute function
*
* @param T double or Jet
* @param q [in]: Quaternion (rotation)
* @param p [in]: translation
* @param residuals[out]: error
*
* @return true: indicate success
*/
template<typename T>
bool operator()(const T* const q, const T* const p, T* residuals)const
{
Eigen::Matrix<T, 3, Eigen::Dynamic> points;
ProjectPoint2Image<T>(q,p, K.cast<T>(), pts.cast<T>(), points);
//输出的points是图像平面的坐标点
//points:3*n n是点的数量
//coef:3*1 是abc三个参数
//所以coef.transpose()*points = 1*3*3*n = 1*n
//
Eigen::Matrix<T, Eigen::Dynamic, 1> tmp = (coef.transpose().cast<T>())*points;
residuals[0] = tmp.cwiseAbs().sum(); //算出来有正有负，这个cwiseAbs是求绝对值。然后加和
return true; // important
}
};这里面需要注意的主要有三点：
- 第零点：自动求导的定义：
new ceres::AutoDiffCostFunction<Edge2EdgeError, 1,4,3>(new Edge2EdgeError(T1，T2，....));这个地方比较重要的地方有两个；
①自动求导的构造函数输入，就是自定义的cost function的实例的指针。这个具体在下一点讲。
②模板参数<Edge2EdgeError, 1,4,3>:
第一个是cost function。
第二个是残差的维度
第三个是待优化参数的维度。这里是4，表示旋转的四元数表示。
第四个也是待优化参数的维度。这里是3，表示平移量的xyz。
也可以有56789个，ceres内部根据传入参数的数量进行了重载。
但是，不管有多少个待优化的参数，这里的4,3的顺序是和后面的AddResidualBlock()的输入对应的！
- 第一点：构造函数的定义。也就是下面这行需要注意的：
new Edge2EdgeError( K_, ply.pc->edges[i].points, ply.img->edges[ply.ids[i]].coef )在实例化这一个cost function的时候，把就按残差所需要的数据进行了传入。
- 第二点：实现对operator()的重载构造仿函数。
在对符号进行重载的函数里，实现了对残差的计算。
4.2 对整个问题也就是problem添加AddResidualBlock();也即如下：
problem.AddResidualBlock(cost, loss_function_edge,q.coeffs().data(), p.data());注意，这里的AddResidualBlock(T1，T2，T3，T4)一共输入了四个参数。
其中，T1是cost的
T2是损失函数，也即是核函数。
T3是前面所强调的四元数
T4是平移量。
需要注意的是，这里的待优化参数的传入是变量的地址！
2.求解器的分类
上面所举的例子的分析是使用的自动求导。实际上ceres还有其他的求导方式。他们分别是：
1.自动求导（Automatic Differentiation）
2.解析求导（Analytic Differentiation）
3.数值求导（numeric Differentiation）
下面我们分别对这三种求导方式进行对比分析
2.1 自动求导（Automatic Differentiation）
struct CostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
CostFunction* cost_function = new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, nullptr, &x);2.2 数值求导（numeric Differentiation）
struct NumericDiffCostFunctor {
bool operator()(const double* const x, double* residual) const {
residual[0] = 10.0 - x[0];
return true;
}
};
CostFunction* cost_function = new NumericDiffCostFunction<NumericDiffCostFunctor, ceres::CENTRAL, 1, 1>(new NumericDiffCostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x);注意，和自动求导相比，唯一的区别在于模板参数里多了一个ceres::CENTRAL
所以，在ceres的官方文档里对这两种求导方法进行了对比：
Generally speaking we recommend automatic differentiation instead of numeric differentiation. The use of C++ templates makes automatic differentiation efficient, whereas numeric differentiation is expensive, prone to numeric errors, and leads to slower convergence.简单来说，就是自动求导又快又好。数值求导又慢又不稳定。
2.3 解析求导（Analytic Differentiation）
但是不是什么情况下都是可以使用自动求导的。在某些情况下，使用闭式求解会更快更有效率，而不是拘泥于自动求导的链式求导规则。
//Minimize 0.5 (10 - x)^2 using analytic jacobian matrix.
class QuadraticCostFunction
: public SizedCostFunction<1 /* number of residuals */,
1 /* size of first parameter */> {
public:
virtual ~QuadraticCostFunction() {}
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
double x = parameters[0][0];
// f(x) = 10 - x.
residuals[0] = 10 - x;
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -1;
}
return true;
}
};
//
// Set up the only cost function (also known as residual).
CostFunction* cost_function = new QuadraticCostFunction;
problem.AddResidualBlock(cost_function, NULL, &x);注意，解析求导和之前两个不一样的地方有两点：
1.最后CostFunction* 指向的直接就是自己定义的残差函数
2.自己定义的残差函数需要继承ceres自己定义的：
SizedCostFunction<1 /* number of residuals */,
1 /* size of first parameter */> {这里面的模板参数定义了残差和待优化变量的维度。
同时没有了之前的仿函数，转而对Evaluate进行了继承和重载。
virtual bool Evaluate(double const* const* parameters,
double* residuals,
double** jacobians) const {
double x = parameters[0][0];
// f(x) = 10 - x.
residuals[0] = 10 - x;
if (jacobians != NULL && jacobians[0] != NULL) {
jacobians[0][0] = -1;
}
return true;
}输入的参数有
double const* const* parameters：输入参数二维矩阵。
double* residuals：残差，一维向量
double** jacobians：雅克比矩阵，二维矩阵
这里的输入的参数的维度该如何理解呢？为什么直接parameters就是二维矩阵了呢？
留待下文分析！
本文章为转载内容，我们尊重原作者对文章享有的著作权。如有内容错误或侵权问题，欢迎原作者联系我们进行内容更正或删除文章。
