仿照getAffineTransform三点映射的方法实现三维空间的四点映射

仿照getAffineTransform三点映射的方法实现三维空间的四点映射

#include <iostream>
#include <opencv.hpp>
#include <vector>


class GetgaussJordan
{
protected:
	cv::Mat newM;
public:
	void step0(double m, double newMat[15][15]) {
		for (int i = 0; i < 15; i++) {
			for (int j = 0; j < 15; j++) {
				if (i == j) {
					newMat[i][j] = 1;
				}
				else {
					newMat[i][j] = 0;
				}
			}
		}
	}

	void step1(double m, double swap[15], double l[15][15], double mat11[15][15]) {
		for (int i = 0; i < 15; i++) {
			swap[i] = i;
			for (int j = 0; j < 15; j++) {
				l[i][j] = 0;
			}
		}
		for (int i = 0; i < 15; i++) {
			double max_row = mat11[i][i];
			int row = i;
			for (int j = i; j < 15; j++) {
				if (mat11[j][i] >= max_row) {
					max_row = mat11[j][i];
					row = j;
				}
			}
			swap[i] = row;
			if (row != i) {
				for (int j = 0; j < 15; j++) {
					double swapk = mat11[i][j];
					mat11[i][j] = mat11[row][j];
					mat11[row][j] = swapk;
				}
			}
			for (int j = i + 1; j < 15; j++) {
				if (mat11[j][i] != 0) {
					l[j][i] = mat11[j][i] / mat11[i][i];
					for (int k = 0; k < 15; k++) {
						mat11[j][k] = mat11[j][k] - (l[j][i] * mat11[i][k]);
					}
				}
			}
		}
	}

	void step2(double m, double mat11[15][15], double l1[15][15]) {
		int longM = m - 1;
		for (int i = 0; i < 15; i++) {
			for (int j = 0; j < 15; j++) {
				l1[i][j] = 0;
			}
		}
		for (int i = 0; i < 15 - 1; i++) {
			for (int j = 0; j < longM - i; j++) {
				if ((mat11[longM - i - j - 1][longM - i] != 0) && (mat11[longM - i][longM - i] != 0)) {
					l1[longM - i - j - 1][longM - i] = mat11[longM - i - j - 1][longM - i] / mat11[longM - i][longM - i];
					for (int k = 0; k < 15; k++) {
						mat11[longM - i - j - 1][k] = mat11[longM - i - j - 1][k] - l1[longM - i - j - 1][longM - i] * mat11[longM - i][k];
					}
				}
			}
		}
	}

	void step3(double m, double mat11[15][15], double l2[15]) {
		for (int i = 0; i < 15; i++) {
			l2[i] = mat11[i][i];
		}
	}

	void gaussJordan(int m, double mat11[15][15], double newMat[15][15]) {
		double swap[15], l[15][15], l1[15][15], l2[15];
		step0(15, newMat);
		step1(15, swap, l, mat11);
		step2(15, mat11, l1);
		step3(15, mat11, l2);

		for (int i = 0; i < 15; i++) {
			if (swap[i] != i) {
				for (int j = 0; j < 15; j++) {
					double swapk1 = newMat[i][j];
					int k1 = swap[i];
					newMat[i][j] = newMat[k1][j];
					newMat[k1][j] = swapk1;
				}
			}
			for (int j = i + 1; j < 15; j++) {
				for (int k = 0; k < 15; k++) {
					if (l[j][i] != 0) {
						newMat[j][k] = newMat[j][k] - l[j][i] * newMat[i][k];
					}
				}
			}
		}
		for (int i = 0; i < 15 - 1; i++) {
			for (int j = 0; j < 15 - i - 1; j++) {
				if (l1[15 - 1 - i - j - 1][15 - 1 - i] != 0) {
					for (int k = 0; k < 15; k++) {
						newMat[15 - 1 - i - j - 1][k] = newMat[15 - 1 - i - j - 1][k] - l1[15 - 1 - i - j - 1][15 - i - 1] * newMat[15 - i - 1][k];
					}
				}
			}
		}
		for (int i = 0; i < 15; i++) {
			for (int j = 0; j < 15; j++) {
				newMat[i][j] = newMat[i][j] / l2[i];
			}
		}
	}
};

void getNewMat(double srcPoints[5][3], double destPoints[5][3], double mat[4][4]) {
	int nums = 4;
	double A[15][15], B[15][1];
	for (int i = 0; i < 15; i++) {
		B[i][0] = 0.0;
		for (int j = 0; j < 15; j++) {
			A[i][j] = 0.0;
		}
	}
	for (int i = 0; i < 5; i++) {
		std::cout << srcPoints[i][0] << std::endl;
		double A_i[3] = { srcPoints[i][0], srcPoints[i][1], srcPoints[i][2] };
		double B_i[3] = { destPoints[i][0], destPoints[i][1], destPoints[i][2] };
		double A_x1[15] = { A_i[0], A_i[1], A_i[2], 1, 0, 0, 0, 0, 0, 0, 0, 0, -A_i[0] * B_i[0], -A_i[1] * B_i[0], -A_i[2] * B_i[0] };
		for (int k = 0; k < 15; k++) {
			A[3 * i][k] = A_x1[k];
		}
		B[3 * i][0] = B_i[0];
		double A_x2[15] = { 0, 0, 0, 0, A_i[0], A_i[1], A_i[2], 1, 0, 0, 0, 0, -A_i[0] * B_i[1], -A_i[1] * B_i[1], -A_i[2] * B_i[1] };
		for (int k = 0; k < 15; k++) {
			A[3 * i + 1][k] = A_x2[k];
		}
		B[3 * i + 1][0] = B_i[1];
		double A_x3[15] = { 0, 0, 0, 0, 0, 0, 0, 0, A_i[0], A_i[1], A_i[2], 1, -A_i[0] * B_i[2], -A_i[1] * B_i[2], -A_i[2] * B_i[2] };
		for (int k = 0; k < 15; k++) {
			A[3 * i + 2][k] = A_x3[k];
		}
		B[3 * i + 2][0] = B_i[1];
	}
	double newData[15][15];
	GetgaussJordan geta;
	geta.gaussJordan(15, A, newData);
	cv::Mat newA = cv::Mat(15, 15, CV_32FC1);
	cv::Mat newB = cv::Mat(15, 1, CV_32FC1);
	for (int i = 0; i < 15; i++) {
		newB.at<float>(i, 0) = B[i][0];
		for (int j = 0; j < 15; j++) {
			newA.at<float>(i, j) = newData[i][j];
		}
	}
	//std::cout << newA << std::endl;
	//std::cout << newB << std::endl;

	cv::Mat warp = newA * newB;
	double mat1[4][4] = { {warp.at<float>(0), warp.at<float>(1), warp.at<float>(2), warp.at<float>(3)},
						{warp.at<float>(4), warp.at<float>(5), warp.at<float>(6), warp.at<float>(7)},
						{warp.at<float>(8), warp.at<float>(9), warp.at<float>(10), warp.at<float>(11)},
						{warp.at<float>(12), warp.at<float>(13), warp.at<float>(14), 1.0 } };
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			mat[i][j] = mat1[i][j];
		}
	}
}

void getNewPoint(double point[3], double newPoint[3], double mat[4][4]) {
	cv::Mat newP = cv::Mat(4, 1, CV_32FC1);
	double P[4][1] = { point[0], point[1], point[2], 1.0 };
	for (int i = 0; i < 4; i++) {
		newP.at<float>(i, 0) = P[i][0];
	}
	cv::Mat newMat = cv::Mat(4, 4, CV_32FC1);
	for (int i = 0; i < 4; i++) {
		for (int j = 0; j < 4; j++) {
			newMat.at<float>(i, j) = mat[i][j];
		}
	}

	cv::Mat restMat;
	restMat = newMat * newP;
	std::cout << restMat << std::endl;
	newPoint[0] = restMat.at<float>(0) / restMat.at<float>(3);
	newPoint[1] = restMat.at<float>(1) / restMat.at<float>(3);
	newPoint[2] = restMat.at<float>(2) / restMat.at<float>(3);
	std::cout << newPoint[0] << std::endl;
	std::cout << newPoint[1] << std::endl;
	std::cout << newPoint[2] << std::endl;
}

int main() {
	double srcPoints[5][3] = { { 0, 0, 1 }, { 100, 0, 1 }, { 100, 100, 1 }, { 0, 100, 1 }, { 0, 50, 1 } };
	double destPoints[5][3] = { { 10, 10, 3 }, { 130, 20, 3 }, { 140, 140, 3 }, { 30, 120, 3 }, { 10, 70, 3} };
	double mat[4][4];
	double point[3] = { 0, 100, 1 };
	double newPoint[3];
	getNewMat(srcPoints, destPoints, mat);
	getNewPoint(point, newPoint, mat);

}