【C++】ceres solver实践

$$y=2\ast x^2+1$$$$y=a\ast x^2+b$$
拟合曲线求$a,b$

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

using namespace std;
using namespace ceres;

//代价函数计算模型
struct costfunction{
    costfunction(double  x, double y) : _x(x) , _y(y){}
    template<typename T>
    bool operator()(const T* const ab, T *residual) const{
        residual[0] = T(_y) - ( ab[0] * T(_x) * T(_x) + ab[1]);
        return true;
    }
    const double _x,_y;
};

int main(int argc, char** argv){
    double ar = 2.0, br = 1.0;
    double ae = 0, be = 2;
    int N = 100;
    double w_sigma = 0.8;
    cv::RNG rng;

    vector<double> x_data,y_data;
    for(int i = 0; i < N; i++){
        double  x = -5.0 + 0.1 * i;
        x_data.push_back(x);
        double y = ar*x*x + br + rng.gaussian(w_sigma*w_sigma);
        y_data.push_back(y);
    }
    double  ab[2] = {ae,be};

    Problem problem;
    for(int i = 0; i < N; i++){
        problem.AddResidualBlock(new AutoDiffCostFunction<costfunction,1,2>(new costfunction(x_data[i],y_data[i])),
                                 nullptr,
                                 ab);
    }
    Solver::Options options;
    options.linear_solver_type = DENSE_NORMAL_CHOLESKY;
    options.minimizer_progress_to_stdout = true;

    Solver::Summary summary;
    chrono::steady_clock::time_point t1 = chrono::steady_clock::now();
    Solve(options,&problem,&summary);
    chrono::steady_clock::time_point t2 = chrono::steady_clock::now();
    chrono::duration<double> time_used = chrono::duration_cast<chrono::duration<double>>(t2-t1);
    cout<< "solve time cost = " << time_used.count()<< " seconds. "<<endl;

    //输出结果
    cout << summary.BriefReport() <<endl;
    cout << "estimated a,b = ";
    for(auto a:ab) cout<<a<<" ";
    cout<<endl;
    return 0;
}

CMakeLists.txt

cmake_minimum_required(VERSION 3.17)
project(untitled)

set(CMAKE_CXX_FLAGS "-std=c++14")

# 定义自己编译的Ceres的库目录
set(Ceres_LIBS_DIR /usr/local/lib)

# 定义CeresConfig.cmake所在文件目录
set(Ceres_DIR ${Ceres_LIBS_DIR}/cmake/Ceres)

# 找Ceres库
find_package(Ceres REQUIRED)
message(STATUS "Found Ceres: ${CERES_FOUND}")
message(STATUS "    Ceres Include Dirs: ${CERES_INCLUDE_DIRS}")
message(STATUS "    Ceres Libs: ${CERES_LIBRARIES}")

find_package(OpenCV REQUIRED)
# Print some message showing some of them
message(STATUS "OpenCV library status:")
message(STATUS "    version: ${OpenCV_VERSION}")
message(STATUS "    libraries: ${OpenCV_LIBS}")
message(STATUS "    include path: ${OpenCV_INCLUDE_DIRS}")

# Add OpenCV headers location to your include paths
include_directories(${OpenCV_INCLUDE_DIRS})

# 添加包含目录
include_directories(${CERES_INCLUDE_DIRS})

add_executable(${PROJECT_NAME} main.cpp)

# 定义需要的库
# ${CERES_LIBRARIES}只有一项ceres，后面很多库都没有。。。所以需要自己定义需要链接那些库。
set(LIBS ${Ceres_LIBS_DIR}/libceres.a umfpack cxsparse glog gflags gomp
        ccolamd btf klu cholmod lapack blas camd amd pthread)

target_link_libraries(${PROJECT_NAME} ${LIBS} ${OpenCV_LIBS})

结果

/home/rock/CLionProjects/untitled/cmake-build-debug/untitled
iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  2.351639e+04    0.00e+00    2.42e+04   0.00e+00   0.00e+00  1.00e+04        0    8.07e-04    8.51e-04
   1  2.153953e+01    2.35e+04    2.50e+00   2.22e+00   1.00e+00  3.00e+04        1    8.35e-04    1.75e-03
   2  2.153893e+01    6.01e-04    1.94e-04   4.00e-03   1.00e+00  9.00e+04        1    8.07e-04    2.57e-03
solve time cost = 0.00261232 seconds. 
Ceres Solver Report: Iterations: 3, Initial cost: 2.351639e+04, Final cost: 2.153893e+01, Termination: CONVERGENCE
estimated a,b = 2.00218 1.0271 

Process finished with exit code 0