//相對定向的迭代結(jié)果有些偏差
//絕對定向模型比例尺確定有問題?結(jié)果未收斂?偏差較大
#include?“stdafx.h“
#include?
#include
#include
#include
#include
#include?
using?namespace?std;
#define?N1?1
#define?N3?117



struct?data_camera_result
{
	double?x0;
	double?y0;//像主點(diǎn)坐標(biāo)
	double?f;
	//主距
	double?CCD;//CCD像素大小
	double?k1;
	double?k2;//徑向畸變差參數(shù)
	double?p1;
	double?p2;//切向畸變差參數(shù)
	double?a;?
	double?b;//像元仿射變換參數(shù)
};


struct?data_point_result
{

	int?point;//疑問：若用char?point?會出現(xiàn)Unhandled?exception?in?.exe?0xC0000005?access?violation?的錯誤
????double?X;
	double?Y;
	double?Z;//控制點(diǎn)坐標(biāo)
	int?m;
	int?n;
	int?p;
	double?x1;
	double?y1;?//左影像上的像點(diǎn)坐標(biāo)
	int?q;
	double?x2;
	double?y2;//右影像上的像點(diǎn)坐標(biāo)
};

/*相機(jī)參數(shù)文件導(dǎo)入*/
int?read_data1（struct?data_camera_result?da1[]）???
{
	FILE?*fp;
	int?i=0;

	if（（fp?=?fopen（“bundleadjustment_SCBA_Camera_Result.scbacmr““r“））?==?NULL）
	{
		printf（“\n文件無法打開，程序退出！\n“）;
		exit（0）;
	}
	//檢驗
	cout<	while（fscanf（fp?“%lf%lf%lf%lf%lf%lf%lf%lf%lf%lf“?&da1[i].x0?&da1[i].y0?&da1[i].f?&da1[i].CCD?&da1[i].k1&da1[i].k2&da1[i].p1&da1[i].p2&da1[i].a&da1[i].b）?>1）?
	{//檢驗
		cout<	???//檢驗
		cout<		?i++;
	}
	return?i;
}


void?printf_data1（struct?data_camera_result?da1[]?int?n）
{
	int?i;
	for（i=0;i		printf（“\n%.4f%.4f%.4f%.4f%.3e%.3e%.3e%.3e%.3e%.3e\n“da1[i].x0?da1[i].y0?da1[i].f?da1[i].CCD?da1[i].k1da1[i].k2da1[i].p1da1[i].p2da1[i].ada1[i].b）;
}



/*控制點(diǎn)及其像點(diǎn)信息文件數(shù)據(jù)導(dǎo)入*/
int?read_data3（struct?data_point_result?da3[]）???
{
	FILE?*fp;
	int?i?=?0;

	if（（fp?=?fopen（“bundleadjustment_SCBA_Point_Result.scbapts““r“））?==?NULL）
	{
		printf（“\n文件無法打開，程序退出！\n“）;
		exit（0）;
	}	
	while（fscanf（fp?“%d%lf%lf%lf%d%d%d%lf%lf%d%lf%lf“?&da3[i].point?&da3[i].X?&da3[i].Y?&da3[i].Z?&da3[i].m&da3[i].n&da3[i].p&da3[i].x1&da3[i].y1&da3[i].q&da3[i].x2&da3[i].y2）?>?1）?
	{?//檢驗
		cout<		i++;
	}
	return?i;
}


void?printf_data3（struct?data_point_result?da3[]?int?n）
{
	int?i;
	for（i=0;i		printf（“\n%4d%.4f%.4f%.4f%d%d%d%.4f%.4f%d%.4f%.4f\n“?da3[i].point?da3[i].X?da3[i].Y?da3[i].Z?da3[i].mda3[i].nda3[i].pda3[i].x1da3[i].y1da3[i].qda3[i].x2da3[i].y2）;
}

/*矩陣相乘子程序*/
void?Mult（double?*m1double?*m2double?*resultint?i_1int?j_12int?j_2）
{?int?i=0j=0k=0;
??for（i=0;i	?for（j=0;j		result[i*j_2+j]=0.0;
		for（k=0;k		????result[i*j_2+j]+=m1[i*j_12+k]*m2[j+k*j_2];
	?}
}

/*矩陣求逆子程序*/
int?Invers_matrix（double?*m1int?n）
{?
	int?*is*js;
	int?i=0jkluv;