高斯-牛頓法 曲線拟合執行個體

原理

曲線 y=a*x*x + b*x +c;

實際采樣值有噪聲, 建構誤差函數

f(x) = y_gauss - (a*x*x + b*x +c)

(a*x*x + b*x +c)是理論計算值， y_gauss就是實際值

有很多點 F(x) = 1/2

f(x)*f(x)

對f(x)在x附近展開， f(x) = f(x) + f'(x) * delta_x;

帶入F(x), 要求其極小值，對其求導令其為0

(f(x) + f'(x) * delta_x)* (f(x) + f'(x) * delta_x)

這裡的f'(x)對于多元變量來說是雅可比矩陣J

展開 求導得到

JTJ * delta_x = - f(x) * J

求得到 delta_x，

這裡注意，J是對三個未知參數 a b c分别求偏導數得到的一個3維向量Vector3d，不是對x求導

判斷有沒有收斂，即判斷兩次的增量delta_x 之間的差的平方小于門檻值即可

求delta_x 用Eigen 庫裡的H.ldlt.solve(b)即可, delta_x  是三個參數的增量

#include <iostream>
#include <opencv4/opencv2/core/core.hpp>
#include <ceres/ceres.h>
#include "eigen3/Eigen/Core"
#include "eigen3/Eigen/Dense"
#include <gflags/gflags.h>

using namespace std;


DEFINE_double(a, 5, " parm" );
DEFINE_double(b, 6, " parm" );
DEFINE_double(c, 7, " parm" );
int main(int argc, char** argv)
{
   google::ParseCommandLineFlags(&argc, &argv, true);//gflags

  double a=1.0, b=2.0, c=1.0;
  int N=100;
  double w_sigma = 1.0;
  cv::RNG rng;

  

  std::cout<<"run like this  ./ceres_cure_fitting -a 2 -b 3 -c 2"<<std::endl;

  double abc[3] = {FLAGS_a , FLAGS_b , FLAGS_c};
  std::vector<double> x_data, y_data;

 
  
  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*w_sigma));
        
        //printf("rng.gaussian(w_sigma*w_sigma) is %f\n", rng.gaussian(w_sigma*w_sigma));

        //CURE_FITTING_COST* cure_fit_cost = new ceres::AutoDiffCostFunction<CURE_FITTING_COST, 1, 3>(new CURE_FITTING_COST(x_data[i], y_data[i]));
    
        // problem.AddResidualBlock(cure_fit_cost, nullptr, abc);
        // cout<<x_data[i]<<" "<<y_data[i]<<endl;
  }
 
  
 
  
  int iteration = 100;
  double lastcost = 0;
  double last_abc[3]={0,0,0};
 
  for(int i=0; i < iteration; i++)
  {
      double cost =0;
       
       double error=0;
      Eigen::Vector3d bb = Eigen::Vector3d::Zero();
      Eigen::Matrix3d H = Eigen::Matrix3d::Zero();
      //printf("error %lf", error);
      for(int j=0; j < N; j++)
      {

        error = y_data[j] - exp(abc[0] * x_data[j] * x_data[j] + abc[1] * x_data[j] + abc[2] );

        Eigen::Vector3d Jac;
        Jac(0)=  -x_data[j] * x_data[j] * exp(abc[0] * x_data[j] * x_data[j] + abc[1] * x_data[j] + abc[2]);
        Jac(1) = -x_data[j] * exp(abc[0] * x_data[j] * x_data[j] + abc[1] * x_data[j] + abc[2]);
        Jac(2) = -exp(abc[0] * x_data[j] * x_data[j] + abc[1] * x_data[j] + abc[2]);
    
      
        H += Jac * Jac.transpose();

        
        bb += -error * Jac;

        cost += error* error;
        
      }
     // printf("cost is %f \n", cost);
      Eigen::Vector3d delta_abc;
      delta_abc = H.ldlt().solve(bb);

     // printf("delta_abc is %f  %f  %f\n", delta_abc[0], delta_abc[1], delta_abc[2]);

     // if(abs(cost - lastcost) < 0.01)break;//convergence 


      
      abc[0] += delta_abc(0);
      abc[1] += delta_abc(1);
      abc[2] += delta_abc(2);
      lastcost = cost;
      
    //判斷是否收斂 兩次的增量小于一定門檻值
     if( (pow((abc[0] - last_abc[0]), 2) + 
     pow((abc[1] - last_abc[1]), 2) + 
     pow((abc[2] - last_abc[2]), 2) ) < 0.0001) 
     {
    
         break;
     }
     


      last_abc[0] = abc[0];
      last_abc[1] = abc[1];
      last_abc[2] = abc[2];

      std::cout<<"abc is "<<abc[0]<<" "<<abc[1]<<" "<<abc[2]<<std::endl;

     
  }
  

 
   std::cout<<"abc is "<<abc[0]<<" "<<abc[1]<<" "<<abc[2]<<std::endl;
   return 0;
 




}