MatrixXd Utils::calculateHomographyMatrixFromFiveOrtoghonalLines(QList<Line*> firstOrtoghonalLines, QList<Line*> secondOrthogonalLines,
QList<Line*> thirdOrthogonalLines, QList<Line*> fourthOrthogonalLines,
QList<Line*> fifthOrthogonalLines)
{
// A * x = b.
MatrixXd A(5, 6);
MatrixXd b(5, 1);
MatrixXd x(5, 1);
Vector3d l1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(0));
Vector3d m1 = getLineInHomogeneousCoordinates(firstOrtoghonalLines.at(1));
Vector3d l2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(0));
Vector3d m2 = getLineInHomogeneousCoordinates(secondOrthogonalLines.at(1));
Vector3d l3 = getLineInHomogeneousCoordinates(thirdOrthogonalLines.at(0));
Vector3d m3 = getLineInHomogeneousCoordinates(thirdOrthogonalLines.at(1));
Vector3d l4 = getLineInHomogeneousCoordinates(fourthOrthogonalLines.at(0));
Vector3d m4 = getLineInHomogeneousCoordinates(fourthOrthogonalLines.at(1));
Vector3d l5 = getLineInHomogeneousCoordinates(fifthOrthogonalLines.at(0));
Vector3d m5 = getLineInHomogeneousCoordinates(fifthOrthogonalLines.at(1));
b << -l1(1)*m1(1), -l2(1)*m2(1), -l3(1)*m3(1), -l4(1)*m4(1), -l5(1)*m5(1);
A << l1(0)*m1(0), (l1(0)*m1(1)+l1(1)*m1(0))/2, l1(1)*m1(1), (l1(0)*m1(2)+l1(2)*m1(0))/2, (l1(1)*m1(2)+l1(2)*m1(1))/2, l1(2)*m1(2),
l2(0)*m2(0), (l2(0)*m2(1)+l2(1)*m2(0))/2, l2(1)*m2(1), (l2(0)*m2(2)+l2(2)*m2(0))/2, (l2(1)*m2(2)+l2(2)*m2(1))/2, l2(2)*m2(2),
l3(0)*m3(0), (l3(0)*m3(1)+l3(1)*m3(0))/2, l3(1)*m3(1), (l3(0)*m3(2)+l3(2)*m3(0))/2, (l3(1)*m3(2)+l3(2)*m3(1))/2, l3(2)*m3(2),
l4(0)*m4(0), (l4(0)*m4(1)+l4(1)*m4(0))/2, l4(1)*m4(1), (l4(0)*m4(2)+l4(2)*m4(0))/2, (l4(1)*m4(2)+l4(2)*m4(1))/2, l4(2)*m4(2),
l5(0)*m5(0), (l5(0)*m5(1)+l5(1)*m5(0))/2, l5(1)*m5(1), (l5(0)*m5(2)+l5(2)*m5(0))/2, (l5(1)*m5(2)+l5(2)*m5(1))/2, l5(2)*m5(2);
x = A.colPivHouseholderQr().solve(b);
x/=x(2);
Matrix3d C;
C << x(0), x(1)/2, x(3)/2,
x(1)/2, x(2), x(4)/2,
x(3)/2, x(4)/2, 1;
Matrix2d kkt;
kkt << C(0,0), C(0,1),
C(1,0), C(1,1);
MatrixXd vKKt(1,2);
vKKt << C(2,0), C(2,1);
MatrixXd V(1,2);
V = vKKt * kkt.inverse();
LLT<MatrixXd> llt(kkt);
MatrixXd U = llt.matrixU();
MatrixXd J (3,3);
J << U(0,0), U(0,1),0, U(1,0), U(1,1),0, V(0), V(1), 1;
return J;
}
void MetricRectification::solve()
{
//std::vector<Eigen::Vector3d> vertices;
//vertices.push_back(Eigen::Vector3d(52.3467, 125.102, 1.0));
//vertices.push_back(Eigen::Vector3d(340.253, 130.147, 1.0));
//vertices.push_back(Eigen::Vector3d(193.28, 126.111, 1.0));
//vertices.push_back(Eigen::Vector3d(225.493, 360.173, 1.0));
//vertices.push_back(Eigen::Vector3d(42.28, 263.32, 1.0));
//vertices.push_back(Eigen::Vector3d(296.967, 397.502, 1.0));
//vertices.push_back(Eigen::Vector3d(212.407, 269.373, 1.0));
//vertices.push_back(Eigen::Vector3d(34.2267, 391.449, 1.0));
//
//vertices.push_back(Eigen::Vector3d(294.953, 318.809, 1.0));
//vertices.push_back(Eigen::Vector3d(456.02, 322.844, 1.0));
//vertices.push_back(Eigen::Vector3d(492.26, 208.84, 1.0));
//vertices.push_back(Eigen::Vector3d(429.847, 400.529, 1.0));
//vertices.push_back(Eigen::Vector3d(299.987, 31.2756, 1.0));
//vertices.push_back(Eigen::Vector3d(555.68, 273.409, 1.0));
//vertices.push_back(Eigen::Vector3d(545.613, 39.3467, 1.0));
//vertices.push_back(Eigen::Vector3d(236.567, 250.204, 1.0));
//
//vertices.push_back(Eigen::Vector3d(95.6333, 264.329, 1.0));
//vertices.push_back(Eigen::Vector3d(501.32, 273.409, 1.0));
//vertices.push_back(Eigen::Vector3d(302.00, 29.2578, 1.0));
//vertices.push_back(Eigen::Vector3d(297.973, 398.511, 1.0));
//std::vector<Eigen::Vector3d> vertices;
//vertices.push_back(Eigen::Vector3d(58, 426, 1.0));
//vertices.push_back(Eigen::Vector3d(510, 89, 1.0));
//vertices.push_back(Eigen::Vector3d(546, 413, 1.0));
//vertices.push_back(Eigen::Vector3d(213, 186, 1.0));
//vertices.push_back(Eigen::Vector3d(105, 247, 1.0));
//vertices.push_back(Eigen::Vector3d(299, 130, 1.0));
//vertices.push_back(Eigen::Vector3d(334, 388, 1.0));
//vertices.push_back(Eigen::Vector3d(76, 171, 1.0));
//
//vertices.push_back(Eigen::Vector3d(302, 420, 1.0));
//vertices.push_back(Eigen::Vector3d(495, 240, 1.0));
//vertices.push_back(Eigen::Vector3d(569, 375, 1.0));
//vertices.push_back(Eigen::Vector3d(362, 244, 1.0));
//vertices.push_back(Eigen::Vector3d(140, 134, 1.0));
//vertices.push_back(Eigen::Vector3d(554, 127, 1.0));
//vertices.push_back(Eigen::Vector3d(407, 314, 1.0));
//vertices.push_back(Eigen::Vector3d(363, 22, 1.0));
//
//vertices.push_back(Eigen::Vector3d(224, 89, 1.0));
//vertices.push_back(Eigen::Vector3d(191, 324, 1.0));
//vertices.push_back(Eigen::Vector3d(39, 187, 1.0));
//vertices.push_back(Eigen::Vector3d(385, 181, 1.0));
//lineVertices = vertices;
for (auto pl : lineVertices)
std::cout << pl.x() << ", " << pl.y() << ", " << pl.z() << std::endl;
std::vector<Eigen::Vector3d> lines;
for (int i = 0; i < lineVertices.size() - 1; i += 2)
lines.push_back(normalizeLine(lineVertices[i], lineVertices[i + 1]));
for (int i = 0; i < lines.size(); ++i)
std::cout << "l" << i << " : " << lines[i].x() << ", " << lines[i].y() << ", " << lines[i].z() << std::endl;
std::cout << std::endl << std::endl;
//lines[0] = Eigen::Vector3d(-0.000141084, 0.00805224, -0.999968);
//lines[1] = Eigen::Vector3d(-0.00568419, 0.000782299, 0.999984);
//lines[2] = Eigen::Vector3d(-0.00218568, 0.00414856, -0.999989);
//lines[3] = Eigen::Vector3d(-0.0016513, -0.00241022, 0.999996);
//lines[4] = Eigen::Vector3d(-8.04546e-05, 0.00321109, -0.999995);
//lines[5] = Eigen::Vector3d(-0.00178489, -0.000581155, 0.999998);
//lines[6] = Eigen::Vector3d(-0.00374583, 0.0039556, 0.999985);
//lines[7] = Eigen::Vector3d(-0.00165759, -0.00242947, 0.999996);
//lines[8] = Eigen::Vector3d(-8.53647e-05, 0.00381402, -0.999993);
//lines[9] = Eigen::Vector3d(-0.00330775, -3.60706e-05, 0.999995);
Eigen::MatrixXd A(5, 5);
Eigen::Vector3d l, m;
Eigen::VectorXd b(5);
for (int i = 0; i < 5; ++i)
{
l = lines[i * 2 + 0];
m = lines[i * 2 + 1];
A(i, 0) = l[0] * m[0];
A(i, 1) = (l[0] * m[1] + l[1] * m[0]) / 2.0;
A(i, 2) = l[1] * m[1];
A(i, 3) = (l[0] * m[2] + l[2] * m[0]) / 2.0;
A(i, 4) = (l[1] * m[2] + l[2] * m[1]) / 2.0;
b[i] = -l[2] * m[2];
}
Eigen::MatrixXd x = A.colPivHouseholderQr().solve(b);
std::cout << "A: \n" << A << std::endl << std::endl;
std::cout << "b: \n" << b << std::endl << std::endl;
std::cout << "x: \n" << x << std::endl << std::endl;
Eigen::MatrixXd C(3, 3);
C(0, 0) = x(0);
C(0, 1) = x(1) / 2.0;
C(0, 2) = x(3) / 2.0;
C(1, 0) = x(1) / 2.0;
C(1, 1) = x(2);
C(1, 2) = x(4) / 2.0;
C(2, 0) = x(3) / 2.0;
C(2, 1) = x(4) / 2.0;
C(2, 2) = 1.0;
std::cout << "C : " << std::endl << C << std::endl << std::endl;
Eigen::MatrixXd KKt(2, 2);
KKt(0, 0) = C(0, 0);
KKt(0, 1) = C(1, 0);
KKt(1, 0) = C(0, 1);
KKt(1, 1) = C(1, 1);
std::cout << "KKt : " << std::endl << KKt << std::endl << std::endl;
Eigen::MatrixXd vKKt(1, 2);
vKKt(0, 0) = C(2, 0);
vKKt(0, 1) = C(2, 1);
std::cout << "vKKT : " << std::endl << vKKt << std::endl << std::endl;
Eigen::MatrixXd V(1, 2);
V = vKKt * KKt.inverse();
std::cout << "V : " << std::endl << V << std::endl << std::endl;
Eigen::MatrixXd K = Eigen::LDLT<Eigen::MatrixXd>(KKt).matrixU();
std::cout << "K : " << std::endl << K << std::endl << std::endl;
Eigen::MatrixXd H(3, 3);
H(0, 0) = K(0, 0);
H(0, 1) = K(0, 1);
H(0, 2) = 0;
H(1, 0) = K(1, 0);
H(1, 1) = K(1, 1);
H(1, 2) = 0;
H(2, 0) = V(0);
H(2, 1) = V(1);
H(2, 2) = 1;
h = H;
std::cout << "H: " << std::endl << H << std::endl << std::endl;
}
