Ceres-Solver使用指南

使用Ceres求解非线性优化问题一般包含如下步骤：

1. 构建cost function
2. 构建待求解的优化问题
3. 配置求解器参数并进行求解

求解 1 2 ( 10 − x 2 ) frac{1}{2}(10-x^2) 21​(10−x2)的根：

#include 
#include 


class CostFunc{
public:
    CostFunc()=default;
    template
    bool operator()(const T* x, T* residual) const{
        *residual = 0.5 * pow(T(10.0) - *x, 2);
        return true;
    }
};

int main(int argc, char** argv){
    
    ceres::Problem problem;
    double x = 9.0; // 待优化参数，初始为9
    // 使用自动求导, <代价函数, 残差维度, 待优化参数的维度>
    ceres::CostFunction* cost_function 
        = new ceres::AutoDiffCostFunction(new CostFunc);
    problem.AddResidualBlock(   cost_function,  // 代价函数
                             	nullptr,        // 核函数
                             	&x              // 待优化参数
                            );
	
    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 << summary.BriefReport() << std::endl;
    std::cout << "result: " << x << std::endl;
}

结果为 x = 9.99951 x = 9.99951 x=9.99951

以 y = 3 ⋅ s i n ( x ) + 2 ⋅ c o s ( x ) + 1 y=3·sin(x)+2·cos(x)+1 y=3⋅sin(x)+2⋅cos(x)+1为真实值生成观测数据，如下图所示，原始数据见附录。

使用最小二乘法求解，令观测模型为：
y = a ⋅ s i n ( x ) + b ⋅ c o s ( x ) + c y=a·sin(x)+b·cos(x)+c y=a⋅sin(x)+b⋅cos(x)+c
令观测向量 h i = [ s i n ( x i ) c o s ( x i ) 1 ] T h_i=begin{bmatrix}sin(x_i)&cos(x_i)&1end{bmatrix}^T hi​=[sin(xi​)​cos(xi​)​1​]T，待优化参数 p = [ a b c ] T p=begin{bmatrix}a&b&cend{bmatrix}^T p=[a​b​c​]T，则 y i = h i T p y_i=h_i^Tp yi​=hiT​p。将观测向量 h i h_i hi​按行排列得到观测矩阵：
H = [ h 1 T h 2 T ⋮ h n T ] H=begin{bmatrix}h_1^T\h_2^T\vdots\h_n^Tend{bmatrix} H=⎣⎢⎢⎢⎡​h1T​h2T​⋮hnT​​⎦⎥⎥⎥⎤​
观测方程如下：
Y = [ y 1 y 2 ⋮ y n ] = H p Y=begin{bmatrix}y_1\y_2\vdots\y_nend{bmatrix}=Hp Y=⎣⎢⎢⎢⎡​y1​y2​⋮yn​​⎦⎥⎥⎥⎤​=Hp
两端同时乘以观测矩阵的转置 H T H^T HT，得到：
H T = H T H p H^T=H^THp HT=HTHp
由于矩阵乘以矩阵的转置其结果一定可逆，所以解得：
p = ( H T H ) − 1 H T Y p=(H^TH)^{-1}H^TY p=(HTH)−1HTY
这就是待求解参数的最小二乘解。Matlab求解程序如下：

x = 0 : 0.2 : 10;
x = x';
y_true = 3 * sin(x) + 2 * cos(x) + 1; % 真实值

% 生成噪声
w = wgn(length(x), 1, 10*log(0.4));
% 产生观测值
z = y_true + w;
% 生成观测矩阵
H = [sin(x), cos(x), x.^0];
% 计算最小二乘估计
p = (H' * H)^(-1) * H' * z;
% 计算均方误差
y = H * p;
mse = (y - y_true)' * (y - y_true) / length(y);
fprintf("mse: %fn", mse);
% 可视化
plot(x, y_true)
hold on 
plot(x, z)
hold on 
plot(x, y)
legend("真实值", "观测值", "拟合值")

结果如下，与真实值基本一致。

p=
    3.0207
    1.9253
    0.9424

使用ceres求解，程序如下：

#include 
#include 

#include 
#include 


class CostFunc{
public:
    CostFunc()=default;
    CostFunc(double x, double y) : x_(x), y_(y){ }
    template
    bool operator()(const T* abc, T* residual) const{
        // residual = y - f(x)
        // f(x) = a·sin(x) + b·cos(x) + c
        *residual = T(y_) - (abc[0] * sin(T(x_)) + abc[1] * cos(T( x_)) + abc[2]);
        return true;
    }
private:
    double x_, y_;
};

int main(int argc, char** argv){
    ceres::Problem problem;
    double abc[] = {0, 0, 0}; // 待优化参数，全部初始化为0
    
    std::fstream data_fs("lse_data.txt"); // 从文件读取观测数据
    double x, y; // 观测数据
    // 添加代价项
    for(int i = 0; i < 51; i++){
        data_fs >> x >> y;
        // 使用自动求导, <代价函数, 残差维度, 待优化参数的维度>
        ceres::CostFunction* cost_function 
            = new ceres::AutoDiffCostFunction(new CostFunc(x, y));
        problem.AddResidualBlock(   cost_function,  // 代价函数
                                    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 << summary.BriefReport() << std::endl;
    std::cout << "result: " << abc[0] << " " << abc[1] << " " << abc[2] << std::endl;
}

结果如下：

iter      cost      cost_change  |gradient|   |step|    tr_ratio  tr_radius  ls_iter  iter_time  total_time
   0  2.147215e+02    0.00e+00    8.29e+01   0.00e+00   0.00e+00  1.00e+04        0    2.19e-05    4.89e-05
   1  1.659848e-06    2.15e+02    7.21e-03   3.74e+00   1.00e+00  3.00e+04        1    2.10e-05    9.11e-05
   2  1.511573e-15    1.66e-06    2.15e-07   3.39e-04   1.00e+00  9.00e+04        1    1.60e-05    1.11e-04
Ceres Solver Report: Iterations: 3, Initial cost: 2.147215e+02, Final cost: 1.511573e-15, Termination: CONVERGENCE
result: 3 2 1

曲线拟合观测数据

0                  3
0.200000000000000  3.55614114806767
0.400000000000000  4.01037701493172
0.600000000000000  4.34459865000446
0.800000000000000  4.54548169139290
1                  4.60501756615997
1.20000000000000   4.52083276685503
1.40000000000000   4.29628347576586
1.60000000000000   3.94032176452194
1.80000000000000   3.46713870324841
2                  2.89559860738276
2.20000000000000   2.24848697694808
2.40000000000000   1.55160211057096
2.60000000000000   0.832726608726498
2.80000000000000   0.120519769130397
3                  -0.556624969021289
3.20000000000000   -1.17171198187225
3.40000000000000   -1.70021969123942
3.60000000000000   -2.12107816255285
3.80000000000000   -2.41750909665699
4                  -2.57769472765101
4.20000000000000   -2.59524895992216
4.40000000000000   -2.46947196162539
4.60000000000000   -2.20537806477050
4.80000000000000   -1.81349585962863
5                  -1.30944845306296
5.20000000000000   -0.713330624559708
5.40000000000000   -0.0489077107826954
5.60000000000000   0.657331843403535
5.80000000000000   1.37723249564137
6                  2.08209407870395
6.20000000000000   2.74381598559394
6.40000000000000   3.33601745206787
6.60000000000000   3.83508927445719
6.80000000000000   4.22113503411548
7                  4.47876430484298
7.20000000000000   4.59770622061197
7.40000000000000   4.57321894258366
7.60000000000000   4.40627870125897
7.80000000000000   4.10354087724912
8                  3.67707467225292
8.20000000000000   3.14388194807165
8.40000000000000   2.52521941603147
8.60000000000000   1.84575119898232
8.80000000000000   1.13256555055197
9                  0.414094931955916
9.20000000000000   -0.281017500507584
9.40000000000000   -0.925059807710340
9.60000000000000   -1.49235605525719
9.80000000000000   -1.96028993196529
10                 -2.31020639082101