从零开始学SLAM: Ceres/G2O求解优化问题

Ceres求解优化问题

从《视觉SLAM十四讲》和ceres的tutorial开始学起，同时复习一下C++中的语法。

struct CURVE_FITTING_COST
{
        CURVE_FITTING_COST ( double x,double y): _x(x),_y(y) {}
        template <typename T>
        bool operator()(
                        const T* const abc,
                        T* residual
                        )const
        {
                residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
                return true;
        }
        const double _x,_y;
}

要使用Ceres，首先要定义求解问题的cost function<CURVE_FITTING_COST>。
在结构体中首先构造构造函数：

CURVE_FITTING_COST ( double x,double y): _x(x),_y(y) {} //结构体的构造函数初始化列表
//相当于CURVE_FITTING_COST ( double x,double y) {_x=x; _y=y}

接下来，定义模板template

 template <typename T>

template的使用是为了简化不同类型的函数和类的重复定义，模版实例化时可以替换任意类型，不仅包括内置类型（int等），也包括自定义类型class。编译器在实例化之后才知道的数据的类型。

bool operator()( const T* const abc,  T* residual )const
        {
                residual[0]=T(_y)-ceres::exp(abc[0]*T(_x)*T(_x)+abc[1]*T(_x)+abc[2]);
                return true;
        }

重载（）符号，仿函数的小技巧，使结构体的一个实例具有类似一个函数的性质，在代码编写过程中能当做一个函数一样来使用。
对结构体、类的一个实例，比如Myclass类的一个实例Obj1，如果Myclass里对（）进行了重载，那Obj1被创建之后，就可以将Obj1这个实例当做函数来用，比如Obj（x）。在这里CURVE_FITTING_COST（）中共传入了两个变量，包括3维参数向量abc以及残差residual。

int main ( int argc, char** argv )
{
    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;
    }

    // 构建最小二乘