仿照getAffineTransform三点映射的方法实现三维空间的四点映射
程序员文章站
2022-04-03 23:19:49
...
仿照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);
}