单片空间后方交会[C#]

原创 2007年10月15日 20:07:00

 

 

using System;
using System.Collections.Generic;
using System.Text;

namespace _33
{
    
class Program
    
{
        
public class Data
        
{
            
//内方位元素
            double f = 0.15324, m = 50000,x0,y0;          
            
//地面点坐标
            double[] X = new double[3];
            
double[] Y = new double[3];
            
double[] Z = new double[3];
            
//X,Y,Z和
            double SX, SY, SZ;
            
//像点坐标
            double[] x = new double[3];
            
double[] y = new double[3];
            
//x,y近似值
            double[] _x = new double[3];
            
double[] _y = new double[3];
            
//方便代入公式,定义_Z
            double[] _Z = new double[3];
            
//外方位元素
            double XS, YS, ZS, r = 0, w = 0, k = 0;
            
//旋转矩阵各值
            double a1, a2, a3, b1, b2, b3, c1, c2, c3;
            
//解方程所用到矩阵
            double[,] matrix_A = new double[66];
            
double[,] matrix_AT = new double[66];
            
double[,] matrix_AA = new double[66];
            
double[,] matrix_AAtemp = new double[612];
            
double[,] matrix_AAR = new double[66];
            
double[,] matrix_AARAT = new double[66];
            
double[,] L = new double[61];
            
double[,] matrix_X = new double[61];


            
public void Input()    //坐标输入及数值初始化
            {
                
//输入x,y,X,Y,Z坐标
                for (int i = 0; i < 3; i++)
                
{                
                    Console.WriteLine(
"======= 输入第{0}个已知点坐标 =======",i+1);
                    Console.WriteLine(
"<像点坐标> x{0}(mm)",i+1);
                    
string userInput_x = Console.ReadLine();
                    x[i] 
= double.Parse(userInput_x);
                    Console.WriteLine(
"<像点坐标> y{0}(mm)", i + 1);
                    
string userInput_y = Console.ReadLine();
                    y[i] 
= double.Parse(userInput_y);
                    Console.WriteLine(
"<地面点坐标> X{0}(m)", i + 1);
                    
string userInput_X = Console.ReadLine();
                    X[i] 
= double.Parse(userInput_X);
                    Console.WriteLine(
"<地面点坐标> Y{0}(m)", i + 1);
                    
string userInput_Y = Console.ReadLine();
                    Y[i] 
= double.Parse(userInput_Y);

                    Console.WriteLine(
"<地面点坐标> Z{0}(m)", i + 1);
                    
string userInput_Z = Console.ReadLine();
                    Z[i] 
= double.Parse(userInput_Z);
                    Console.WriteLine();
                    
                    
//求X,Y,Z和
                   
                    SX 
+= X[i];
                    SY 
+= Y[i];
                    SZ 
+= Z[i];
                }

                
//x,y值换算成m
                for (int i = 0; i < 3; i++)
                
{
                    x[i] 
/= 1000;
                    y[i] 
/= 1000;

                }

                
//定义初始值
                x0 = y0 = 0;
                r 
= w = k = 0;
                XS 
= SX / 3;
                YS 
= SY / 3;
                ZS 
= SZ / 3 + m * f;
            }


            
public void Matrix()//计算旋转矩阵R
              {
                    a1 
= Math.Cos(r) * Math.Cos(k) - Math.Sin(r) * Math.Sin(w) * Math.Sin(k);
                    a2 
= -Math.Cos(r) * Math.Sin(k) - Math.Sin(r) * Math.Sin(w) * Math.Cos(k);
                    a3 
= -Math.Sin(r) * Math.Cos(w);
                    b1 
= Math.Cos(w) * Math.Sin(k);
                    b2 
= Math.Cos(w) * Math.Cos(k);
                    b3 
= -Math.Sin(w);
                    c1 
= Math.Sin(r) * Math.Cos(k) + Math.Cos(r) * Math.Sin(w) * Math.Sin(k);
                    c2 
= -Math.Sin(r) * Math.Sin(k) + Math.Cos(r) * Math.Sin(w) * Math.Cos(k);
                    c3 
= Math.Cos(r) * Math.Cos(w);

                    
for (int i = 0; i < 3; i++)
                    
{
                        
//计算_Z
                        _Z[i] = a3 * (X[i] - XS) + b3 * (Y[i] - YS) + c3 * (Z[i] - ZS);

                        
//计算x,y近似值
                        _x[i] = -* (a1 * (X[i] - XS) + b1 * (Y[i] - YS) + c1 * (Z[i] - ZS)) / (a3 * (X[i] - XS) + b3 * (Y[i] - YS) + c3 * (Z[i] - ZS));
                        _y[i] 
= -* (a2 * (X[i] - XS) + b2 * (Y[i] - YS) + c2 * (Z[i] - ZS)) / (a3 * (X[i] - XS) + b3 * (Y[i] - YS) + c3 * (Z[i] - ZS));
                    }


                  
//Matrix_A赋值
                    for (int i = 0; i < 3; i++)
                    
{
                        matrix_A[
2 * i, 0= 1 / _Z[i] * (a1 * f + a3 * x[i]);
                        matrix_A[
2 * i, 1= 1 / _Z[i] * (b1 * f + b3 * x[i]);
                        matrix_A[
2 * i, 2= 1 / _Z[i] * (c1 * f + c3 * x[i]);
                        matrix_A[
2 * i, 3= y[i] * Math.Sin(w) - (x[i] / f * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) + f * Math.Cos(k)) * Math.Cos(w);
                        matrix_A[
2 * i, 4= -* Math.Sin(k) - x[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
                        matrix_A[
2 * i, 5= y[i];
                        matrix_A[
2 * i + 10= 1 / _Z[i] * (c2 * f + c3 * y[i]);
                        matrix_A[
2 * i + 11= 1 / _Z[i] * (b2 * f + b3 * y[i]);
                        matrix_A[
2 * i + 12= 1 / _Z[i] * (c2 * f + c3 * y[i]);
                        matrix_A[
2 * i + 13= -x[i] * Math.Sin(w) - (y[i] / f * (x[i] * Math.Cos(k) - y[i] * Math.Sin(k)) - f * Math.Sin(k)) * Math.Cos(w);
                        matrix_A[
2 * i + 14= -* Math.Cos(k) - y[i] / f * (x[i] * Math.Sin(k) + y[i] * Math.Cos(k));
                        matrix_A[
2 * i + 15= -x[i];
                        L[
2 * i, 0= x[i] - _x[i];
                        L[
2 * i + 10= y[i] - _y[i];
                    }


                    
////得到法方程的解

                    
////求matrix_A的转置

                    for (int i = 0; i < 6; i++)
                    
{

                        
for (int j = 0; j < 6; j++)
                        
{

                            matrix_AT[i, j] 
= matrix_A[j, i];
                        }

                    }


                    
////A的转置与A的乘积,存在matrix_AA中

                    
for (int i = 0; i < 6; i++)
                    
{
                        
for (int j = 0; j < 6; j++)
                        
{
                            matrix_AA[i, j] 
= 0;

                            
for (int vv = 0; vv < 6; vv++)
                            
{
                                matrix_AA[i, j] 
+= matrix_AT[i, vv] * matrix_A[vv, j];
                            }

                        }

                    }


                    
//matrix_AA的逆阵

                    
//定义matrix_AAtemp--临时数组

                    
for (int vv = 0; vv < 6; vv++)
                    
{
                        
for (int t = 0; t < 12; t++)
                        
{
                            matrix_AAtemp[vv, t] 
= 0;
                        }

                    }


                    
//把matrix_AA各值赋给matrix_AAtemp

                    
for (int i = 0; i < 6; i++)
                    
{
                        
for (int j = 0; j < 6; j++)
                        
{
                            matrix_AAtemp[i, j] 
= matrix_AA[i, j];
                        }

                    }


                    
////在matrix_AAtemp加入初等方阵
                   
                    
for (int v = 0; v < 6; v++)
                    
{

                        matrix_AAtemp[v, v 
+ 6= 1;
                    }


                    
//初等变换
                    for (int vv = 0; vv < 6; vv++)
                    
{

                        
if (matrix_AAtemp[vv, vv] != 1)
                        
{

                            
double bs = matrix_AAtemp[vv, vv];
                            matrix_AAtemp[vv, vv] 
= 1;
                            
for (int p = vv + 1; p < 12; p++)
                            
{
                                matrix_AAtemp[vv, p] 
/= bs;
                            }

                        }

                        
for (int q = 0; q < 6; q++)
                        
{
                            
if (q != vv)
                            
{
                                
double bs = matrix_AAtemp[q, vv];
                                
for (int p = 0; p < 12; p++)
                                
{
                                    matrix_AAtemp[q, p] 
-= bs * matrix_AAtemp[vv, p];
                                }

                            }

                            
else
                            
{
                                
continue;
                            }

                        }

                    }


                    
//得到matrix_AA的逆阵后存在matrix_AAR

                    
for (int i = 0; i < 6; i++)
                    
{
                        
for (int j = 0; j < 6; j++)
                        
{
                            matrix_AAR[i, j] 
= matrix_AAtemp[i, j + 6];
                        }

                    }


                    
//matrix_AAR * matrix_AT存在matrix_AARAT

                    
for (int i = 0; i < 6; i++)
                    
{
                        
for (int j = 0; j < 6; j++)
                        
{
                            matrix_AARAT[i, j] 
= 0;
                            
for (int vv = 0; vv < 6; vv++)
                            
{

                                matrix_AARAT[i, j] 
+= matrix_AAR[i, vv] * matrix_AT[vv, j];
                            }

                        }

                    }



                    
//matrix_AARAT x L
                    
//存在matrix_X中

                    
for (int i = 0; i < 6; i++)
                    
{
                        
for (int j = 0; j < 1; j++)
                        
{
                            matrix_X[i, j] 
= 0;
                            
for (int vv = 0; vv < 6; vv++)
                            
{
                                matrix_X[i, j] 
+= matrix_AARAT[i, vv] * L[vv, j];
                            }

                        }

                    }

                  
                   
//计算外方位元素值
                    XS += matrix_X[00];
                    YS 
+= matrix_X[10];
                    ZS 
+= matrix_X[20];
                    r 
+= matrix_X[30];
                    w 
+= matrix_X[40];
                    k 
+= matrix_X[50];
                                 
 }

            
public void Judge(int n,double d) //反复调用Matrix()方法进行迭代,并判断是否满足限差
            {   

                
if (Math.Abs(matrix_X[30]) >= d || Math.Abs(matrix_X[40]) >= d || Math.Abs(matrix_X[50]) >= d)
                
{

                    Console.WriteLine(
"================== 迭代开始 ==================");
                    Console.WriteLine();
                    
for (int nu = 0; nu< n; nu++)
                    
{  
                       Matrix();    
                       Console.WriteLine(
"======第{0}次迭代 φ, w ,k 值====== ", nu + 1);
                        
                        
for (int i = 3; i < 6; i++)
                        
{
                            
for (int j = 0; j < 1; j++)
                            
{
                                Console.WriteLine(matrix_X[i, j]);

                            }

                        }


                        
if (Math.Abs(matrix_X[30]) <= d && Math.Abs(matrix_X[40]) <= d && Math.Abs(matrix_X[50]) <= d)
                        
{
                            Console.WriteLine(
"===============在第{0}次迭代改正数值小于限差==============", nu + 1);
                            Console.WriteLine();
                            Console.WriteLine(
"===========================迭代结束==========================");
                            Console.WriteLine();
                            Console.WriteLine(
"Enter键显示计算结果......");
                            Console.ReadLine();
                            Output();
                            TestRusults();

                            
break;
                        }


                    }

                    
if (Math.Abs(matrix_X[30]) >= d || Math.Abs(matrix_X[40]) >= d || Math.Abs(matrix_X[50]) >= d)
                    
{
                        Console.WriteLine(
"            -------------------------  错误!-------------------------");
                        Console.WriteLine(
"已达到指定迭代次数,仍不能满足指定限差要求,程序无法继续执行。");
                        Console.WriteLine();
                        Console.WriteLine(
"大虾请检查数据后重新来过.");                   
                    }

                }

                
else 
                
{
                    Console.WriteLine();
                    Console.WriteLine(
"====满足限差要求,无需迭代====");
                    Console.WriteLine();
                    Console.WriteLine(
"Enter键显示计算结果......");
                    Console.ReadLine();
                    Output();
                    TestRusults();
                }
    
                   
                   
               
            

            }

            
public void Output()//输出计算结果
            {

                Console.WriteLine(
"================= 计算结果 =================");
                Console.WriteLine(
"XS={0}",XS);
                Console.WriteLine(
"YS={0}",YS);
                Console.WriteLine(
"ZS={0}",ZS);
                Console.WriteLine(
"φ={0}",r);
                Console.WriteLine(
"w={0}",w);
                Console.WriteLine(
"k={0}",k);
                Console.WriteLine();
                Console.WriteLine(
"Enter键进入第四点检验.....");
                Console.ReadLine();

              }

            
public void TestRusults()//测试结果
            {
                
//像点坐标和地面点坐标
                double t_X, t_Y, t_Z;
                
double test_x, test_y; 

                Console.WriteLine(
"================ 第四点验检验 ================");
                Console.WriteLine(
"输入地面点坐标 X(m) ");
                
string strTest_X = Console.ReadLine();
                t_X 
= double.Parse(strTest_X);
                Console.WriteLine(
"输入地面点坐标 Y(m)");
                
string strTest_Y = Console.ReadLine();
                t_Y 
= double.Parse(strTest_Y);
                Console.WriteLine(
"输入地面点坐标 Z(m)");
                
string strTest_Z = Console.ReadLine();
                t_Z 
= double.Parse(strTest_Z);
                Console.WriteLine();

                test_x 
= -* (a1 * (t_X - XS) + b1 * (t_Y - YS) + c1 * (t_Z - ZS)) / (a3 * (t_X - XS) + b3 * (t_Y - YS) + c3 * (t_Z - ZS)) + x0;
                test_y 
= -* (a2 * (t_X - XS) + b2 * (t_Y - YS) + c2 * (t_Z - ZS)) / (a3 * (t_X - XS) + b3 * (t_Y - YS) + c3 * (t_Z - ZS)) + y0;

                Console.WriteLine(
"======== 该点像点坐标(mm) ========");
                Console.WriteLine(
"x={0}", test_x * 1000);
                Console.WriteLine(
"y={0}",test_y*1000);
                Console.WriteLine(
"Enter退出......");
                Console.ReadLine();
                
                
            }


        }
//end of public class Data
        public static void Main()//主方法
            {
                
try
                
{
                    Console.WriteLine(
"============ 输入迭代次数  ============");
                    
string str_n = Console.ReadLine();
                    
int n = int.Parse(str_n);
                    Console.WriteLine(
"============ 输入限差 ============");
                    
string str_d = Console.ReadLine();
                    
double d = double.Parse(str_d);
                    
//生成实例
                    Data p = new Data();
                    
//调用方法
                    p.Input();
                    p.Matrix();
                    p.Judge(n,d);
      
                    
               }

                
catch(Exception)
                
{
                    Console.WriteLine(
"发生错误,大虾请重试!");
                    Console.ReadLine();
                }
  
                

            }


        }
//end of class Program
    }


 
相关文档:
http://hi.baidu.com/huqing7002/blog/item/3ceb3cd8816e283732fa1c50.html
http://blog.mop.com/bys02201/2006/11/21/2591919.html
http://221.232.129.83/jpkc2005/syclx/4.teaching%20guide/img/houjiao.pdf

摄影测量空间后方交会外方位元素的解算程序

航摄相片的外方位元素表示的是摄影摄影瞬间相片上的点对于地面上的点之间的关系的一些参数,在测绘工作中,如果求出了一张航拍相片的外方位元素,那么就可以根据像素点的坐标计算出对应的地面点的坐标,而这些解算过...
  • ld15102891672
  • ld15102891672
  • 2017年05月02日 12:57
  • 1656

摄影测量后方交会MATLAB

%aa = textread('物方控制点坐标.txt');fid=fopen('CONTROL.txt'); %打开数据总文件 B=textscan(fid,'%f %f %f %f');%把每一...
  • qq_34069180
  • qq_34069180
  • 2017年07月08日 14:11
  • 276

摄影测量后方交会-前方交会(C#)

双像解析摄影测量中的后交-前交解法,常用于已知像片的外方位元素、需确定少量待定点坐标的情况。解算过程分为两个阶段:后方交会、前方交会。 思路为利用后方交会计算外方位元素,利用前方交会解算待定地面点坐...
  • u011670396
  • u011670396
  • 2015年09月05日 20:39
  • 3050

空间后方交会

///< 像素的行列号内定向转化为相空间坐标系坐标 m_ImgPts = new st_point[iPntCount]; double dImgX, dImgY; for (int i=0; ...
  • OrangeMo0n
  • OrangeMo0n
  • 2013年07月18日 22:45
  • 609

摄影测量后方交会

摄影测量后方交会涉及到的初始数据较多,在这里讨论一下其单位问题一个求解的基本思想...
  • u010260855
  • u010260855
  • 2014年04月12日 20:00
  • 1425

Python新浪博客签到

#coding:utf-8 __author__ = 'zy' import urllib2 import cookielib import urllib import re import sys '...
  • qq_34069180
  • qq_34069180
  • 2017年04月22日 15:18
  • 346

摄影测量学后方交会

namespace ConsoleApplication1 { class Program { static void Main(string[] args) ...
  • qq_34069180
  • qq_34069180
  • 2016年06月03日 21:56
  • 248

共线方程求解外方位元素--单片空间后方交会

单片空间后方交会是利用三个不在一条直线的已知点计算相片外方位元素的方法。 获取已知数据,包括: x[]、y[]:改正后的控制点的像方坐标 X[]、Y[]、Z[]:控制点的物方坐标 f: 相机焦距 ...
  • u012176176
  • u012176176
  • 2017年08月02日 14:13
  • 215

JAVA:角度后方交会算法GUI实现

在待定点P观测水平角和检查角,进而确定P的平面坐标:(1)计算已知边BC的边长和方位角(2)计算α1(3)由点C推算出点P的坐标...
  • cooelf
  • cooelf
  • 2014年10月30日 21:07
  • 812

JAVA:距离后方交会算法GUI实现

在待测点P上观测距离距离S1,S2,S3,进而确定P的平面坐标:(1)计算已知边AB的边长和方位角(2)计算α2(3)由点P1推算出P坐标...
  • cooelf
  • cooelf
  • 2014年10月30日 20:28
  • 919
内容举报
返回顶部
收藏助手
不良信息举报
您举报文章:单片空间后方交会[C#]
举报原因:
原因补充:

(最多只允许输入30个字)