Task03 Syrma Timur ITMO

libs/3rdparty/libgtest/googletest/src/gtest-death-test.cc

@@ -46,6 +46,7 @@
 # include <errno.h>
 # include <fcntl.h>
 # include <limits.h>
+# include <cstdint>
 
 # if GTEST_OS_LINUX
 #  include <signal.h>

src/phg/sfm/ematrix.cpp

@@ -18,26 +18,15 @@ namespace {
         copy(Ecv, E);
 
         Eigen::JacobiSVD<Eigen::MatrixXd> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
-        throw std::runtime_error("not implemented yet");
-// TODO
+        Eigen::VectorXd s = svd.singularValues();
+        double avg = (s[0] + s[1]) / 2.0;
+        Eigen::VectorXd s_new(3);
+        s_new << avg, avg, 0;
+        E = svd.matrixU() * s_new.asDiagonal() * svd.matrixV().transpose();
 
         copy(E, Ecv);
     }
 
-}
-
-cv::Matx33d phg::fmatrix2ematrix(const cv::Matx33d &F, const phg::Calibration &calib0, const phg::Calibration &calib1)
-{
-    throw std::runtime_error("not implemented yet");
-//    matrix3d E = TODO;
-//
-//    ensureSpectralProperty(E);
-//
-//    return E;
-}
-
-namespace {
-
     matrix34d composeP(const Eigen::MatrixXd &R, const Eigen::VectorXd &t)
     {
         matrix34d result;
@@ -61,107 +50,108 @@ namespace {
 
     bool depthTest(const vector2d &m0, const vector2d &m1, const phg::Calibration &calib0, const phg::Calibration &calib1, const matrix34d &P0, const matrix34d &P1)
     {
-        throw std::runtime_error("not implemented yet");
-//        // скомпенсировать калибровки камер
-//        vector3d p0 = TODO;
-//        vector3d p1 = TODO;
-//
-//        vector3d ps[2] = {p0, p1};
-//        matrix34d Ps[2] = {P0, P1};
-//
-//        vector4d X = phg::triangulatePoint(Ps, ps, 2);
-//        if (X[3] != 0) {
-//            X /= X[3];
-//        }
-//
-//        // точка должна иметь положительную глубину для обеих камер
-//        return TODO && TODO;
+        vector3d p0 = calib0.unproject(m0);
+        vector3d p1 = calib1.unproject(m1);
+
+        vector3d ps[2] = {p0, p1};
+        matrix34d Ps[2] = {P0, P1};
+
+        vector4d X = phg::triangulatePoint(Ps, ps, 2);
+        if (X[3] != 0) {
+            X /= X[3];
+        }
+
+        vector3d x0_proj = P0 * X;
+        vector3d x1_proj = P1 * X;
+
+        return x0_proj[2] > 0 && x1_proj[2] > 0;
     }
+
+}
+
+cv::Matx33d phg::fmatrix2ematrix(const cv::Matx33d &F, const phg::Calibration &calib0, const phg::Calibration &calib1)
+{
+    matrix3d E = calib1.K().t() * F * calib0.K();
+    ensureSpectralProperty(E);
+    return E;
 }
 
-// Матрицы камер для фундаментальной матрицы определены с точностью до проективного преобразования
-// То есть, можно исказить трехмерный мир (применив 4-мерную однородную матрицу), и одновременно поменять матрицы P0, P1 так, что проекции в пикселях не изменятся
-// Если мы знаем калибровки камер (матрицы K0, K1 в структуре матриц P0, P1), то можем наложить дополнительные ограничения, в частности, известно, что
-// существенная матрица (Essential matrix = K1t * F * K0) имеет ровно два совпадающих ненулевых сингулярных значения, тогда как для фундаментальной матрицы они могут различаться
-// Это дополнительное ограничение позволяет разложить существенную матрицу с точностью до 4 решений, вместо произвольного проективного преобразования (см. Hartley & Zisserman p.258)
-// Обычно мы можем использовать одну общую калибровку, более менее верную для большого количества реальных камер и с ее помощью выполнить
-// первичное разложение существенной матрицы (а из него, взаимное расположение камер) для последующего уточнения методом нелинейной оптимизации
 void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d &Ecv, const std::vector<cv::Vec2d> &m0, const std::vector<cv::Vec2d> &m1, const Calibration &calib0, const Calibration &calib1)
 {
-    throw std::runtime_error("not implemented yet");
-//    if (m0.size() != m1.size()) {
-//        throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()");
-//    }
-//
-//    using mat = Eigen::MatrixXd;
-//    using vec = Eigen::VectorXd;
-//
-//    mat E;
-//    copy(Ecv, E);
-//
-//    // (см. Hartley & Zisserman p.258)
-//
-//    Eigen::JacobiSVD<mat> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
-//
-//    mat U = svd.matrixU();
-//    vec s = svd.singularValues();
-//    mat V = svd.matrixV();
-//
-//    // U, V must be rotation matrices, not just orthogonal
-//    if (U.determinant() < 0) U = -U;
-//    if (V.determinant() < 0) V = -V;
-//
-//    std::cout << "U:\n" << U << std::endl;
-//    std::cout << "s:\n" << s << std::endl;
-//    std::cout << "V:\n" << V << std::endl;
-//
-//
-//    mat R0 = TODO;
-//    mat R1 = TODO;
-//
-//    std::cout << "R0:\n" << R0 << std::endl;
-//    std::cout << "R1:\n" << R1 << std::endl;
-//
-//    vec t0 = TODO;
-//    vec t1 = TODO;
-//
-//    std::cout << "t0:\n" << t0 << std::endl;
-//
-//    P0 = matrix34d::eye();
-//
-//    // 4 possible solutions
-//    matrix34d P10 = composeP(R0, t0);
-//    matrix34d P11 = composeP(R0, t1);
-//    matrix34d P12 = composeP(R1, t0);
-//    matrix34d P13 = composeP(R1, t1);
-//    matrix34d P1s[4] = {P10, P11, P12, P13};
-//
-//    // need to select best of 4 solutions: 3d points should be in front of cameras (positive depths)
-//    int best_count = 0;
-//    int best_idx = -1;
-//    for (int i = 0; i < 4; ++i) {
-//        int count = 0;
-//        for (int j = 0; j < (int) m0.size(); ++j) {
-//            if (depthTest(m0[j], m1[j], calib0, calib1, P0, P1s[i])) {
-//                ++count;
-//            }
-//        }
-//        std::cout << "decomposeEMatrix: count: " << count << std::endl;
-//        if (count > best_count) {
-//            best_count = count;
-//            best_idx = i;
-//        }
-//    }
-//
-//    if (best_count == 0) {
-//        throw std::runtime_error("decomposeEMatrix : can't decompose ematrix");
-//    }
-//
-//    P1 = P1s[best_idx];
-//
-//    std::cout << "best idx: " << best_idx << std::endl;
-//    std::cout << "P0: \n" << P0 << std::endl;
-//    std::cout << "P1: \n" << P1 << std::endl;
+    if (m0.size() != m1.size()) {
+        throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()");
+    }
+
+    using mat = Eigen::MatrixXd;
+    using vec = Eigen::VectorXd;
+
+    mat E;
+    copy(Ecv, E);
+
+    Eigen::JacobiSVD<mat> svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV);
+
+    mat U = svd.matrixU();
+    vec s = svd.singularValues();
+    mat V = svd.matrixV();
+
+    if (U.determinant() < 0)
+        U = -U;
+    if (V.determinant() < 0)
+        V = -V;
+
+    std::cout << "U:\n" << U << std::endl;
+    std::cout << "s:\n" << s << std::endl;
+    std::cout << "V:\n" << V << std::endl;
+
+    mat W(3, 3);
+    W << 0, -1, 0,
+         1,  0, 0,
+         0,  0, 1;
+
+    mat R0 = U * W * V.transpose();
+    mat R1 = U * W.transpose() * V.transpose();
+
+    std::cout << "R0:\n" << R0 << std::endl;
+    std::cout << "R1:\n" << R1 << std::endl;
+
+    vec t0 = U.col(2);
+    vec t1 = -U.col(2);
+
+    std::cout << "t0:\n" << t0 << std::endl;
+
+    P0 = matrix34d::eye();
+
+    matrix34d P10 = composeP(R0, t0);
+    matrix34d P11 = composeP(R0, t1);
+    matrix34d P12 = composeP(R1, t0);
+    matrix34d P13 = composeP(R1, t1);
+    matrix34d P1s[4] = {P10, P11, P12, P13};
+
+    int best_count = 0;
+    int best_idx = -1;
+    for (int i = 0; i < 4; ++i) {
+        int count = 0;
+        for (int j = 0; j < (int)m0.size(); ++j) {
+            if (depthTest(m0[j], m1[j], calib0, calib1, P0, P1s[i])) {
+                ++count;
+            }
+        }
+        std::cout << "decomposeEMatrix: count: " << count << std::endl;
+        if (count > best_count) {
+            best_count = count;
+            best_idx = i;
+        }
+    }
+
+    if (best_count == 0) {
+        throw std::runtime_error("decomposeEMatrix : can't decompose ematrix");
+    }
+
+    P1 = P1s[best_idx];
+
+    std::cout << "best idx: " << best_idx << std::endl;
+    std::cout << "P0: \n" << P0 << std::endl;
+    std::cout << "P1: \n" << P1 << std::endl;
 }
 
 void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P)
@@ -174,7 +164,7 @@ void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Ma
     O(2) = O_mat(2);
 
     if (cv::determinant(R) < 0) {
-        R *= -1;   
+        R *= -1;
     }
 }