0


优化库——ceres(三)实战案例

实战案例

1.1 CmakeLists.txt配置

cmake_minimum_required(VERSION 2.8)
project(ceres)

find_package(Ceres REQUIRED)
include_directories(${CERES_INCLUDE_DIRS})

add_executable(test test.cpp)
target_link_libraries(test ${CERES_LIBRARIES})

1.2 示例:ceres入门例子

一个简单的求解

      min
     
     
      ⁡
     
    
    
     x
    
   
   
    (
   
   
    10
   
   
    −
   
   
    x
   
   
    
     )
    
    
     2
    
   
  
  
   \min _{x} (10-x)^{2}
  
 
minx​(10−x)2 的优化问题代码如下:
#include<iostream>#include<ceres/ceres.h>usingnamespace std;usingnamespace ceres;//第一部分:构建代价函数,重载()符号,仿函数的小技巧structCostFunctor{template<typenameT>booloperator()(const T*const x, T* residual)const{
     residual[0]=T(10.0)- x[0];returntrue;}};//主函数intmain(int argc,char** argv){
  google::InitGoogleLogging(argv[0]);// 寻优参数x的初始值,为5double initial_x =5.0;double x = initial_x;// 第二部分:构建寻优问题
  Problem problem;//使用自动求导,将之前的代价函数结构体传入,第一个1是输出维度,即残差的维度,第二个1是输入维度,即待寻优参数x的维度。
  CostFunction* cost_function =new AutoDiffCostFunction<CostFunctor,1,1>(new CostFunctor);//向问题中添加误差项,本问题比较简单,添加一个就行。
  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";return0;}

1.3 应用:曲线拟合(使用的是自动求导,不用写雅克比)

以下内容来源与参考:

一文助你Ceres 入门——Ceres Solver新手向全攻略

问题:拟合非线性函数的曲线

     y
    
    
     =
    
    
     
      e
     
     
      
       3
      
      
       
        x
       
       
        2
       
      
      
       +
      
      
       2
      
      
       x
      
      
       +
      
      
       1
      
     
    
   
   
    y=e^{3 x^{2}+2 x+1}
   
  
 y=e3x2+2x+1

整个代码的思路还是先构建代价函数结构体,然后在[0,1]之间均匀生成待拟合曲线的1000个数据点,加上方差为1的白噪声,数据点用两个vector储存(x_data和y_data),然后构建待求解优化问题,最后求解,拟合曲线参数。
(PS. 本段代码中使用OpenCV的随机数产生器,要跑代码的同学可能要先装一下OpenCV)

#include<iostream>#include<opencv2/core/core.hpp>#include<ceres/ceres.h>usingnamespace std;usingnamespace cv;//构建代价函数结构体,abc为待优化参数,residual为残差。structCURVE_FITTING_COST{CURVE_FITTING_COST(double x,double y):_x(x),_y(y){}template<typenameT>booloperator()(const T*const abc,T* residual)const{
    residual[0]=_y-ceres::exp(abc[0]*_x*_x+abc[1]*_x+abc[2]);returntrue;}constdouble _x,_y;};//主函数intmain(){//参数初始化设置,abc初始化为0,白噪声方差为1(使用OpenCV的随机数产生器)。double a=3,b=2,c=1;double w=1;
  RNG rng;double abc[3]={0,0,0};//生成待拟合曲线的数据散点,储存在Vector里,x_data,y_data。
  vector<double> x_data,y_data;for(int i=0;i<1000;i++){double x=i/1000.0;
    x_data.push_back(x);
    y_data.push_back(exp(a*x*x+b*x+c)+rng.gaussian(w));}//反复使用AddResidualBlock方法(逐个散点,反复1000次)//将每个点的残差累计求和构建最小二乘优化式//不使用核函数,待优化参数是abc
  ceres::Problem problem;for(int i=0;i<1000;i++){
    problem.AddResidualBlock(new ceres::AutoDiffCostFunction<CURVE_FITTING_COST,1,3>(newCURVE_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;
  ceres::Solver::Summary summary;
  ceres::Solve(options,&problem,&summary);
  cout<<"a= "<<abc[0]<<endl;
  cout<<"b= "<<abc[1]<<endl;
  cout<<"c= "<<abc[2]<<endl;return0;}}

1.4 应用: 基于李代数的视觉SLAM位姿优化(解析求导)

以下内容来源与参考:

Ceres-Solver 从入门到上手视觉SLAM位姿优化问题

下面以 基于李代数的视觉SLAM位姿优化问题 为例,介绍 Ceres Solver 的使用。

(1)残差(预测值 - 观测值)

     r
    
    
     (
    
    
     ξ
    
    
     )
    
    
     =
    
    
     K
    
    
     exp
    
    
     ⁡
    
    
     
      (
     
     
      
       ξ
      
      
       ∧
      
     
     
      )
     
    
    
     P
    
    
     −
    
    
     u
    
   
   
     r(\xi)=K \exp \left(\xi^{\wedge}\right) P-u 
   
  
 r(ξ)=Kexp(ξ∧)P−u

(2)雅克比矩阵

     J
    
    
     =
    
    
     
      
       ∂
      
      
       r
      
      
       (
      
      
       ξ
      
      
       )
      
     
     
      
       ∂
      
      
       ξ
      
     
    
    
    
     =
    
    
     
      [
     
     
      
       
        
         
          
           
            f
           
           
            x
           
          
          
           
            Z
           
           
            ′
           
          
         
        
       
       
        
         
          0
         
        
       
       
        
         
          
           −
          
          
           
            
             
              X
             
             
              ′
             
            
            
             
              f
             
             
              x
             
            
           
           
            
             Z
            
            
             
              ′
             
             
              2
             
            
           
          
         
        
       
       
        
         
          
           −
          
          
           
            
             
              X
             
             
              ′
             
            
            
             
              Y
             
             
              ′
             
            
            
             
              f
             
             
              x
             
            
           
           
            
             Z
            
            
             
              ′
             
             
              2
             
            
           
          
         
        
       
       
        
         
          
           
            f
           
           
            x
           
          
          
           +
          
          
           
            
             
              X
             
             
              
               ′
              
              
               2
              
             
            
            
             
              f
             
             
              x
             
            
           
           
            
             Z
            
            
             
              ′
             
             
              2
             
            
           
          
         
        
       
       
        
         
          
           −
          
          
           
            
             
              Y
             
             
              ′
             
            
            
             
              f
             
             
              x
             
            
           
           
            
             Z
            
            
             ′
            
           
          
         
        
       
      
      
       
        
         
          0
         
        
       
       
        
         
          
           
            f
           
           
            y
           
          
          
           
            Z
           
           
            ′
           
          
         
        
       
       
        
         
          
           −
          
          
           
            
             
              Y
             
             
              ′
             
            
            
             
              f
             
             
              y
             
            
           
           
            
             Z
            
            
             2
            
           
          
         
        
       
       
        
         
          
           −
          
          
           
            f
           
           
            y
           
          
          
           −
          
          
           
            
             
              Y
             
             
              
               ′
              
              
               2
              
             
            
            
             
              f
             
             
              y
             
            
           
           
            
             Z
            
            
             
              ′
             
             
              2
             
            
           
          
         
        
       
       
        
         
          
           
            
             X
            
            
             ′
            
           
           
            
             Y
            
            
             ′
            
           
           
            
             f
            
            
             y
            
           
          
          
           
            Z
           
           
            
             ′
            
            
             2
            
           
          
         
        
       
       
        
         
          
           
            
             X
            
            
             ′
            
           
           
            
             f
            
            
             y
            
           
          
          
           
            Z
           
           
            ′
           
          
         
        
       
      
     
     
      ]
     
    
    
     ∈
    
    
     
      R
     
     
      
       2
      
      
       ×
      
      
       6
      
     
    
   
   
     J=\frac{\partial r(\xi)}{\partial \xi} \\ =\left[\begin{array}{ccccc} \frac{f_{x}}{Z^{\prime}} & 0 & -\frac{X^{\prime} f_{x}}{Z^{\prime 2}} & -\frac{X^{\prime} Y^{\prime} f_{x}}{Z^{\prime 2}} & f_{x}+\frac{X^{\prime 2} f_{x}}{Z^{\prime 2}} & -\frac{Y^{\prime} f_{x}}{Z^{\prime}} \\ 0 & \frac{f_{y}}{Z^{\prime}} & -\frac{Y^{\prime} f_{y}}{Z^{2}} & -f_{y}-\frac{Y^{\prime 2} f_{y}}{Z^{\prime 2}} & \frac{X^{\prime} Y^{\prime} f_{y}}{Z^{\prime 2}} & \frac{X^{\prime} f_{y}}{Z^{\prime}} \end{array}\right] \in \mathbb{R}^{2 \times 6} 
   
  
 J=∂ξ∂r(ξ)​=[Z′fx​​0​0Z′fy​​​−Z′2X′fx​​−Z2Y′fy​​​−Z′2X′Y′fx​​−fy​−Z′2Y′2fy​​​fx​+Z′2X′2fx​​Z′2X′Y′fy​​​−Z′Y′fx​​Z′X′fy​​​]∈R2×6

(3)核心代码

代价函数的构造:

//应该是重投影误差,这样的话才会有残差2维的,优化变量是6维度的classBAGNCostFunctor:public ceres::SizedCostFunction<2,6>{public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW //对齐//? 二维,三维? 第一个是像素观测值   第二个是准备用来重投影的三维观测值BAGNCostFunctor(Eigen::Vector2d observed_p, Eigen::Vector3d observed_P):observed_p_(observed_p),observed_P_(observed_P){}virtual~BAGNCostFunctor(){}virtualboolEvaluate(//参数 残差  雅克比doubleconst*const* parameters,double*residuals,double**jacobians)const{

        Eigen::Map<const Eigen::Matrix<double,6,1>>T_se3(*parameters);

        Sophus::SE3 T_SE3 = Sophus::SE3::exp(T_se3);

        Eigen::Vector3d Pc = T_SE3 * observed_P_;

        Eigen::Matrix3d K;double fx =520.9, fy =521.0, cx =325.1, cy =249.7;
        K << fx,0, cx,0, fy, cy,0,0,1;//! 计算残差
        Eigen::Vector2d residual =  observed_p_ -(K * Pc).hnormalized();

        residuals[0]= residual[0];
        residuals[1]= residual[1];if(jacobians !=NULL){if(jacobians[0]!=NULL){// 2*6的雅克比矩阵
                Eigen::Map<Eigen::Matrix<double,2,6, Eigen::RowMajor>>J(jacobians[0]);double x = Pc[0];double y = Pc[1];double z = Pc[2];double x2 = x*x;double y2 = y*y;double z2 = z*z;J(0,0)=  fx/z;J(0,1)=0;J(0,2)=-fx*x/z2;J(0,3)=-fx*x*y/z2;J(0,4)=  fx+fx*x2/z2;J(0,5)=-fx*y/z;J(1,0)=0;J(1,1)=  fy/z;J(1,2)=-fy*y/z2;J(1,3)=-fy-fy*y2/z2;J(1,4)=  fy*x*y/z2;J(1,5)=  fy*x/z;}}returntrue;}private:const Eigen::Vector2d observed_p_;const Eigen::Vector3d observed_P_;};

构造优化问题,并求解相机位姿:

Sophus::Vector6d se3;//在当前problem中添加代价函数残差块,损失函数为NULL采用默认的最小二乘误差即L2范数,优化变量为 se3
ceres::Problem problem;for(int i=0; i<n_points;++i){
    ceres::CostFunction *cost_function;
    cost_function =newBAGNCostFunctor(p2d[i], p3d[i]);
    problem.AddResidualBlock(cost_function,NULL, se3.data());}

ceres::Solver::Options options;
options.dynamic_sparsity =true;
options.max_num_iterations =100;//迭代100次
options.sparse_linear_algebra_library_type = ceres::SUITE_SPARSE;
options.minimizer_type = ceres::TRUST_REGION;
options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
options.trust_region_strategy_type = ceres::DOGLEG;
options.minimizer_progress_to_stdout =true;
options.dogleg_type = ceres::SUBSPACE_DOGLEG;

ceres::Solver::Summary summary;
ceres::Solve(options,&problem,&summary);
std::cout << summary.BriefReport()<<"\n";

std::cout <<"estimated pose: \n"<< Sophus::SE3::exp(se3).matrix()<< std::endl;
标签:

本文转载自: https://blog.csdn.net/jdy_lyy/article/details/119361019
版权归原作者 雨luo凡城 所有, 如有侵权,请联系我们删除。

“优化库——ceres(三)实战案例”的评论:

还没有评论