《视觉slam十四讲》初学小白笔记（6）

 看了两天的公式，头都大了。什么状态估计，最小二乘（不得不感叹一句，数学好差啊~还好有个牛皮的师兄给我答疑解惑）

今天看着例程敲了一下Ceres的代码，基本是个固定模板https://blog.csdn.net/MyArrow/article/details/82906738

不能深究，不然发现自己又有一堆一堆不懂的东西，可怕~

慢慢学，之后再来填坑~

#include<iostream>
#include<opencv2/core/core.hpp>
#include<ceres/ceres.h>
#include<chrono> //计时

using namespace std;

/*
 * 第一步：定义残差函数fi：定义一个代价函数的结构体(struct)，在结构体内重载（）运算符
 * 第二步：创建一个CostFunction（把fi作为其参数）
 * 第三步：增加一个残差块（Problem.AddResidualBlock）
 * 第四步：问题求解（Problem.Solve）
 * 
 */


//代价函数的计算模型
struct CURVE_FITTING_COST{
  CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}	//struct的用法
  //残差的计算
  template <typename T>		//固定模板，T是一个类型
  bool operator()(const T* const abc,	//参数模型，3维
		  T* residual) const	//残差
    {
      
      //y-exp(ax^2+bx+c)
      residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
      return true;      
    }
    const double _x,_y;
};

int main(int agrc,char** agrv){
  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<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(exp(a*x*x+b*x+c)+rng.gaussian(w_sigma));	//加上高斯噪声
    cout<<x_data[i]<<" "<<y_data[i]<<endl;  
  }
  
  //构建最小二乘问题（ceres的核心问题，还不是很懂）
  ceres::Problem problem;
  for(int i=0;i<N;i++){
    //AddResidualBlock()顾名思义主要用于向Problem类传递残差模块的信息，函数原型如下，
    //传递的参数主要包括代价函数模块、损失函数模块和参数模块。
    problem.AddResidualBlock(		
      //自动求导的代价函数AutoDiffCostFunction,模板参数：误差类型、输入维度(x)、输出维度(y)
      new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>( 
	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;	//输出到cout
  
  ceres::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 over="<<time_used.count()<<" s"<<endl;
  
  cout<<summary.BriefReport()<<endl;
  cout<<"estimate a,b,c";
  for(auto a:abc) cout<<a<<" ";
  cout<<endl;
  
  return 0;
}