求解單獨像對相對定向元素（數字攝影測量中的解析法相對定向）

時間 2019-11-12

標籤求解單獨相對定向元素數字測量解析简体版

原文原文鏈接

yogurt今天要和你們分享的是我本身在選修課數字攝影測量上學到的一點關於立體像對的解析法相對定向的知識，首先yogurt必須給你們梳理一下攝影測量幾種解析方式的區別與聯繫，將這些關係梳理好了以後寫程序的思路纔會更清晰哦！數組

--------------------------------------------------------yogurt小課堂開課啦-----------------------------------------------------------ide

在攝影測量中，爲了從獲取的影像中肯定被研究物體的位置、形狀、大小及其相互關係，除了應用到雙向立體測圖法（模擬法、解析法及數字法）外，還能夠利用物方和像方之間的解析關係式經過計算來獲取（《攝影測量學》第五章前言）。涉及到座標系統、內外方位元素以及共線條件方程式這三個大方面。其中最最重要的解析關係式非共線條件方程莫屬！函數

(一）、座標系統：spa

二維的框標座標系x/y 、像平面座標系x/y ；3d

三維的像空間座標系S-X/Y/Z 、像空間輔助座標系S-U/V/W 、地面測量座標系(左手系)T-Xt/Yt/Zt 、地面攝影測量座標系D-X/Y/Z 。調試

(二)、內外方位元素：code

3個內方位元素：主距f、像主點在框標座標系中的座標x0、y0；blog

6個外方位元素：3個直線元素：攝影中心在地面空間座標系中的座標值Xs/Ys/Zs，一般選擇的是地面攝影測量座標系；ip

3個角元素：航向傾角、旁向傾角、像片旋角。event

(三)、由共線條件方程在不一樣條件下應用求得關於攝影測量的信息如：

一、單像空間後方交會

已知：一張攝影像片的內方位元素x0/y0/F、攝影比例尺m、地面控制點的地面座標(X/Y/Z)及其對應的像點像平面座標(x/y)，求解：該像片的六個外方位元素。

二、立體像對的前方交會

已知：一對立體像對中兩張像片各自的內外方位元素和像點像平面座標(x/y)，求解：相應地面點的地面座標(X/Y/Z)。

三、立體像對的解析法相對定向：

肯定立體像對兩張像片相對位置和姿態，創建一個與拍攝時類似的立體模型，以肯定模型點的三維座標。分爲求解連續像對相對定向元素和求解單獨像對相對定向元素兩種：

(1)求解連續像對相對定向元素：以左像片爲基礎，求出右像片相對於左像片的五個相對定向元素（攝影中心在地面攝影測量座標系中副軸和第三旋轉軸方向上的座標值V/W、航向傾角ψ、旁向傾角ω、像片旋角κ）

此時，左片外方位元素U1=V1=W1=ψ1=ω1=κ1=0；右片外方位元素中U2只涉及比例尺，V二、W二、ψ二、ω二、κ2未知；

(2)求解單獨像對相對定向元素：以基線做爲主軸，左主核面爲uw平面，創建像空間輔助座標系S1-U1/V1/W1及S2-U2V2W2,求出此時的五個相對定向元素（左像片的航向傾角ψ一、像片旋角κ1以及右像片的航向傾角ψ二、旁向傾角ω二、像片旋角κ2）

此時，左片外方位元素U1=V1=W1=0，ω1=0，剩下的ψ一、κ1未知；右片外方位元素U2=B（攝影基線）、V2=W=02，剩下的ψ二、ω二、κ2。

-----------------------------------------------------------------下課啦---------------------------------------------------------------

該程序涉及到的基礎操做有讀寫文件、矩陣的乘法、轉置和求逆。接下來讓咱們一塊兒練習一下吧！

要求：根據某對立體像對中同名像點的左、右影像的行、列號數據，按單獨像對相對定向方法，求解該立體像對的相對定向元素？

數據：相似這個樣子就能夠啦，這是我數據的一部分哈，第一排是像對數，剩下的每一排四個數據的前兩個是左像片的某像元的行列數，後兩個是右像片的相對像元的行列數：

已知：影像尺寸（像素）：寬度 = 8000; 高度= 11500;像元尺寸0.009毫米，像片主距50.2毫米。像主點：行號（像素）574九、列號（像素）3999。

程序思路：那首先yogurt仍是帶着你們看一看程序思路吧~~

一、 讀入數據並將行列號轉換爲X、Y座標

1.1 從文件中讀入立體像對中同名像點的左、右影像的行、列號數據，分別將左、右影像的數據存入left和right數組中（數組元素是Point，如：left[3].x表示第三對立體像對的同名像點在左影像上的列數；right[3].y表示第三對立體像對的同名像點在右影像上的行數；）。

1.2 利用像主點的行列號，將同名像點的行列號轉換爲XY座標，計算公式以下：

x=(列數-COL像主點)*SIZE；

y=(ROW像主點-行數)*SIZE。

二、 求解左右影像像主點的像空輔座標系下的座標(u,v,w)

2.1 求旋轉矩陣R[3][3]，根絕旋轉矩陣R的元素計算公式能夠獲得R矩陣，計算公式以下：

R2[0][0] = cos(ψ2)*cos(κ2) - sin(ψ2)*sin(ω2)*sin(κ2);

R2[0][1] = -cos(ψ2)*sin(κ2) - sin(ψ2)*sin(ω2)*cos(κ2);

R2[0][2] = -sin(ψ2)*cos(ω2);

R2[1][0] = cos(ω2)*sin(κ2);

R2[1][1] = cos(ω2)*cos(κ2);

R2[1][2] = -sin(ω2);

R2[2][0] = sin(ψ2)*cos(κ2) + cos(ψ2)*sin(ω2)*sin(κ2);

R2[2][1] = -sin(ψ2)*sin(κ2) + cos(ψ2)*sin(ω2)*cos(κ2);

R2[2][2] = cos(ψ2)*cos(ω2)。

2.1 利用旋轉矩陣以及像平面座標，利用矩陣乘法，獲得像空輔座標。

右像片的與左像片的像空輔座標計算方法同樣。

三、 計算偏差方程式的係數和常數項

3.1 係數項計算公式我

A[i][0] = u1*v2 / w2;

A[i][1] = -u2*v1 / w1;

A[i][2] = F*(1 + v1*v2 / (w1*w2));

A[i][3] = -u1;

A[i][4] = u2。

3.2 常數項計算公式

L[i][0] = -F*v1 / w1 + F*v2 / w2。

四、 解算未知數

4.1 利用矩陣的乘法，矩陣轉置以及矩陣求逆的函數（在主函數前已經聲明且定義），計算改正數。

4.2 根據改正數更新未知數，並判斷是否已經知足偏差標準，即每一個改正數的絕對值是否都小於0.3*0.0001。若知足則中止迭代，不然繼續進行迭代，直到知足偏差限制爲止。

嘿嘿，你必定知道yogurt又要直接把平時寫的做業中的代碼粘上來炫耀了。

// test.cpp : 定義控制檯應用程序的入口點。
//

#include "stdafx.h"
#include<math.h>
#include<stdlib.h>
#define WIDTH 8000;
#define HEIGHT 11500;
#define SIZE 0.009;
#define COL 3999
#define ROW 5749
#define F 50.2
#define NUM 63
#define NUM2 5
#define TOLERANCE 0.3*0.0001
#define Basic 24.223500

typedef struct POINT
{
    double x, y;
}point;

void read(point *left, point *right, char *filename)
{
    FILE * fp = fopen(filename, "r");
    if (fp==NULL)
    {
        printf("ERROR");
        return;
    }
    int num;
    fscanf(fp, "%d", &num);
    for (int i = 0; i < num; i++)
    {
        fscanf(fp, "%lf,%lf,%lf,%lf", &left[i].y, &left[i].x, &right[i].y, &right[i].x);
    }
    fclose(fp);
    return;
}

void matrixmultiply(double **R, double **A, double **B,int a,int b,int c)
{
    double sum;
    for (int i = 0; i < a; i++)
    {
        for (int j = 0; j < c; j++)
        {
            sum = 0;
            for (int k = 0; k < b; k++)
            {
                sum += R[i][k] * A[k][j];
            }
            B[i][j] = sum;
        }
    }

    return;
}

void matrixtran(double **A, double **AT)
{
    for (int i = 0; i < NUM; i++)
    {
        for (int j = 0; j < NUM2; j++)
        {
            AT[j][i] = A[i][j];
        }
    }
    return;
}

void matrixinverse(double **A, double **B)
{
    double z[NUM2][NUM2*2],h;
    int i, j, m;

    //增廣矩陣(A | E)存入二維數組z中
    for (i = 0; i < NUM2; i++)
        for (j = 0; j < NUM2; j++)
            z[i][j] = A[i][j];

    for (i = 0; i < NUM2; i++)
        for (j = NUM2; j < NUM2 * 2; j++)
            z[i][j] = 0;

    for (i = 0; i < NUM2; i++)
        z[i][i+NUM2] = 1;

    
    //調整擴展矩陣,若某一列全爲0則不存在逆矩陣
    for (i = 0; i < NUM2; i++)//行
    {
        if (z[i][i] == 0)//某行對角線數值爲0，須要調整
        {
            for (j = 0; j < NUM2; j++)//行
            {
                if (z[j][i] != 0)//第j行i列不爲0，交換第i行和第j行
                {
                    for (m = 0; m < NUM2*2; m++)//列
                    {
                        h = z[i][m];
                        z[i][m] = z[j][m];
                        z[j][m] = h;
                    }    
                    break;
                }//交換完畢
            }
            if (j > NUM2)
                printf("該列沒有不爲0的行，沒有逆矩陣");
        }//調整完畢
    }    

    //計算擴展矩陣
    for (i =0; i < NUM2; i++)//行
    {
        double first = z[i][i];
        //使z[i][i]=1
        for (j = 0; j < NUM2*2; j++)//列
        {
            z[i][j] /= first;
        }
        //把其餘行在該列的數值變爲0
        for (m = 0; m < NUM2; m++)
        {
            if (m == i)//遇到本身這行
                continue;

            double beishu = z[m][i];
            for (int n = 0; n < NUM2 * 2; n++)//轉換m行的每一列，保證z[m][i]=0
            {
                z[m][n] -= z[i][n] * beishu;
            }
        }
    }

    //取擴展矩陣後的NUM2*NUM2矩陣，放到B中
    for (i = 0; i < NUM2; i++)
    {
        for (j = NUM2; j < NUM2 * 2; j++)
        {
            B[i][j - NUM2] = z[i][j];
        }
    }
}

int _tmain(int argc, _TCHAR* argv[])
{
    /*同名像點行列號rows,cols-->像平面座標x,y,像平面座標結果輸出到"coordinate.txt" */

    //開闢數組left和right分別存儲左右同名像點的行列號
    point left[63], right[63];

    //從txt文件中讀入左右同名像點的行列號
    read(left, right, "同名點左右影像行列號文件.txt");

    //將行列號轉換爲座標（公式：x=(c-COL)*SIZE,y=(ROW-h)*SIZE），並輸出
    double xleft[NUM], yleft[NUM], xright[NUM], yright[NUM];
    FILE *fp = fopen("coordinate.txt", "w");
    if (fp == NULL)
    {
        printf("ERROR");
        return 0;
    }
    fprintf(fp, "%d\n", NUM);
    for (int i = 0; i < NUM; i++)
    {
        //左影像
        xleft[i] = (left[i].x - COL)*SIZE;
        yleft[i] = (ROW - left[i].y)*SIZE;
        //右影像
        xright[i] = (right[i].x - COL)*SIZE;
        yright[i] = (ROW - right[i].y)*SIZE;
        //輸出
        fprintf(fp, "%d\t%lf\t%lf\t%lf\t%lf\n", i, xleft[i], yleft[i], xright[i], yright[i]);
    }
    fclose(fp);


    /*第一次迭代的偏差方程的係數矩陣A、常數矩陣L輸出到"parameter matrix.txt" */

    //設置待求的相對定向元素的初始值
    double ψ1 = 0, ω1 = 0, κ1 = 0, ψ2 = 0, ω2 = 0, κ2 = 0;
    double u1, v1, w1, u2, v2, w2;
    //係數矩陣
    double **A = (double **)malloc(sizeof(double*)*NUM);//NUM*NUM2矩陣
    for (int w = 0; w < NUM; w++)
    {
        A[w] = (double *)malloc(sizeof(double)*NUM2);
    }
    //常數矩陣
    double **L = (double **)malloc(sizeof(double*)*NUM);//NUM*1矩陣
    for (int w = 0; w < NUM; w++)
    {
        L[w] = (double *)malloc(sizeof(double) * 1);
    }
    //改正數矩陣
    double **X = (double **)malloc(sizeof(double*)*NUM2);//NUM2*1矩陣
    for (int w = 0; w < NUM2; w++)
    {
        X[w] = (double *)malloc(sizeof(double) * 1);
    }
    int number = 0;     //計算迭代次數
    //定義旋轉矩陣
    double **R1 = (double **)malloc(sizeof(double*) * 3);//3*3矩陣
    for (int w = 0; w < 3; w++)
    {
        R1[w] = (double *)malloc(sizeof(double) * 3);
    }
    double **R2 = (double **)malloc(sizeof(double*) * 3);//3*3矩陣
    for (int w = 0; w < 3; w++)
    {
        R2[w] = (double *)malloc(sizeof(double) * 3);
    }
    //定義中間矩陣
    double **AT = (double **)malloc(sizeof(double*)*NUM2);//NUM2*NUM矩陣
    for (int w = 0; w < NUM2; w++)
    {
        AT[w] = (double *)malloc(sizeof(double) * NUM);
    }
    double **p1 = (double **)malloc(sizeof(double*)*NUM2);//NUM2*NUM2矩陣
    for (int w = 0; w < NUM2; w++)
    {
        p1[w] = (double *)malloc(sizeof(double) * NUM2);
    }
    double **p2 = (double **)malloc(sizeof(double*)*NUM2);//NUM2*NUM2矩陣
    for (int w = 0; w < NUM2; w++)
    {
        p2[w] = (double *)malloc(sizeof(double) * NUM2);
    }
    double **p3 = (double **)malloc(sizeof(double*)*NUM2);//NUM2*NUM矩陣
    for (int w = 0; w < NUM2; w++)
    {
        p3[w] = (double *)malloc(sizeof(double) * NUM);
    }
    while (1)
    {
        //對於每一對同名像點
        for (int i = 0; i < NUM; i++)
        {
            //計算像點的像空間輔助座標(u1,v1,w1)和(u2,v2,w2),須要求旋轉矩陣R1,R2
            //求旋轉矩陣R1,並計算u1,v1,w1    
            R1[0][0] = cos(ψ1)*cos(κ1) - sin(ψ1)*sin(ω1)*sin(κ1);
            R1[0][1] = -cos(ψ1)*sin(κ1) - sin(ψ1)*sin(ω1)*cos(κ1);
            R1[0][2] = -sin(ψ1)*cos(ω1);
            R1[1][0] = cos(ω1)*sin(κ1);
            R1[1][1] = cos(ω1)*cos(κ1);
            R1[1][2] = -sin(ω1);
            R1[2][0] = sin(ψ1)*cos(κ1) + cos(ψ1)*sin(ω1)*sin(κ1);
            R1[2][1] = -sin(ψ1)*sin(κ1) + cos(ψ1)*sin(ω1)*cos(κ1);
            R1[2][2] = cos(ψ1)*cos(ω1);
            double **M = (double **)malloc(sizeof(double*) * 3);//3*1矩陣
            for (int w = 0; w < 3; w++)
            {
                M[w] = (double *)malloc(sizeof(double) * 1);
            }
            double **B = (double **)malloc(sizeof(double*) * 3);//3*1矩陣
            for (int w = 0; w < 3; w++)
            {
                B[w] = (double *)malloc(sizeof(double) * 1);
            }
            M[0][0] = { xleft[i] };
            M[1][0] = { yleft[i] };
            M[2][0] = { -F };
            matrixmultiply(R1, M, B, 3, 3, 1);
            u1 = B[0][0];
            v1 = B[1][0];
            w1 = B[2][0];

            //求旋轉矩陣R2,並計算u2,v2,w2
            R2[0][0] = cos(ψ2)*cos(κ2) - sin(ψ2)*sin(ω2)*sin(κ2);
            R2[0][1] = -cos(ψ2)*sin(κ2) - sin(ψ2)*sin(ω2)*cos(κ2);
            R2[0][2] = -sin(ψ2)*cos(ω2);
            R2[1][0] = cos(ω2)*sin(κ2);
            R2[1][1] = cos(ω2)*cos(κ2);
            R2[1][2] = -sin(ω2);
            R2[2][0] = sin(ψ2)*cos(κ2) + cos(ψ2)*sin(ω2)*sin(κ2);
            R2[2][1] = -sin(ψ2)*sin(κ2) + cos(ψ2)*sin(ω2)*cos(κ2);
            R2[2][2] = cos(ψ2)*cos(ω2);
            double **C = (double **)malloc(sizeof(double*) * 3);//3*1矩陣
            for (int w = 0; w < 3; w++)
            {
                C[w] = (double *)malloc(sizeof(double) * 1);
            }
            double **D = (double **)malloc(sizeof(double*) * 3);//3*1矩陣
            for (int w = 0; w < 3; w++)
            {
                D[w] = (double *)malloc(sizeof(double) * 1);
            }
            C[0][0] = { xright[i] };
            C[1][0] = { yright[i] };
            C[2][0] = { -F };
            matrixmultiply(R2, C, D, 3, 3, 1);
            u2 = D[0][0];
            v2 = D[1][0];
            w2 = D[2][0];

            //逐點計算偏差方程式的係數和常數
            A[i][0] = u1*v2 / w2;
            A[i][1] = -u2*v1 / w1;
            A[i][2] = F*(1 + v1*v2 / (w1*w2));
            A[i][3] = -u1;
            A[i][4] = u2;
            L[i][0] = -F*v1 / w1 + F*v2 / w2;
        }

        //解法方程，求改正數
        
        //計算
        matrixtran(A, AT);
        matrixmultiply(AT, A, p1, NUM2, NUM, NUM2);
        matrixinverse(p1, p2);
        matrixmultiply(p2, AT, p3, NUM2, NUM2, NUM);
        matrixmultiply(p3, L, X, NUM2, NUM, 1);

        //迭代結果：相對定向元素的改正數dψ1,dκ1和dψ2,dω2,dκ2
        ψ1 += X[0][0];
        ψ2 += X[1][0];
        ω2 += X[2][0];
        κ1 += X[3][0];
        κ2 += X[4][0];

        number++;
        //輸出第一次迭代的係數矩A和常數矩陣L
        if (number == 1)
        {
            FILE *fp = fopen("parameter matrix.txt", "w");
            if (fp == NULL)
            {
                printf("ERROR");
                return 0;
            }
            fprintf(fp, "第一次的係數矩陣A：\n");
            for (int m = 0; m < NUM; m++)
            {
                for (int n = 0; n < 5; n++)
                {
                    fprintf(fp, "%lf\t", A[m][n]);
                }
                fprintf(fp, "\n");
            }
            fprintf(fp, "\n第一次的常數矩陣L：\n");
            for (int m = 0; m < NUM; m++)
            {
                fprintf(fp, "%lf\n", L[m][0]);
            }
            fclose(fp);
        }

        //判斷迭代是否繼續，設迭代終止閾值爲0.3*pow(10,-4)
        int test = 0;
        for (int j = 0; j < 5; j++)
        {
            if (fabs(X[j][0]) < TOLERANCE)
                test++;
        }
        if (test == 5)
        {
            printf("左影像的相對定向旋轉矩陣：\n");
            for (int row = 0; row < 3; row++)
            {
                for (int col = 0; col < 3; col++)
                {
                    printf("%lf\t", R1[row][col]);
                }
                printf("\n");
            }
            printf("右影像的相對定向旋轉矩陣：\n");
            for (int row = 0; row < 3; row++)
            {
                for (int col = 0; col < 3; col++)
                {
                    printf("%lf\t", R2[row][col]);
                }
                printf("\n");
            }
            break;
        }
    }

    /*輸出相對定向線元素*/
    printf("\n相對定向線元素：\n");
    printf("左：\t0.000000\t0.000000\t0.000000");
    printf("\n右：\t%lf\t0.000000\t0.000000",Basic);
    /*輸出相對定向角元素*/
    printf("\n相對定向角元素：\n");
    printf("左：\t%lf\t%lf\t%lf", ψ1, ω1, κ1);
    printf("\n右：\t%lf\t%lf\t%lf", ψ2, ω2, κ2);
    /*輸出迭代閾值，迭代次數和5個相對定向元素的最終值 */
    printf("\n\n迭代閾值：%lf\n迭代次數：%d", TOLERANCE, number);
}