立体像对相对定向

立体像对相对定向

按照老师ppt的运算过程写出来的结果和老师给的答案不一样，检查了很多遍发现老师也不是用ppt上的运算过程写的。
为了交作业，就只能改改自己的，凑出来老师的答案emmmm。感觉挺不对的。
其实大致过程是对的，就是几个参数的位置改了改，交作业要用，就先这么着吧。
破案了破案了，ppt上的过程不对，这下边代码就可以了

#include<iostream>
#include <iomanip>
#include"math.h"using namespace std;
void PrintArray(double* a, int b, int c);//打印矩阵
void TranspositionArray(double* a, double* aT, int b, int c);//转置矩阵
void MultiplyArray(double* a, double* b, double* c, int m, int n, int l);//矩阵相乘
void make2array(double**& a, int n);
void deletarray(double**& a, int n);
int ConverseArray(double* ip, double* rp, int n);//矩阵求逆
void SubtratArray(double* a, double* b, double* c, int m, int n);//矩阵相减int main() {double f = 24;//焦距double LeftImage[6][3] = {1.983 , -6.091 ,-f,0.924 ,  7.098 ,-f,1.068 ,  4.538 ,-f,1.208 ,  6.858 ,-f,-0.514 , -10.05 ,-f,1.293 , -8.089 ,-f};//左像片像点坐标double RightImage[6][3] = {-3.202, -5.564, -f,-2.83 ,  7.694, -f,-2.878,  5.098, -f,-2.578,  7.429, -f,-5.642, -9.152, -f,-3.981, -7.441, -f};//右像片像点坐标for (int i = 0; i < 6; i++) {for (int j = 0; j < 3; j++) {LeftImage[i][j] /= 1000;RightImage[i][j] /= 1000;}}double u = 0, v = 0, pitch = 0, roll = 0, yaw = 0;//相对方位元素初始化double Rotate[3][3] = { 0 };//右片旋转矩阵	double A[6][5] = { 0 };//误差方程的系数矩阵double A_T[5][6] = { 0 }, A_TA[5][5] = { 0 }, A_TA_N[5][5] = { 0 }, A_TA_N_T[5][6] = { 0 };//中间矩阵double L[6] = { 0 };//误差方程的常数double X[5] = { 0 };//相对方位元素未知数改正数double v_v[6] = { 0 }, m0 = 0, m[5] = { 0 }, AX[6] = { 0 }, vv = 0;double XYZ[6][3] = { 0 };//模型坐标int n = 0;//累计循环次数while (1){	n++;Rotate[0][0] = cos(pitch) * cos(yaw) - sin(pitch) * sin(roll) * sin(yaw);Rotate[0][1] = (-1.0) * cos(pitch) * sin(yaw) - sin(pitch) * sin(roll) * cos(yaw);Rotate[0][2] = (-1.0) * sin(pitch) * cos(roll);Rotate[1][0] = cos(roll) * sin(yaw);Rotate[1][1] = cos(roll) * cos(yaw);Rotate[1][2] = (-1.0) * sin(roll);Rotate[2][0] = sin(pitch) * cos(yaw) + cos(pitch) * sin(roll) * sin(yaw);Rotate[2][1] = (-1.0) * sin(pitch) * sin(yaw) + cos(pitch) * sin(roll) * cos(yaw);Rotate[2][2] = cos(pitch) * cos(roll);double Bx = RightImage[0][0] - LeftImage[0][0], By = Bx * u, Bz = Bx * v;for (int i = 0; i < 6; i++) {double Right_new[3] = { 0 };double Right_New[3] = { 0 };//右相片像空间辅助坐标double Left_N = 0, Right_N = 0;//点投影系数for (int j = 0; j < 3; j++){Right_new[j] = RightImage[i][j];}MultiplyArray(*Rotate, Right_new, Right_New, 3, 3, 1);Left_N = (Bx * Right_New[2] - Bz * Right_New[0]) / (LeftImage[i][0] * Right_New[2] - LeftImage[i][2] * Right_New[0]);Right_N = (Bx * LeftImage[i][2] - Bz * LeftImage[i][0]) / (LeftImage[i][0] * Right_New[2] - LeftImage[i][2] * Right_New[0]);A[i][0] = Bx;A[i][1] = (-1.0) * Right_New[1] / Right_New[2] * Bx;A[i][2] = (-1.0) * Right_New[1] * Right_New[0] / Right_New[2] * Right_N;A[i][3] = (-1.0) * (Right_New[2] + (Right_New[1] * Right_New[1]) / Right_New[2]) * Right_N;A[i][4] = Right_New[0] * Right_N;L[i] = Left_N * LeftImage[i][1] - Right_N * Right_New[1] - By;	//模型坐标XYZ[i][0] = Left_N * LeftImage[i][0];XYZ[i][1] = 0.5 * (Left_N * LeftImage[i][1] + Right_N * Right_New[1] + By);XYZ[i][2] = Left_N * LeftImage[i][2];}TranspositionArray(*A, *A_T, 6, 5);MultiplyArray(*A_T, *A, *A_TA, 5, 6, 5);ConverseArray(*A_TA, *A_TA_N, 5);MultiplyArray(*A_TA_N, *A_T, *A_TA_N_T, 5, 5, 6);MultiplyArray(*A_TA_N_T, L, X, 5, 6, 1);u += X[0];v += X[1];pitch += X[2];roll += X[3];yaw += X[4];if ((fabs(X[2]) < 3e-5) && (fabs(X[3]) < 3e-5) && (fabs(X[4]) < 3e-5)){	cout << "累计循环了" << n << "次！" << endl;cout << "最后一次循环后的改正数为：";for (int i = 0; i < 5; i++)cout << setiosflags(ios::fixed) << X[i] << "  ";cout << endl;break;}}cout << "相对方位元素最终结果为：";cout << setiosflags(ios::fixed) << u << "   " << v << "   " << pitch << "   " << roll << "   " << yaw << "   " << endl;MultiplyArray(*A, X, AX, 6, 5, 1);SubtratArray(AX, L, v_v, 6, 1);//精度评定for (int i = 0; i < 6; i++){vv += v_v[i] * v_v[i];}m0 = sqrt(vv);cout << setiosflags(ios::fixed) << m[0];cout << "相对方位元素精度评定结果为：";for (int i = 0; i < 5; i++){m[i] = m0 * sqrt(fabs(A_TA_N[i][i]));cout << m[i] << "   ";}cout << endl;cout << "模型坐标为：" << endl;PrintArray(*XYZ, 6, 3);return 0;
}
void PrintArray(double* a, int b, int c) {//打印矩阵	for (int i = 0; i < b; i++) {for (int j = 0; j < c; j++) {cout << a[i * c + j] << " ";}cout << endl;}
}void TranspositionArray(double* a, double* aT, int b, int c) {//转置矩阵for (int i = 0; i < b; i++) {for (int j = 0; j < c; j++) {aT[j * b + i] = a[i * c + j];}}
}void MultiplyArray(double* a, double* b, double* c, int m, int n, int l) {//矩阵相乘for (int i = 0; i < m * l; i++) {//矩阵初始化c[i] = 0;}for (int i = 0; i < m; i++) {for (int j = 0; j < l; j++) {for (int k = 0; k < n; k++) {c[i * l + j] += a[i * n + k] * b[k * l + j];}}}
}void make2array(double**& a, int n)
{int i, j;a = new double* [n];for (i = 0; i < n; i++){a[i] = new double[2 * n];}for (i = 0; i < n; i++){for (j = 0; j < 2 * n; j++){a[i][j] = 0;}}
}
void deletarray(double**& a, int n)
{int i;for (i = 0; i < n; i++){delete[]a[i];}delete[]a;
}
//矩阵求逆
//ip为要求逆的矩阵，rp是返回的逆矩阵，矩阵维数为n  
//行列式为0，返回0；否则返回值为1
int ConverseArray(double* ip, double* rp, int n)
{double** mat = NULL; //做行变换的矩阵   int i, j, r;double k, temp;//初始化mat为0,大小为n*2nmake2array(mat, n);for (i = 0; i < n; i++){for (j = 0; j < n; j++){mat[i][j] = ip[i * n + j];}mat[i][n + i] = 1; //初始化右侧的单位矩阵   }//做行变换化为上三角阵   for (i = 0; i < n; i++){if (mat[i][i] == 0) //第i行i列为0   {for (j = i + 1; j < n; j++){if (mat[j][i] != 0)     //找一个非0行   {for (r = i; r < 2 * n; r++){temp = mat[j][r];mat[j][r] = mat[i][r];mat[i][r] = temp;}break; //跳出j循环   }}}if (mat[i][i] == 0)   return   0; //行列式为0则返回   for (j = i + 1; j < n; j++){if (mat[j][i] != 0) //i行i列下方的j行i列元素不为0   {k = -mat[j][i] / mat[i][i]; //做行变换   for (r = i; r < 2 * n; r++)mat[j][r] = mat[j][r] + k * mat[i][r];}}}//化成单位矩阵   for (i = n - 1; i >= 0; i--){k = mat[i][i];for (r = i; r < 2 * n; r++)mat[i][r] = mat[i][r] / k;for (j = i - 1; j >= 0; j--){k = -mat[j][i];for (r = i; r < 2 * n; r++)mat[j][r] = mat[j][r] + k * mat[i][r];}}//将结果输出   for (i = 0; i < n; i++)for (j = 0; j < n; j++)rp[i * n + j] = mat[i][j + n];//mat矩阵释放 deletarray(mat, n);return 1;
}void SubtratArray(double* a, double* b, double* c, int m, int n)
{int i, j;for (i = 0; i < m; i++){for (j = 0; j < n; j++){c[i * n + j] = a[i * n + j] - b[i * n + j];}}
}