近景摄影测量-直接线性变换DLT原理详解与代码示例

直接线性变换算法的计算步骤详解

直接线性变换是建立像点坐标仪坐标和相应物点物方空间坐标之间直接的线性关系的算法。可以用照片中若干控制点的像点坐标和物方坐标计算内外方位元素、畸变系数,以及前交计算未知点的物方坐标。它有以下特点:

  1. 照片不需要归心、定向
  2. 无需内方位元素值和外方位元素的初始近似值
  3. 特别适用于非量测相机的摄影处理
  4. 需要物方的一组控制点
  5. 本质上是一个后方交会-前方交会的过程

直接线性变化的结算过程,包括l系数的解算、空间未知点坐标(XYZ)的解算两个解算步骤。还包括将L系数转化为畸变系数、内外方位元素的转化过程与中误差计算。这四大步骤将在下文按照我自己学习与理解逐个拆分详细讲解。

一、按照直接线性变换基本关系式,以像点坐标量测值为观测值,将各系数改正值作为未知数,根据控制点解算直接线性变换系数L_{1-11}、比例不归一系数d_s、坐标系不正交系数d_\beta(包括估计值解算,像主点解算,精确值解算三个步骤)。

        直接线性变换基本关系式原则上也是通过共线条件方程式演绎而来,通过考虑比例尺不归一误差d_s和坐标系不正交误差d_\beta,将原式经过代数演绎,化简为如下的DLT基本关系式:

        其中:L_{1-11}是内方位元素、外方位元素、dβ、ds的函数。

        1)解算L系数的近似值,该步骤不需要迭代平差,只需要解方程即可,求解11个未知数,共需要5.5个控制点:

        2)求解像主点坐标:

        3)迭代求解L系数的精确值,并加入畸变差系数:

        此处使用的畸变模型包含k_1 k_2 P_1 P_2:

        加入观测值改正数和像点坐标非线性改正后的方程为:

        取符号:

        误差方程式如下:

        可以写作V = ML - W的形式

        其中L=[l_1l_2l_3l_4l_5l_6l_7l_8l_9l_{10}l_{11}k_1k_2P_1P_2],注意此处未知数并不是改正值。

        然后利用计算,利用计算得到的未知数值替换原来的值进行迭代求解,每次迭代要重新计算像主点坐标\left ( x_0,y_0\right )和A,直到上一次与本次计算的结果之差小于某一个阈值,停止迭代,输出结果,我设置的是0.001。

二、再利用解求得到的系数,利用直接线性变换基本关系式列出计算物方空间坐标的方程式,解算待求点物方坐标(包括像点坐标改正、物方坐标估计值计算、精确值计算),待求点包括活动控制架上的人工标志点和检查点。

        1)像点坐标改正:

        根据上述计算得到的畸变系数,改正每一个像点的非线性误差:

        2)待求点空间坐标近似值(X,Y,Z)计算

        依照基本关系式,可以得到如下计算XYZ值的方程式:

        计算空间坐标相当于前方交会,需要左右两张影像,至少三个方程求解,两 张影像的L系数分别为L和L',当略去四个方程中的一个时,有如下方程组:

        即可计算物方坐标的近似值

        3)待求点空间坐标精确值(X,Y,Z)计算

        把改正了非线性误差的x+Δx作为x,y+Δy作为y。

        得到以XYZ作为未知数的误差方程式:

        将所有点列出的误差方程合并为一个大方程组:

        其中 S=[X YZ]^T,注意此处也不是改正值作为未知数。

        利用最小二乘平差原理计算未知数的值:,然后利用计算得到的S代替原来的未知数作为下一次迭代的计算数据,若本次计算结果与上一次计算结果之差小于某一个阈值,则结束迭代,退出循环,输出结果。我设置的阈值为0.01。

三、并将L系数转化为ds、dβ、影像的内外方位元素、畸变系数 k_1 k_2 P_1 P_2

        中间使用到的代数符号:

        其中:

        再计算:

        计算比例不归一误差ds和坐标系不正交系数dβ

        计算内方位元素:

        

        计算外方位元素:

        线元素(解方程组):

        

        角元素:

四、计计算精度:像点坐标观测值单位权中误差、各未知数的中误差、像点观测值残差、检查点的物方空间坐标差值及精度统计(外精度)。

代码示例:

#include "opencv2/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include <math.h>
#include<fstream>
#include<sstream>
#include<vector>
#include<iostream>
#include<iomanip>
using namespace std;
using namespace cv;

#define ROWS 4160
#define COLS 6240
#define PIXSIZE 0.00376

//-----------------------------------------结构体----------------------------------------------------
struct ControlPoint//控制点
{
	int flag;
	double X;
	double Y;
	double Z;
};

struct imgPoint//像点
{
	int flag;
	double x;
	double y;
};

struct mergedpt//组合点
{
	int flag;
	double X;
	double Y;
	double Z;
	double x;
	double y;
};

//外方位元素结构
struct LastParam {
	//内方位元素
	double x0 = 0;
	double y0 = 0;
	double f = 0;
	//畸变系数
	double k1 = 0;
	double k2 = 0;
	double p1 = 0;
	double p2 = 0;
	//外方位元素
	double Phi = 0;
	double Omega = 0;
	double Kappa = 0;
	double Xs = 0;
	double Ys = 0;
	double Zs = 0;
	//比例尺不一系数
	double dp = 0;
	double ds = 0;

};

//-----------------------------------------读取坐标函数----------------------------------------------------
void readClptData(char* file, vector<ControlPoint>& ControlPoints)
{
	ifstream inFile(file);
	if (!inFile)
	{
		cout << "文件读取失败,请检查文件路径" << endl;
		return;
	}
	string line;
	string firstLine;
	getline(inFile, firstLine);
	while (getline(inFile, line)) {
		ControlPoint clpt;
		istringstream iss(line);
		iss >> clpt.flag;
		iss >> clpt.Z;
		iss >> clpt.X;
		iss >> clpt.Y;
		clpt.Z = -1.0 * clpt.Z;
		ControlPoints.push_back(clpt);
	}
	inFile.close();
}

void readImgData(char* file, vector<imgPoint>& imgPoints)
{
	ifstream inFile(file);
	if (!inFile)
	{
		cout << "文件读取失败,请检查文件路径" << endl;
		return;
	}
	string line;
	while (getline(inFile, line)) {
		imgPoint imgpt;
		istringstream iss(line);
		iss >> imgpt.flag;
		iss >> imgpt.x;
		iss >> imgpt.y;
		imgPoints.push_back(imgpt);
	}
	inFile.close();
}

//-----------------------------------------坐标系转换&坐标合并----------------------------------------------------


void cvtPix2Img(vector<imgPoint>& imgPoints)
{
	for (int i = 0; i < imgPoints.size(); i++)
	{
		imgPoints[i].x = (imgPoints[i].x - COLS / 2) * PIXSIZE;
		imgPoints[i].y = -1.0 * (imgPoints[i].y - ROWS / 2) * PIXSIZE;
	}
}

void merge_point(vector<imgPoint>& imgPoints, vector<ControlPoint>& ControlPoints, vector<mergedpt>& mergedpts)
{
	mergedpt pair;
	for (int i = 0; i < imgPoints.size(); i++)
	{
		for (int j = 0; j < ControlPoints.size(); j++)
		{
			if (imgPoints[i].flag == ControlPoints[j].flag)
			{
				pair.flag = imgPoints[i].flag;
				pair.X = ControlPoints[j].X;
				pair.Y = ControlPoints[j].Y;
				pair.Z = ControlPoints[j].Z;
				pair.x = imgPoints[i].x;
				pair.y = imgPoints[i].y;
				mergedpts.push_back(pair);
				break;
			}
		}
	}
}

//-----------------------------------------求解L系数----------------------------------------------------
// 1求粗略解 2求x0y0 3求l系数精确解
// 注意:求l系数精确解的时候要同时更新x0和y0
// 
//求解L系数近似值
Mat cal_L_ApproValue(vector<mergedpt> selectedPairs)
{
	//列方程(舍弃一个方程)
	Mat A = Mat::zeros(2 * selectedPairs.size(), 11, CV_64FC1);
	Mat Li = Mat::zeros(11, 1, CV_64FC1);
	Mat C = Mat::zeros(2 * selectedPairs.size(), 1, CV_64FC1);
	//填充系数阵和常数阵
	for (int i = 0; i < selectedPairs.size(); i++)
	{
		A.at<double>(i * 2, 0) = selectedPairs[i].X;
		A.at<double>(i * 2, 1) = selectedPairs[i].Y;
		A.at<double>(i * 2, 2) = selectedPairs[i].Z;
		A.at<double>(i * 2, 3) = 1.0;
		A.at<double>(i * 2, 4) = 0.0;
		A.at<double>(i * 2, 5) = 0.0;
		A.at<double>(i * 2, 6) = 0.0;
		A.at<double>(i * 2, 7) = 0.0;
		A.at<double>(i * 2, 8) = selectedPairs[i].x * selectedPairs[i].X;
		A.at<double>(i * 2, 9) = selectedPairs[i].x * selectedPairs[i].Y;
		A.at<double>(i * 2, 10) = selectedPairs[i].x * selectedPairs[i].Z;
		A.at<double>(i * 2 + 1, 0) = 0.0;
		A.at<double>(i * 2 + 1, 1) = 0.0;
		A.at<double>(i * 2 + 1, 2) = 0.0;
		A.at<double>(i * 2 + 1, 3) = 0.0;
		A.at<double>(i * 2 + 1, 4) = selectedPairs[i].X;
		A.at<double>(i * 2 + 1, 5) = selectedPairs[i].Y;
		A.at<double>(i * 2 + 1, 6) = selectedPairs[i].Z;
		A.at<double>(i * 2 + 1, 7) = 1.0;
		A.at<double>(i * 2 + 1, 8) = selectedPairs[i].y * selectedPairs[i].X;
		A.at<double>(i * 2 + 1, 9) = selectedPairs[i].y * selectedPairs[i].Y;
		A.at<double>(i * 2 + 1, 10) = selectedPairs[i].y * selectedPairs[i].Z;
		 
		C.at<double>(i * 2, 0) = selectedPairs[i].x;
		C.at<double>(i * 2 + 1, 0) = selectedPairs[i].y;
	}
	//cout << A << endl;
	//求解
	Li = (A.t() * A).inv() * A.t() * C;
	return Li;
}
 
//求解内方位元素x0 y0 dp ds
void cal_InteriorParams(Mat Li, LastParam& Last)
{
	double l1 = Li.at<double>(0, 0);
	double l2 = Li.at<double>(1, 0);
	double l3 = Li.at<double>(2, 0);
	double l5 = Li.at<double>(4, 0);
	double l6 = Li.at<double>(5, 0);
	double l7 = Li.at<double>(6, 0);
	double l9 = Li.at<double>(8, 0);
	double l10 = Li.at<double>(9, 0);
	double l11 = Li.at<double>(10, 0);
	Last.x0 = -1.0 * (l1 * l9 + l2 * l10 + l3 * l11) / (l9 * l9 + l10 * l10 + l11 * l11);
	Last.y0 = -1.0 * (l5 * l9 + l6 * l10 + l7 * l11) / (l9 * l9 + l10 * l10 + l11 * l11);
	double A = (l1 * l1 + l2 * l2 + l3 * l3) / (l9 * l9 + l10 * l10 + l11 * l11) - Last.x0 * Last.x0;
	double B = (l5 * l5 + l6 * l6 + l7 * l7) / (l9 * l9 + l10 * l10 + l11 * l11) - Last.y0 * Last.y0;
	double C = (l1 * l5 + l2 * l6 + l3 * l7) / (l9 * l9 + l10 * l10 + l11 * l11) - Last.x0 * Last.y0;
	Last.f = sqrt((A * B - C * C) / B);
	double dp = asin(sqrt(C * C / (A * B)));
	Last.ds = sqrt(A / B) - 1;
	if (dp * C > 0)
	{
		Last.dp = -1.0 * dp;
		return;
	}
	Last.dp = dp;
}

//填充精确解系数
void cal_L_AccurateValue(Mat Li, Mat& A, Mat& l, LastParam& Last, vector<mergedpt> pairs)
{
	double l1 = Li.at<double>(0, 0);
	double l2 = Li.at<double>(1, 0);
	double l3 = Li.at<double>(2, 0);
	double l5 = Li.at<double>(4, 0);
	double l6 = Li.at<double>(5, 0);
	double l7 = Li.at<double>(6, 0);
	double l9 = Li.at<double>(8, 0);
	double l10 = Li.at<double>(9, 0);
	double l11 = Li.at<double>(10, 0);
	double f = Last.f;
	double x0 = Last.x0;
	double y0 = Last.y0;
	for (int i = 0; i < pairs.size(); i++)
	{
		double X = pairs[i].X;
		double Y = pairs[i].Y;
		double Z = pairs[i].Z;
		double x = pairs[i].x;
		double y = pairs[i].y;
		double a = l9 * pairs[i].X + l10 * pairs[i].Y + l11 * pairs[i].Z + 1;
		double r_2 = pow(x - x0, 2) + pow(y - y0, 2);
		A.at<double>(2 * i, 0) = -1.0 * X / a;
		A.at<double>(2 * i, 1) = -1.0 * Y / a;
		A.at<double>(2 * i, 2) = -1.0 * Z / a;
		A.at<double>(2 * i, 3) = -1.0 * 1 / a;
		A.at<double>(2 * i, 4) = 0;
		A.at<double>(2 * i, 5) = 0;
		A.at<double>(2 * i, 6) = 0;
		A.at<double>(2 * i, 7) = 0;
		A.at<double>(2 * i, 8) = -1.0 * x * X / a;
		A.at<double>(2 * i, 9) = -1.0 * x * Y / a;
		A.at<double>(2 * i, 10) = -1.0 * x * Z / a;
		A.at<double>(2 * i, 11) = -1.0 * (x - x0) * r_2;
		A.at<double>(2 * i, 12) = -1.0 * (x - x0) * r_2 * r_2;
		A.at<double>(2 * i, 13) = -1.0 * (r_2 + 2 * (x - x0) * (x - x0));
		A.at<double>(2 * i, 14) = -2.0 * (x - x0) * (y - y0);

		A.at<double>(2 * i + 1, 0) = 0.0;
		A.at<double>(2 * i + 1, 1) = 0.0;
		A.at<double>(2 * i + 1, 2) = 0.0;
		A.at<double>(2 * i + 1, 3) = 0.0;
		A.at<double>(2 * i + 1, 4) = -1.0 * X / a;
		A.at<double>(2 * i + 1, 5) = -1.0 * Y / a;
		A.at<double>(2 * i + 1, 6) = -1.0 * Z / a;
		A.at<double>(2 * i + 1, 7) = -1.0 * 1 / a;
		A.at<double>(2 * i + 1, 8) = -1.0 * y * X / a;
		A.at<double>(2 * i + 1, 9) = -1.0 * y * Y / a;
		A.at<double>(2 * i + 1, 10) = -1.0 * y * Z / a;
		A.at<double>(2 * i + 1, 11) = -1.0 * (y - y0) * r_2;
		A.at<double>(2 * i + 1, 12) = -1.0 * (y - y0) * r_2 * r_2;
		A.at<double>(2 * i + 1, 13) = -2.0 * (y - y0) * (x - x0);
		A.at<double>(2 * i + 1, 14) = -1.0 * (r_2 + 2 * (y - y0) * (y - y0));

		l.at<double>(2 * i, 0) = x / a;
		l.at<double>(2 * i + 1, 0) = y / a;
	}
}

//-----------------------------------------求解物方坐标----------------------------------------------------
// 1将像点坐标畸变改正 2计算粗略值 3计算精确值
// 注意:更新x0y0--A
//

//求解改正了非线性误差的像点坐标
void modifyImgPoints(vector<imgPoint>& mergedpts, LastParam Last)
{
	for (int i = 0; i < mergedpts.size(); i++)
	{
		double x = mergedpts[i].x;
		double y = mergedpts[i].y;
		double x0 = Last.x0;
		double y0 = Last.y0;
		double k1 = Last.k1;
		double k2 = Last.k2;
		double p1 = Last.p1;
		double p2 = Last.p2;
		double r_2 = (x - x0) * (x - x0) + (y - y0) * (y - y0);
		mergedpts[i].x = x + (x - x0) * (k1 * r_2 + k2 * r_2 * r_2) + p1 * (r_2 + 2 * (x - x0) * (x - x0)) + 2 * p2 * (x - x0) * (y - y0);
		mergedpts[i].y = y + (y - y0) * (k1 * r_2 + k2 * r_2 * r_2) + p2 * (r_2 + 2 * (y - y0) * (y - y0)) + 2 * p1 * (x - x0) * (y - y0);

	}
}

//计算物方坐标粗略值
Mat cal_XYZ_ApproValue(imgPoint left_pair, imgPoint right_pair, Mat left_Li, Mat right_Li)
{
	Mat XYZ_RoughValue = Mat::zeros(3, 1, CV_64FC1);
	Mat A = Mat::zeros(4, 3, CV_64FC1);
	Mat C = Mat::zeros(4, 1, CV_64FC1);
	//填充系数阵和常数阵
	A.at<double>(0, 0) = left_Li.at<double>(0, 0) + left_pair.x * left_Li.at<double>(8, 0);
	A.at<double>(0, 1) = left_Li.at<double>(1, 0) + left_pair.x * left_Li.at<double>(9, 0);
	A.at<double>(0, 2) = left_Li.at<double>(2, 0) + left_pair.x * left_Li.at<double>(10, 0);
	A.at<double>(1, 0) = left_Li.at<double>(4, 0) + left_pair.y * left_Li.at<double>(8, 0);
	A.at<double>(1, 1) = left_Li.at<double>(5, 0) + left_pair.y * left_Li.at<double>(9, 0);
	A.at<double>(1, 2) = left_Li.at<double>(6, 0) + left_pair.y * left_Li.at<double>(10, 0);
	A.at<double>(2, 0) = right_Li.at<double>(0, 0) + right_pair.x * right_Li.at<double>(8, 0);
	A.at<double>(2, 1) = right_Li.at<double>(1, 0) + right_pair.x * right_Li.at<double>(9, 0);
	A.at<double>(2, 2) = right_Li.at<double>(2, 0) + right_pair.x * right_Li.at<double>(10, 0);
	A.at<double>(3, 0) = right_Li.at<double>(4, 0) + right_pair.y * right_Li.at<double>(8, 0);
	A.at<double>(3, 1) = right_Li.at<double>(5, 0) + right_pair.y * right_Li.at<double>(9, 0);
	A.at<double>(3, 2) = right_Li.at<double>(6, 0) + right_pair.y * right_Li.at<double>(10, 0);

	C.at<double>(0, 0) = -1.0 * (left_Li.at<double>(3, 0) + left_pair.x);
	C.at<double>(1, 0) = -1.0 * (left_Li.at<double>(7, 0) + left_pair.y);
	C.at<double>(2, 0) = -1.0 * (right_Li.at<double>(3, 0) + right_pair.x);
	C.at<double>(3, 0) = -1.0 * (right_Li.at<double>(7, 0) + right_pair.y);

	XYZ_RoughValue = (A.t() * A).inv() * A.t() * C;
	return XYZ_RoughValue;
}

//物方坐标精确值计算
ControlPoint cal_XYZ_AccurateValue(Mat left_Li, Mat right_Li, ControlPoint pointInOS, imgPoint left_Point, imgPoint right_Point, LastParam left_Last, LastParam right_Last)
{
	Mat A = Mat::zeros(4, 3, CV_64FC1);
	Mat l = Mat::zeros(4, 1, CV_64FC1);
	double l_l1 = left_Li.at<double>(0, 0);	double r_l1 = right_Li.at<double>(0, 0);
	double l_l2 = left_Li.at<double>(1, 0);	double r_l2 = right_Li.at<double>(1, 0);
	double l_l3 = left_Li.at<double>(2, 0);	double r_l3 = right_Li.at<double>(2, 0);
	double l_l4 = left_Li.at<double>(3, 0);	double r_l4 = right_Li.at<double>(3, 0);
	double l_l5 = left_Li.at<double>(4, 0);	double r_l5 = right_Li.at<double>(4, 0);
	double l_l6 = left_Li.at<double>(5, 0);	double r_l6 = right_Li.at<double>(5, 0);
	double l_l7 = left_Li.at<double>(6, 0);	double r_l7 = right_Li.at<double>(6, 0);
	double l_l8 = left_Li.at<double>(7, 0);	double r_l8 = right_Li.at<double>(7, 0);
	double l_l9 = left_Li.at<double>(8, 0); double r_l9 = right_Li.at<double>(8, 0);
	double l_l10 = left_Li.at<double>(9, 0); double r_l10 = right_Li.at<double>(9, 0);
	double l_l11 = left_Li.at<double>(10, 0); double r_l11 = right_Li.at<double>(10, 0);
	double former_X = 0.0; double former_Y = 0.0; double former_Z = 0.0;
	double X = pointInOS.X; double Y = pointInOS.Y; double Z = pointInOS.Z;
	while (true)
	{
		double l_x = left_Point.x; double r_x = right_Point.x;
		double l_y = left_Point.y; double r_y = right_Point.y;
		double l_a = l_l9 * X + l_l10 * Y + l_l11 * Z + 1; double r_a = r_l9 * X + r_l10 * Y + r_l11 * Z + 1;
		//左片
		A.at<double>(0, 0) = -1.0 * (l_l1 + l_l9 * l_x) / l_a;
		A.at<double>(0, 1) = -1.0 * (l_l2 + l_l10 * l_x) / l_a;
		A.at<double>(0, 2) = -1.0 * (l_l3 + l_l11 * l_x) / l_a;
		A.at<double>(1, 0) = -1.0 * (l_l5 + l_l9 * l_y) / l_a;
		A.at<double>(1, 1) = -1.0 * (l_l6 + l_l10 * l_y) / l_a;
		A.at<double>(1, 2) = -1.0 * (l_l7 + l_l11 * l_y) / l_a;
		//右片
		A.at<double>(2, 0) = -1.0 * (r_l1 + r_l9 * r_x) / r_a;
		A.at<double>(2, 1) = -1.0 * (r_l2 + r_l10 * r_x) / r_a;
		A.at<double>(2, 2) = -1.0 * (r_l3 + r_l11 * r_x) / r_a;
		A.at<double>(3, 0) = -1.0 * (r_l5 + r_l9 * r_y) / r_a;
		A.at<double>(3, 1) = -1.0 * (r_l6 + r_l10 * r_y) / r_a;
		A.at<double>(3, 2) = -1.0 * (r_l7 + r_l11 * r_y) / r_a;

		l.at<double>(0, 0) = (l_l4 + l_x) / l_a;
		l.at<double>(1, 0) = (l_l8 + l_y) / l_a;
		l.at<double>(2, 0) = (r_l4 + r_x) / r_a;
		l.at<double>(3, 0) = (r_l8 + r_y) / r_a;

		Mat XYZ_AcValue = (A.t() * A).inv() * A.t() * l;
		if (abs(former_X - X < 0.01) && abs(former_Y - Y) < 0.01 && abs(former_Z - Z) < 0.01)
		{
			pointInOS.X = XYZ_AcValue.at<double>(0, 0);
			pointInOS.Y = XYZ_AcValue.at<double>(1, 0);
			pointInOS.Z = XYZ_AcValue.at<double>(2, 0);
			break;
		}
		former_X = X; former_Y = Y; former_Z = Z;
		X = XYZ_AcValue.at<double>(0, 0);
		Y = XYZ_AcValue.at<double>(1, 0);
		Z = XYZ_AcValue.at<double>(2, 0);
	}
	return pointInOS;
}

//-----------------------------------------转换为内外方位元素和畸变系数----------------------------------------------------
// 
// 
//计算内外方位元素和dp、ds
void cal_ExteriorParams(Mat final, LastParam& Last, vector<mergedpt> pairs)
{
	double l1 = final.at<double>(0, 0);
	double l2 = final.at<double>(1, 0);
	double l3 = final.at<double>(2, 0);
	double l4 = final.at<double>(3, 0);
	double l5 = final.at<double>(4, 0);
	double l6 = final.at<double>(5, 0);
	double l7 = final.at<double>(6, 0);
	double l8 = final.at<double>(7, 0);
	double l9 = final.at<double>(8, 0);
	double l10 = final.at<double>(9, 0);
	double l11 = final.at<double>(10, 0);
	Mat A = Mat::zeros(3, 3, CV_64FC1);
	Mat C = Mat::zeros(3, 1, CV_64FC1);
	A.at<double>(0, 0) = l1;
	A.at<double>(0, 1) = l2;
	A.at<double>(0, 2) = l3;
	A.at<double>(1, 0) = l5;
	A.at<double>(1, 1) = l6;
	A.at<double>(1, 2) = l7;
	A.at<double>(2, 0) = l9;
	A.at<double>(2, 1) = l10;
	A.at<double>(2, 2) = l11;

	C.at<double>(0, 0) = -1.0 * l4;
	C.at<double>(1, 0) = -1.0 * l8;
	C.at<double>(2, 0) = -1.0;

	//外方位线元素
	Mat X = A.inv() * C;
	Last.Xs = X.at<double>(0, 0);
	Last.Ys = X.at<double>(1, 0);
	Last.Zs = X.at<double>(2, 0);

	double factor = sqrt(l9 * l9 + l10 * l10 + l11 * l11);
	double a3 = l9 / factor;
	double b3 = l10 / factor;
	double c3 = l11 / factor;

	//内方位元素和ds dp
	double x0 = Last.x0;
	double y0 = Last.y0;
	double f = Last.f;
	double ds = Last.ds;
	double dp = Last.dp;
	double b1 = (l2 / factor + b3 * x0 + (y0 * b3 + l6) * (1 + ds) * sin(dp)) / f;
	double b2 = (l6 / factor + b3 * y0) * (1 + ds) * cos(dp) / f;

	//外方位角元素
	Last.Phi = atan(-1.0 * a3 / c3);
	Last.Omega = asin(-1.0 * b3);
	Last.Kappa = atan(b1 / b2);

	//畸变系数直接从结构体中读取
}


int main()
{
//--------------------------------------------初始化------------------------------------------------------
	//解算内方位元素初值
	LastParam left_Last; left_Last.x0 = 0; left_Last.y0 = 0; left_Last.f = 0;
	left_Last.Xs = 0; left_Last.Ys = 0; left_Last.Zs = 0;
	left_Last.Kappa = 0; left_Last.Omega = 0; left_Last.Phi = 0;
	left_Last.k1 = 0; left_Last.k2 = 0; left_Last.p1 = 0; left_Last.p2 = 0;
	LastParam right_Last;
	right_Last.x0 = 0; right_Last.y0 = 0; right_Last.f = 0;
	right_Last.Xs = 0; right_Last.Ys = 0; right_Last.Zs = 0;
	right_Last.Kappa = 0; right_Last.Omega = 0; right_Last.Phi = 0;
	right_Last.k1 = 0;  right_Last.k2 = 0; right_Last.p1 = 0; right_Last.p2 = 0;

//-----------------------------------------读取坐标函数----------------------------------------------------

	char clptfile[] = "./data2/ctrl.txt";
	//char left_file[] = "./data2/coordinates_left2.txt";
	//char right_file[] = "./data2/coordinates_right2.txt";
	char left_file[] = "C:/Users/LENOVO/Desktop/近景摄影测量/datapt/pointallLL.txt";
	char right_file[] = "C:/Users/LENOVO/Desktop/近景摄影测量/datapt/pointallRR.txt";
	char l_unknown_file[] = "./data2/coordinates_unknown_left.txt";
	char r_unknown_file[] = "./data2/coordinates_unknown_right.txt";
	char l_ckptfile[] = "./data2/coordinates_check_left.txt";
	char r_ckptfile[] = "./data2/coordinates_check_right.txt";
	//读取控制点数据和左右片标志点的像素坐标
	vector<ControlPoint> ControlPoints;
	vector<imgPoint> left_imgPoints;
	vector<imgPoint> right_imgPoints;
	vector<imgPoint> left_unPoints;
	vector<imgPoint> right_unPoints;
	vector<imgPoint> left_ckpts;
	vector<imgPoint> right_ckpts;
	readClptData(clptfile, ControlPoints);
	readImgData(left_file, left_imgPoints);
	readImgData(right_file, right_imgPoints);
	readImgData(l_unknown_file, left_unPoints);
	readImgData(r_unknown_file, right_unPoints);
	readImgData(l_ckptfile, left_ckpts);
	readImgData(r_ckptfile, right_ckpts);

//-----------------------------------------坐标系转换&坐标合并----------------------------------------------------
// 
	//将像素坐标(pixel)转换为图像坐标(mm)
	cvtPix2Img(left_imgPoints);
	cvtPix2Img(right_imgPoints);
	cvtPix2Img(left_unPoints);
	cvtPix2Img(right_unPoints);
	cvtPix2Img(left_ckpts);
	cvtPix2Img(right_ckpts);

	//合成点对
	vector<mergedpt> left_mergedpts;
	vector<mergedpt> right_mergedpts;
	vector<mergedpt> left_ckpt_Pairs;
	vector<mergedpt> right_ckpt_Pairs;
	merge_point(left_imgPoints, ControlPoints, left_mergedpts);
	merge_point(right_imgPoints, ControlPoints, right_mergedpts);
	merge_point(left_ckpts, ControlPoints, left_ckpt_Pairs);
	merge_point(right_ckpts, ControlPoints, right_ckpt_Pairs);


//-----------------------------------------求解L系数----------------------------------------------------
// 1求粗略解 2求x0y0 3求l系数精确解
// 注意:求l系数精确解的时候要同时更新x0和y0
// 
// 
	//解算l系数初值
	Mat left_L_ApporValue = cal_L_ApproValue(left_mergedpts);
	Mat right_L_RoughValue = cal_L_ApproValue(right_mergedpts);

	//计算x0和y0
	cal_InteriorParams(left_L_ApporValue, left_Last);
	cal_InteriorParams(right_L_RoughValue, right_Last);

	//解算l系数精确值
	//确定系数
	Mat left_A = Mat::zeros(2 * left_mergedpts.size(), 15, CV_64FC1);
	Mat left_l = Mat::zeros(2 * left_mergedpts.size(), 1, CV_64FC1);
	Mat right_A = Mat::zeros(2 * right_mergedpts.size(), 15, CV_64FC1);
	Mat right_l = Mat::zeros(2 * right_mergedpts.size(), 1, CV_64FC1);
	double former_left_f = left_Last.f;
	double former_right_f = right_Last.f;
	double former_left_x0 = left_Last.x0;
	double former_left_y0 = left_Last.y0;
	Mat left_L_AccurateValue;
	Mat right_L_AccurateValue;
	//迭代左片
	while (true)
	{
		cal_L_AccurateValue(left_L_ApporValue, left_A, left_l, left_Last, left_mergedpts);
		left_L_AccurateValue = (left_A.t() * left_A).inv() * left_A.t() * left_l;
		left_Last.k1 = left_L_AccurateValue.at<double>(11, 0);
		left_Last.k2 = left_L_AccurateValue.at<double>(12, 0);
		left_Last.p1 = left_L_AccurateValue.at<double>(13, 0);
		left_Last.p2 = left_L_AccurateValue.at<double>(14, 0);
		
		left_L_ApporValue.at<double>(0, 0) = left_L_AccurateValue.at<double>(0, 0);
		left_L_ApporValue.at<double>(1, 0) = left_L_AccurateValue.at<double>(1, 0);
		left_L_ApporValue.at<double>(2, 0) = left_L_AccurateValue.at<double>(2, 0);
		left_L_ApporValue.at<double>(3, 0) = left_L_AccurateValue.at<double>(3, 0);
		left_L_ApporValue.at<double>(4, 0) = left_L_AccurateValue.at<double>(4, 0);
		left_L_ApporValue.at<double>(5, 0) = left_L_AccurateValue.at<double>(5, 0);
		left_L_ApporValue.at<double>(6, 0) = left_L_AccurateValue.at<double>(6, 0);
		left_L_ApporValue.at<double>(7, 0) = left_L_AccurateValue.at<double>(7, 0);
		left_L_ApporValue.at<double>(8, 0) = left_L_AccurateValue.at<double>(8, 0);
		left_L_ApporValue.at<double>(9, 0) = left_L_AccurateValue.at<double>(9, 0);
		left_L_ApporValue.at<double>(10, 0) = left_L_AccurateValue.at<double>(10, 0);
		cal_InteriorParams(left_L_AccurateValue, left_Last);//更新内方位元素
		if (abs(left_Last.f - former_left_f) < 0.001 && abs(left_Last.x0 - former_left_x0 < 0.001) && abs(left_Last.y0 - former_left_y0 < 0.001))
		{

			cout << "------------------------------" << endl;
			cout << "左片Li精确值为:" << endl;
			for (int k = 0; k < left_L_AccurateValue.rows - 4; k++)
				cout << "L" << k + 1 << "\t" << left_L_AccurateValue.at<double>(k, 0) << endl;
			//精度统计
			Mat V = left_A * left_L_AccurateValue - left_l;
			Mat sigma0 = (V.t() * V / (2 * left_mergedpts.size() - 15));
			cout << "------------------------------" << endl;
			cout << "左片像点坐标观测值的单位权中误差/mm:" << sqrt(sigma0.at<double>(0, 0)) << endl;
			cout << "------------------------------" << endl;
			Mat Qxx = (left_A.t() * left_A).inv();
			vector<string> str = { "L1", "L2", "L3", "L4", "L5", "L6", "L7", "L8", "L9", "L10", "L11", "k1", "k2", "p1", "p2" };
			cout << "未知数中误差:" << endl;
			for (int i = 0; i < 15; i++)
			{
				double sigmaXi = sqrt(Qxx.at<double>(i, i)) * sigma0.at<double>(0, 0);
				cout << str[i] << ": " << sigmaXi << endl;
			}
			cout << "------------------------------" << endl;
			int num = 0;
			cout << "左片像点坐标观测值的残差/pixel:" << endl;
			cout << "点号" << "\t" << "Vx" << "\t\t" << "Vy" << endl;

			for (int i = 0; i < V.rows; i += 2)
			{
				cout << left_mergedpts[num].flag  << "\t" << V.at<double>(i, 0) / PIXSIZE << "\t" << V.at<double>(i + 1, 0) / PIXSIZE << endl;
				num++;
			}
			cout.unsetf(ios::scientific);
			
			
			break;
		}
		former_left_f = left_Last.f;
		former_left_x0 = left_Last.x0;
		former_left_y0 = left_Last.y0;
	}
	//迭代右片
	while (true)
	{
		cal_L_AccurateValue(right_L_RoughValue, right_A, right_l, right_Last, right_mergedpts);
		right_L_AccurateValue = (right_A.t() * right_A).inv() * right_A.t() * right_l;
		right_Last.k1 = right_L_AccurateValue.at<double>(11, 0);
		right_Last.k2 = right_L_AccurateValue.at<double>(12, 0);
		right_Last.p1 = right_L_AccurateValue.at<double>(13, 0);
		right_Last.p2 = right_L_AccurateValue.at<double>(14, 0);
		right_L_RoughValue.at<double>(0, 0) = right_L_AccurateValue.at<double>(0, 0);
		right_L_RoughValue.at<double>(1, 0) = right_L_AccurateValue.at<double>(1, 0);
		right_L_RoughValue.at<double>(2, 0) = right_L_AccurateValue.at<double>(2, 0);
		right_L_RoughValue.at<double>(3, 0) = right_L_AccurateValue.at<double>(3, 0);
		right_L_RoughValue.at<double>(4, 0) = right_L_AccurateValue.at<double>(4, 0);
		right_L_RoughValue.at<double>(5, 0) = right_L_AccurateValue.at<double>(5, 0);
		right_L_RoughValue.at<double>(6, 0) = right_L_AccurateValue.at<double>(6, 0);
		right_L_RoughValue.at<double>(7, 0) = right_L_AccurateValue.at<double>(7, 0);
		right_L_RoughValue.at<double>(8, 0) = right_L_AccurateValue.at<double>(8, 0);
		right_L_RoughValue.at<double>(9, 0) = right_L_AccurateValue.at<double>(9, 0);
		right_L_RoughValue.at<double>(10, 0) = right_L_AccurateValue.at<double>(10, 0);
		cal_InteriorParams(right_L_AccurateValue, right_Last);
		if (abs(right_Last.f - former_right_f) < 0.0001)
		{
			cout << "------------------------------" << endl;
			cout << "右片Li精确值为:" << endl;
			for (int k = 0; k < right_L_AccurateValue.rows - 4; k++)
				cout << "L" << k + 1 << "\t" << right_L_AccurateValue.at<double>(k, 0) << endl;
			//精度统计
			Mat V = right_A * right_L_AccurateValue - right_l;
			Mat sigma0 = (V.t() * V / (2 * right_mergedpts.size() - 15));
			cout << "------------------------------" << endl;
			cout << "右片像点坐标观测值的单位权中误差/mm:" << sqrt(sigma0.at<double>(0, 0)) << endl;
			cout << "------------------------------" << endl;
			Mat Qxx = (left_A.t() * left_A).inv();
			vector<string> str = { "L1", "L2", "L3", "L4", "L5", "L6", "L7", "L8", "L9", "L10", "L11", "k1", "k2", "p1", "p2" };
			cout << "未知数中误差:" << endl;
			for (int i = 0; i < 15; i++)
			{
				double sigmaXi = sqrt(Qxx.at<double>(i, i)) * sigma0.at<double>(0, 0);
				cout << str[i] << ": " << sigmaXi << endl;
			}
			cout << "------------------------------" << endl;
			int num = 0;
			cout << "右片像点坐标观测值的残差/pixel:" << endl;
			cout << "点号" << "\t" << "Vx" << "\t\t" << "Vy" << endl;
			for (int i = 0; i < V.rows; i += 2)
			{
				cout << right_mergedpts[num].flag  << "\t" << V.at<double>(i, 0) / PIXSIZE << "\t" << V.at<double>(i + 1, 0) / PIXSIZE << endl;
				num++;
			}
			cout.unsetf(ios::scientific);
			
			break;
		}
		former_right_f = right_Last.f;
	}


//-----------------------------------------转换为内外方位元素和畸变系数----------------------------------------------------


	cal_ExteriorParams(left_L_AccurateValue, left_Last, left_mergedpts);
	cal_ExteriorParams(right_L_AccurateValue, right_Last, right_mergedpts);
	cout << "------------------------------" << endl;
	cout << "左相机内外方位元素和畸变系数(mm):" << endl;
	cout << "外—线元素:" << endl;
	cout << "Xs:" << left_Last.Xs << " Ys:" << left_Last.Ys << " Zs:" << left_Last.Zs << endl;
	cout << "外-角元素:" << endl;
	cout << "Phi:" << left_Last.Phi << " Omega:" << left_Last.Omega << " Kappa:" << left_Last.Kappa << endl;
	cout << "内方位元素:" << endl;
	cout << "x0:" << left_Last.x0 << " y0:" << left_Last.y0 << " f:" << left_Last.f << endl;
	cout << "畸变系数:" << endl;
	cout << "k1:" << left_Last.k1 << " k2:" << left_Last.k2 << " p1:" << left_Last.p1 << " p2:" << left_Last.p2 << endl;
	cout << "比例系数 正交系数:" << endl;
	cout << "ds: " << left_Last.ds << "dp: " << left_Last.dp << endl;
	cout << "------------------------------" << endl;
	cout << "右相机内外方位元素和畸变系数(mm):" << endl;
	cout << "外—线元素:" << endl;
	cout << "Xs:" << right_Last.Xs << " Ys:" << right_Last.Ys << " Zs:" << right_Last.Zs << endl;
	cout << "外-角元素:" << endl;
	cout << "Phi" << right_Last.Phi << " Omega:" << right_Last.Omega << " Kappa:" << right_Last.Kappa << endl;
	cout << "内方位元素:" << endl;
	cout << "x0:" << right_Last.x0 << " y0:" << right_Last.y0 << " f:" << right_Last.f << endl;
	cout << "畸变系数:" << endl;
	cout << "k1:" << right_Last.k1 << " k2:" << right_Last.k2 << " p1:" << right_Last.p1 << " p2:" << right_Last.p2 << endl;
	cout << "比例系数 正交系数:" << endl;
	cout << "ds: " << right_Last.ds << "dp: " << right_Last.dp << endl;


//-------------------------------------------求解物方坐标和欧氏距离----------------------------------------------------
// 
// 1将像点坐标畸变改正 2计算粗略值 3计算精确值
// 注意:更新x0y0--A
// 
	//畸变纠正
	modifyImgPoints(left_unPoints, left_Last);
	modifyImgPoints(right_unPoints, right_Last);

	//解算待定点的坐标初值
	vector<ControlPoint> unPoints;
	for (int i = 0; i < left_unPoints.size(); i++)
	{
		Mat un_res = cal_XYZ_ApproValue(left_unPoints[i], right_unPoints[i], left_L_AccurateValue, right_L_AccurateValue);
		ControlPoint p;
		p.flag = left_unPoints[i].flag;
		p.X = un_res.at<double>(0, 0);
		p.Y = un_res.at<double>(1, 0);
		p.Z = un_res.at<double>(2, 0);
		unPoints.push_back(p);
		
	}

	//解算待定点物方空间坐标精确值
	for (int i = 0; i < left_unPoints.size(); i++)
	{
		unPoints[i] = cal_XYZ_AccurateValue(left_L_AccurateValue, right_L_AccurateValue, unPoints[i], left_unPoints[i], right_unPoints[i], left_Last, right_Last);
	}

	//计算各点相对于点52号点的欧式距离
	ControlPoint p52;
	for (int i = 0; i < unPoints.size(); i++)
	{
		if (unPoints[i].flag == 52)
		{
			p52 = unPoints[i];
			break;
		}
	}
	vector<double> distance;
	cout << "------------------------------" << endl;
	cout << "待定点相对于点52的欧式距离/mm:" << endl;
	for (int i = 0; i < unPoints.size(); i++)
	{
		if (unPoints[i].flag == 52)
			continue;
		double dis = sqrt(pow(unPoints[i].X - p52.X, 2) + pow(unPoints[i].Y - p52.Y, 2) + pow(unPoints[i].Z - p52.Z, 2));
		distance.push_back(dis);
		cout << unPoints[i].flag << " " << dis << endl;
	}

	//-------------------------------------------求解检查点物方坐标和精度检查----------------------------------------------------

	//先改正像点坐标畸变
	modifyImgPoints(left_ckpts, left_Last);
	modifyImgPoints(right_ckpts, right_Last);
	vector<ControlPoint> ckPoints;

	//计算近似值
	for (int i = 0; i < left_ckpts.size(); i++)
	{
		Mat ck_res = cal_XYZ_ApproValue(left_ckpts[i], right_ckpts[i], left_L_ApporValue, right_L_RoughValue);
		ControlPoint p;
		p.flag = left_ckpts[i].flag;
		p.X = ck_res.at<double>(0, 0);
		p.Y = ck_res.at<double>(1, 0);
		p.Z = ck_res.at<double>(2, 0);
		ckPoints.push_back(p);
		//cout << p.flag << " " << p.X << " " << p.Y << " " << p.Z << endl;
	}

	//解算检查点物方空间坐标精确值
	for (int i = 0; i < left_ckpts.size(); i++)
	{
		ckPoints[i] = cal_XYZ_AccurateValue(left_L_AccurateValue, right_L_AccurateValue, ckPoints[i], left_ckpts[i], right_ckpts[i], left_Last, right_Last);
	}

	//计算残差
	cout << "-------------------" << endl;
	cout << "检查点物方空间坐标残差/mm" << endl;
	cout << "点号\t" << "X\t" << "Y\t" << "Z\t" << endl;
	for (int i = 0; i < left_ckpt_Pairs.size(); i++)
	{
		for (int j = 0; j < ckPoints.size(); j++)
		{
			if (left_ckpt_Pairs[i].flag == ckPoints[j].flag)
			{
				double dx = left_ckpt_Pairs[i].X - ckPoints[j].X;
				double dy = left_ckpt_Pairs[i].Y - ckPoints[j].Y;
				double dz = left_ckpt_Pairs[i].Z - ckPoints[j].Z;
				cout << left_ckpt_Pairs[i].flag << "\t" << dx << "\t" << dy << "\t" << dz << endl;
			}
		}
	}
	return 0;
}

评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值