ceres library使用
ceres library:
- 使用自动求导
#include<iostream>
#include<ceres/ceres.h>
using namespace std;
using namespace ceres;
//第一部分：构建代价函数，重载（）符号，仿函数的小技巧
struct CostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[0] = T(10.0) - x[0];
return true;
}
};
//主函数
int main(int argc, char** argv) {
google::InitGoogleLogging(argv[0]);
// 寻优参数x的初始值，为5
double initial_x = 5.0;
double x = initial_x;
// 第二部分：构建寻优问题
Problem problem;
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
problem.AddResidualBlock(cost_function, NULL, &x); //向问题中添加误差项. 三个参数分别表示：误差项，核函数（这里不使用，为空），待估计参数
//第三部分： 配置并运行求解器
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR; //配置增量方程的解法
options.minimizer_progress_to_stdout = true;//输出到cout
Solver::Summary summary;//优化信息
Solve(options, &problem, &summary);//求解!!!
std::cout << summary.BriefReport() << "\n";//输出优化的简要信息
//最终结果
std::cout << "x : " << initial_x
<< " -> " << x << "\n";
return 0;
}
构建最小二乘问题的相关方法均包含在Ceres::Problem类中，涉及的成员函数主要包括Problem::AddResidualBlock()和Problem::AddParameterBlock()
1Problem::AddResidualBlock():
ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function,
LossFunction *loss_function,
const vector<double *> parameter_blocks)
ResidualBlockId Problem::AddResidualBlock(CostFunction *cost_function,
LossFunction *loss_function,
double *x0, double *x1, ...)
cost function代价函数：包含了参数模块的维度信息，内部使用仿函数定义误差函数的计算方式。
损失函数loss function：用于处理参数中含有野值的情况，避免错误量测对估计的影响，常用参数包括HuberLoss、CauchyLoss等（完整的参数列表参见Ceres API文档）；该参数可以取NULL或nullptr，此时损失函数为单位函数。
参数模块：待优化的参数，可一次性传入所有参数的指针容器vector<double*>或依次传入所有参数的指针double*。
2Problem::AddParameterBlock()：
void Problem::AddParameterBlock(double *values, int size)
void Problem::AddParameterBlock(double *values, int size, LocalParameterization *local_parameterization)
其中，第一种函数原型除了会增加一些额外的参数检查之外，功能上和隐式传递参数并没有太大区别。第二种函数原型则会额外传入LocalParameterization参数，用于重构优化参数的维数，这里我们着重讲解一下LocalParameterization类。
2.1LocalParameterization:
LocalParameterization类的作用是解决非线性优化中的过参数化问题。所谓过参数化，即待优化参数的实际自由度小于参数本身的自由度。例如在SLAM中，当采用四元数表示位姿时，由于四元数本身的约束（模长为1），实际的自由度为3而非4。此时，若直接传递四元数进行优化，冗余的维数会带来计算资源的浪费，需要使用Ceres预先定义的QuaternionParameterization对优化参数进行重构：
problem.AddParameterBlock(quaternion, 4);// 直接传递4维参数
ceres::LocalParameterization* local_param = new ceres::QuaternionParameterization();
problem.AddParameterBlock(quaternion, 4, local_param)//重构参数，优化时实际使用的是3维的等效旋转矢量
2.2自定义LocalParameterization:
LocalParaneterization本身是一个虚基类，详细定义如下。用户可以自行定义自己需要使用的子类，或使用Ceres预先定义好的子类。
class LocalParameterization {
public:
virtual ~LocalParameterization() {}
//
virtual bool Plus(const double* x,
const double* delta,
double* x_plus_delta) const = 0;//参数正切空间上的更新函数
virtual bool ComputeJacobian(const double* x, double* jacobian) const = 0; //雅克比矩阵
virtual bool MultiplyByJacobian(const double* x,
const int num_rows,
const double* global_matrix,
double* local_matrix) const;//一般不用
virtual int GlobalSize() const = 0; // 参数的实际维数
virtual int LocalSize() const = 0; // 正切空间上的参数维数
};
class CERES_EXPORT QuaternionParameterization : public LocalParameterization {
public:
virtual ~QuaternionParameterization() {}
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 4; }
virtual int LocalSize() const { return 3; }
};
可以看到，GlobalSize()的返回值为4，即四元数本身的实际维数；由于在内部优化时，ceres采用的是旋转矢量，维数为3，因此LocalSize()的返回值为3。
重载的Plus函数给出了四元数的更新方法，接受参数分别为优化前的四元数x，用旋转矢量表示的增量delta，以及更新后的四元数x_plus_delta。函数首先将增量由旋转矢量转换为四元数，随后采用标准四元数乘法对四元数进行更新。
bool QuaternionParameterization::Plus(const double* x,
const double* delta,
double* x_plus_delta) const {
// 将旋转矢量转换为四元数形式
const double norm_delta =
sqrt(delta[0] * delta[0] + delta[1] * delta[1] + delta[2] * delta[2]);
if (norm_delta > 0.0) {
const double sin_delta_by_delta = (sin(norm_delta) / norm_delta);
double q_delta[4];
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];
// 采用四元数乘法更新
QuaternionProduct(q_delta, x, x_plus_delta);
} else {
for (int i = 0; i < 4; ++i) {
x_plus_delta[i] = x[i];
}
}
return true;
}
- 自己求导
#include "ceres/ceres.h"
#include "glog/logging.h"
//自己计算雅各比矩阵
//f1=x1^2+2*x2^2-1
//f2=2*x1+x2-2
//min(f1^2+f2^2)
class f1 : public ceres::FirstOrderFunction {
public:
virtual ~f1() {}
virtual bool Evaluate(const double* parameters,
double* cost,
double* gradient) const {
const double x = parameters[0];
const double y = parameters[1];
cost[0] = (x * x + 2.0*y*y-1.0)*(x * x + 2.0*y*y-1.0)+(2.0*x+y-2.0)*(2.0*x+y-2.0);
if (gradient != NULL) {
gradient[0] = 2.0 *(x * x + 2.0*y*y-1.0)+2*(2.0*x+y-2.0);
gradient[1] = 2.0 *(x * x + 2.0*y*y-1.0)*4.0*y+2*(2.0*x+y-2.0);
}
return true;
}
virtual int NumParameters() const { return 2; }
};
int main(int argc, char** argv) {
double parameters[2] = {1, 0.1};
ceres::GradientProblem problem(new f1());
ceres::GradientProblemSolver::Options options;//3配置并运行求解器
options.minimizer_progress_to_stdout = true;//输出到cout
ceres::GradientProblemSolver::Summary summary;//优化信息
ceres::Solve(options, problem, parameters, &summary);//求解
std::cout << summary.FullReport() << "\n";
std::cout << " x: " << parameters[0]
<< " y: " << parameters[1] << "\n";
return 0;
}
浙公网安备 33010602011771号