// ceres_solver_test.cpp : 此文件包含 "main" 函数。程序执行将在此处开始并结束。
//

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

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

//第一部分：构建代价函数，重载（）符号，仿函数的小技巧
struct CostFunctor {
    template <typename T>
    bool operator()(const T* const x, T* residual) const {
        residual[0] = T(10.0) - x[0];
        return true;
    }
};

//构建代价函数结构体，abc为待优化参数，residual为残差。
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] = _y - ceres::exp(abc[0] * _x * _x + abc[1] * _x + abc[2]);
        return true;
    }
    const double _x, _y;
};

//构建代价函数结构体，abc为待优化参数，residual为残差。
struct SIN_FITTING_COST
{
    SIN_FITTING_COST(double x, double y) :_x(x), _y(y) {}
    template <typename T>
    bool operator()(const T* const abc, T* residual)const
    {
        residual[0] = _y - (abc[0] + abc[1] * ceres::sin(_x + abc[2]));
        return true;
    }
    const double _x, _y;
};


//主函数
int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);

#if 0
    // 1/2*(10-x)^2
    // 寻优参数x的初始值，为5
    double initial_x = 5.0;
    double x = initial_x;

    // 第二部分：构建寻优问题
    Problem problem;
    CostFunction* cost_function =
        new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor); //使用自动求导，将之前的代价函数结构体传入，第一个1是输出维度，即残差的维度，第二个1是输入维度，即待寻优参数x的维度。
    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";
#endif

#if 0
    // y = e^(a * x * x + b * x + c)
    //参数初始化设置，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(std::exp(double(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>(
                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;
    ceres::Solver::Summary summary;
    ceres::Solve(options, &problem, &summary);
    cout << "a= " << abc[0] << endl;
    cout << "b= " << abc[1] << endl;
    cout << "c= " << abc[2] << endl;
    
#endif

#if 1
    // y = a+b*sin(x+c)
    //参数初始化设置，abc初始化为0，白噪声方差为1（使用OpenCV的随机数产生器）。
    double a = 0.5, b = 0.4, c = CV_PI/6;
    double w = 0.01;
    RNG rng;
    double abc[3] = { 0,0,0 };

    //生成待拟合曲线的数据散点，储存在Vector里，x_data，y_data。
    vector<double> x_data, y_data;
    double step = (2 * CV_PI) / 200;
    for (int i = 0; i < 200; i++)
    {
        double x = i * step;
        x_data.push_back(x);
        y_data.push_back(double(a + b * std::sin(x+c)) + rng.gaussian(w));
    }

    //反复使用AddResidualBlock方法（逐个散点，反复1000次）
    //将每个点的残差累计求和构建最小二乘优化式
    //不使用核函数，待优化参数是abc
    ceres::Problem problem;
    for (int i = 0; i < 200; i++)
    {
        problem.AddResidualBlock(
            new ceres::AutoDiffCostFunction<SIN_FITTING_COST, 1, 3>(
                new SIN_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);
    std::cout << "a= " << abc[0] << std::endl;
    std::cout << "b= " << abc[1] << std::endl;
    std::cout << "c= " << abc[2] << std::endl;

#endif
    return 0;
}