什么时候需要分析求导雅克比？
总结了下主要考虑下面方面
（1）需要考虑求解效率的时候，有时采用代数求导速度会快于自动求导
下面将ceres官方文档http://ceres-solver.org/analytical_derivatives.html中的例子完整实现一下

1.问题描述

假设下面曲线
y = b 1 ( 1 + e b 2 − b 3 x ) 1 / b 4 + w y = \frac{b_1}{(1+e^{b_2-b_3x})^{1/b_4}}+w y=(1+eb2−b3x)1/b4b1+w
其中a,b为曲线的参数，w为高斯噪声。假设我们有N个关于x,y的观测数据点，想根据这些数据点求出曲线的参数。那么可以求解下面的最小二乘问题。
min ⁡ a , b , c 1 2 ∑ i = 1 N ∥ b 1 ( 1 + e b 2 − b 3 x i ) 1 / b 4 − y i ) ∥ 2 \min_{a,b,c}\frac{1}{2}\sum_{i=1}^{N}\left \| \frac{b_1}{(1+e^{b_2-b_3x_i})^{1/b_4}}-y_i) \right \|^{2} a,b,cmin21i=1∑N∥∥∥∥(1+eb2−b3xi)1/b4b1−yi)∥∥∥∥2

2.自动推导代码

#include "ceres/ceres.h"
#include <opencv2/core/core.hpp>
#include <chrono>
using namespace std;
using namespace ceres;
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;struct CostFunctor
{CostFunctor( double x, double y ) : _x ( x ), _y ( y ) {}template <typename T> bool operator()(const T* const bb,      //模型参数，有4维T* residual) const      //残差{residual[0] = bb[0]/pow(1.0+exp(bb[1]-bb[2]*T(_x)),1.0/bb[3])-T(_y);return true;}const double _x, _y;    // x,y数据
};int main(int argc, char** argv)
{double b1=1,b2=2,b3=3,b4=4;         // 真实参数值int N=100;                          // 数据点double w_sigma=0.001;                 // 噪声Sigma值cv::RNG rng;                        // OpenCV随机数产生器double bEst[4] = {1,1,1,1};            // b参数的估计值vector<double> x_data, y_data;      // 数据cout<<"generating data: "<<endl;for ( int i=0; i<N; i++ ){double x = i/100.0;x_data.push_back ( x );y_data.push_back (b1/pow(1.0+exp(b2-b3*x),1.0/b4)+ rng.gaussian ( w_sigma ));cout<<x_data[i]<<" "<<y_data[i]<<endl;}// Build the problem.Problem problem;for ( int i=0; i<N; i++ ){CostFunction* cost_function =new AutoDiffCostFunction<CostFunctor, 1, 4>(new CostFunctor( x_data[i], y_data[i] ));problem.AddResidualBlock(cost_function, NULL, bEst);}// Run the solver!Solver::Options options;options.minimizer_progress_to_stdout = true;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<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;std::cout << summary.BriefReport() << "\n";std::cout << "output b1: " << bEst[0] << "\n";std::cout << "output b2: " << bEst[1] << "\n";std::cout << "output b3: " << bEst[2] << "\n";std::cout << "output b4: " << bEst[3] << "\n";return 0;
}

运行完成后结果

solve time cost = 0.00308766 seconds.
Ceres Solver Report: Iterations: 33, Initial cost: 7.821481e+00, Final cost: 5.100971e-05, Termination: CONVERGENCE
output b1: 1.0014
output b2: 1.98773
output b3: 2.98419
output b4: 3.96682

3.分析求导雅克比代码

曲线y分别对b1,b2,b3,b4求导如下

#include "ceres/ceres.h"
#include <opencv2/core/core.hpp>
#include <chrono>
using namespace std;
using namespace ceres;
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;
class Rat43Analytic : public SizedCostFunction<1,4> {public:Rat43Analytic(const double x, const double y) : x_(x), y_(y) {}virtual ~Rat43Analytic() {}virtual bool Evaluate(double const* const* parameters,double* residuals,double** jacobians) const {const double b1 = parameters[0][0];const double b2 = parameters[0][1];const double b3 = parameters[0][2];const double b4 = parameters[0][3];residuals[0] = b1 *  pow(1 + exp(b2 -  b3 * x_), -1.0 / b4) - y_;if (!jacobians) return true;double* jacobian = jacobians[0];if (!jacobian) return true;jacobian[0] = pow(1 + exp(b2 - b3 * x_), -1.0 / b4);jacobian[1] = -b1 * exp(b2 - b3 * x_) *pow(1 + exp(b2 - b3 * x_), -1.0 / b4 - 1) / b4;jacobian[2] = x_ * b1 * exp(b2 - b3 * x_) *pow(1 + exp(b2 - b3 * x_), -1.0 / b4 - 1) / b4;jacobian[3] = b1 * log(1 + exp(b2 - b3 * x_)) *pow(1 + exp(b2 - b3 * x_), -1.0 / b4) / (b4 * b4);return true;}private:const double x_;const double y_;};int main(int argc, char** argv)
{double b1=1,b2=2,b3=3,b4=4;         // 真实参数值int N=100;                          // 数据点double w_sigma=0.001;                 // 噪声Sigma值cv::RNG rng;                        // OpenCV随机数产生器double bEst[4] = {1,1,1,1};            // b参数的估计值vector<double> x_data, y_data;      // 数据cout<<"generating data: "<<endl;for ( int i=0; i<N; i++ ){double x = i/100.0;x_data.push_back ( x );y_data.push_back (b1/pow(1.0+exp(b2-b3*x),1.0/b4)+ rng.gaussian ( w_sigma ));cout<<x_data[i]<<" "<<y_data[i]<<endl;}// Build the problem.Problem problem;for ( int i=0; i<N; i++ ){CostFunction* cost_function =new Rat43Analytic( x_data[i], y_data[i]);problem.AddResidualBlock(cost_function, NULL, bEst);}Solver::Options options;//  options.minimizer_progress_to_stdout = true;// Run the solver!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<double> time_used = chrono::duration_cast<chrono::duration<double>>( t2-t1 );cout<<"solve time cost = "<<time_used.count()<<" seconds. "<<endl;std::cout << summary.BriefReport() << "\n";std::cout << "output b1: " << bEst[0] << "\n";std::cout << "output b2: " << bEst[1] << "\n";std::cout << "output b3: " << bEst[2] << "\n";std::cout << "output b4: " << bEst[3] << "\n";return 0;
}

结果如下

solve time cost = 0.00466533 seconds.
Ceres Solver Report: Iterations: 33, Initial cost: 7.821481e+00, Final cost: 5.100971e-05, Termination: CONVERGENCE
output b1: 1.0014
output b2: 1.98773
output b3: 2.98419
output b4: 3.96682

ceres非线性优化(分析推导雅克比矩阵例子)相关推荐

VINS-Mono之后端非线性优化 (目标函数中视觉残差和IMU残差，及其对状态量的雅克比矩阵、协方差递推方程的推导)
文章目录 1. 前言 2. 非线性最小二乘 2.1 Guass-Newton 和 Levenberg-Marquardt 2.2 鲁棒核函数下状态量增量方程的构建 3. 局部Bundle Adjust ...
VINS-Mono之IMU预积分，预积分误差、协方差及误差对状态量雅克比矩阵的递推方程的推导
文章目录 1. 前言 2. IMU模型 3. 基于世界坐标系下的IMU运动模型 3.1 连续形式下的IMU运动模型 3.2 离散形式下的IMU运动模型 3.2.1 欧拉法离散形式 3.2.2 中值法离 ...
激光SLAM后端优化——雅克比矩阵推导
激光SLAM后端优化--雅克比矩阵推导 Jacobi Matrix Jacobi Matrix In the EKF system, the maintained state quantities i ...
机械臂的雅克比矩阵推导
1. 线速度和角速度的递推通式推导 p i = p i − 1 + R i − 1 r i − 1 , i i − 1 \mathbf{p}_{i}=\mathbf{p}_{i-1}+\mathbf{ ...
机械臂中的四元素转为旋转矩阵_雅克比矩阵（上）雅克比推导
1.前言回顾前面几期的内容,在第一期中介绍了机器人的正/逆运动学建模,正运动学解决的问题是如何从关节空间的关节变量描述操作空间的位姿,反之则是逆运动学的内容.将操作空间和关节的空间的关系用以下关系式 ...
雅克比矩阵（上）-----雅克比推导
1.前言回顾前面几期的内容,在第一期中介绍了机器人的正/逆运动学建模,正运动学解决的问题是如何从关节空间的关节变量描述操作空间的位姿,反之则是逆运动学的内容.将操作空间和关节的空间的关系用以下关系式 ...
雅克比矩阵（Jacobian Matrix）在正运动学中的应用
转载地址:http://www.360doc.com/content/17/0828/17/1489589_682803176.shtml 说到逆运动学(IK),其中最重要的一部分就是利用雅克比矩阵表 ...
MATLAB机器人工具箱【1】——建模+正逆运动学+雅克比矩阵
MATLAB机器人工具箱[1]-- 机械臂建模+正逆运动学+雅克比矩阵 1. 二维空间位姿描述 2. 三维空间位姿描述 3. 建立机器人模型 3.1 Link 类 3.2 SerialLink 类 3 ...
机器人的雅克比矩阵、海森矩阵、可操作度雅克比矩阵
1.前言: 本文简单记录下关于机器人的Jacobian矩阵-,Hessian矩阵[在运动学逆解,reactive motion controllers,基于模型的控制设计中常常使用jacobian m ...

ceres非线性优化(分析推导雅克比矩阵例子)

1.问题描述

2.自动推导代码

3.分析求导雅克比代码

ceres非线性优化(分析推导雅克比矩阵例子)相关推荐

最新文章

热门文章