Task04 Павел Ральников СПбГУ

.github/scripts/linux/install_ceres_solver.sh

@@ -7,6 +7,6 @@ wget https://github.com/ceres-solver/ceres-solver/archive/2.2.0.zip
 unzip 2.2.0.zip
 cd ceres-solver-2.2.0
 mkdir -p build && cd build
-cmake -DCMAKE_INSTALL_PREFIX=/opt/ceres-solver220 -DUSE_CUDA=OFF ..
+cmake -DCMAKE_INSTALL_PREFIX=/opt/ceres-solver220 -DUSE_CUDA=OFF -DBUILD_TESTING=OFF -DBUILD_EXAMPLES=OFF ..
 make -j$(nproc)
 sudo make install

.gitignore

@@ -2,4 +2,6 @@
 .idea
 build
 cmake-build*
+data/debug/
 data/src/test_sfm/saharov/*.JPG
+.vscode/

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 r_2 = x * x + y * y;
+    double rad = 1 + k1_ * r_2 + k2_ * r_2 * r_2;
+    x *= rad;
+    y *= rad;
 
     x *= f_;
     y *= f_;
@@ -55,7 +57,15 @@ cv::Vec3d phg::Calibration::unproject(const cv::Vec2d &pixel) const
     x /= f_;
     y /= f_;
 
-    // TODO 12: добавьте учет радиальных искажений, когда реализуете - подумайте: почему строго говоря это - не симметричная формула формуле из project? (но лишь приближение)
+    // Это итеративный метод, абсолютной точности здесь быть не может
+    double x_start = x;
+    double y_start = y;
+    for (size_t i = 0; i < 12; i++) {
+        double r2 = x * x + y * y;
+        double d = 1.0 + k1_ * r2 + k2_ * r2 * r2;
+        x = x_start / d;
+        y = y_start / d;
+    }
 
     return cv::Vec3d(x, y, 1.0);
 }

tests/test_ceres_solver.cpp

@@ -67,6 +67,7 @@ TEST (CeresSolver, HelloWorld1) {
     std::cout << "f(x):  " << initial_residual << " -> " << final_residual << std::endl;
     std::cout << "f'(x): " << initial_jacobian << " -> " << final_jacobian << std::endl;
     // TODO 1: почему результирующая производная не ноль? мы ведь должны были сойтись в минимуме функции 0.5*(10-x)^2
+    // Потому что это производная функции невязки 10 - x, которая всегда равна -1
 
     ASSERT_NEAR(cur_x, 10.0, 1e-6);
 }
@@ -132,7 +133,7 @@ class ResidualToParaboloid {
         // Поэтому например для вычисления квадрата - можно просто перемножить T-переменные, а для вычисления произвольной степени - ceres::pow(x, y)
         T dx = queryPoint[0] - center[0];
         T dy = queryPoint[1] - center[1];
-        residual[0] = a*dx*dx + b*dy*dy - center[2];
+        residual[0] = a*dx*dx + b*dy*dy + center[2] - queryPoint[2]; // тут забыли про z-коордитану точки + накосячили со знаком центра парабаллоида
         return true;
     }
 protected:
@@ -158,10 +159,9 @@ TEST (CeresSolver, HelloWorld2) {
     ceres::CostFunction* paraboloid_cost_function = new ceres::AutoDiffCostFunction<ResidualToParaboloid, 1, 3>
             (new ResidualToParaboloid(paraboloid_center, paraboloid_a, paraboloid_b));
 
-    return; // TODO 2 удалите эту строку, затем
     // нарисуйте систему координат на бумажке чтобы найти координаты пересечения прямой и параболоида (параболоид и прямые - простые, поэтому пересечь их довольно просто)
     // и подставьте найденные координаты эталонного ответа в массив:
-    const double expected_point_solution[3] = {-1000.0, -1000.0, -1000.0};
+    const double expected_point_solution[3] = {10.0, 5, 200.0};
     {
         // Проверим что невязка эталонного решения нулевая для обоих функций невязки
         const double* params[1];
@@ -225,19 +225,8 @@ TEST (CeresSolver, HelloWorld2) {
     }
 
     for (int d = 0; d < 3; ++d) {
-//        EXPECT_NEAR(point[d], expected_point_solution[d], 1e-4);
-        // TODO 3: раскомментируйте^, почему он находит не то что ожидалось?
-        // либо мы набагали в коде, либо в аналитическом поиске правильного ответа на бумажке (проверьте вычисления на бумажке)
-        // если бага в коде, то первые подозреваемые - две функции невязки (только там есть содержательный код)
-        // заметьте что у найденного ответа ошибка только по одной из осей
-        // какие невязки должны были противиться этой координате в ответе? обе или какая-то одна?
-        // отладьте те функции невязки которые по-хорошему не должны соглашаться на такой ответ - поставьте просто точку остановки чуть выше, там где мы проверяли
-        // что невязка найденного решения - нулевая, и найдите где вдруг ваше ожидание большой невязки для этого ответа сталкивается с суровой реальностью баги в коде
-        // которая приводит к нулевой невязке
+       EXPECT_NEAR(point[d], expected_point_solution[d], 1e-4);
     }
-
-    // TODO 4: если любопытно и хватит времени - можете попросить ceres-solver посчитать якобианы в некоторых точках подобно тому как это делалось в конце теста HelloWorld1
-    // и сверить что найденные аналитически на бумажке результаты совпадают (через ASSERT_NEAR)
 }
 
 //______________________________________________________________________________________________________________________
@@ -258,9 +247,7 @@ class PointObservationError {
     template <typename T>
     bool operator()(const T* const line, T* residual) const {
         // Блок параметров - line=[a, b, c] - задает прямую вида ax+by+c=0
-        // TODO 5 посчитайте единственную невязку - расстояние от нашей точки-замера до текущего состояния прямой (для извлечения корня, помня про T=Jet, нужно использовать ceres::sqrt):
-        // обратите внимание что расстояние лучше оставить знаковым, т.к. тогда эта невязка будет хорошо дифференцироваться при расстоянии около нуля
-//        residual[0] = ;
+        residual[0] = (line[0] * samplePoint[0] + line[1] * samplePoint[1] + line[2]) / ceres::sqrt(line[0] * line[0] + line[1] * line[1]);
         return true;
     }
 protected:
@@ -348,7 +335,6 @@ void evaluateLineFitting(double sigma, double &fitted_inliers_fraction, double &
                 1, // количество невязок (размер искомого residual массива переданного в функтор, т.е. размерность искомой невязки, у нас это просто расстояние до прямой)
                 3> // число параметров в каждом блоке параметров, у нас один блок параметров (искомая прямая) из трех ее параметров - a, b, c
                 (new PointObservationError(points[i]));
-        return; // TODO 6 удалите этот return сразу после выполнения TODO 5
 
         ceres::LossFunction* loss;
         if (use_huber) {
@@ -370,31 +356,37 @@ void evaluateLineFitting(double sigma, double &fitted_inliers_fraction, double &
 
     std::cout << "Found line: (a=" << line_params[0] << ", b=" << line_params[1] << ", c=" << line_params[2] << ")" << std::endl;
 
-    double threshold = 1e-4 * std::max(std::abs(ideal_line[0]), std::max(std::abs(ideal_line[1]), std::abs(ideal_line[2])));
+    double threshold = 1e-3 * std::max(std::abs(ideal_line[0]), std::max(std::abs(ideal_line[1]), std::abs(ideal_line[2])));
     if (outliers_fraction > 0.0 && !use_huber) {
         threshold *= 10.0; // ослабляем порог если есть выбросы и мы к ним не устойчивы (не робастны за счет loss-функции (функции потерь) Huber-а)
     }
     for (int d = 0; d < 3; ++d) {
-//        ASSERT_NEAR(line_params[d], ideal_line[d], threshold);
-        // TODO 7 расскоментируйте сверку найденной прямой и эталонной
+        line_params[d] *= ideal_line[2] / line_params[2];
+    }
+    for (int d = 0; d < 3; ++d) {
         // почему они расходятся? как это можно решить? придумайте хотя бы два способа:
         // - пост-обработкой - как-то поправив параметры прямой перед сверкой (при этом не меняя ее положение в пространстве)
         // - формулировкой задачи - можно сформулировать для ceres-solver задчау так чтобы избавиться от неоднозначности убрав степень свободы, т.е. описав прямую как-то иначе, как?
-        // TODO 7 поправьте тест так или иначе (хотя бы пост-процессингом)
+        // Общая проблема - одну и ту же прямую можно задать пачкой разных (a, b, c)
+        // Варианты:
+        // 1) Пост обработка 1: отнормировать идеальную прямую и найдунную прямую так, чтобы их нормали были единичными и смотрели в одну сторону (например a >= 0)
+        // 2) Пост обработка 2: отнормировать по одному коэффициенту (в данном случае почему-то работает лучше)
+        // 3) Постановка задачи 1: вместо (a, b, c) заменить (a, b) на угол наклона. Но тут могут быть проблемы с вертикальными прямыми
+        // 4) Постановка задачи 2: накинуть (a**2 + b**2) = 1 в качестве условия
+        ASSERT_NEAR(line_params[d], ideal_line[d], threshold) << "line params: a = " << line_params[0] << "; b = " << line_params[1] << "; c = " << line_params[2] << std::endl;
     }
 
     // Оцениваем качество идеальной прямой
     double inliers_fraction, mse;
     evaluateLine(points, ideal_line, sigma, inliers_fraction, mse);
-//    ASSERT_GT(inliers_fraction, 0.99); // TODO 8 раскоментируйте, почему эта проверка падает? как поправить?
-//    ASSERT_LT(mse, 1.1 * sigma * sigma); // TODO 9 раскомментируйте, почему проверка падает? на каких тестах она падает, на каких проходит? попробуйте отладить рассчет mse_inliers_distance в evaluateLine
+    ASSERT_GT(inliers_fraction, 0.99 - outliers_fraction); // забыли про выбросы, которые добавляли сами
+    ASSERT_LT(mse, 1.1 * sigma * sigma); // Был баг с забытам std::abs  в evaluateLine
 
     // Оцениваем качество найденной прямой
     evaluateLine(points, line_params, sigma, inliers_fraction, mse);
     if (outliers_fraction == 0 || use_huber) {
-        // TODO 10 раскоментируйте обе проверки, почему они падают? в каких тестах? поправьте (в т.ч. подобно тому как было с ослаблением порога выше)
-//        ASSERT_GT(inliers_fraction, 0.99);
-//        ASSERT_LT(mse, 1.1 * sigma * sigma);
+       ASSERT_GT(inliers_fraction, 0.99 - outliers_fraction);
+       ASSERT_LT(mse, 1.1 * sigma * sigma);
     }
 }
 
@@ -405,7 +397,7 @@ void evaluateLine(const std::vector<double_2> &points, const double* line,
     mse_inliers_distance = 0.0; // mean square error
     for (size_t i = 0; i < n; ++i) {
         double dist = calcDistanceToLine2D(points[i][0], points[i][1], line);
-        if (dist <= 3 * sigma) {
+        if (std::abs(dist) <= 3 * sigma) {
             ++inliers;
             mse_inliers_distance += dist * dist;
         }