vs2015中使用ceres-solver库实现空间后方交会

vs2015中使用ceres-solver库实现空间后方交会

空间后方交会是指利用航摄像片上三个以上不在一条直线上的控制点按共线方程计算该像片外方位元素的方法。是单幅影像解析过程中的一个步骤。

首先给出共线方程：

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

首先有个构造函数，传入的参数共有6个，其中五个是控制点坐标（像方坐标两个xy和物方坐标三个XYZ），另外一个是相机的焦距f。
在重载()函数中，需要的参数初值和残差。初值一共有六个，三个线元素和三个角元素。将三个角元素处理成旋转矩阵所需要的九个参数。
之后按照共线方程编写残差计算公式，这里需要注意的是，一组点可以计算出两个残差，分别是X和Y。
接下来是调用函数，使用的数据是参考书（《摄影测量学》张剑清 潘励 王树根武汉大学出版社 P17-P23）P39第9题。题目中给定的相机焦距为153.24mm，坐标点如下表（编辑成文本，4行5列，前两列为想点坐标x、y,后三列为地面坐标X、Y、Z，单位为mm）：
-86.15 -68.99 36589.41 25273.32 2195.17
-53.40 82.21 37631.08 31324.51 728.69
-14.78 -76.63 39100.97 24934.98 2386.50
10.46 64.43 40426.54 30319.81 757.31

然后对上面的代码进行说明，首先读取坐标点对，一共有4对。一对点包含五个值。将所有的点都读到数组pData中。
然后定义一个数组用来保存未知数和迭代初值。根据后方交会流程，三个角元素的初值在垂直摄影时可以认为其值为0，而对于三个线元素，Xs和Ys初值用所有的控制点的物方横坐标和纵坐标的均值，而Zs根据焦距和摄影比例尺坟墓共同决定。
然后构造一个Problem对象，并将每个控制点作为观测值构造一个误差方程，需要注意的是像平面坐标单位要和物方坐标单位一致，由于像方坐标和焦距都是毫米，所以都要除以1000换算为米。
代码如下：

#include <ceres\ceres.h>
#include <glog\logging.h>
using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solver;

using namespace std;

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：0## 标题-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]