ceres优化库的一点总结（问题构建总结和求导方式的分析）

最新推荐文章于 2024-03-21 08:20:39 发布

SpapaS

最新推荐文章于 2024-03-21 08:20:39 发布

阅读量3.2k

点赞数 7

分类专栏： ceres 代码

本文链接：https://blog.csdn.net/supengufo/article/details/106462460

版权

代码同时被 2 个专栏收录

7 篇文章 1 订阅

订阅专栏

ceres

1 篇文章 1 订阅

订阅专栏

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就是二维矩阵了呢？

留待下文分析！

SpapaS

关注

7
点赞
踩
37

收藏

觉得还不错? 一键收藏
0
评论
ceres优化库的一点总结（问题构建总结和求导方式的分析）

1.Ceres中求解一个优化问题的结构背景：在SLAM中，很多问题都是在求解Translation（包含旋转和平移量），因此这里以其为代表，来分析使用ceres如何对其近求导。void Calibrator::Optimize(Eigen::Matrix4d& tf){ //待优化参数分别为rotation和t Eigen::Matrix3d rot = T_.topLeftCorner(3,3); Eigen::Quaterniond q (rot); E
复制链接

扫一扫