Ceres-Solver库入门
示例1:求极值
首先我们以Ceres库官网中的Hello World例子来进行说明。这里例子的目的是为了计算方程取得最小值时x的值。从这个方程很容易看出来当x=10时,f(x)取得最小值0。这个方程虽然没有什么实际意义,但是为了演示Ceres库还是很不错的例子。
1、编写一个g(x)=10-x的残差方程。代码如下:
structCostFunctor {
template <typename T>
bool operator()(const T* const x, T* residual) const {
residual[0] = T(10.0) - x[0];
return true;
}
};
这里值得注意的是,必须要编写一个重载()运算,而且必须使用模板类型,所有的输入参数和输出参数都要使用T类型。
2、当我们写完了上面的计算残差的方程,接下来就可以使用Ceres库来构造一个求解非线性最小二乘法的Problem来进行求解未知数了。代码如下:
int main(int argc, char** argv)
{
google::InitGoogleLogging(argv[0]); // 指定求解的未知数的初值,这里设置为5.0
double initial_x = 5.0;
double x = initial_x; // 建立Problem
Problem problem; // 建立CostFunction(残差方程)
CostFunction* cost_function =
new AutoDiffCostFunction<CostFunctor, 1, 1>(new CostFunctor);
problem.AddResidualBlock(cost_function, NULL, &x); // 求解方程!
Solver::Options options;
options.linear_solver_type = ceres::DENSE_QR;
options.minimizer_progress_to_stdout = true;
Solver::Summary summary;
Solve(options, &problem, &summary); std::cout << summary.BriefReport() << "\n";
std::cout << "x : " << initial_x
<< " -> " << x << "\n";
return 0;
}
AutoDiffCostFunction需要CostFunctor作为输入。上面的例子编译执行后,会输出下面的信息:
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
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
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
CeresSolver Report: Iterations: 2, Initial cost: 1.250000e+01, Final cost:1.388518e-16, Termination: PARAMETER_TOLERANCE.
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:曲线拟合
下面的例子是指定一系列的点对来拟合一个曲线的系数。这一系列点对是通过曲线插值的点然后添加了标准差的高斯噪声。我们要拟合的曲线形式为:
首先我们定义一个残差结构体,用于计算每一个观测值的残差。代码如下:
struct ExponentialResidual {
ExponentialResidual(double x, double y)
: x_(x), y_(y) {} template <typename T>
bool operator()(const T* const m, const T* const c, T* residual) const {
residual[0] = T(y_) - exp(m[0] * T(x_) + c[0]);
return true;
} private:
// 观测值
const double x_;
const double y_;
};
假设观测点对全部存在数组data中,data中共有2n个数,下面的代码用来演示如何将每个观测点对都加入到Problem中用于后续求解。
double m = 0.0;
double c = 0.0; Problem problem;
for (int i = 0; i < kNumObservations; ++i)
{
CostFunction* cost_function =
new AutoDiffCostFunction<ExponentialResidual, 1, 1, 1>(
new ExponentialResidual(data[2 * i], data[2 * i + 1]));
problem.AddResidualBlock(cost_function, NULL, &m, &c);
}
对上面的代码大致说明下,首先定义m和c的初值都为0,然后定义一个Problem对象,用于后续求解。观测点的个数共有kNumObservations个,遍历每个观测点对,然后new一个残差方程并将其加入Problem中。过程和示例1一样,只不过这里的观测值比较多,需要用一个循环来进行处理。
后续的处理和示例1完全一样,定义一个求解选项和求解报告,然后调用Solve函数进行求解即可。下面时输出的求解过程以及m和c的结果。
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
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
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
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
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
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
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
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
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
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
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
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
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
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
Ceres Solver Report: Iterations: 13, Initial cost: 1.211734e+02, Final cost: 1.056751e+00, Termination: FUNCTION_TOLERANCE.
Initial m: 0 c: 0
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的值的残差还要大。所以说这些点最精确的值应该是我们计算的值,而不是理论值(与添加的高斯噪声有很大的关系)。下面是观测点、拟合点以及曲线的图像。
示例3:单像空间后方交会
通过上面两个示例大致应该熟悉了Ceres库的使用方式,下面我们编写一个求解摄影测量学里面最基础的一个问题,也就是单张像片的空间后方交会程序。学过《摄影测量学》的人肯定知道,后方交会的求解过程,是非常的麻烦,需要将共线方程求偏导数线性化,然后迭代计算外方位元素。
首先给出共线方程:
式中:x0、y0和f为内方位元素,一般为已知量,abc为旋转矩阵,使用三个角元素来表示为:
按照教科书上的求解外方位元素的步骤,都需要10步左右,而且中间还需要求改正数迭代计算,非常复杂。这部分内容可以参考下面基本教科书中的内容:
[1]《摄影测量学》张剑清 潘励 王树根武汉大学出版社 P17-P23
[2]《摄影测量学》王佩军 徐亚明 武汉大学出版社 P60-P67
[3]《摄影测量学》林君建 苍桂华 国防工业出版社 P49-P55
下面我们按照Ceres库的方法来编写后方交会的代码,首先编写一个带有模板重载函数()的结构体,用于计算残差。
struct BackCrossResidual
{
/*
* X, Y, Z, x, y 分别为观测值,f为焦距
*/
BackCrossResidual(double X, double Y, double Z, double x, double y, double f)
:_X(X), _Y(Y), _Z(Z), _x(x), _y(y), _f(f){} /*
* pBackCrossParameters:-2分别为Xs、Ys、Zs,3-5分别为Phi、Omega、Kappa
*/
template <typename T>
bool operator () (const T * const pBackCrossParameters, T* residual) const
{
T dXs = pBackCrossParameters[0];
T dYs = pBackCrossParameters[1];
T dZs = pBackCrossParameters[2];
T dPhi = pBackCrossParameters[3];
T dOmega = pBackCrossParameters[4];
T dKappa = pBackCrossParameters[5]; T a1 = cos(dPhi)*cos(dKappa) - sin(dPhi)*sin(dOmega)*sin(dKappa);
T a2 = -cos(dPhi)*sin(dKappa) - sin(dPhi)*sin(dOmega)*cos(dKappa);
T a3 = -sin(dPhi)*cos(dOmega);
T b1 = cos(dOmega)*sin(dKappa);
T b2 = cos(dOmega)*cos(dKappa);
T b3 = -sin(dOmega);
T c1 = sin(dPhi)*cos(dKappa) + cos(dPhi)*sin(dOmega)*sin(dKappa);
T c2 = -sin(dPhi)*sin(dKappa) + cos(dPhi)*sin(dOmega)*cos(dKappa);
T c3 = cos(dPhi)*cos(dOmega); // 有两个残差
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))));
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)))); return true;
} private:
const double _X;
const double _Y;
const double _Z;
const double _x;
const double _y;
const double _f;
};
对上面的代码解释下,首先有个构造函数,传入的参数共有6个,其中五个是控制点坐标(像方坐标两个xy和物方坐标三个XYZ),另外一个是相机的焦距f。
在重载()函数中,需要的参数初值和残差。初值一共有六个,三个线元素和三个角元素。将三个角元素处理成旋转矩阵所需要的九个参数。
最后按照共线方程编写残差计算公式,这里需要注意的是,一组点可以计算出两个残差,分别是X和Y。
接下来是调用函数,使用的数据是参考书[1]P39第9题(或参考书[2]P89第3题)。题目中给定的相机焦距为153.24mm,坐标点如下表:
点号 |
像点坐标 |
地面坐标 |
|||
x(mm) |
y(mm) |
X(m) |
Y(m) |
Z(m) |
|
1 |
-86.15 |
-68.99 |
36589.41 |
25273.32 |
2195.17 |
2 |
-53.40 |
82.21 |
37631.08 |
31324.51 |
728.69 |
3 |
-14.78 |
-76.63 |
39100.97 |
24934.98 |
2386.50 |
4 |
10.46 |
64.43 |
40426.54 |
30319.81 |
757.31 |
代码如下:
int main(int argc, char** argv)
{
google::InitGoogleLogging(argv[0]); const char *pszFilename= "C:\\后方交会\\sourcedata.txt"; FILE* fid = fopen(pszFilename,"rt");
if(!fid)
return NULL; int nCount = 4;
double* pData = new double[5 * nCount];
memset(pData, 0, sizeof(double)* 5 * nCount); double dBackCrossParameters[6] = {0}; //初值
for(int i=0;i<nCount;i++)
{
double* pTmp = pData+5*i;
fscanf(fid,"%lf %lf %lf%lf %lf",pTmp,pTmp+1,pTmp+2,pTmp+3,pTmp+4); dBackCrossParameters[0]+= pTmp[2]; //X
dBackCrossParameters[1]+= pTmp[3]; //Y
} fclose(fid); dBackCrossParameters[0]/= nCount;
dBackCrossParameters[1]/= nCount; double df = 153.24; //mm
dBackCrossParameters[2]= 50*df; Problem problem;
for (int i = 0; i < nCount;++i)
{
double* pPoint = pData+ 5*i; BackCrossResidual*pResidualX = newBackCrossResidual(pPoint[2],pPoint[3], pPoint[4],pPoint[0]/1000, pPoint[1]/1000,df/1000);
problem.AddResidualBlock(newAutoDiffCostFunction<BackCrossResidual, 2, 6>(pResidualX), NULL,dBackCrossParameters);
} Solver::Options m_options;
Solver::Summary m_summary;
m_options.max_num_iterations = 25;
m_options.linear_solver_type = ceres::DENSE_QR;
m_options.minimizer_progress_to_stdout = true; Solve(m_options, &problem,&m_summary); fprintf(stdout,"Xs=%lfYs=%lf Zs=%lf\n",dBackCrossParameters[0],dBackCrossParameters[1],dBackCrossParameters[2]);
fprintf(stdout,"Phi=%lfOmega=%lf Kappa=%lf\n",dBackCrossParameters[3],dBackCrossParameters[4],dBackCrossParameters[5]);
fprintf(stdout,"%s\n",m_summary.FullReport().c_str()); return 0;
}
接下来对上面的代码进行说明,首先读取坐标点对,一共有4对。一对点包含五个值。将所有的点都读到数组pData中。
接下来定义一个数组用来保存未知数和迭代初值。根据后方交会流程,三个角元素的初值在垂直摄影时可以认为其值为0,而对于三个线元素,Xs和Ys初值用所有的控制点的物方横坐标和纵坐标的均值,而Zs根据焦距和摄影比例尺坟墓共同决定。
然后构造一个Problem对象,并将每个控制点作为观测值构造一个误差方程,需要注意的是像平面坐标单位要和物方坐标单位一致,由于像方坐标和焦距都是毫米,所以都要除以1000换算为米。
程序执行的结果如下图所示,计算的结果与参考书[1]中给出的参考答案完全一样。此外用我之前用偏导数线性化求解的结果也是一致的,从上面的代码可以看出,使用Ceres库完全可以将很复杂的非线性问题用非常简单的代码来求解。
Ceres-Solver库入门的更多相关文章
- Ceres Solver: 高效的非线性优化库(一)
Ceres Solver: 高效的非线性优化库(一) 注:本文基于Ceres官方文档,大部分由英文翻译而来.可作为非官方参考文档. 简介 Ceres,原意是谷神星,是发现不久的一颗轨道在木星和火星之间 ...
- VINS(九)Ceres Solver优化(未完待续)
使用Ceres Solver库处理后端优化问题,首先系统的优化函数为
- Ceres Solver 入门稍微多一点
其实ceres solver用了挺多的,可能是入门不精,有时候感觉感觉不理解代码上是怎么实现的,这次就通过ceres的官网仔细看了一些介绍,感觉对cpp了解更好了一些. 跟g2o的比较的话,感觉cer ...
- Ceres Solver: 高效的非线性优化库(二)实战篇
Ceres Solver: 高效的非线性优化库(二)实战篇 接上篇: Ceres Solver: 高效的非线性优化库(一) 如何求导 Ceres Solver提供了一种自动求导的方案,上一篇我们已经看 ...
- Ceres Solver for android
最近开发中,需要对图片做一些处理与线性技术,这时就用到了Ceres Solver.如何把Ceres Solver集成到Android里呢? 官网给了一个解决方案,简洁明了: Downloa ...
- Ceres 四重奏 之 入门简介
Ceres 翻译为谷神星,是太阳系中的一颗矮行星,于1801年被意大利神父 Piazzi 首次观测到,但随后 Piazzi 因为生病,跟丢了它的运行轨迹. 几个月后,德国数学家 Gauss,利用最小二 ...
- 数据分析与展示——NumPy库入门
这是我学习北京理工大学嵩天老师的<Python数据分析与展示>课程的笔记.嵩老师的课程重点突出.层次分明,在这里特别感谢嵩老师的精彩讲解. NumPy库入门 数据的维度 维度是一组数据的组 ...
- 数据分析与展示——Matplotlib库入门
Matplotlib库入门 Matplotlib库介绍 Matliotlib库是Python优秀的数据可视化第三方库. Matliotlib库的效果见:http://matplotlib.org/ga ...
- Python数据分析与展示(1)-数据分析之表示(1)-NumPy库入门
Numpy库入门 从一个数据到一组数据 维度:一组数据的组织形式 一维数据:由对等关系的有序或无序数据构成,采用线性方式组织. 可用类型:对应列表.数组和集合 不同点: 列表:数据类型可以不同 数组: ...
随机推荐
- 万众瞩目之下,ANGULAR 2终于正式发布啦!
转载:https://angular.io/ 怀着期盼的心情,终于盼到了稳定版本,那么我就可以专心研究了,不再为不定期的修复烦恼咯. 今天,在 Google 总部一个特别的聚会上,我们发布了 Angu ...
- 如何将一个二进制的xxx.bin文件轻松转为C语言数组
今天在写一个SPI-flash读写程序,目的是要将一个二进制文件写到SPI_FLASH中,最后通过开机读取,实际上这个.bin文件就是uboot和second-boot的结合体.通过SD卡写到SPI- ...
- springMVC源码分析--RequestMappingHandlerAdapter(五)
上一篇博客springMVC源码分析--HandlerAdapter(一)中我们主要介绍了一下HandlerAdapter接口相关的内容,实现类及其在DispatcherServlet中执行的顺序,接 ...
- 使用maven执行单元测试总结
maven本身没有单元测试框架,但是maven的default生命周期的test阶段绑定了maven-surefire-plugin插件,该插件可以调用Junit3.Junit4.TestNG等Jav ...
- Java进阶(四十七)Socket通信
Java进阶(四十七)Socket通信 今天讲解一个 Hello Word 级别的 Java Socket 通信的例子.具体通讯过程如下: 先启动Server端,进入一个死循环以便一直监听某端口是 ...
- Gazebo機器人仿真學習探索筆記(三)機器人模型
gazebo_models:https://bitbucket.org/osrf/gazebo_models 模型庫下載,可以參考如下命令: ~/Rob_Soft/Gazebo7$ hg clone ...
- 今天我点亮了CSDN博客专家殊荣
很久以前,看着csdn博客学习第一篇博客时,我依旧记得,是一个名叫蒋老夫子的博客专家,文章写的非常认真.内心很崇拜.在想,若干年后,在哪个地方?以什么样的一种状态,也得到此殊荣,华为有句口号,叫勇敢做 ...
- Dynamics CRM 开启EmailRouter日志记录
找到mailrouter的安装路径,在service文件夹下找到"Microsoft.Crm.Tools.EmailAgent.xml"这个文件,已管理员方式打开,找到loglev ...
- OpenMP基础----以图像处理中的问题为例
OpenMP2.5规范中,对于可以多线程执行的循环有如下5点约束: 1.循环语句中的循环变量必须是有符号整形,如果是无符号整形就无法使用,OpenMP3.0中取消了这个约束 2.循环语句中的比 ...
- 深入解剖unsigned int 和 int
就如同int a:一样,int 也能被其它的修饰符修饰.除void类型外,基本数据类型之前都可以加各种类型修饰符,类型修饰符有如下四种: 1.signed----有符号,可修饰char.int.Int ...