#include <iostream>
#include <fstream>
#include <vector>
#include <math.h>
#include <algorithm>
#include "CCMat.h"
#include "Point.h"
using namespace::std;
//坐标：
//f,H
double f,H,m;
int N;
//a1-c3
double a1,a2,a3,b1,b2,b3,c1,c2,c3;
//XYZ的代号：
double Xb,Yb,Zb;
//六个外方为元素：
double g_Xs,g_Ys,g_Zs;
double dXs, dYs, dZs;
double g_fai,g_omga,g_kama;
double dfai, domga, dkama;
//像点坐标的表示：
double x,x_estimate;
double y,y_estimate;
//12个系数：
double a00,a01,a02,a03,a04,a05,a10,a11,a12,a13,a14,a15;
//控制点坐标：
vector<Point> vpoints;
//计算a1-c3
void abc_get()
{
a1 = cos(g_fai)*cos(g_kama) - sin(g_omga)*sin(g_fai)*sin(g_kama);
a2 = -cos(g_fai)*sin(g_kama)-sin(g_omga)*sin(g_fai)*cos(g_kama);
a3 = -sin(g_fai)*cos(g_omga);
b1 = cos(g_omga)*sin(g_kama);
b2 = cos(g_omga)*cos(g_kama);
b3 = -sin(g_omga);
c1 = sin(g_omga)*cos(g_kama) + cos(g_fai)*sin(g_omga)*sin(g_kama);
c2 = -sin(g_omga)*sin(g_kama) + cos(g_fai)*sin(g_omga)*cos(g_kama);
c3 = cos(g_omga)*cos(g_fai);
}
//计算XbYbZb
void XbYbZb_get(double X,double Y,double Z)
{
Xb = a1*(X - g_Xs)+b1*(Y - g_Ys)+c1*(Z - g_Zs);
Yb = a2*(X - g_Xs)+b2*(Y - g_Ys)+c2*(Z - g_Zs);
Zb = a3*(X - g_Xs)+b3*(Y - g_Ys)+c3*(Z - g_Zs);
}
//计算像点坐标的近似值：
//double x_or_y_get(char flg,double X,double Y,double Z);
double x_or_y_get(char flg)
{
if (flg == 0)
return -f*Xb/Zb;
else
return -f*Yb/Zb;
}
//计算a00到a15
CCMat a00a15_get(double x,double y)
{
a00 = (a1 * f + a3 * x)/Zb;
a01 = (b1 * f + b3 * x)/Zb;
a02 = (c1 * f + c3 * x)/Zb;
a10 = (a2 * f + a3 * y)/Zb;
a11 = (b2 * f + b3 * y)/Zb;
a12 = (c2 * f + c3 * y)/Zb;
a03 = y * sin(g_omga) -(x*(x*cos(g_kama)-y*sin(g_kama))/f + f*cos(g_kama))*cos(g_omga);
a04 = -f*sin(g_kama) - x*(x*sin(g_kama)+y*cos(g_kama))/f;
a05 = y;
a13 = -x * sin(g_omga) -(y*(x*cos(g_kama)-y*sin(g_kama))/f - f*sin(g_kama))*cos(g_omga);
//a13 = -x * sin(g_omga) -(x*(x*sin(g_kama)+y*cos(g_kama))/f - f*sin(g_kama))*cos(g_omga);
a14 = -f*cos(g_kama) - y*(x*sin(g_kama)+y*cos(g_kama))/f;
a15 = -x;
vector<double> vetmat;
vetmat.push_back(2);
vetmat.push_back(6);
vetmat.push_back(a00);vetmat.push_back(a01);vetmat.push_back(a02);
vetmat.push_back(a03);vetmat.push_back(a04);vetmat.push_back(a05);
vetmat.push_back(a10);vetmat.push_back(a11);vetmat.push_back(a12);
vetmat.push_back(a13);vetmat.push_back(a14);vetmat.push_back(a15);
CCMat matret(vetmat);
return matret;
}
//计算L
CCMat lxly_get()
{
vector<double> vetmat;
vetmat.push_back(2);vetmat.push_back(1);
vetmat.push_back(x - x_estimate);
vetmat.push_back(y - y_estimate);
CCMat matret(vetmat);
return matret;
}
//读取初始条件：
void readcondition(int numberofDot,istream streamin,ostream streamout )
{
N = numberofDot;
//控制点数据：
for (int i(0);i< numberofDot;i++)
{
//streamout<<"请输入第"<<i+1<<"个坐标(x,y,xp,yp坐标用空格隔开)：";
Point point;
streamin>>point.m_x>>point.m_y>>point.m_z>>point.m_xp>>point.m_yp;
//streamin.clear();
vpoints.push_back(point);
//streamout<<endl;
}
//关于摄影比例尺子、航高：
//streamout<<"请输入摄影比例尺的分母：";
streamin>>m;
//streamin.clear();
//streamout<<endl;
//streamout<<"请输入航高：";
streamin>>H;
//streamin.clear();
//streamout<<"请输入焦距：";
streamin>>f;
//streamout<<endl;
//六个外方位元素的初始值：
g_omga = g_kama = g_fai = 0;
for (i = 0;i<vpoints.size();i++)
{
g_Xs += vpoints[i].m_x;
g_Ys += vpoints[i].m_y;
}
g_Xs/=vpoints.size();
g_Ys/=vpoints.size();
g_Zs = H;
}
main()
{
ifstream inf;
inf.open("file1.txt");
//获取已知数据：控制点的像点坐标、地面坐标、航高……
//计算未知数初始值
readcondition(4,inf,cout);
inf.close();
g_fai = g_kama = g_omga = 0.001;
CCMat result_i;
for (int j(1);;j++)
{
//计算旋转矩阵：
abc_get();
CCMat matA,matL;
for (int i(0);i<N;i++)
{
x = vpoints[i].m_xp;
y = vpoints[i].m_yp;
//
XbYbZb_get(vpoints[i].m_x,vpoints[i].m_y,vpoints[i].m_z);
x_estimate = x_or_y_get(0);
y_estimate = x_or_y_get(1);
CCMat mata,matl;
mata = a00a15_get(x,y);
matA.add(0,mata);
matl = lxly_get();
matL.add(0,matl);
}
CCMat rmat,lmat;
lmat = ((matA.turnAround()).subMat(matA)).reverseMat();
rmat = (matA.turnAround()).subMat(matL);
result_i = lmat.subMat(rmat);
cout<<j<<"->\n";
printMat(cout,result_i);
//result_i = matA.turnAround().subMat(matA).reverseMat().subMat(matA.turnAround()).subMat(matL);
double a;
a = result_i[0][0];
g_Xs += result_i[0][0];
g_Ys += result_i[1][0];
g_Zs += result_i[2][0];
g_fai += result_i[3][0];
g_omga += result_i[4][0];
g_kama += result_i[5][0];
if((result_i[0][0]<=0.0000001 && result_i[0][0]>= -0.0000001 )
&&
(result_i[1][0]<=0.0000001 && result_i[1][0]>= -0.0000001 )
&&
(result_i[2][0]<=0.0000001 && result_i[2][0]>= -0.0000001 )
)
break;
}
cout<<"迭代次数是："<<j<<"次"<<endl;
cout<<"结果是（Xs,Ys,Zs,fai,omga,kama）：\n"<<" 线元素：\n "<<g_Xs<<" "<<g_Ys<<" "
<<g_Zs<<"\n 角元素：\n "
<<g_fai-(int)(g_fai/3.141592653589793*3.141592653589793)<<" "
<<g_omga-(int)(g_omga/3.141592653589793*3.141592653589793)<<" "
<<g_kama-(int)(g_kama/3.141592653589793*3.141592653589793)<<endl;
cout<<"\n\n";
cout<<"如果保存结果请输入零值，反之输入非零值 -> ";
cout.flush();
int _end;
cin>>_end;
cin.clear();
if(_end == 0)
{
ofstream file;
file.open("result.txt");
file<<"迭代次数是："<<j<<"次"<<endl;
file<<"结果是（Xs,Ys,Zs,fai,omga,kama）：\n"<<" 线元素：\n "<<g_Xs<<" "<<g_Ys<<" "<<g_Zs<<"\n 角元素：\n "
<<g_fai-(int)(g_fai/3.141592653589793*3.141592653589793)<<" "
<<g_omga-(int)(g_omga/3.141592653589793*3.141592653589793)<<" "
<<g_kama-(int)(g_kama/3.141592653589793*3.141592653589793)<<endl;
file<<"\n\n";
file.close();
}
return 0;
}
//CCMat.h
#ifndef _CCMAT_
#define _CCMAT_ 40
#include <iostream>
#include <fstream>
#include <exception>
#include <vector>
#include <stdlib.h>
#include <algorithm>
#include <math.h>
#include <time.h>
using namespace::std;
class Clit
{
public:
Clit(){it = NULL;}
vector<double>* vet;
vector<double>::iterator it;
double& operator[](int j)
{
int i(it - (*vet).begin() +j);
double& r=(*vet)[i];
return r;
}
};
class CCMat
{
public:
CCMat():m_nX(0),m_nY(0){m_elements.clear();};
CCMat(CCMat* mat);
CCMat(vector<double> vet);
CCMat(ifstream& file);
CCMat(char* file);
CCMat(CCMat& mat);
CCMat operator=(CCMat& mat);
~CCMat(){m_elements.clear();}
Clit operator[](int i);
void add(int flg,CCMat& othermat);
CCMat subMat(CCMat& rMat); //乘法
CCMat plusMat(CCMat& rmat); //加法
CCMat turnAround(); //转置
CCMat reverseMat(); //求逆矩阵
double valueofMat(const CCMat& mat, int i,int j);//求解代数余子式
void tobttingle(CCMat& mat); //化简为上三角行列式
private:
public:int m_nX,m_nY;
private:
vector<double> m_elements;
};
void printMat(ostream& out,CCMat& mat);
#else
#endif
#include <iostream>
#include <fstream>
#include <exception>
#include <vector>
#include <stdlib.h>
#include <algorithm>
#include <math.h>
#include <time.h>
#include "CCMat.h"
using namespace::std;
CCMat::CCMat(CCMat* mat)
{
m_nX=mat->m_nX;m_nY=mat->m_nY;m_elements = mat->m_elements;
}
CCMat::CCMat(CCMat& mat)
{
m_nX = mat.m_nX;
m_nY = mat.m_nY;
m_elements.resize(mat.m_elements.size());
copy(mat.m_elements.begin(),mat.m_elements.end(),m_elements.begin());
}
CCMat::CCMat(vector<double> vet)
{
if(vet.size()<3 || vet.size()!=vet[0]*vet[1] + 2) return;
m_nX = vet[0];
m_nY = vet[1];
m_elements.resize(m_nX*m_nY);
copy(vet.begin()+2,vet.end(),m_elements.begin());
}
CCMat::CCMat(ifstream& file)
{
file>>m_nX;
file>>m_nY;
double val;
while(!file.eof()&&!file.fail()&&m_elements.size()!=m_nX*m_nY)
{
file>>val;
m_elements.push_back(val);
}
file.close();
if(m_elements.size()!=m_nX*m_nY)
delete this;
}
CCMat::CCMat(char* file)
{
ifstream ofile(file);
CCMat mat(ofile);
m_nX = mat.m_nX;
m_nY = mat.m_nY;
m_elements = mat.m_elements;
}
Clit CCMat::operator[](int i)
{
Clit lit;
if (i>=m_nX)
throw new exception("The index may be out of range!");
if (i < 0 )
throw new exception("The index may be zero!");
lit.it = m_elements.begin()+(i*m_nY);
lit.vet = &m_elements;
return lit;
}
CCMat CCMat::operator=(CCMat& mat)
{
if(&mat == this) return *this;
m_nX = mat.m_nX;
m_nY = mat.m_nY;
m_elements.resize(mat.m_elements.size());
copy(mat.m_elements.begin(),mat.m_elements.end(),m_elements.begin());
return *this;
}
CCMat CCMat::turnAround()
{
CCMat newmat,oldmat;
oldmat = *this;
newmat.m_nX = m_nY;
newmat.m_nY = m_nX;
for(int i=0;i<m_nY;i++)
for(int j= 0;j<m_nX;j++)
newmat.m_elements.push_back(oldmat[j][i]) ;
return newmat;
}
CCMat CCMat::subMat(CCMat& rMat)
{
CCMat newmat;
if (rMat.m_nX!=m_nY)
{
newmat.m_nX = newmat.m_nY = 0;
return newmat;
}
newmat.m_nX = m_nX;
newmat.m_nY = rMat.m_nY;
newmat.m_elements.resize(m_nX*rMat.m_nY);
newmat.m_elements.assign(m_nX * rMat.m_nY,0);
for(int i= 0;i<m_nX;i++){
for(int j = 0;j<rMat.m_nY;j++)
for(int k = 0;k<m_nY;k++)
{
newmat[i][j] += (*this)[i][k] * rMat[k][j];
}
}
return newmat;
}
CCMat CCMat::plusMat(CCMat& rmat)
{
CCMat newmat;
newmat.m_nX = rmat.m_nX;
newmat.m_nY = rmat.m_nY;
newmat.m_elements.resize(rmat.m_nX *rmat.m_nY);
for (int i(0);i<rmat.m_elements.size();i++)
{
newmat.m_elements[i]=m_elements[i]+rmat.m_elements[i];
}
return newmat;
}
void CCMat::tobttingle(CCMat& mat)
{
if(mat.m_nX != mat.m_nY) return;
int N(mat.m_nX);
double* buffer;
buffer = (double*)malloc(sizeof(double)*N);
int flg(0); //这是列信号
while(flg!=N){
for(int j=flg + 1;j<N;j++) //这是行信号
{ //生成减数（只是针对一行的）
for (int l(0);l<N;l++)
{
double M;
if(mat[flg][j-1]==0)
buffer[l] = 0;
else{
M = mat[j][flg] / mat[flg][flg];
buffer[l]= mat[flg][l] * M;
}
}
//对这一行做变换
for(int p(0);p<N;p++)
{ mat[j][p] -= buffer[p]; double aa;
aa = mat[j][p];
}
}
flg++;
}
free(buffer);
}
double CCMat::valueofMat(const CCMat& mat,int h,int v) //mat[h][v]的代数余子式
{
if(mat.m_nX*mat.m_nY==0||mat.m_nX*mat.m_nY<0||mat.m_nX != mat.m_nY) return 0;
double _result(pow(-1,h+v));
CCMat newmat;
if(h<0 && v<0)
{
newmat.m_nX = mat.m_nX;
newmat.m_nY = mat.m_nY;
}else{
newmat.m_nX = mat.m_nX - 1;
newmat.m_nY = mat.m_nY - 1;
}
newmat.m_elements.clear();
for(int i(0);i<mat.m_nX;i++){
for (int j(0);j<mat.m_nY;j++)
{
if(i==h || j==v) continue;
double aa;
aa = (*this)[i][j];
newmat.m_elements.push_back(aa);
}
}
if(newmat.m_elements.size()!=newmat.m_nX*newmat.m_nY) return 0;
tobttingle(newmat);
for(int i1(0);i1<newmat.m_nX;i1++){
_result*=newmat[i1][i1];
}
return _result;
return 0;
}
CCMat CCMat::reverseMat()
{
CCMat newmat;double di;
if(m_nX!=m_nY) return newmat;
di = valueofMat((*this),-1,-1);
if(di==0) return newmat;
newmat.m_nX = m_nX;
newmat.m_nY = m_nY;
for(int i(0);i<m_nX;i++){
for(int j(0);j<m_nY;j++)
newmat.m_elements.push_back(valueofMat((*this),i,j)/di);
}
return newmat.turnAround();
}
void printMat(ostream& out,CCMat& mat)
{
for (int j= 0;j<mat.m_nX;j++)
{
for(int l= 0;l<mat.m_nY;l++)
out<<mat[j][l]<<" ";
out<<endl;
}
out.flush();
out<<endl;
out.clear();
}
void CCMat::add(int flg,CCMat& othermat)
{
/*
if (flg == 0)
{
if(othermat.m_nY != m_nY)
return NULL;
m_nX+=othermat.m_nX;
}
else
{*/
if(m_nX!=0 && othermat.m_nY!= m_nY)
return ;
m_nX += othermat.m_nX;
m_nY = othermat.m_nY;
for (int i(0);i<othermat.m_nX;i++)
{
for (int j(0);j<othermat.m_nY;j++)
{
m_elements.push_back(othermat[i][j]);
}
}
//}
return ;
}
4
112
6KB
2011-06-24
9
