#include<iostream>
#include<iomanip>
using namespace std;
void swap(float a[], float b[], int n)
{
float t;
for (int i = 0; i<n; i++)
{
t = a[i];
a[i] = b[i];
b[i] = t;
}
}
void chu(float a[], int j, int n)
{
float t;
t = a[j];
for (int i = 0; i<n; i++)
a[i] = a[i] / t;
}
void jia(float a[], float b[], float m, int n)
{
for (int i = 0; i<n; i++)
{
a[i] = a[i] + m*b[i];
}
}
void ni_Matrix(float * * p){
int n, i, j;
float t;
//输入矩阵
cout << "请输入矩阵的阶数" << endl;
cin >> n;
float **p_aij = new float*[2 * n];
for (i = 0; i < 2 * n; i++){
p_aij[i] = new float[2 * n];
if (i < n){
for (j = 0; j < n; j++){
p_aij[i][j] = p[i][j];
}
}
}
//后部归零
for (i = 0; i<2 * n; i++)
{
for (j = n; j<2 * n; j++)p_aij[i][j] = 0;
}
//添加单位阵
for (i = 0; i<n; i++)
for (j = n; j<2 * n; j++)
{
if ((j - n) == i){ p_aij[i][j] = 1; }
}
//正向行化简
for (j = 0; j<n; j++)
{
for (i = j; i<n; i++){ if (p_aij[i][j] != 0)break; }
swap(p_aij[j], p_aij[i], 2 * n);
chu(p_aij[j], j, 2 * n);
for (i = j + 1; i<n; i++)
{
jia(p_aij[i], p_aij[j], -p_aij[i][j], 2 * n);
}
}
//逆向行化简
for (j = n - 1; j >= 0; j--)
{
for (i = j; i >= 0; j--){ if (p_aij[i][j] != 0)break; }
swap(p_aij[j], p_aij[i], 2 * n);
chu(p_aij[j], j, 2 * n);
for (i = j - 1; i >= 0; i--)
{
jia(p_aij[i], p_aij[j], -p_aij[i][j], 2 * n);
}
}
//输出
cout << "AtA的逆矩阵：\n";
for (i = 0; i<n; i++)
{
cout << "(";
for (j = n; j < 2 * n; j++){
p[i][j - n] = p_aij[i][j];
cout << setw(4) << p[i][j-n] << " ";
}
cout <<")"<< endl;
}
cin.get();
cin.get();
for (int i = 0; i < 2 * n; i++)
delete[] p_aij[i];
delete[] p_aij;
}
int main()
{
//创建世界坐标矩阵
cout << "请输入十组世界坐标（x,y,z）:\n";
float **p_world = new float *[10];
for (int i = 0; i < 10; i++){
p_world[i] = new float[3];
for (int j = 0; j < 3; j++){
cin >> p_world[i][j];
if (!cin){
cin.clear();
while (cin.get() != '\n')
continue;
cout << "Bad input!!";
break;
}
}
}
//创建旋转矩阵
cout << "请输入旋转角度（x,y,z）:\n";
float ** p_rotation = new float *[3];
float x_angle, y_angle, z_angle;
cout << "x轴："; cin >> x_angle;
cout << "y轴："; cin >> y_angle;
cout << "z轴："; cin >> z_angle;
for (int i = 0; i < 3; i++)
p_rotation[i] = new float[3];
p_rotation[0][0] = cos(x_angle)*cos(z_angle) - sin(x_angle)*sin(y_angle)*sin(z_angle);
p_rotation[0][1] = cos(y_angle)*sin(z_angle) + sin(x_angle)*sin(y_angle)*cos(z_angle);
p_rotation[0][2] = -cos(x_angle)*sin(y_angle);
p_rotation[1][0] = -cos(x_angle)*sin(z_angle);
p_rotation[1][1] = cos(x_angle)*cos(z_angle);
p_rotation[1][2] = sin(x_angle);
p_rotation[2][0] = sin(y_angle)*cos(z_angle) + sin(x_angle)*cos(y_angle)*sin(z_angle);
p_rotation[2][1] = sin(y_angle)*sin(z_angle) - sin(x_angle)*cos(y_angle)*cos(z_angle);
p_rotation[2][2] = cos(x_angle)*cos(y_angle);
//创建平移矩阵
cout << "输入平移向量（x,y,z）:\n";
float * p_trans = new float[3];
for (int j = 0; j < 3; j++){
cin >> p_trans[j];
if (!cin){
cin.clear();
while (cin.get() != '\n')
continue;
cout << "Bad input!!";
break;
}
}
//创建内参数矩阵
cout << "按顺序输入相机内参数Ku,Kv,Uo,Vo：\n";
float Ku, Kv, Uo, Vo;
cin >> Ku >> Kv >> Uo >> Vo; cout << endl;
float * *p_nei = new float *[3];
for (int i = 0; i < 3; i++){
for (int j = 0; j < 3; j++){
p_nei[i] = new float[3];
p_nei[i][j] = 0;
}
}
p_nei[0][0] = Ku; p_nei[0][2] = Uo; p_nei[1][1] = Kv; p_nei[1][2] = Vo; p_nei[2][2] = 1;
//打印内参数矩阵
cout << "\n得相机内参数矩阵为：\n";
for (int i = 0; i < 3; i++){
cout << "(";
for (int j = 0; j < 3; j++){
cout << p_nei[i][j] << " ";
}
cout << ")\n";
}
//创建外参数矩阵
cout << "\n由旋转和平移矩阵得外参数矩阵：\n";
float ** p_wai = new float*[3];
for (int i = 0; i < 3; i++){
p_wai[i] = new float[4];
for (int j = 0; j < 3; j++){
p_wai[i][j] = p_rotation[i][j];
}
p_wai[i][3] = p_trans[i];
}
//打印外参数矩阵
for (int i = 0; i < 3; i++){
cout << "(";
for (int j = 0; j < 4; j++)
cout << p_wai[i][j] << ",";
cout << ")\n";
}
//由内外参数矩阵得相机的参数矩阵
cout << "\n由内外参数矩阵得相机的参数矩阵:\n";
float ** p_camera = new float *[3];
for (int i = 0; i < 3; i++){
p_camera[i] = new float[4];
cout << "(";
for (int j = 0; j < 4; j++){
p_camera[i][j] = p_nei[i][0] * p_wai[0][j] + p_nei[i][1] * p_wai[1][j] + p_nei[i][2] * p_wai[2][j];
cout << p_camera[i][j] << " ";
}
cout << ")\n";
}
//显示世界坐标
cout << "\n世界坐标：\n";
for (int i = 0; i < 10; i++){
cout << "(";
for (int j = 0; j < 3; j++){
cout << p_world[i][j] << ",";
}
cout << ")\n";
}
//根据世界坐标得相应图像坐标
cout << "\n十组世界坐标对应的图像坐标：\n";
float ** p_image = new float *[10];
int Eu = 0.5, Ev = 0.5;
for (int i = 0; i < 10; i++){
cout << "(";
p_image[i] = new float[2];
float su = (p_world[i][0] * p_camera[0][0] + p_world[i][1] * p_camera[0][1] + p_world[i][2] * p_camera[0][2] + p_camera[0][3]);
float s = (p_world[i][0] * p_camera[2][0] + p_world[i][1] * p_camera[2][1] + p_world[i][2] * p_camera[2][2] + p_camera[2][3]);
float sv = (p_world[i][0] * p_camera[1][0] + p_world[i][1] * p_camera[1][1] + p_world[i][2] * p_camera[1][2] + p_camera[1][3]);
p_image[i][0] = su / s; cout << p_image[i][0] << ",";
p_image[i][1] = sv / s; cout << p_image[i][1] << ")\n";
}
//给图像加干扰后得到的图像坐标
float ** p_errorimage = new float *[10];
cout << "\n加干扰后求得的图像坐标：\n";
for (int i = 0; i < 10; i++){
cout << "(";
p_errorimage[i] = new float[2];
p_errorimage[i][0] = p_image[i][0] + 0.02; cout << p_errorimage[i][0] << ",";
p_errorimage[i][1] = p_image[i][1] + 0.01; cout << p_errorimage[i][1] << ")\n";
}
//构造A矩阵p_a1+p_a2=p_A
float ** p_a1 = new float *[10];
for (int i = 0; i < 10; i++){
p_a1[i] = new float[11];
for (int j = 0; j < 3; j++)
p_a1[i][j] = p_world[i][j];
p_a1[i][3] = 1;
for (int j = 4; j < 8; j++)
p_a1[i][j] = 0;
for (int j = 8; j < 11; j++)
p_a1[i][j] = -p_errorimage[i][0] * p_world[i][j - 8];
}
cout << "\nThe a1 Matrix:\n";
for (int i = 0; i < 10; i++){
cout << "(";
for (int j = 0; j < 11; j++)
cout << p_a1[i][j] << " ";
cout << ")\n";
}
//end p_a1
float ** p_a2 = new float *[10];
for (int i = 0; i < 10; i++){
p_a2[i] = new float[11];
for (int j = 0; j < 4; j++)
p_a2[i][j] = 0;
for (int j = 4; j < 7; j++)
p_a2[i][j] = p_world[i][j - 4];
p_a2[i][7] = 1;
for (int j = 8; j < 11; j++)
p_a2[i][j] = -p_image[i][1] * p_world[i][j - 8];//后需改为errorimage！！！！！！！！！！！
}
cout << "\nThe a2 Matrix:\n";
for (int i = 0; i < 10; i++){
cout << "(";
for (int j = 0; j < 11; j++)
cout << p_a2[i][j] << " ";
cout << ")\n";
}
//end p_a2
//构造A
float ** p_A = new float *[20];
for (int i = 0; i < 20; i++){
p_A[i] = new float[11];
if (i % 2 == 0){
for (int j = 0; j < 11; j++)
p_A[i][j] = p_a1[i / 2][j];
}
else if (i % 2 == 1){
for (int j = 0; j < 11; j++)
p_A[i][j] = p_a2[(i - 1) / 2][j];
}
}
//打印A矩阵
cout << "\nThe A Matrix:\n";
for (int i = 0; i < 20; i++){
cout << "(";
for (int j = 0; j < 11; j++)
cout << p_A[i][j] << " ";
cout << ")\n";
}//end A
//构造AT
cout << "\nThe At Matrix:\n";
float **p_At = new float *[11];
for (int i = 0; i < 11; i++){
p_At[i] = new float[20];
cout << "(";
for (int j = 0; j < 20; j++){
p_At[i][j] = p_A[j][i];
cout << p_At[i][j] << " ";
}
cout << ")\n";
}//end AT
//构造ATA
cout << "\nThe ATA Matrix :\n";
float ** p_AtA = new float *[11];
for (int i = 0; i < 11; i++){
cout << "(";
p_AtA[i] = new float[11];
for (int j = 0; j < 11; j++){
p_AtA[i][j] = 0;
for (int k = 0; k < 20; k++)
p_AtA[i][j] += p_At[i][k] * p_A[k][j];
cout << p_AtA[i][j] << " ";
}
cout << ")\n";
}//end ATA
//构造AtA的逆矩阵
//********求任何一个矩阵的逆********
ni_Matrix(p_AtA);
//此时p_AtA矩阵为原矩阵的逆矩阵(成功)！！！！！
//----------------------------
//构造B20*1矩阵
//----------------------------
float ** p_B = new float*[20];
for (int i = 0; i < 20; i++){
p_B[i]= new float[1];
}
cout << "The B Matrix:\n";
for (int i = 0; i < 20; i++){
if (i % 2 == 0)
p_B[i][0] = p_errorimage[i / 2][0];
else
p_B[i][0] = p_errorimage[(i - 1) / 2][1];
cout << "(" << p_B[i][0] << ")" << endl;
}//end B
//--------------
//计算X即求参数矩阵C
//---------------
cout << "The C1 matrix:\n";
float ** p_C1 = new float *[11];
for (int i = 0; i < 11; i++){
cout << i<<"行 (";
p_C1[i] = new float[20];
for (int j = 0; j < 20; j++){
p_C1[i][j] = 0;
for (int k = 0; k < 11; k++)
p_C1[i][j] += p_AtA[i][k] * p_At[k][j];
cout << " " << p_C1[i][j];
}
cout << ")\n";
}
cout << "\nThe computed C2 Matrix :\n";
float ** p_C2 = new float *[11];
for (int i = 0; i < 11; i++){
cout<< "(";
p_C2[i] = new float[1];
for (int j = 0; j < 1; j++){
p_C2[i][j] = 0;
for (int k = 0; k < 11; k++)
p_C2[i][j] += p_C1[i][k] * p_B[k][j];
cout <<" "<< p_C2[i][j];
}
cout<< ")\n" ;
}
cout << "\nThe computed C Matrix :\n";
float ** p_C = new float *[3];
for (int i = 0; i < 3; i++){
cout << "(";
p_C[i] = new float[4];
if (i<2)
{
for (int j = 0; j < 4; j++){
p_C[i][j] = p_C2[j][0];
cout <<" "<<p_C[i][j];
}
cout << ")\n";
}
else
{
p_C[i][0] = p_C2[8][0];
p_C[i][1] = p_C2[9][0];
p_C[i][2] = p_C2[10][0];
p_C[i][3]= p_trans[2];
for (int j = 0; j < 4; j++){ cout <<" "<< p_C[i][j]; }
cout << ")\n";
}
}//end C
cin.get();
cin.get();
//释放二维数组
for (int i = 0; i < 10; i++)
delete[]p_world[i];
delete[]p_world;
for (int i = 0; i < 3; i++)
delete[] p_rotation[i];
delete[]p_rotation;
for (int i = 0; i < 3; i++)
delete[] p_wai[i];
delete[] p_wai;
for (int i = 0; i < 3; i++)
delete[] p_nei[i];
delete[] p_nei;
delete[] p_trans;
for (int i = 0; i < 3; i++)
delete[] p_camera[i];
delete[] p_camera;
for (int i = 0; i < 10; i++)
delete[]p_image[i];
delete[] p_image;
for (int i = 0; i < 10; i++)
delete[]p_errorimage[i];
delete[] p_errorimage;
for (int i = 0; i < 10; i++)
delete[]p_a1[i];
delete[] p_a1;
for (int i = 0; i < 10; i++)
delete[]p_a2[i];
delete[] p_a2;
for (int i = 0; i < 20; i++)
delete[]p_A[i];
delete[] p_A;
for (int i = 0; i < 11; i++)
delete[]p_At[i];
delete[] p_At;
for (int i = 0; i < 11; i++)
delete[]p_AtA[i];
delete[] p_AtA;
for (int i = 0; i < 20; i++)
delete[]p_B[i];
delete[] p_B;
for (int i = 0; i < 11; i++)
delete[]p_C2[i];
delete[] p_C2;
for (int i = 0; i < 11; i++)
delete[]p_C1[i];
delete[] p_C1;
for (int i = 0; i < 3; i++)
delete[]p_C[i];
delete[] p_C;
}
扫一扫
1080
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
