Ceres库与最小二乘优化

#include<iostream>

using namespace std;

//结构体定义结束后别忘了加封号
struct myStruct
{
  double x_,y_;

  //结构体的构造函数，传入x、y
  myStruct(double x,double y)
  {
    x_=x;
    y_=y;
  }
};


int main()
{
  //创建一个结构体的时候要写成指针形式，否则会报错
  myStruct* s1 = new myStruct(3.4,6.1);
  //注意指针形式的调用成员变量是箭头，不是点
  cout<<s1->x_<<endl;
  cout<<s1->y_<<endl;
  return 0;
}

#include<iostream>

using namespace std;

//模板函数定义的标准开头
template <typename T>
T add(T a,T b)
{
  T sum = a+b;
  return sum;
}


int main()
{
  //注意模板函数调用的方式，需要在函数名后面用尖括号制定模板类型
  double sum1 = add<double>(3.5,1.1);
  double sum2 = add<int>(5,4);
  string sum3 = add<string>("a","b");
  cout<<"Typename double "<<sum1<<endl;
  cout<<"Typename int "<<sum2<<endl;
  cout<<"Typename string "<<sum3<<endl;
  return 0;
}

  int b = 500;  
  const int* a = &b;         [1]  
  int const *a = &b;         [2]  
  int* const a = &b;         [3]  
  const int* const a = &b;   [4]  

void fun() const
{
	//your code here...
};

<函数类型> operator <运算符>(<参数表>)
{
	<函数体>
} 

<对象名>.operator <运算符>(<参数>)
它等价于
<对象名><运算符><参数>

#include<iostream>

using namespace std;

struct MyStruct
{
  //声明一个结构体的成员变量
  double x;
  
  //结构体构造函数
  MyStruct(double x_)
  {
    x = x_;
  }
  
  //在结构体中重载+运算符
  //注意+左边的数字不需要显式指出，默认是在结构体中的成员变量
  double operator+(double y)
  {
    return x*2+y*2;
  }
};

int main()
{
  //新建一个结构体，并以数字3初始化
  MyStruct* my = new MyStruct(3);
  //注意重载运算符的使用方式，这里my是结构体指针，因此使用箭头使用成员函数，不是点
  double res = my->operator+(4);
  //也可以利用星号将指针变成对象，这样就可以使用第二种调用方式了
  double res2 = *my+6;
  cout<<res<<endl;
  cout<<res2<<endl;
  return 0;
}

#include<iostream>

using namespace std;

struct Functor
{
    //在结构体中重载括号运算符，构造仿函数
    bool operator()(double x, double y)
    {
      return x > y;
    }
};

int main()
{
  Functor* fun = new Functor();
  //第一种使用方法，和重载运算符使用方法一样
  cout<<fun->operator()(4.9,6.9)<<endl;
  //第二种使用方法，建立一个临时变量
  cout<<Functor()(6,4)<<endl;
  return 0;
}

#include<iostream>

using namespace std;

struct Functor
{
    //成员变量
    double bias;
    
    //构造函数
    Functor(double b)
    {
      bias = b;
    }
  
    //模板仿函数
    template <typename T>
    bool operator()(const T &x, T &y) const
    {
      y = x + bias;
      return true;
    }
};

int main()
{
  Functor* fun = new Functor(1.2);
  double x=3.0,y;
  fun->operator()<double>(x,y);
  cout<<y<<endl;
  return 0;
}

# CMake
sudo apt-get install cmake
# google-glog + gflags
sudo apt-get install libgoogle-glog-dev libgtest-dev libgflags-dev
# BLAS & LAPACK
sudo apt-get install libatlas-base-dev liblapack-dev
# Eigen3
sudo apt-get install libeigen3-dev
# SuiteSparse and CXSparse (optional)
sudo apt-get install libsuitesparse-dev libcxsparse3.1.2

mkdir build
cd build
cmake ..
make -j3
make install

cmake_minimum_required(VERSION 2.6)
project(ceres_test)

set( CMAKE_CXX_FLAGS "-std=c++11 -O3" )

# 添加cmake模块以使用ceres库
list( APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake_modules )

# 寻找Ceres库并添加它的头文件
find_package( Ceres REQUIRED )
include_directories( ${CERES_INCLUDE_DIRS} )

# OpenCV
find_package( OpenCV REQUIRED )
include_directories( ${OpenCV_DIRS} )

add_executable(ceres_test main.cpp)

# 与Ceres和OpenCV链接
target_link_libraries( ceres_test ${CERES_LIBRARIES} ${OpenCV_LIBS} )

install(TARGETS ceres_test RUNTIME DESTINATION bin)

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

using namespace std;
using namespace cv;
using namespace ceres;

//vector,用于存放x、y的观测数据
//待估计函数为y=3.5x^3+1.6x^2+0.3x+7.8
vector<double> xs;
vector<double> ys;

//定义CostFunctor结构体用于描述代价函数
struct CostFunctor{
  
  double x_guan,y_guan;
  
  //构造函数，用已知的x、y数据对其赋值
  CostFunctor(double x,double y)
  {
    x_guan = x;
    y_guan = y;
  }
  
  //重载括号运算符，两个参数分别是估计的参数和由该参数计算得到的残差
  //注意这里的const，一个都不能省略，否则就会报错
  template <typename T>
  bool operator()(const T* const params,T* residual)const
  {
    residual[0]=y_guan-(params[0]*x_guan*x_guan*x_guan+params[1]*x_guan*x_guan+params[2]*x_guan+params[3]);
    return true;  
  }
};

//生成实验数据
void generateData()
{
  RNG rng;
  double w_sigma = 1.0;
  for(int i=0;i<100;i++)
  {
    double x = i;
    double y = 3.5*x*x*x+1.6*x*x+0.3*x+7.8;
    xs.push_back(x);
    ys.push_back(y+rng.gaussian(w_sigma));
  }
  for(int i=0;i<xs.size();i++)
  {
    cout<<"x:"<<xs[i]<<" y:"<<ys[i]<<endl;
  }
}

//简单描述我们优化的目的就是为了使我们估计参数算出的y'和实际观测的y的差值之和最小
//所以代价函数(CostFunction)就是y'-y，其对应每一组观测值与估计值的残差。
//由于我们优化的是残差之和，因此需要把代价函数全部加起来，使这个函数最小，而不是单独的使某一个残差最小
//默认情况下，我们认为各组的残差是等权的，也就是核函数系数为1。
//但有时可能会出现粗差等情况，有可能不等权，但这里不考虑。
//这个求和以后的函数便是我们优化的目标函数
//通过不断调整我们的参数值，使这个目标函数最终达到最小，即认为优化完成
int main(int argc, char **argv) {
  
  generateData();
  
  //创建一个长度为4的double数组用于存放参数
  double params[4]={1.0};

  //第一步，创建Problem对象，并对每一组观测数据添加ResidualBlock
  //由于每一组观测点都会得到一个残差，而我们的目的是最小化所有残差的和
  //所以采用for循环依次把每个残差都添加进来
  Problem problem;
  for(int i=0;i<xs.size();i++)
  {
    //利用我们之前写的结构体、仿函数，创建代价函数对象，注意初始化的方式
    //尖括号中的参数分别为误差类型，输出维度(因变量个数)，输入维度(待估计参数的个数)
    CostFunction* cost_function = new AutoDiffCostFunction<CostFunctor,1,4>(new CostFunctor(xs[i],ys[i]));
    //三个参数分别为代价函数、核函数和待估参数
    problem.AddResidualBlock(cost_function,NULL,params);
  }
  
  //第二步，配置Solver
  Solver::Options options;
  //配置增量方程的解法
  options.linear_solver_type=ceres::DENSE_QR;
  //是否输出到cout
  options.minimizer_progress_to_stdout=true;
  
  //第三步，创建Summary对象用于输出迭代结果
  Solver::Summary summary;
  
  //第四步，执行求解
  Solve(options,&problem,&summary);
  
  //第五步，输出求解结果
  cout<<summary.BriefReport()<<endl;
  
  cout<<"p0:"<<params[0]<<endl;
  cout<<"p1:"<<params[1]<<endl;
  cout<<"p2:"<<params[2]<<endl;
  cout<<"p3:"<<params[3]<<endl;
  return 0;
}