ceres Analytic Derivatives

ceres是一个非线性最小二乘的库
其中主要求解方式有Analytic Derivatives，AutoDerivatives & Numeric Derivatives这三种。AutoDerivatives最常用也最方便使用，Analytic Derivatives在雅各比矩阵计算上可能会快于AutoDerivatives，所以在某些地方可以使用Analytic Derivatives来加速优化过程。

Analytic Derivatives需要完成ceres::SizedCostFunction中Evaluate的定义
其中，SizedCostFunction声明为

这里<>中的第一个参数为残差的数量（残差可以是一个向量），后面的int表示参数块的大小，可以输入多个参数块（比如多个数组或者vector等），然后在AddResidualBlock（）时，输入的参数的大小要和<>中的一致

Evaluate的函数声明为：

这里parameters为参数块，由于参数块可以输入多个，所以他是一个指向double指针的指针，residulas为残差，最多是一位向量，所以是一个指向double的指针。雅各比矩阵显然是一个二维的（如果有m维残差向量，n维参数向量，那么雅各比是一个mxn的矩阵），这里重点是雅各比是如何存储的

比如现在有两个参数块，分别是vector p1, vector p2，大小分别为3，2
p1[0]在parameters[0][0]中，p1[1]在parameters[0][1]，p1[2]在parameters[0][2]中
p2[0]在parameters[1][0]中，p2[1]在parameters[1][1]中

然后残差是二维的r1 在residuals[0]中，r2在residuals[1]中

重点为雅各比矩阵存储，第i个参数块都放在parameters[i]指向的double数组中，所有残差依次对第i个参数块中所有参数求导的结果都依次放在jacobians[i]中

这里，c为parameters[I]中的第c个参数
延用上面的例子，
jacobians[0][0]存放r1对p1[0]求导
jacobians[0][1]存放r1对p1[1]求导
jacobians[0][2]存放r1对p1[2]求导
jacobians[0][3]存放r2对p1[0]求导
jacobians[0][4]存放r2对p1[1]求导
jacobians[0][5]存放r2对p1[2]求导

这里给出两个SizedCostFunction为多维的情况的代码，一个是slambook2中的位姿估计的改写，一个是曲线拟合

下面是slam十四讲中ch7，用ceres Analytic Derivatives对pose_estimation_3d2d的修改，放了SizedCostFunction

class pnp_optimization : public ceres::SizedCostFunction<2, 6>{
public:pnp_optimization(const Eigen::Vector3d &point3d, const Eigen::Vector2d &point2d, const Eigen::Matrix3d &K) :_point3d(point3d), _point2d(point2d), Kk(K) {}virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) const {typedef Eigen::Matrix<double, 6, 1> vector6d;vector6d se3;se3 << parameters[0][0], parameters[0][1], parameters[0][2], parameters[0][3],parameters[0][4], parameters[0][5];Sophus::SE3d T = Sophus::SE3d::exp(se3);double fx = Kk(0, 0);double fy = Kk(1, 1);double cx = Kk(0, 2);double cy = Kk(1, 2);Eigen::Vector3d pc = T * _point3d;Eigen::Vector2d proj(fx * pc[0] / pc[2] + cx, fy * pc[1] / pc[2] + cy);Eigen::Vector2d e = _point2d - proj;residuals[0] = e[0];residuals[1] = e[1];//cout << residuals[0] << " " << residuals[1] << endl;if (jacobians != nullptr && jacobians[0] != nullptr){double inv_z = 1.0 / pc[2];double inv_z2 = inv_z * inv_z;jacobians[0][0] = -fx * inv_z;jacobians[0][1] = 0;jacobians[0][2] = fx * pc[0] * inv_z2;jacobians[0][3] = fx * pc[0] * pc[1] * inv_z2;jacobians[0][4] = -fx + fx * pc[0] * pc[0] * inv_z2;jacobians[0][5] = fx * pc[1] * inv_z;jacobians[0][6] = 0;jacobians[0][7] = -fy * inv_z;jacobians[0][8] = fy * pc[1] * inv_z2;jacobians[0][9] = fy + fy * pc[1] * pc[1] * inv_z2;jacobians[0][10] = -fy * pc[0] * pc[1] * inv_z2;jacobians[0][11] = -fy * pc[0] * inv_z;}return true;}
private:const Eigen::Vector3d _point3d;const Eigen::Vector2d _point2d;const Eigen::Matrix3d Kk;
};

下面是自己写的一个曲线拟合的例子，sizedcostfuction为2x2

#include <iostream>
#include <ceres/ceres.h>#define DATA_LEN 100using namespace std;
float getRand();class test : public ceres::SizedCostFunction<2,2> {
public:test (const double x, const vector<double> y) : x_(x), y_(y) {}virtual bool Evaluate (double const* const* parameters,double* residuals,double** jacobians) const {double a, b;a = parameters[0][0];b = parameters[0][1];residuals[0] = y_[0] - a*x_*x_ - b*x_;residuals[1] = y_[1] - a*x_ - b;if (jacobians != nullptr) {if (jacobians[0] != nullptr) {jacobians[0][0] = -x_*x_;jacobians[0][1] = -x_;jacobians[0][2] = -x_;jacobians[0][3] = 0;//printf("%lf, %lf, %lf, %lf\n", jacobians[0][0], jacobians[0][1], jacobians[0][2], jacobians[0][3]);}}}private:const double x_;const vector<double> y_;
};int main() {double para[2];vector<vector<double>> y_true(DATA_LEN, vector<double>(2, 0));vector<vector<double>> y_in(DATA_LEN, vector<double>(2, 0));para[0] = 2;para[1] = 0.5;for (int i = 0; i < DATA_LEN; i++) {//x = 0 : 1 : DATA_LEN//y1 = ax^2+bx;//y2 = ax + b;double x = i;y_true[i][0] = para[0] * x * x + para[1] * x;y_true[i][1] = para[0] * x + para[1];y_in[i][0] = y_true[i][0] + getRand();y_in[i][1] = y_true[i][1] + getRand();}double mypara[2] = {1, 2};ceres::Problem problem;for (int i = 0; i < DATA_LEN; i++) {ceres::CostFunction *pCostFunction = new test(i, y_in[i]);problem.AddResidualBlock(pCostFunction, NULL, mypara);}ceres::Solver::Options options;options.linear_solver_type = ceres::DENSE_NORMAL_CHOLESKY;options.minimizer_progress_to_stdout = true;ceres::Solver::Summary summary;ceres::Solve(options, &problem, &summary);cout << mypara[0] << endl << mypara[1] << endl;return 0;
}float getRand()
{return 2.0 * rand() / RAND_MAX - 1.0;
}

ceres Analytic Derivatives相关推荐

ceres解析导数（Analytic Derivatives）进阶
解析导数(Analytic Derivatives)进阶一.问题定义二.代价函数定义三.完整代码四.输出结果五.配置文件六.适用情况 ceres中有三种求导的方式,分别是自动求导.数值 ...
Ceres 库：基础使用，以手写高斯-牛顿法为例
Ceres 库简介 Ceres库为Google开发的开源C++非线性优化库,被广泛使用于求解最小二乘问题. Ceres库的Github主页如下: 安装首先,下载Cere的源码: git clone ...
Automatic differentiation
看算法看到这个了,转载一下,备忘. From Wikipedia, the free encyclopedia Jump to: navigation, search In mathematics a ...
Optimization Options Reference
Optimization Options 下表介绍了优化选项. 使用optimoptions函数或fminbnd,fminsearch,fzero或lsqnonneg的optimset创建选项. 有关 ...
Ceres Solver: 高效的非线性优化库（二）实战篇
Ceres Solver: 高效的非线性优化库(二)实战篇接上篇: Ceres Solver: 高效的非线性优化库(一) 如何求导 Ceres Solver提供了一种自动求导的方案,上一篇我们已经看 ...
【slam十四讲第二版】【课本例题代码向】【第九讲~后端Ⅰ】【安装Meshlab】【BAL数据集格式】【ceres求解BA】【g2o求解BA】
[slam十四讲第二版][课本例题代码向][第九讲~后端Ⅰ][安装Meshlab][BAL数据集格式][ceres求解BA][g2o求解BA] 0 前言 1 安装Meshlab: 三维几何网格处理 2 ...
SLAM14讲第七讲习题10:Ceres实现ICP优化
很好的学习ceres的习题,难度很低,容易入手. ceres结构体构造: struct ICPCeres {/*** @brief Construct a new ICPCeres object* * ...
Ceres库，从入门到放弃
前言首先当然是安装了,安装十分简单. 官网文件有很多人翻译了,例如:https://blog.csdn.net/wzheng92/article/details/79634069 使用Ceres库主 ...
Ceres入门——Ceres的基本使用方法
Ceres入门--Ceres的基本使用方法 1.使用流程 2.实例分析--HelloWorld 2.1 构建代价函数(cost function) 2.2 构建寻优问题 2.3 配置并运行求解器 2. ...

ceres Analytic Derivatives

ceres Analytic Derivatives

ceres Analytic Derivatives相关推荐

最新文章

热门文章