-
Ceres学习笔记002--使用Ceres求解Powell方程
使用Ceres求解Powell方程
1 Powell方程
Powell 方程如下：
F ( x ) = [ f 1 ( x ) , f 2 ( x ) , f 3 ( x ) , f 4 ( x ) ] (1) F(x) = [f_1(x),f_2(x),f_3(x),f_4(x)]\tag{1} F(x)=[f1(x),f2(x),f3(x),f4(x)](1)
其中：
{ f 1 ( x ) = x 1 + 10 ∗ x 2 f 2 ( x ) = 5 ∗ ( x 3 − x 4 ) f 3 ( x ) = ( x 2 − 2 ∗ x 3 ) 2 f 4 ( x ) = 10 ∗ ( x 1 − x 4 ) 2 (2) \left\{\right.\tag{2} ⎩ ⎨ ⎧f1(x)=x1+10∗x2f2(x)=5∗(x3−x4)f3(x)=(x2−2∗x3)2f4(x)=10∗(x1−x4)2(2)f 1 ( x ) = x 1 + 10 ∗ x 2 f 2 ( x ) = 5 ∗ ( x 3 − x 4 ) f 3 ( x ) = ( x 2 − 2 ∗ x 3 ) 2 f 4 ( x ) = 10 ∗ ( x 1 − x 4 ) 2
Powell方程中，含有4个参数，4个残差项，目标是找到参数块 [ x 1 , x 2 , x 3 , x 4 ] [x_1,x_2,x_3,x_4] [x1,x2,x3,x4]，使得下式能够取得最小值（实际上一般是极小值）：
1 2 ∗ ∣ ∣ F ( x ) ∣ ∣ 2 = 1 2 ∗ ( ∣ ∣ f 1 ( x ) ∣ ∣ 2 + ∣ ∣ f 2 ( x ) ∣ ∣ 2 + ∣ ∣ f 3 ( x ) ∣ ∣ 2 + ∣ ∣ f 4 ( x ) ∣ ∣ 2 ) (3) \frac{1}{2} * ||F(x)||^2 = \frac{1}{2} * (||f_1(x)||^2 + ||f_2(x)||^2 + ||f_3(x)||^2 + ||f_4(x)||^2) \tag{3} 21∗∣∣F(x)∣∣2=21∗(∣∣f1(x)∣∣2+∣∣f2(x)∣∣2+∣∣f3(x)∣∣2+∣∣f4(x)∣∣2)(3)2 Ceres使用基本步骤
Ceres求解过程主要有两大步骤，构建最小二乘问题和求解最小二乘问题，具体步骤如下：
2.1 构建最小二乘问题
- 用户自定义残差计算模型，可能存在多个；
- 构建Ceres代价函数（CostFunction），将用户自定义残差计算模型添加至CostFunction，可能存在多个CostFunction，为每个CostFunction添加用户自定义残差计算模型，并指定用户自定义残差计算模型的导数计算方法；
- 构建Ceres问题（Problem），并在Problem中添加残差块（ResidualBlock），可能存在多个ResidualBlock，为每个ResidualBlock指定CostFunction，LossFunction以及参数块（ParameterBlock）。
2.2 求解最小二乘问题
- 配置求解器参数Options，即设置Problem求解方法及参数。例如迭代次数、步长等等；
- 输出日志内容Summary；
- 优化求解Solve。
构建最小二乘问题时的一些思考
- 目标函数有4个维度，可以拆成4个代价函数，每个代价函数包含1项残差；也可以合并成1个代价函数，包含4项残差；
- 有4个参数，可以拆成4个参数块，每个参数块包含1个参数；也可以合并成1个参数块，包含4个参数；
- 计算导数时，可以使用自动微分法、数值微分法、解析解法。
接下来将通过多种方式实现。
3 代码实现
3.1 cost4_x4_auto
- 4个代价函数（CostFunction），每个代价函数的输出残差维度为1；
- 4个参数块（ParametersBlock），每个参数块的输入维度为1
- 导数计算使用自动微分。
完整代码如下：
#include "ceres/ceres.h" #include "gflags/gflags.h" #include "glog/logging.h" // 用户自定义第1个维度的残差计算模型 // 输入2个参数块，每个参数块的维度为1 // 输出1个残差块，残差块的维度为1 struct F1 { templatebool operator()(const T* const x1, const T* const x2, T* residual) const { residual[0] = x1[0] + 10.0 * x2[0]; return true; } }; // 用户自定义第1个维度的残差计算模型 struct F2 { template bool operator()(const T* const x3, const T*const x4, T* residual) const { residual[0] = sqrt(5.0) * (x3[0] - x4[0]); return true; } }; // 用户自定义第1个维度的残差计算模型 struct F3 { template bool operator()(const T* const x2, const T* const x3, T* residual) const { residual[0] = (x2[0] - 2.0 * x3[0]) * (x2[0] - 2.0 * x3[0]); return true; } }; // 用户自定义第1个维度的残差计算模型 struct F4 { template bool operator()(const T* const x1, const T* const x4, T* residual) const { residual[0] = sqrt(10.0) * ((x1[0] - x4[0]) * (x1[0] - x4[0])); return true; } }; int main(int argc, char** argv) { GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); google::InitGoogleLogging(argv[0]); // 设置参数初始值，输入4个参数块，每个参数块的维度为1 double x1 = 3.0; double x2 = -1.0; double x3 = 0.0; double x4 = 1.0; // 构建非线性最小二乘问题 ceres::Problem problem; // 添加残差块，需要依次指定代价函数、损失函数、参数块 // 本例中损失函数为单位函数，每个成本函数需要输入2个参数块 problem.AddResidualBlock(new ceres::AutoDiffCostFunction - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
- 102
结果如下：
Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1 iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 1.075000e+02 0.00e+00 1.55e+02 0.00e+00 0.00e+00 1.00e+04 0 6.16e-04 1.19e-03 1 5.036190e+00 1.02e+02 2.00e+01 2.16e+00 9.53e-01 3.00e+04 1 1.40e-03 2.77e-03 2 3.148168e-01 4.72e+00 2.50e+00 6.23e-01 9.37e-01 9.00e+04 1 7.74e-04 3.67e-03 3 1.967760e-02 2.95e-01 3.13e-01 3.08e-01 9.37e-01 2.70e+05 1 7.62e-04 4.56e-03 4 1.229900e-03 1.84e-02 3.91e-02 1.54e-01 9.37e-01 8.10e+05 1 5.29e-04 5.20e-03 5 7.687123e-05 1.15e-03 4.89e-03 7.69e-02 9.37e-01 2.43e+06 1 5.19e-04 5.82e-03 6 4.804625e-06 7.21e-05 6.11e-04 3.85e-02 9.37e-01 7.29e+06 1 7.59e-04 6.71e-03 7 3.003028e-07 4.50e-06 7.64e-05 1.92e-02 9.37e-01 2.19e+07 1 7.49e-04 7.58e-03 8 1.877006e-08 2.82e-07 9.54e-06 9.62e-03 9.37e-01 6.56e+07 1 7.62e-04 8.46e-03 9 1.173223e-09 1.76e-08 1.19e-06 4.81e-03 9.37e-01 1.97e+08 1 6.04e-04 9.19e-03 10 7.333425e-11 1.10e-09 1.49e-07 2.40e-03 9.37e-01 5.90e+08 1 5.17e-04 9.82e-03 11 4.584044e-12 6.88e-11 1.86e-08 1.20e-03 9.37e-01 1.77e+09 1 5.39e-04 1.05e-02 12 2.865573e-13 4.30e-12 2.33e-09 6.02e-04 9.37e-01 5.31e+09 1 5.59e-04 1.11e-02 13 1.791438e-14 2.69e-13 2.91e-10 3.01e-04 9.37e-01 1.59e+10 1 5.30e-04 1.17e-02 14 1.120029e-15 1.68e-14 3.64e-11 1.51e-04 9.37e-01 4.78e+10 1 9.31e-04 1.28e-02 Solver Summary (v 2.0.0-eigen-(3.3.8)-no_lapack-eigensparse-no_openmp) Original Reduced Parameter blocks 4 4 Parameters 4 4 Residual blocks 4 4 Residuals 4 4 Minimizer TRUST_REGION Dense linear algebra library EIGEN Trust region strategy LEVENBERG_MARQUARDT Given Used Linear solver DENSE_QR DENSE_QR Threads 1 1 Linear solver ordering AUTOMATIC 4 Cost: Initial 1.075000e+02 Final 1.120029e-15 Change 1.075000e+02 Minimizer iterations 15 Successful steps 15 Unsuccessful steps 0 Time (in seconds): Preprocessor 0.000576 Residual only evaluation 0.000475 (14) Jacobian & residual evaluation 0.002427 (15) Linear solver 0.005271 (14) Minimizer 0.013570 Postprocessor 0.000089 Total 0.014236 Termination: CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.642190e-11 <= 1.000000e-10) Initial x1 = 0.000146222, x2 = -1.46222e-05, x3 = 2.40957e-05, x4 = 2.40957e-05- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
根据优化结果，当 x 1 = x 2 = x 3 = x 4 = 0 x_1 = x_2 = x_3 = x_4 = 0 x1=x2=x3=x4=0 时，能够得到最小值。
3.2 cost1_x1_auto
- 1个代价函数（CostFunction），输出残差维度为4；
- 1个参数块（ParametersBlock），输入参数维度为4；
- 导数计算采用自动微分。
完整代码如下：
#include "ceres/ceres.h" #include "gflags/gflags.h" #include "glog/logging.h" // 用户自定义残差计算模型 // 输入1个参数块，参数块维度为4 // 输出1个残差块，残差块维度为4 struct F1234 { templatebool operator()(const T* const x, T* residual) const { residual[0] = x[0] + 10.0 * x[1]; residual[1] = sqrt(5.0) * (x[2] - x[3]); residual[2] = (x[1] - 2.0 * x[2]) * (x[1] - 2.0 * x[2]); residual[3] = sqrt(10.0) * (x[0] - x[3]) * (x[0] - x[3]); return true; } }; int main(int argc, char** argv) { GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); google::InitGoogleLogging(argv[0]); // 设置参数初始值，输入1个参数块，参数块的维度为4 double x1234[4] = { 3, -1, 0, 1 }; // 构建非线性最小二乘问题 ceres::Problem problem; // 添加残差块，需要依次指定代价函数、损失函数、参数块 // 本例中损失函数为单位函数，代价函数需要输入1个参数块 problem.AddResidualBlock(new ceres::AutoDiffCostFunction - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
结果如下：
Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1 iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 1.075000e+02 0.00e+00 1.55e+02 0.00e+00 0.00e+00 1.00e+04 0 1.01e-03 1.54e-03 1 5.036190e+00 1.02e+02 2.00e+01 2.16e+00 9.53e-01 3.00e+04 1 1.61e-03 3.42e-03 2 3.148168e-01 4.72e+00 2.50e+00 6.23e-01 9.37e-01 9.00e+04 1 6.19e-04 4.22e-03 3 1.967760e-02 2.95e-01 3.13e-01 3.08e-01 9.37e-01 2.70e+05 1 7.00e-04 5.03e-03 4 1.229900e-03 1.84e-02 3.91e-02 1.54e-01 9.37e-01 8.10e+05 1 5.11e-04 5.69e-03 5 7.687123e-05 1.15e-03 4.89e-03 7.69e-02 9.37e-01 2.43e+06 1 4.93e-04 6.29e-03 6 4.804625e-06 7.21e-05 6.11e-04 3.85e-02 9.37e-01 7.29e+06 1 4.78e-04 6.88e-03 7 3.003028e-07 4.50e-06 7.64e-05 1.92e-02 9.37e-01 2.19e+07 1 5.01e-04 7.49e-03 8 1.877006e-08 2.82e-07 9.54e-06 9.62e-03 9.37e-01 6.56e+07 1 7.43e-04 8.35e-03 9 1.173223e-09 1.76e-08 1.19e-06 4.81e-03 9.37e-01 1.97e+08 1 7.21e-04 9.21e-03 10 7.333425e-11 1.10e-09 1.49e-07 2.40e-03 9.37e-01 5.90e+08 1 7.08e-04 1.00e-02 11 4.584044e-12 6.88e-11 1.86e-08 1.20e-03 9.37e-01 1.77e+09 1 5.00e-04 1.07e-02 12 2.865573e-13 4.30e-12 2.33e-09 6.02e-04 9.37e-01 5.31e+09 1 4.85e-04 1.13e-02 13 1.791438e-14 2.69e-13 2.91e-10 3.01e-04 9.37e-01 1.59e+10 1 4.82e-04 1.19e-02 14 1.120029e-15 1.68e-14 3.64e-11 1.51e-04 9.37e-01 4.78e+10 1 6.79e-04 1.27e-02 Solver Summary (v 2.0.0-eigen-(3.3.8)-no_lapack-eigensparse-no_openmp) Original Reduced Parameter blocks 1 1 Parameters 4 4 Residual blocks 1 1 Residuals 4 4 Minimizer TRUST_REGION Dense linear algebra library EIGEN Trust region strategy LEVENBERG_MARQUARDT Given Used Linear solver DENSE_QR DENSE_QR Threads 1 1 Linear solver ordering AUTOMATIC 1 Cost: Initial 1.075000e+02 Final 1.120029e-15 Change 1.075000e+02 Minimizer iterations 15 Successful steps 15 Unsuccessful steps 0 Time (in seconds): Preprocessor 0.000531 Residual only evaluation 0.000361 (14) Jacobian & residual evaluation 0.001866 (15) Linear solver 0.005511 (14) Minimizer 0.014814 Postprocessor 0.000128 Total 0.015473 Termination: CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.642190e-11 <= 1.000000e-10) Initial x1 = 0.000146222, x2 = -1.46222e-05, x3 = 2.40957e-05, x4 = 2.40957e-05- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
3.3 cost1_x4_auto
- 1个代价函数（CostFunction），输出残差维度为4；
- 4个参数块（ParametersBlock），每个参数块的输入参数维度为1；
- 导数计算采用自动微分。
完整代码如下：
#include "ceres/ceres.h" #include "gflags/gflags.h" #include "glog/logging.h" // 用户自定义残差计算模型 // 输入4个参数块，参数块维度为1 // 输出1个残差块，残差块维度为4 struct F1234 { templatebool operator()(const T* const x1, const T* const x2, const T* const x3, const T* const x4, T* residual) const { residual[0] = x1[0] + 10.0 * x2[0]; residual[1] = sqrt(5.0) * (x3[0] - x4[0]); residual[2] = (x2[0] - 2.0 * x3[0]) * (x2[0] - 2.0 * x3[0]); residual[3] = sqrt(10.0) * (x1[0] - x4[0]) * (x1[0] - x4[0]); return true; } }; int main(int argc, char** argv) { GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); google::InitGoogleLogging(argv[0]); // 设置参数初始值，输入4个参数块，每个参数块的维度为1 double x1 = 3.0; double x2 = -1.0; double x3 = 0.0; double x4 = 1.0; // 构建非线性最小二乘问题 ceres::Problem problem; // 添加残差块，需要依次指定代价函数、损失函数、参数块 // 本例中损失函数为单位函数，成本函数需要输入4个参数块 problem.AddResidualBlock(new ceres::AutoDiffCostFunction - 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
结果如下：
Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1 iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 1.075000e+02 0.00e+00 1.55e+02 0.00e+00 0.00e+00 1.00e+04 0 5.29e-04 9.91e-04 1 5.036190e+00 1.02e+02 2.00e+01 2.16e+00 9.53e-01 3.00e+04 1 1.15e-03 2.31e-03 2 3.148168e-01 4.72e+00 2.50e+00 6.23e-01 9.37e-01 9.00e+04 1 4.61e-04 2.87e-03 3 1.967760e-02 2.95e-01 3.13e-01 3.08e-01 9.37e-01 2.70e+05 1 4.50e-04 3.43e-03 4 1.229900e-03 1.84e-02 3.91e-02 1.54e-01 9.37e-01 8.10e+05 1 4.57e-04 3.98e-03 5 7.687123e-05 1.15e-03 4.89e-03 7.69e-02 9.37e-01 2.43e+06 1 5.65e-04 4.66e-03 6 4.804625e-06 7.21e-05 6.11e-04 3.85e-02 9.37e-01 7.29e+06 1 5.15e-04 5.31e-03 7 3.003028e-07 4.50e-06 7.64e-05 1.92e-02 9.37e-01 2.19e+07 1 4.49e-04 5.86e-03 8 1.877006e-08 2.82e-07 9.54e-06 9.62e-03 9.37e-01 6.56e+07 1 7.00e-04 6.65e-03 9 1.173223e-09 1.76e-08 1.19e-06 4.81e-03 9.37e-01 1.97e+08 1 4.69e-04 7.24e-03 10 7.333425e-11 1.10e-09 1.49e-07 2.40e-03 9.37e-01 5.90e+08 1 4.77e-04 7.82e-03 11 4.584044e-12 6.88e-11 1.86e-08 1.20e-03 9.37e-01 1.77e+09 1 6.49e-04 8.58e-03 12 2.865573e-13 4.30e-12 2.33e-09 6.02e-04 9.37e-01 5.31e+09 1 6.43e-04 9.33e-03 13 1.791438e-14 2.69e-13 2.91e-10 3.01e-04 9.37e-01 1.59e+10 1 6.26e-04 1.01e-02 14 1.120029e-15 1.68e-14 3.64e-11 1.51e-04 9.37e-01 4.78e+10 1 7.81e-04 1.10e-02 Solver Summary (v 2.0.0-eigen-(3.3.8)-no_lapack-eigensparse-no_openmp) Original Reduced Parameter blocks 4 4 Parameters 4 4 Residual blocks 1 1 Residuals 4 4 Minimizer TRUST_REGION Dense linear algebra library EIGEN Trust region strategy LEVENBERG_MARQUARDT Given Used Linear solver DENSE_QR DENSE_QR Threads 1 1 Linear solver ordering AUTOMATIC 4 Cost: Initial 1.075000e+02 Final 1.120029e-15 Change 1.075000e+02 Minimizer iterations 15 Successful steps 15 Unsuccessful steps 0 Time (in seconds): Preprocessor 0.000462 Residual only evaluation 0.000325 (14) Jacobian & residual evaluation 0.001673 (15) Linear solver 0.004690 (14) Minimizer 0.010687 Postprocessor 0.000069 Total 0.011219 Termination: CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.642190e-11 <= 1.000000e-10) Final x1 = 0.000146222, x2 = -1.46222e-05, x3 = 2.40957e-05, x4 = 2.40957e-05- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
同样，也可以按照cost4_x1_auto, cost2_x2_auto等多种方式实现，读者可举一反三，自行实现验证，关键是一定要确保ceres::AutoDiffCostFunction<>中，输出残差维度和输入每个参数块维度，要和用户自定义残差计算模型（或代价函数）中，输出残差维度和输入每个参数块维度保持一致（通俗但不严谨的讲，可以理解为x[]和residual[]的长度），否则很有可能无法迭代出正确结果。
另外，按照cost1_x1指定输入和输出的维度，分别通过数值法和解析解法求解Powell方程的最小值。
3.4 cost1_x1_numeric
- 1个代价函数（CostFunction），输出残差维度为4；
- 1个参数块（ParametersBlock），输入参数维度为4；
- 导数计算采用数值微分法。
完整代码如下：
#include "ceres/ceres.h" #include "gflags/gflags.h" #include "glog/logging.h" // 用户自定义残差计算模型 // 输入1个参数块，参数块维度为4 // 输出1个残差块，残差块维度为4 // 用户自定义残差计算模型 struct F1234 { bool operator()(const double* const x, double* residual) const { residual[0] = x[0] + 10.0 * x[1]; residual[1] = sqrt(5.0) * (x[2] - x[3]); residual[2] = (x[1] - 2.0 * x[2]) * (x[1] - 2.0 * x[2]); residual[3] = sqrt(10.0) * (x[0] - x[3]) * (x[0] - x[3]); return true; } }; int main(int argc, char** argv) { GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); google::InitGoogleLogging(argv[0]); // 设置参数初始值，输入1个参数块，参数块的维度为4 double x1234[4] = { 3, -1, 0, 1 }; // 构建非线性最小二乘问题 ceres::Problem problem; // 添加残差块，需要依次指定代价函数、损失函数、参数块 // 本例中损失函数为单位函数，代价函数中需要输入1个参数块 // 本例中数值计算导数方法为CENTRAL problem.AddResidualBlock(new ceres::NumericDiffCostFunction- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
结果如下：
Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1 iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 1.075000e+02 0.00e+00 1.55e+02 0.00e+00 0.00e+00 1.00e+04 0 5.58e-04 1.08e-03 1 5.036190e+00 1.02e+02 2.00e+01 2.16e+00 9.53e-01 3.00e+04 1 1.09e-03 2.36e-03 2 3.148168e-01 4.72e+00 2.50e+00 6.23e-01 9.37e-01 9.00e+04 1 4.22e-04 2.88e-03 3 1.967760e-02 2.95e-01 3.13e-01 3.08e-01 9.37e-01 2.70e+05 1 4.18e-04 3.40e-03 4 1.229900e-03 1.84e-02 3.91e-02 1.54e-01 9.37e-01 8.10e+05 1 4.14e-04 3.91e-03 5 7.687123e-05 1.15e-03 4.89e-03 7.69e-02 9.37e-01 2.43e+06 1 4.15e-04 4.42e-03 6 4.804625e-06 7.21e-05 6.11e-04 3.85e-02 9.37e-01 7.29e+06 1 6.04e-04 5.14e-03 7 3.003028e-07 4.50e-06 7.64e-05 1.92e-02 9.37e-01 2.19e+07 1 4.81e-04 5.75e-03 8 1.877006e-08 2.82e-07 9.54e-06 9.62e-03 9.37e-01 6.56e+07 1 5.56e-04 6.42e-03 9 1.173223e-09 1.76e-08 1.19e-06 4.81e-03 9.37e-01 1.97e+08 1 4.80e-04 7.01e-03 10 7.333425e-11 1.10e-09 1.49e-07 2.40e-03 9.37e-01 5.90e+08 1 6.15e-04 7.73e-03 11 4.584044e-12 6.88e-11 1.86e-08 1.20e-03 9.37e-01 1.77e+09 1 6.46e-04 8.50e-03 12 2.865573e-13 4.30e-12 2.33e-09 6.02e-04 9.37e-01 5.31e+09 1 6.46e-04 9.28e-03 13 1.791438e-14 2.69e-13 2.91e-10 3.01e-04 9.37e-01 1.59e+10 1 6.57e-04 1.01e-02 14 1.120029e-15 1.68e-14 3.64e-11 1.51e-04 9.37e-01 4.78e+10 1 6.38e-04 1.09e-02 Solver Summary (v 2.0.0-eigen-(3.3.8)-no_lapack-eigensparse-no_openmp) Original Reduced Parameter blocks 1 1 Parameters 4 4 Residual blocks 1 1 Residuals 4 4 Minimizer TRUST_REGION Dense linear algebra library EIGEN Trust region strategy LEVENBERG_MARQUARDT Given Used Linear solver DENSE_QR DENSE_QR Threads 1 1 Linear solver ordering AUTOMATIC 1 Cost: Initial 1.075000e+02 Final 1.120029e-15 Change 1.075000e+02 Minimizer iterations 15 Successful steps 15 Unsuccessful steps 0 Time (in seconds): Preprocessor 0.000526 Residual only evaluation 0.000313 (14) Jacobian & residual evaluation 0.001281 (15) Linear solver 0.004963 (14) Minimizer 0.010534 Postprocessor 0.000071 Total 0.011131 Termination: CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.642190e-11 <= 1.000000e-10) Final x1 = 0.000146222, x2 = -1.46222e-05, x3 = 2.40957e-05, x4 = 2.40957e-05- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
3.5 cost1_x1_analytic
- 1个代价函数（CostFunction），输出残差维度为4；
- 1个参数块（ParametersBlock），输入参数维度为4；
- 导数计算采用解析法。
Ceres中，每个雅可比分量表示如下：
j a c b i a n [ i ] [ r ∗ p a r a m e t e r s _ b l o c k _ s i z e _ [ i ] + c ] = ∂ r e s i d u a l [ r ] ∂ p a r a m e t e r s [ i ] [ c ] (4) jacbian[i][r * parameters\_block\_size\_[i] + c] = \frac{\partial residual[r]}{\partial parameters[i][c]} \tag{4} jacbian[i][r∗parameters_block_size_[i]+c]=∂parameters[i][c]∂residual[r](4)
式中， i i i 表示参数块索引， c c c 表示某个参数块中的参数索引， r r r 表示残差的索引。由此可见，雅可比可以理解成是一个二维的数组，（不同于 C++ 中的二维数组，因为 C++ 中的二维数组列数都一样，类似于 std::vector
对于本例，只有1个参数块，因此只有 jacobian[0] ，残差的维度为4，参数块中参数数量为4，因此 jacobian[0] 中共有4*4个分量。
完整代码如下：
#include "ceres/ceres.h" #include "gflags/gflags.h" #include "glog/logging.h" class CostFunction1234 : public ceres::SizedCostFunction<4, /*输出（residual）维度大小*/\ 4 /*输入参数块维度大小*/> { public: virtual ~CostFunction1234() {} // 用户自定义残差计算方法 virtual bool Evaluate(double const* const* x, /*输入参数块*/\ double* residual, /*输出残差*/\ double** jacobians /*输出雅可比矩阵*/) const { // 本例中1个参数块中有4个输入参数 double x1 = x[0][0]; double x2 = x[0][1]; double x3 = x[0][2]; double x4 = x[0][3]; // 本例中输出残差维度为4 residual[0] = x1 + 10.0 * x2; residual[1] = sqrt(5.0) * (x3 - x4); residual[2] = (x2 - 2.0 * x3) * (x2 - 2.0 * x3); residual[3] = sqrt(10.0) * (x1 - x4) * (x1 - x4); if(jacobians != NULL && jacobians[0] != NULL) { // 第1个维度残差对参数块中的参数依次求偏导 jacobians[0][0] = 1.0; jacobians[0][1] = 10.0; jacobians[0][2] = 0.0; jacobians[0][3] = 0.0; // 第2个维度残差对参数块中的参数依次求偏导 jacobians[0][4] = 0.0; jacobians[0][5] = 0.0; jacobians[0][6] = sqrt(5.0); jacobians[0][7] = -sqrt(5.0); // 第3个维度残差对参数块中的参数依次求偏导 jacobians[0][8] = 0.0; jacobians[0][9] = 2 * (x2 - 2 * x3); jacobians[0][10] = -4 * (x2 - 2 * x3); jacobians[0][11] = 0.0; // 第4个维度残差对参数块中的参数依次求偏导 jacobians[0][12] = 2 * sqrt(10.0) * (x1 - x4); jacobians[0][13] = 0.0; jacobians[0][14] = 0.0; jacobians[0][15] = -2 * sqrt(10.0) * (x1 - x4); } return true; } }; int main(int argc, char** argv) { GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true); google::InitGoogleLogging(argv[0]); // 设置参数初始值，输入1个参数块，参数块的维度为4 double x1234[4] = { 3, -1, 0, 1 }; // 构建非线性最小二乘问题 ceres::Problem problem; // 添加残差块，需要依次指定代价函数、损失函数、参数块 // 本例中损失函数为单位函数，代价函数需要输入1个参数块 problem.AddResidualBlock(new CostFunction1234, NULL, x1234); // 配置求解器参数 ceres::Solver::Options options; // 设置最大迭代次数 options.max_num_iterations = 100; // 指定线性求解器来求解问题 options.linear_solver_type = ceres::DENSE_QR; // 输出每次迭代的信息 options.minimizer_progress_to_stdout = true; // 输出日志内容 ceres::Solver::Summary summary; std::cout << "Initial x1 = " << x1234[0] << ", x2 = " << x1234[1] << ", x3 = " << x1234[2] << ", x4 = " << x1234[3] << "\n"; // 开始优化求解 ceres::Solve(options, &problem, &summary); // 输出优化过程及结果 std::cout << summary.FullReport() << "\n"; std::cout << "Final x1 = " << x1234[0] << ", x2 = " << x1234[1] << ", x3 = " << x1234[2] << ", x4 = " << x1234[3] << "\n"; std::system("pause"); return 0; }- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
- 70
- 71
- 72
- 73
- 74
- 75
- 76
- 77
- 78
- 79
- 80
- 81
- 82
- 83
- 84
- 85
- 86
- 87
- 88
- 89
- 90
- 91
- 92
- 93
- 94
- 95
- 96
- 97
- 98
- 99
- 100
- 101
结果如下：
Initial x1 = 3, x2 = -1, x3 = 0, x4 = 1 iter cost cost_change |gradient| |step| tr_ratio tr_radius ls_iter iter_time total_time 0 1.075000e+02 0.00e+00 1.55e+02 0.00e+00 0.00e+00 1.00e+04 0 4.30e-04 8.54e-04 1 5.036190e+00 1.02e+02 2.00e+01 2.16e+00 9.53e-01 3.00e+04 1 1.17e-03 2.19e-03 2 3.148168e-01 4.72e+00 2.50e+00 6.23e-01 9.37e-01 9.00e+04 1 5.59e-04 2.88e-03 3 1.967760e-02 2.95e-01 3.13e-01 3.08e-01 9.37e-01 2.70e+05 1 5.91e-04 3.59e-03 4 1.229900e-03 1.84e-02 3.91e-02 1.54e-01 9.37e-01 8.10e+05 1 5.45e-04 4.25e-03 5 7.687123e-05 1.15e-03 4.89e-03 7.69e-02 9.37e-01 2.43e+06 1 1.02e-03 5.47e-03 6 4.804625e-06 7.21e-05 6.11e-04 3.85e-02 9.37e-01 7.29e+06 1 4.56e-04 6.06e-03 7 3.003028e-07 4.50e-06 7.64e-05 1.92e-02 9.37e-01 2.19e+07 1 4.22e-04 6.59e-03 8 1.877006e-08 2.82e-07 9.54e-06 9.62e-03 9.37e-01 6.56e+07 1 6.06e-04 7.30e-03 9 1.173223e-09 1.76e-08 1.19e-06 4.81e-03 9.37e-01 1.97e+08 1 5.80e-04 8.00e-03 10 7.333425e-11 1.10e-09 1.49e-07 2.40e-03 9.37e-01 5.90e+08 1 5.90e-04 8.71e-03 11 4.584044e-12 6.88e-11 1.86e-08 1.20e-03 9.37e-01 1.77e+09 1 5.94e-04 9.42e-03 12 2.865573e-13 4.30e-12 2.33e-09 6.02e-04 9.37e-01 5.31e+09 1 4.47e-04 9.97e-03 13 1.791438e-14 2.69e-13 2.91e-10 3.01e-04 9.37e-01 1.59e+10 1 6.11e-04 1.07e-02 14 1.120029e-15 1.68e-14 3.64e-11 1.51e-04 9.37e-01 4.78e+10 1 4.42e-04 1.13e-02 Solver Summary (v 2.0.0-eigen-(3.3.8)-no_lapack-eigensparse-no_openmp) Original Reduced Parameter blocks 1 1 Parameters 4 4 Residual blocks 1 1 Residuals 4 4 Minimizer TRUST_REGION Dense linear algebra library EIGEN Trust region strategy LEVENBERG_MARQUARDT Given Used Linear solver DENSE_QR DENSE_QR Threads 1 1 Linear solver ordering AUTOMATIC 1 Cost: Initial 1.075000e+02 Final 1.120029e-15 Change 1.075000e+02 Minimizer iterations 15 Successful steps 15 Unsuccessful steps 0 Time (in seconds): Preprocessor 0.000423 Residual only evaluation 0.000351 (14) Jacobian & residual evaluation 0.000778 (15) Linear solver 0.005656 (14) Minimizer 0.011010 Postprocessor 0.000057 Total 0.011491 Termination: CONVERGENCE (Gradient tolerance reached. Gradient max norm: 3.642190e-11 <= 1.000000e-10) Final x1 = 0.000146222, x2 = -1.46222e-05, x3 = 2.40957e-05, x4 = 2.40957e-05- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
-
相关阅读:
TP5 模型查询的返回值、返回值的判断以及所使用的SQL
数据结构：AVL树的旋转（高度平衡树）
07-Zookeeper分布式一致性协议ZAB源码剖析
常用的基本命令（必掌握）
最高人民法院：跨境赌博利用区块链、虚拟货币等新兴技术逃避打击
JUC的线程池架构
React18源码: reconciler执行流程
职场成长的三种方式
WebStorm配置less编译wxss或css
【微信小程序】页面tabBar切换、下拉刷新
- 原文地址：https://blog.csdn.net/qq_45006390/article/details/127647723
