调用Ceres编程实现摄影测量空间前方交会

调用Ceres编程实现摄影测量空间前方交会

有博主写了使用ceres实现后方交会
https://blog.csdn.net/enjoyxiong/article/details/103001241
由于最近在做毕设，需要实现前方交会，所以自己照着用ceres写了一个。

1.空间前方交会

2.实验数据

左影像：x0=0; y0=0; f=152.91;
Xs=970302.448784;
Ys=-1138644.971216;
Zs=3154.584941;
phi=0.010425;
omega=-0.012437;
kapa=0.003380;
右影像：x0=0; y0=0; f=152.91;
Xs=971265.303768;
Ys=-1138634.245942;
Zs=3154.784258;
phi=0.008870;
omega=-0.005062;
kapa=-0.008703;
同名像点坐标：左：（0.153，91.798），右：（-78.672，89.122）

3.ceres编程实现

#include <vector>

#include "ceres/ceres.h"
#include "gflags/gflags.h"
#include "glog/logging.h"

using ceres::AutoDiffCostFunction;
using ceres::CostFunction;
using ceres::Problem;
using ceres::Solve;
using ceres::Solver;

using namespace std;

struct FrontCross {
	FrontCross(double x, double y, double f, double* eo)
		:_x(x), _y(y), _f(f), _eo(eo) {}//f为焦距，eo为外方位元素

	template<typename T>
	bool operator()(const T* const pControlPt, T* residual) const {
		T dX = pControlPt[0];
		T dY = pControlPt[1];
		T dZ = pControlPt[2];

		T Xs = T(_eo[0]); T Ys = T(_eo[1]); T Zs = T(_eo[2]);
		T Phi = T(_eo[3]); T Omega = T(_eo[4]); T Kappa = T(_eo[5]);

		T a1 = cos(Phi) * cos(Kappa) - sin(Phi) * sin(Omega) * sin(Kappa);
		T a2 = -cos(Phi) * sin(Kappa) - sin(Phi) * sin(Omega) * cos(Kappa);
		T a3 = -sin(Phi) * cos(Omega);
		T b1 = cos(Omega) * sin(Kappa);
		T b2 = cos(Omega) * cos(Kappa);
		T b3 = -sin(Omega);
		T c1 = sin(Phi) * cos(Kappa) + cos(Phi) * sin(Omega) * sin(Kappa);
		T c2 = -sin(Phi) * sin(Kappa) + cos(Phi) * sin(Omega) * cos(Kappa);
		T c3 = cos(Phi) * cos(Omega);

		T l1 = T(_f) * a1 + T(_x) * a3;
		T l2 = T(_f) * b1 + T(_x) * b3;
		T l3 = T(_f) * c1 + T(_x) * c3;
		T lx = T(_f) * a1 * Xs + T(_f) * b1 * Ys + T(_f) * c1 * Zs + T(_x) * a3 * Xs + T(_x) * b3 * Ys + T(_x) * c3 * Zs;
		T l4 = T(_f) * a2 + T(_y) * a3;
		T l5 = T(_f) * b2 + T(_y) * b3;
		T l6 = T(_f) * c2 + T(_y) * c3;
		T ly = T(_f) * a2 * Xs + T(_f) * b2 * Ys + T(_f) * c2 * Zs + T(_y) * a3 * Xs + T(_y) * b3 * Ys + T(_y) * c3 * Zs;

		/*residual[0] = T(_x) + T(_f) * T((a1 * (dX - Xs) + b1 * (dY - Ys) + c1 * (dZ - Zs)) / ((a3 * (dX - Xs) + b3 * (dY - Ys) + c3 * (dZ - Zs))));
		residual[1] = T(_y) + T(_f) * T((a2 * (dX - Xs) + b2 * (dY - Ys) + c2 * (dZ - Zs)) / ((a3 * (dX - Xs) + b3 * (dY - Ys) + c3 * (dZ - Zs))));*/
		residual[0] = l1 * dX + l2 * dY + l3 * dZ - lx;
		residual[1] = l4 * dX + l5 * dY + l6 * dZ - ly;
		return true;
	}
private:
	const double _x;
	const double _y;
	const double _f;
	const double* _eo;
};

int main(int argc, char** argv) {
	GFLAGS_NAMESPACE::ParseCommandLineFlags(&argc, &argv, true);
	google::InitGoogleLogging(argv[0]);
	const char* FileName = "forwardcross.txt";//存放一组同名点左右相片的坐标"0.153 91.798 -78.672 89.122"
	FILE* f = fopen(FileName, "rt");

	double* pData = new double[4];
	memset(pData, 0, sizeof(double) * 2 * 2);

	double dControlPt[3] = { 0 };
	double* pTmp = pData;
	fscanf(f, "%lf %lf %lf %lf", pTmp, pTmp + 1, pTmp + 2, pTmp + 3);
	fclose(f);


	//左右相片的外方位元素
	double eoL[12] = { 970302.448784 ,-1138644.971216 ,3154.584941 ,0.010425 ,-0.012437 ,0.003380,971265.303768 ,-1138634.245942 ,3154.784258 ,0.008870 ,-0.005062 ,-0.008703 };
	double* eo = eoL;
	double df = 152.91;
	Problem problem;
	for (int i = 0; i < 2; i++) {
		FrontCross* pResidualX = new FrontCross(pData[i * 2 + 0], pData[i * 2 + 1], df, eo + 6 * i);
		problem.AddResidualBlock(new AutoDiffCostFunction<FrontCross, 2, 3>(pResidualX), NULL, dControlPt);
	}


	Solver::Options options;
	Solver::Summary summary;
	options.max_num_iterations = 25;
	options.linear_solver_type = ceres::DENSE_QR;
	options.minimizer_progress_to_stdout = true;

	Solve(options, &problem, &summary);
	fprintf(stdout, "X=%lf  Y=%lf  Z=%lf\n", dControlPt[0], dControlPt[1], dControlPt[2]);
	fprintf(stdout, "%s\n", summary.FullReport().c_str());
	system("pause");
	return 0;
}

3.结果

4.遇到的问题

如果是这样列残差方程，那么X、Y、Z是需要赋初值的，否则收敛不了，或者即时收敛也是错误的结果。因为这个错误我盯了一下午代码…

residual[0] = T(_x) + T(_f) * T((a1 * (dX - Xs) + b1 * (dY - Ys) + c1 * (dZ - Zs)) / ((a3 * (dX - Xs) + b3 * (dY - Ys) + c3 * (dZ - Zs))));
residual[1] = T(_y) + T(_f) * T((a2 * (dX - Xs) + b2 * (dY - Ys) + c2 * (dZ - Zs)) / ((a3 * (dX - Xs) + b3 * (dY - Ys) + c3 * (dZ - Zs))));

按照下面这种列残差方程，就不需要赋初值

residual[0] = l1 * dX + l2 * dY + l3 * dZ - lx;
residual[1] = l4 * dX + l5 * dY + l6 * dZ - ly;

5.参考博客

https://blog.csdn.net/u011713916/article/details/102511714?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-6.control&dist_request_id=&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromBaidu-6.control