锐客网

  1. 首页 > it技术 > >

SLAM|Ceres库，从入门到放弃

slam非线性优化

前言 首先当然是安装了，安装十分简单。
官网文件有很多人翻译了，例如：https://blog.csdn.net/wzheng92/article/details/79634069
使用Ceres库主要来求解有界约束的非线性最小二乘问题的形式：
SLAM|Ceres库，从入门到放弃
文章图片

求和符号后面的我们称之为误差项。这里我们称 ρ(·)为核函数。一般为恒等的函数（这个函数可以针对不同部分对误差的权重进行调整）；官方文件称之为LossFunction（损失函数），所以很容易混淆。这个标量函数用来减少他的作用减少异常值对非线性最小二乘问题求解的影响。
里面的fi函数f*（）称之为CostFunction(代价函数)，
SLAM|Ceres库，从入门到放弃
文章图片
称之为参数块（ParameterBlock）,我们通常会把待优化的量放在这里面。这个块的规模一般很小。下面的s.t.意思是subject to （受限于），也就是参数的优化所在范围。
当下面的受限于条件变成负无穷到正无穷，就是我们非线性最小二乘问题。
第一步：模板 ceres求解问题的模板：

//ceres求解步骤 //定义CostFuntion //构建Problem //配置Solver //定义Summary //开始优化Solve //输出结果SUmmary.BriefReport #include using namespace std；struct CostFunction{ template bool operator()(const T* x, T* residual )const{ residual[0]=cost_function; return true; }//这个步骤是必须的，通过函数重载运算操作符定义代价函数，就是形式里面的f(x)。//x的维度是下面的dim_2 //residual的维度是dim_1//可选 //下面这部分，如果结构中需要传递别的数据，可以采取定义数据，然后采用才C++11标准中结构 //体使用初始化列表初始化数据 /* const data_type_a, _b; CostFunction(data_type a,data_type b): _a ( a ), _b ( b ) { } */ }; int main( int argc ,char** argv) {/* 函数其他部分,初始化 带估计量x的初始化 */ //构建问题 Problem problem; //添加误差项 problem.AddResidualBlock( new DiffCostFunction_type(new CostFunction), nullptr, &x ); // DiffCostFunction_type一般可以使用自动求导 AutoDiffCostFunction //或者使用NumericDiffCostFunction //使用时为了区别，定义结构体名为 NumericDiffCostFunction，其他相似，构建问题参数多了一个 //new NumericDiffCostFunction( //new NumericDiffCostFunctor)，//第一个参数为生成的costfuntion，尖括号中为误差类型，输入维度，输出维度 //dim_2 的维度是上面结构体定义的x维度 //第二个参数为核函数 //第三个参数为待估计参数的地址，如果是数组，则可以直接传入数组首地址 //说明：这个x是main函数初始化部分定义的 //配置求解器 ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_QR; //线性方程求解的方法 options.minimizer_progress_to_stdout =true; //输出优化的过程 //优化信息结果 ceres::Solver::Summary summary; //开始优化 ceres::Solve(options, &problem, &problem ); //输出优化结果简报 cout<

这是个大概的模板，有很多变种。做以下说明：
1.定义CostFuntion模型时，用了结构体的形式；
2.调用AddResidualBlock将误差添加到目标函数中，由于优化需要梯度，我们有三种选择：
（1）使用Ceres自带的自动求导（Auto Diff）
（2）使用数值求导（Numeric Diff）
（3）使用自己求导的解析求导形式，提供给ceres。
3.自动求导需要指定误差和优化变量的维度。
4.设定好问题，调用solve函数求解，在options里可以配置各种优化的选项，可以选择Line Search或者Trust Region、迭代次数、步长等等
了解了模板之后，看下官方的tutorial的第一个helloworld.cpp文件，应该很好理解。
链接在此：ceres-solver tutorial http://www.ceres-solver.org/nnls_tutorial.html
好了，下面看好了，我要变形了。
第二步：模板变形 数值导数(NumericDiff)，有时候我们不能够完全用模板定义出Costfunction，比如我们可能在定义时候调用了我们不能控制的别的库函数。
解析求导，提供一个参数的输入数组、一个残差的输出数组和一个雅可比矩阵的输出数组。雅可比矩阵是可选的，求值时要检查它是否为非空，如果是，则用残差函数的导数值填充它。
官网的二次函数的代码为例：
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; // f'(x) = -1. Since there's only 1 parameter and that parameter // has 1 dimension, there is only 1 element to fill in the // jacobians. // // Since the Evaluate function can be called with the jacobians // pointer equal to NULL, the Evaluate function must check to see // if jacobians need to be computed. // // For this simple problem it is overkill to check if jacobians[0] // is NULL, but in general when writing more complex // CostFunctions, it is possible that Ceres may only demand the // derivatives w.r.t. a subset of the parameter blocks. if (jacobians != NULL && jacobians[0] != NULL) { jacobians[0][0] = -1; } return true; } };

主要不同就是costfunction 函数的定义。

三、Powell's Function 鲍威尔法是在无约束优化算扼方向，从某个初始点出发，求目标函数在这些方向上的极小值点，然后以该点为新的出发点，取复这一过程直到获得满意解，其优点是不必计算目标函数的梯度就可以在有限步内找到极值点。
SLAM|Ceres库，从入门到放弃
文章图片

大概就是这样子。
#include #include "ceres/ceres.h" #include "gflags/gflags.h" #include "glog/logging.h" using ceres::AutoDiffCostFunction; using ceres::CostFunction; using ceres::Problem; using ceres::Solver; using ceres::Solve; struct F1 { template bool operator()(const T* const x1, const T* const x2, T* residual) const { // f1 = x1 + 10 * x2; residual[0] = x1[0] + 10.0 * x2[0]; return true; } }; struct F2 { template bool operator()(const T* const x3, const T* const x4, T* residual) const { // f2 = sqrt(5) (x3 - x4) residual[0] = sqrt(5.0) * (x3[0] - x4[0]); return true; } }; struct F3 { template bool operator()(const T* const x2, const T* const x3, T* residual) const { // f3 = (x2 - 2 x3)^2 residual[0] = (x2[0] - 2.0 * x3[0]) * (x2[0] - 2.0 * x3[0]); return true; } }; struct F4 { template bool operator()(const T* const x1, const T* const x4, T* residual) const { // f4 = sqrt(10) (x1 - x4)^2 residual[0] = sqrt(10.0) * (x1[0] - x4[0]) * (x1[0] - x4[0]); return true; } }; int main(int argc, char** argv) { CERES_GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); google::InitGoogleLogging(argv[0]); double x1 =3.0; double x2 = -1.0; double x3 =0.0; double x4 =1.0; Problem problem; // Add residual terms to the problem using the using the autodiff // wrapper to get the derivatives automatically. The parameters, x1 through // x4, are modified in place. problem.AddResidualBlock(new AutoDiffCostFunction(new F1), NULL, &x1, &x2); problem.AddResidualBlock(new AutoDiffCostFunction(new F2), NULL, &x3, &x4); problem.AddResidualBlock(new AutoDiffCostFunction(new F3), NULL, &x2, &x3); problem.AddResidualBlock(new AutoDiffCostFunction(new F4), NULL, &x1, &x4); Solver::Options options; options.max_num_iterations = 100; options.linear_solver_type = ceres::DENSE_QR; options.minimizer_progress_to_stdout = true; std::cout << "Initial x1 = " << x1 << ", x2 = " << x2 << ", x3 = " << x3 << ", x4 = " << x4 << "\n"; // Run the solver! Solver::Summary summary; Solve(options, &problem, &summary); std::cout << summary.FullReport() << "\n"; std::cout << "Final x1 = " << x1 << ", x2 = " << x2 << ", x3 = " << x3 << ", x4 = " << x4 << "\n"; return 0; }

如果一个函数的参数受到其他多个函数的约束，针对每个函数，分别定义costfunction，然后调用AddResidualBlock添加到目标函数。
四、曲线拟合 我的模板是根据曲线拟合的例子总结的，所以直接套用，可以体会下，那个我留作可选部分的作用。
下面的程序用于拟合y=exp（ax^2+bx+c）。通过opencv的rng产生误差，然后在拟合。
#include #include #include #include using namespace std; // 代价函数的计算模型 struct CURVE_FITTING_COST { CURVE_FITTING_COST ( double x, double y ) : _x ( x ), _y ( y ) {} // 残差的计算 template bool operator() ( const T* const abc,// 模型参数，有3维 T* residual ) const// 残差 { residual[0] = T ( _y ) - ceres::exp ( abc[0]*T ( _x ) *T ( _x ) + abc[1]*T ( _x ) + abc[2] ); // y-exp(ax^2+bx+c) return true; } const double _x, _y; // x,y数据 }; int main ( int argc, char** argv ) { double a=1.0, b=2.0, c=1.0; // 真实参数值 int N=100; // 数据点 double w_sigma=1.0; // 噪声Sigma值 cv::RNG rng; // OpenCV随机数产生器 double abc[3] = {0,0,0}; // abc参数的估计值vector x_data, y_data; // 数据cout<<"generating data: "< ( new CURVE_FITTING_COST ( x_data[i], y_data[i] ) ), nullptr,// 核函数，这里不使用，为空 abc// 待估计参数 ); }// 配置求解器 ceres::Solver::Options options; // 这里有很多配置项可以填 options.linear_solver_type = ceres::DENSE_QR; // 增量方程如何求解 options.minimizer_progress_to_stdout = true; // 输出到coutceres::Solver::Summary summary; // 优化信息 chrono::steady_clock::time_point t1 = chrono::steady_clock::now(); ceres::Solve ( options, &problem, &summary ); // 开始优化 chrono::steady_clock::time_point t2 = chrono::steady_clock::now(); chrono::duration time_used = chrono::duration_cast>( t2-t1 ); cout<<"solve time cost = "< 这样的函数拟合，对于一些错误的点也会考虑在内，如果存在误差很大的点，会导致整体拟合的效果不好。所以，更加鲁棒（robust）的方法就是使用LossFunction，损失函数。
SLAM|Ceres库，从入门到放弃
文章图片

下面就是重点了，敲黑板！！！
五、BundleAdjustment 中文名光束法平差，我记不住，有些人叫做BA问题，简单好记！主要用于SLAM，在给定一组实测图像特征位置及其对应关系的情况下，进行束调整的目标是找到使重投影误差最小的三维点位置和相机参数。该优化问题通常表示为非线性最小二乘问题，其误差为观测到的特征位置与对应的三维点在相机成像平面上投影之差的L2平方模。
模板针孔相机模型。相机使用9个参数进行参数化:3个参数用于旋转，3个参数用于平移，1个参数用于焦距，2个参数用于径向畸变。主点没有建模(即假设位于图像中心)。
关于相机模型的知识点请自行学习回顾。
#include #include #include "ceres/ceres.h" #include "ceres/rotation.h"//这个类用去读取BAL数据集相机、照片等相关信息的类，大致了解下 // Read a Bundle Adjustment in the Large dataset. class BALProblem { public: ~BALProblem() { delete[] point_index_; delete[] camera_index_; delete[] observations_; delete[] parameters_; } int num_observations()const { return num_observations_; } const double* observations() const { return observations_; } double* mutable_cameras(){ return parameters_; } double* mutable_points(){ return parameters_+ 9 * num_cameras_; } //每个相机对应的内参和外参 double* mutable_camera_for_observation(int i) { return mutable_cameras() + camera_index_[i] * 9; } //对应数据点所在观测下的坐标 double* mutable_point_for_observation(int i) { return mutable_points() + point_index_[i] * 3; } bool LoadFile(const char* filename) { FILE* fptr = fopen(filename, "r"); if (fptr == NULL) { return false; }; FscanfOrDie(fptr, "%d", &num_cameras_); FscanfOrDie(fptr, "%d", &num_points_); FscanfOrDie(fptr, "%d", &num_observations_); point_index_ = new int[num_observations_]; camera_index_ = new int[num_observations_]; observations_ = new double[2 * num_observations_]; num_parameters_ = 9 * num_cameras_ + 3 * num_points_; parameters_ = new double[num_parameters_]; for (int i = 0; i < num_observations_; ++i) { FscanfOrDie(fptr, "%d", camera_index_ + i); FscanfOrDie(fptr, "%d", point_index_ + i); for (int j = 0; j < 2; ++j) { FscanfOrDie(fptr, "%lf", observations_ + 2*i + j); } } for (int i = 0; i < num_parameters_; ++i) { FscanfOrDie(fptr, "%lf", parameters_ + i); } return true; } private: template void FscanfOrDie(FILE *fptr, const char *format, T *value) { int num_scanned = fscanf(fptr, format, value); if (num_scanned != 1) { LOG(FATAL) << "Invalid UW data file."; } } int num_cameras_; int num_points_; int num_observations_; int num_parameters_; int* point_index_; int* camera_index_; double* observations_; double* parameters_; }; //模板针孔相机模型。相机使用9个参数进行参数化:3个参数用于旋转， //3个参数用于平移，1个参数用于焦距，2个参数用于径向畸变。 //主点没有建模(即假设位于图像中心)。struct SnavelyReprojectionError { SnavelyReprojectionError(double observed_x, double observed_y) : observed_x(observed_x), observed_y(observed_y) {}template bool operator()(const T* const camera, const T* const point, T* residuals) const {//把空间点变成像素坐标p=R*Pw+t； T p[3]; ceres::AngleAxisRotatePoint(camera, point, p); // camera[3,4,5] are the translation. p[0] += camera[3]; p[1] += camera[4]; p[2] += camera[5]; //考虑相机的径向畸变，并进行校正，计算最终的像素坐标 T xp = - p[0] / p[2]; T yp = - p[1] / p[2]; const T& l1 = camera[7]; const T& l2 = camera[8]; T r2 = xp*xp + yp*yp; T distortion = 1.0 + r2* (l1 + l2* r2); // Compute final projected point position. const T& focal = camera[6]; T predicted_x = focal * distortion * xp; T predicted_y = focal * distortion * yp; //预测像素坐标和观测坐标在x，y方向上的误差 // The error is the difference between the predicted and observed position. residuals[0] = predicted_x - observed_x; residuals[1] = predicted_y - observed_y; return true; } // Factory to hide the construction of the CostFunction object from // the client code. static ceres::CostFunction* Create(const double observed_x, const double observed_y) { return (new ceres::AutoDiffCostFunction( new SnavelyReprojectionError(observed_x, observed_y))); } double observed_x; double observed_y; }; int main(int argc, char** argv) { google::InitGoogleLogging(argv[0]); if (argc != 2) { std::cerr << "usage: simple_bundle_adjuster \n"; return 1; } BALProblem bal_problem; if (!bal_problem.LoadFile(argv[1])) { std::cerr << "ERROR: unable to open file " << argv[1] << "\n"; return 1; } const double* observations = bal_problem.observations(); ceres::Problem problem; for (int i = 0; i < bal_problem.num_observations(); ++i) {ceres::CostFunction* cost_function = SnavelyReprojectionError::Create(observations[2 * i + 0], observations[2 * i + 1]); problem.AddResidualBlock(cost_function, NULL /* squared loss */, bal_problem.mutable_camera_for_observation(i), bal_problem.mutable_point_for_observation(i)); }ceres::Solver::Options options; options.linear_solver_type = ceres::DENSE_SCHUR; options.minimizer_progress_to_stdout = true; ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); std::cout << summary.FullReport() << "\n"; return 0; }

大致的流程都是相似的。









【SLAM|Ceres库，从入门到放弃】

    推荐阅读


    上一篇：Mathematics|Matlab与数值计算基础

    下一篇：Matlab和ROS数据互联