Task04 Syrma Timur ITMO

src/phg/core/calibration.cpp

@@ -35,8 +35,10 @@ cv::Vec3d phg::Calibration::project(const cv::Vec3d &point) const
     double x = point[0] / point[2];
     double y = point[1] / point[2];
 
-    // TODO 11: добавьте учет радиальных искажений (k1_, k2_) (после деления на Z, но до умножения на f)
-
+    double r2 = x * x + y * y;
+    double dist = 1.0 + k1_ * r2 + k2_ * r2 * r2;
+    x *= dist;
+    y *= dist;
 
     x *= f_;
     y *= f_;
@@ -55,7 +57,12 @@ cv::Vec3d phg::Calibration::unproject(const cv::Vec2d &pixel) const
     x /= f_;
     y /= f_;
 
-    // TODO 12: добавьте учет радиальных искажений, когда реализуете - подумайте: почему строго говоря это - не симметричная формула формуле из project? (но лишь приближение)
+    double r2 = x * x + y * y;
+    double dist = 1.0 + k1_ * r2 + k2_ * r2 * r2;
+    if (dist != 0) {
+        x /= dist;
+        y /= dist;
+    }
 
     return cv::Vec3d(x, y, 1.0);
 }

src/phg/sfm/fmatrix.cpp

@@ -146,7 +146,7 @@ namespace {
 
         // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters
         // будет отличаться от случая с гомографией
-        const int n_trials = 10000;
+        const int n_trials = 100000;
 
         const int n_samples = 8;
         uint64_t seed = 1;

src/phg/sfm/resection.cpp

@@ -92,7 +92,7 @@ namespace {
         // будет отличаться от случая с гомографией
         const int n_trials = 10000;
 
-        const double threshold_px = 3;
+        const double threshold_px = 8;
 
         const int n_samples = 6;
         uint64_t seed = 1;

tests/test_sfm_ba.cpp

@@ -1,3 +1,4 @@
+#include <algorithm>
 #include <gtest/gtest.h>
 
 #include <opencv2/core.hpp>
@@ -22,10 +23,10 @@
 #include <ceres/ceres.h>
 
 // TODO включите Bundle Adjustment (но из любопытства посмотрите как ведет себя реконструкция без BA например для saharov32 без BA)
-#define ENABLE_BA                             0
+#define ENABLE_BA                             1
 
 // TODO когда заработает при малом количестве фотографий - увеличьте это ограничение до 100 чтобы попробовать обработать все фотографии (если же успешно будут отрабаывать только N фотографий - отправьте PR выставив здесь это N)
-#define NIMGS_LIMIT                           10 // сколько фотографий обрабатывать (можно выставить меньше чтобы ускорить экспериментирование, или в случае если весь датасет не выравнивается)
+#define NIMGS_LIMIT                           100 // сколько фотографий обрабатывать (можно выставить меньше чтобы ускорить экспериментирование, или в случае если весь датасет не выравнивается)
 #define INTRINSICS_CALIBRATION_MIN_IMGS       5 // начиная со скольки камер начинать оптимизировать внутренние параметры камеры (фокальную длину и т.п.) - из соображений что "пока камер мало - наблюдений может быть недостаточно чтобы не сойтись к ложной внутренней модели камеры"
 
 #define ENABLE_INSTRINSICS_K1_K2              1 // TODO учитывать ли радиальную дисторсию - коэффициенты k1, k2 попробуйте с ним и и без saharov32, заметна ли разница?
@@ -382,30 +383,42 @@ class ReprojectionError {
                     const T* camera_intrinsics, // внутренние калибровочные параметры камеры: [5] = {k1, k2, f, cx, cy} (одни и те же для всех кадров, т.к. снято на одну и ту же камеру)
                     const T* point_global,      // 3D точка: [3]  = {x, y, z}
                     T* residuals) const {       // невязка:  [2]  = {dx, dy}
-        // TODO реализуйте функцию проекции, все нужно делать в типе T чтобы ceres-solver мог под него подставить как Jet (очень рекомендую посмотреть Jet.h - как класная статья из википедии!), так и double
+        T point[3];
 
         // translation[3] - сдвиг в локальную систему координат камеры
+        const T* translation = &camera_extrinsics[0];
+        for (size_t i = 0; i < 3; ++i) {
+            point[i] = point_global[i] - translation[i];
+        }
 
         // rotation[3] - angle-axis rotation, поворачиваем точку point->p (чтобы перейти в локальную систему координат камеры)
-        // подробнее см. https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation
-        // (P.S. у камеры всмысле вращения три степени свободы)
+        const T* rotation = &camera_extrinsics[3];
+        T pointRotated[3];
+        ceres::AngleAxisRotatePoint(rotation, point, pointRotated);
 
         // Проецируем точку на фокальную плоскость матрицы (т.е. плоскость Z=фокальная длина)
+        T x = pointRotated[0] / pointRotated[2];
+        T y = pointRotated[1] / pointRotated[2];
 
 #if ENABLE_INSTRINSICS_K1_K2
         // k1, k2 - коэффициенты радиального искажения (radial distortion)
+        T r2 = x * x + y * y;
+        T dist = 1.0 + camera_intrinsics[0] * r2 + camera_intrinsics[1] * r2 * r2;
+        x *= dist;
+        y *= dist;
 #endif
 
         // Домножаем на f, тем самым переводя в пиксели
+        x *= camera_intrinsics[2];
+        y *= camera_intrinsics[2];
 
-        // Из координат когда точка (0, 0) - центр оптической оси
-        // Переходим в координаты когда точка (0, 0) - левый верхний угол картинки
-        // cx, cy - координаты центра оптической оси (обычно это центр картинки, но часто он чуть смещен)
+        x += camera_intrinsics[3];
+        y += camera_intrinsics[4];
 
-        // Теперь по спроецированным координатам не забудьте посчитать невязку репроекции
+        residuals[0] = x - observed_x;
+        residuals[1] = y - observed_y;
 
         return true;
-        // TODO сверьте эту функцию с вашей реализацией проекции в src/phg/core/calibration.cpp (они должны совпадать)
     }
 protected:
     double observed_x;
@@ -435,8 +448,12 @@ void runBA(std::vector<vector3d> &tie_points,
     ASSERT_NEAR(calib.cy_, 0.0, 0.3 * calib.height());
 
     // внутренние калибровочные параметры камеры: [5] = {k1, k2, f, cx, cy}
-    // TODO: преобразуйте calib в блок параметров камеры (ее внутренних характеристик) для оптимизации в BA
     double camera_intrinsics[5];
+    camera_intrinsics[0] = calib.k1_;
+    camera_intrinsics[1] = calib.k2_;
+    camera_intrinsics[2] = calib.f_;
+    camera_intrinsics[3] = calib.cx_ + 0.5 * calib.width();
+    camera_intrinsics[4] = calib.cy_ + 0.5 * calib.height();
     std::cout << "Before BA ";
     printCamera(camera_intrinsics);
 
@@ -575,8 +592,11 @@ void runBA(std::vector<vector3d> &tie_points,
 
     std::cout << "After BA ";
     printCamera(camera_intrinsics);
-    // TODO преобразуйте параметры камеры в обратную сторону, чтобы последующая резекция учла актуальное представление о пространстве:
-    // calib.* = camera_intrinsics[*];
+    calib.k1_ = camera_intrinsics[0];
+    calib.k2_ = camera_intrinsics[1];
+    calib.f_  = camera_intrinsics[2];
+    calib.cx_ = camera_intrinsics[3] - 0.5 * calib.width();
+    calib.cy_ = camera_intrinsics[4] - 0.5 * calib.height();
 
     ASSERT_NEAR(calib.f_ , DATASET_F, 0.2 * DATASET_F);
     ASSERT_NEAR(calib.cx_, 0.0, 0.3 * calib.width());
@@ -649,8 +669,17 @@ void runBA(std::vector<vector3d> &tie_points,
             }
 
             if (ENABLE_OUTLIERS_FILTRATION_COLINEAR && ENABLE_BA) {
-                // TODO выполните проверку случая когда два луча почти параллельны, чтобы не было странных точек улетающих на бесконечность (например чтобы угол был хотя бы 2.5 градуса)
-                // should_be_disabled = true;
+                const double min_ray_angle_degrees = 2.5;
+                const double min_ray_angle_cos = std::cos(min_ray_angle_degrees * CV_PI / 180.0);
+                vector3d track_dir = cv::normalize(track_point - camera_origin);
+                should_be_disabled = std::all_of(track.img_kpt_pairs.begin(), track.img_kpt_pairs.begin() + ci, [&](const auto& pair){
+                    int cur_camera_id = pair.first;
+                    matrix3d cur_R;
+                    vector3d cur_camera_origin;
+                    phg::decomposeUndistortedPMatrix(cur_R, cur_camera_origin, cameras[cur_camera_id]);
+                    vector3d cur_track_dir = cv::normalize(track_point - cur_camera_origin);
+                    return std::abs(track_dir.dot(cur_track_dir)) > min_ray_angle_cos;
+                });
             }
 
             {