Ceres-Solver库入门

墨蓝 2022-05-31 07:40 234阅读 0赞

## 示例1：求极值 ##

首先我们以Ceres库官网中的Hello World例子来进行说明。这里例子的目的是为了计算方程![20131118184844031][]取得最小值时x的值。从这个方程很容易看出来当x=10时，f(x)取得最小值0。这个方程虽然没有什么实际意义，但是为了演示Ceres库还是很不错的例子。

1、编写一个g(x)=10-x的残差方程。代码如下：

**\[cpp\]** [view plain][] [copy][view plain]

1. structCostFunctor \{
2. template <typename T>
3. bool operator()(const T\* const x, T\* residual) const \{
4. residual\[0\] = T(10.0) - x\[0\];
5. return true;
6. \}
7. \};

这里值得注意的是，必须要编写一个重载()运算，而且必须使用模板类型，所有的输入参数和输出参数都要使用T类型。

2、当我们写完了上面的计算残差的方程，接下来就可以使用Ceres库来构造一个求解非线性最小二乘法的Problem来进行求解未知数了。代码如下：

**\[cpp\]** [view plain][] [copy][view plain]

1. int main(int argc, char\*\* argv)
2. \{
3. google::InitGoogleLogging(argv\[0\]);
4. 
5. // 指定求解的未知数的初值，这里设置为5.0
6. double initial\_x = 5.0;
7. double x = initial\_x;
8. 
9. // 建立Problem
10. Problem problem;
11. 
12. // 建立CostFunction（残差方程）
13. CostFunction\* cost\_function =
14. new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
15. problem.AddResidualBlock(cost\_function, NULL, &x);
16. 
17. // 求解方程!
18. Solver::Options options;
19. options.linear\_solver\_type = ceres::DENSE\_QR;
20. options.minimizer\_progress\_to\_stdout = true;
21. Solver::Summary summary;
22. Solve(options, &problem, &summary);
23. 
24. std::cout << summary.BriefReport() << "\\n";
25. std::cout << "x : " << initial\_x
26. << " -> " << x << "\\n";
27. return 0;
28. \}

AutoDiffCostFunction需要CostFunctor作为输入。上面的例子编译执行后，会输出下面的信息：

**\[plain\]** [view plain][] [copy][view plain]

1.  0: f: 1.250000e+01 d: 0.00e+00 g: 5.00e+00h: 0.00e+00 rho: 0.00e+00 mu: 1.00e+04 li: 0 it: 6.91e-06 tt: 1.91e-03
2.  1: f: 1.249750e-07 d: 1.25e+01 g: 5.00e-04h: 5.00e+00 rho: 1.00e+00 mu: 3.00e+04 li: 1 it: 2.81e-05 tt: 1.99e-03
3.  2: f: 1.388518e-16 d: 1.25e-07 g: 1.67e-08h: 5.00e-04 rho: 1.00e+00 mu: 9.00e+04 li: 1 it: 1.00e-05 tt: 2.01e-03
4.  CeresSolver Report: Iterations: 2, Initial cost: 1.250000e+01, Final cost:1.388518e-16, Termination: PARAMETER\_TOLERANCE.
5.  x : 5-> 10

3、从上面的输出信息中可以看出，经过三次迭代计算，求得的x值为10时可以取得最小值。接下来我们分析下main函数中的代码。

第2行代码为Google的log库，详细内容请参考Google log库的相关说明。

第5、6行为定义了求解未知数的初值，初值设置为5。

第9行，声明一个Problem对象，用于求解。

第12行，声明一个残差方程，CostFunction通过模板类AutoDiffCostFunction来进行构造，第一个模板参数为残差对象，也就是最开始写的那个那个带有重载()运算符的结构体，第二个模板参数为残差个数，第三个模板参数为未知数个数，最后参数是结构体对象。

第14行，将观测值和残差方程加入Problem对象中，如果有多个观测值，都需要加进去，这点可以看后面的示例。

第16~19行，定义一个求解选项，里面主要包括对方程线性化的方式，迭代次数等，具体参考官网帮助文档。

第20行，定义一个求解结果报告。

第21行，调用Solve函数进行求解，第一个参数就是求解选项，第二个参数为Problem指针，第三个参数为求解报告指针。

第23行，输出求解报告信息。

第24行，输出求解前的初值和求解后的值。

## 示例2：曲线拟合 ##

下面的例子是指定一系列的点对来拟合一个曲线的系数。这一系列点对是通过曲线![20131118185016781][]插值的点然后添加了标准差的高斯噪声。我们要拟合的曲线形式为：

![20131118185051406][]

首先我们定义一个残差结构体，用于计算每一个观测值的残差。代码如下：

**\[cpp\]** [view plain][] [copy][view plain]

1. struct ExponentialResidual \{
2. ExponentialResidual(double x, double y)
3. : x\_(x), y\_(y) \{\}
4. 
5. template <typename T>
6. bool operator()(const T\* const m, const T\* const c, T\* residual) const \{
7. residual\[0\] = T(y\_) - exp(m\[0\] \* T(x\_) + c\[0\]);
8. return true;
9. \}
10. 
11. private:
12. // 观测值
13. const double x\_;
14. const double y\_;
15. \};

假设观测点对全部存在数组data中，data中共有2n个数，下面的代码用来演示如何将每个观测点对都加入到Problem中用于后续求解。

**\[cpp\]** [view plain][] [copy][view plain]

1. double m = 0.0;
2. double c = 0.0;
3. 
4. Problem problem;
5. for (int i = 0; i < kNumObservations; ++i)
6. \{
7. CostFunction\* cost\_function =
8. new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
9. new ExponentialResidual(data\[2 \* i\], data\[2 \* i + 1\]));
10. problem.AddResidualBlock(cost\_function, NULL, &m, &c);
11. \}

对上面的代码大致说明下，首先定义m和c的初值都为0，然后定义一个Problem对象，用于后续求解。观测点的个数共有kNumObservations个，遍历每个观测点对，然后new一个残差方程并将其加入Problem中。过程和示例1一样，只不过这里的观测值比较多，需要用一个循环来进行处理。

后续的处理和示例1完全一样，定义一个求解选项和求解报告，然后调用Solve函数进行求解即可。下面时输出的求解过程以及m和c的结果。

**\[plain\]** [view plain][] [copy][view plain]

1.  0: f: 1.211734e+02 d: 0.00e+00 g: 3.61e+02 h: 0.00e+00 rho: 0.00e+00 mu: 1.00e+04 li: 0 it: 0.00e+00 tt: 0.00e+00
2.  1: f: 1.211734e+02 d:-2.21e+03 g: 3.61e+02 h: 7.52e-01 rho:-1.87e+01 mu: 5.00e+03 li: 1 it: 0.00e+00 tt: 0.00e+00
3.  2: f: 1.211734e+02 d:-2.21e+03 g: 3.61e+02 h: 7.51e-01 rho:-1.86e+01 mu: 1.25e+03 li: 1 it: 0.00e+00 tt: 0.00e+00
4.  3: f: 1.211734e+02 d:-2.19e+03 g: 3.61e+02 h: 7.48e-01 rho:-1.85e+01 mu: 1.56e+02 li: 1 it: 0.00e+00 tt: 0.00e+00
5.  4: f: 1.211734e+02 d:-2.02e+03 g: 3.61e+02 h: 7.22e-01 rho:-1.70e+01 mu: 9.77e+00 li: 1 it: 0.00e+00 tt: 0.00e+00
6.  5: f: 1.211734e+02 d:-7.34e+02 g: 3.61e+02 h: 5.78e-01 rho:-6.32e+00 mu: 3.05e-01 li: 1 it: 0.00e+00 tt: 0.00e+00
7.  6: f: 3.306595e+01 d: 8.81e+01 g: 4.10e+02 h: 3.18e-01 rho: 1.37e+00 mu: 9.16e-01 li: 1 it: 0.00e+00 tt: 0.00e+00
8.  7: f: 6.426770e+00 d: 2.66e+01 g: 1.81e+02 h: 1.29e-01 rho: 1.10e+00 mu: 2.75e+00 li: 1 it: 0.00e+00 tt: 0.00e+00
9.  8: f: 3.344546e+00 d: 3.08e+00 g: 5.51e+01 h: 3.05e-02 rho: 1.03e+00 mu: 8.24e+00 li: 1 it: 0.00e+00 tt: 0.00e+00
10. 9: f: 1.987485e+00 d: 1.36e+00 g: 2.33e+01 h: 8.87e-02 rho: 9.94e-01 mu: 2.47e+01 li: 1 it: 0.00e+00 tt: 0.00e+00
11. 10: f: 1.211585e+00 d: 7.76e-01 g: 8.22e+00 h: 1.05e-01 rho: 9.89e-01 mu: 7.42e+01 li: 1 it: 0.00e+00 tt: 0.00e+00
12. 11: f: 1.063265e+00 d: 1.48e-01 g: 1.44e+00 h: 6.06e-02 rho: 9.97e-01 mu: 2.22e+02 li: 1 it: 0.00e+00 tt: 0.00e+00
13. 12: f: 1.056795e+00 d: 6.47e-03 g: 1.18e-01 h: 1.47e-02 rho: 1.00e+00 mu: 6.67e+02 li: 1 it: 0.00e+00 tt: 0.00e+00
14. 13: f: 1.056751e+00 d: 4.39e-05 g: 3.79e-03 h: 1.28e-03 rho: 1.00e+00 mu: 2.00e+03 li: 1 it: 0.00e+00 tt: 0.00e+00
15. Ceres Solver Report: Iterations: 13, Initial cost: 1.211734e+02, Final cost: 1.056751e+00, Termination: FUNCTION\_TOLERANCE.
16. Initial m: 0 c: 0
17. Final m: 0.291861 c: 0.131439

从上面输出信息可以看出，开始m和c均为0时，残差大小为121.173，迭代结束m=0.291861，c=0.131439时残差大小为1.05675。计算和m和c的值与我们的理论值m=3，c=0.1有不小的差异，当我们将m=3，c=0.1带入方程计算的残差大小为1.082425。可以看出m=3，c=0.1时的残差比我们计算的m和c的值的残差还要大。所以说这些点最精确的值应该是我们计算的值，而不是理论值（与添加的高斯噪声有很大的关系）。下面是观测点、拟合点以及曲线的图像。

![SouthEast][]

## 示例3：单像空间后方交会 ##

通过上面两个示例大致应该熟悉了Ceres库的使用方式，下面我们编写一个求解摄影测量学里面最基础的一个问题，也就是单张像片的空间后方交会程序。学过《摄影测量学》的人肯定知道，后方交会的求解过程，是非常的麻烦，需要将共线方程求偏导数线性化，然后迭代计算外方位元素。

首先给出共线方程：

![20131118185251015][]

式中：x0、y0和f为内方位元素，一般为已知量，abc为旋转矩阵，使用![20131118185511953][]三个角元素来表示为：

![20131118185340906][]

按照教科书上的求解外方位元素的步骤，都需要10步左右，而且中间还需要求改正数迭代计算，非常复杂。这部分内容可以参考下面基本教科书中的内容：

\[1\]《摄影测量学》张剑清 潘励 王树根武汉大学出版社 P17-P23

\[2\]《摄影测量学》王佩军 徐亚明 武汉大学出版社 P60-P67

\[3\]《摄影测量学》林君建 苍桂华 国防工业出版社 P49-P55

下面我们按照Ceres库的方法来编写后方交会的代码，首先编写一个带有模板重载函数()的结构体，用于计算残差。

**\[cpp\]** [view plain][] [copy][view plain]

1. struct BackCrossResidual
2. \{
3. /\*
4. \* X, Y, Z, x, y 分别为观测值，f为焦距
5. \*/
6. BackCrossResidual(double X, double Y, double Z, double x, double y, double f)
7. :\_X(X), \_Y(Y), \_Z(Z), \_x(x), \_y(y), \_f(f)\{\}
8. 
9. /\*
10. \* pBackCrossParameters：-2分别为Xs、Ys、Zs,3-5分别为Phi、Omega、Kappa
11. \*/
12. template <typename T>
13. bool operator () (const T \* const pBackCrossParameters, T\* residual) const
14. \{
15. T dXs = pBackCrossParameters\[0\];
16. T dYs = pBackCrossParameters\[1\];
17. T dZs = pBackCrossParameters\[2\];
18. T dPhi = pBackCrossParameters\[3\];
19. T dOmega = pBackCrossParameters\[4\];
20. T dKappa = pBackCrossParameters\[5\];
21. 
22. T a1 = cos(dPhi)\*cos(dKappa) - sin(dPhi)\*sin(dOmega)\*sin(dKappa);
23. T a2 = -cos(dPhi)\*sin(dKappa) - sin(dPhi)\*sin(dOmega)\*cos(dKappa);
24. T a3 = -sin(dPhi)\*cos(dOmega);
25. T b1 = cos(dOmega)\*sin(dKappa);
26. T b2 = cos(dOmega)\*cos(dKappa);
27. T b3 = -sin(dOmega);
28. T c1 = sin(dPhi)\*cos(dKappa) + cos(dPhi)\*sin(dOmega)\*sin(dKappa);
29. T c2 = -sin(dPhi)\*sin(dKappa) + cos(dPhi)\*sin(dOmega)\*cos(dKappa);
30. T c3 = cos(dPhi)\*cos(dOmega);
31. 
32. // 有两个残差
33. residual\[0\]= T(\_x) +T(\_f) \* T( (a1\*(\_X-dXs) + b1\*(\_Y-dYs) + c1\*(\_Z-dZs)) / ((a3\*(\_X-dXs) + b3\*(\_Y-dYs) + c3\*(\_Z-dZs))));
34. residual\[1\]= T(\_y) +T(\_f) \* T( (a2\*(\_X-dXs) + b2\*(\_Y-dYs) + c2\*(\_Z-dZs)) / ((a3\*(\_X-dXs) + b3\*(\_Y-dYs) + c3\*(\_Z-dZs))));
35. 
36. return true;
37. \}
38. 
39. private:
40. const double \_X;
41. const double \_Y;
42. const double \_Z;
43. const double \_x;
44. const double \_y;
45. const double \_f;
46. \};

对上面的代码解释下，首先有个构造函数，传入的参数共有6个，其中五个是控制点坐标（像方坐标两个xy和物方坐标三个XYZ），另外一个是相机的焦距f。

在重载()函数中，需要的参数初值和残差。初值一共有六个，三个线元素和三个角元素。将三个角元素处理成旋转矩阵所需要的九个参数。

最后按照共线方程编写残差计算公式，这里需要注意的是，一组点可以计算出两个残差，分别是X和Y。

接下来是调用函数，使用的数据是参考书\[1\]P39第9题（或参考书\[2\]P89第3题）。题目中给定的相机焦距为153.24mm，坐标点如下表：

代码如下：

**\[cpp\]** [view plain][] [copy][view plain]

1. int main(int argc, char\*\* argv)
2. \{
3. google::InitGoogleLogging(argv\[0\]);
4. 
5. const char \*pszFilename= "C:\\\\后方交会\\\\sourcedata.txt";
6. 
7. FILE\* fid = fopen(pszFilename,"rt");
8. if(!fid)
9. return NULL;
10. 
11. int nCount = 4;
12. double\* pData = new double\[5 \* nCount\];
13. memset(pData, 0, sizeof(double)\* 5 \* nCount);
14. 
15. double dBackCrossParameters\[6\] = \{0\}; //初值
16. for(int i=0;i<nCount;i++)
17. \{
18. double\* pTmp = pData+5\*i;
19. fscanf(fid,"%lf %lf %lf%lf %lf",pTmp,pTmp+1,pTmp+2,pTmp+3,pTmp+4);
20. 
21. dBackCrossParameters\[0\]+= pTmp\[2\]; //X
22. dBackCrossParameters\[1\]+= pTmp\[3\]; //Y
23. \}
24. 
25. fclose(fid);
26. 
27. dBackCrossParameters\[0\]/= nCount;
28. dBackCrossParameters\[1\]/= nCount;
29. 
30. double df = 153.24; //mm
31. dBackCrossParameters\[2\]= 50\*df;
32. 
33. Problem problem;
34. for (int i = 0; i < nCount;++i)
35. \{
36. double\* pPoint = pData+ 5\*i;
37. 
38. BackCrossResidual\*pResidualX = newBackCrossResidual(pPoint\[2\],pPoint\[3\], pPoint\[4\],pPoint\[0\]/1000, pPoint\[1\]/1000,df/1000);
39. problem.AddResidualBlock(newAutoDiffCostFunction<BackCrossResidual, 2, 6>(pResidualX), NULL,dBackCrossParameters);
40. \}
41. 
42. Solver::Options m\_options;
43. Solver::Summary m\_summary;
44. m\_options.max\_num\_iterations = 25;
45. m\_options.linear\_solver\_type = ceres::DENSE\_QR;
46. m\_options.minimizer\_progress\_to\_stdout = true;
47. 
48. Solve(m\_options, &problem,&m\_summary);
49. 
50. fprintf(stdout,"Xs=%lfYs=%lf Zs=%lf\\n",dBackCrossParameters\[0\],dBackCrossParameters\[1\],dBackCrossParameters\[2\]);
51. fprintf(stdout,"Phi=%lfOmega=%lf Kappa=%lf\\n",dBackCrossParameters\[3\],dBackCrossParameters\[4\],dBackCrossParameters\[5\]);
52. fprintf(stdout,"%s\\n",m\_summary.FullReport().c\_str());
53. 
54. return 0;
55. \}

接下来对上面的代码进行说明，首先读取坐标点对，一共有4对。一对点包含五个值。将所有的点都读到数组pData中。

接下来定义一个数组用来保存未知数和迭代初值。根据后方交会流程，三个角元素的初值在垂直摄影时可以认为其值为0，而对于三个线元素，Xs和Ys初值用所有的控制点的物方横坐标和纵坐标的均值，而Zs根据焦距和摄影比例尺坟墓共同决定。

然后构造一个Problem对象，并将每个控制点作为观测值构造一个误差方程，需要注意的是像平面坐标单位要和物方坐标单位一致，由于像方坐标和焦距都是毫米，所以都要除以1000换算为米。

程序执行的结果如下图所示，计算的结果与参考书\[1\]中给出的参考答案完全一样。此外用我之前用偏导数线性化求解的结果也是一致的，从上面的代码可以看出，使用Ceres库完全可以将很复杂的非线性问题用非常简单的代码来求解。

![SouthEast 1][]

[20131118184844031]: /images/20220531/4037b0ec16774757a5f445e516803441.png
[view plain]: http://blog.csdn.net/liminlu0314/article/details/16808239#
[20131118185016781]: /images/20220531/36034292c66842c98c445ee19afdf32e.png
[20131118185051406]: /images/20220531/61a5e510c10b4a7bbccfdc0a50bf3aea.png
[SouthEast]: /images/20220531/bfe1bc0c379d45f496de2442a83b2f42.png
[20131118185251015]: /images/20220531/16311c51bdac41e494882cba4eff8f21.png
[20131118185511953]: /images/20220531/0ee1e653634c4fc1ab0fb4a755e28918.png
[20131118185340906]: /images/20220531/f53e02d2662442d286df4d4a06a2fe61.png
[SouthEast 1]: /images/20220531/a3f8ba7deaf24f948aa92c70ba76e7ac.png