内定向程序算法

终于写出来第二个摄影测量的程序了，感慨万千。对于编程学习，还是要多付出实践，才会有收获。

 

 

//#include "matrix.h"
#include<stdio.h>
#include<iostream>
#include<opencv2/opencv.hpp>
using namespace std;
using namespace cv;
int main()
{
	//读入坐标
	float saomiao[16] = { 106.003,-106.005,-106.003,-106.003,-106.001,106.003,106.001,106.001,
0.002,-109.998,-110.001,0.002, 0.004,110.004,109.997,0.000 };
	//以像主点为原点的像平面坐标系的像点坐标
	float kuangbiao[16] = { 4858,4930,152,4891,197,185,4903,221,2504,4996,80,2534,2549,113,4966,2578 };
	//扫描坐标系下框标坐标的行列号
	float A_num[16][6] = { 0 };
	//A阵为16行6列

	float x0 = -0.004;			//x偏移量
	float y0 = -0.008;			//y偏移量
	float delta = 0.045;		//像素大小
	float f = 152.72;


	for (int k = 0; k < 16; k+=2 ) 
	{
		A_num[k][0] = 1;
		A_num[k][1] = kuangbiao[k] * delta;
		A_num[k][2] = -kuangbiao[k+1] * delta;
		A_num[k + 1][3] = A_num[k][0];
		A_num[k + 1][4] = A_num[k][1];
		A_num[k + 1][5] = A_num[k][2];

	}



	//定义矩阵
	Mat A(16, 6, CV_32F, A_num);	//A为16行6列，float类型的矩阵
	Mat X(6, 1, CV_32F, 0.0);		//X为6行1列，float类型的矩阵，为要求的运算后的矩阵
	Mat L(16, 1, CV_32F, saomiao);	//L为16行1列的，float类型的常数矩阵

	X = ((A.t()*A).inv())*A.t()*L;
    //矩阵运算  A的转置A.t  再乘以A  矩阵求逆inv()  再乘以A  再乘以L
	std::cout << X << std::endl;

	float m0 = X.at<float>(0, 0);	//X矩阵第一行第一列的数
	float m1 = X.at<float>(1, 0);
	float m2 = X.at<float>(2, 0);
	float n0 = X.at<float>(3, 0);
	float n1 = X.at<float>(4, 0);
	float n2 = X.at<float>(5, 0);


	float x1,x1_dx;
	float y1,y1_dx;
	float z;
	x1 = 2996;		//输入任意一个像素的框标坐标
	y1 = 1486;

	
	x1_dx = m0+ m1*x1*delta-m2*y1*delta-x0;
	y1_dx = n0 + n1*x1*delta-n2*y1*delta-y0;//得到像平面坐标系（减去主点偏移量）
	z = -f;


	cout << x1_dx << endl;
	cout << y1_dx << endl;
	cout << z << endl;					//得到像空间辅助坐标系

	getchar();
	return 0;
}


//运行结果
[-114.85733;
 1.0008425;
 -0.0094435224;
 114.17293;
 0.0083294483;
 1.0008631]
20.7117
48.3762
-152.72