Task04 Кирилл Доровских СПбГУ

src/phg/core/calibration.cpp

@@ -37,6 +37,11 @@ cv::Vec3d phg::Calibration::project(const cv::Vec3d &point) const
 
     // TODO 11: добавьте учет радиальных искажений (k1_, k2_) (после деления на Z, но до умножения на f)
 
+    double r_2 = x * x + y * y;
+    double dist = 1.f + k1_ * r_2 + k2_ * r_2 * r_2;
+
+    x *= dist;
+    y *= dist;
 
     x *= f_;
     y *= f_;
@@ -56,6 +61,10 @@ cv::Vec3d phg::Calibration::unproject(const cv::Vec2d &pixel) const
     y /= f_;
 
     // TODO 12: добавьте учет радиальных искажений, когда реализуете - подумайте: почему строго говоря это - не симметричная формула формуле из project? (но лишь приближение)
+    double r_2 = x * x + y * y;
+    double undist = -k2_ * r_2 * r_2 - k1_ * r_2 + 1.f;
+    x *= undist;
+    y *= undist;
 
     return cv::Vec3d(x, y, 1.0);
 }

tests/test_ceres_solver.cpp

@@ -67,10 +67,12 @@ 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
+    // Потому что мы минимизируем resudial, а у него градиент равен -1 [= (10 - x)']
 
     ASSERT_NEAR(cur_x, 10.0, 1e-6);
 }
 
+
 //______________________________________________________________________________________________________________________
 // Пусть есть два фиксированных 3D объекта - параболоид и прямая.
 // Хотим найти их точку пересечения. Да, это из пушки по воробьям, но как иллюстрация для тренировки - полезно :)
@@ -132,7 +134,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] = queryPoint[2] - (a*dx*dx + b*dy*dy + center[2]);
         return true;
     }
 protected:
@@ -144,6 +146,7 @@ class ResidualToParaboloid {
 TEST (CeresSolver, HelloWorld2) {
     // Две невязки: расстояние до 3D прямой и расстояние до параболоида, иначе говоря мы ищем точку их пересечения
 
+
     // Формулируем обе Cost Function
     const double line_point[3]  = {10.0, 5.0, 0.0};
     const double line_direction[3] = {0.0, 0.0, 1.0};
@@ -158,10 +161,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.0, 200.0};
     {
         // Проверим что невязка эталонного решения нулевая для обоих функций невязки
         const double* params[1];
@@ -224,8 +226,9 @@ TEST (CeresSolver, HelloWorld2) {
         ASSERT_NEAR(residual, 0.0, 1e-6);
     }
 
+
     for (int d = 0; d < 3; ++d) {
-//        EXPECT_NEAR(point[d], expected_point_solution[d], 1e-4);
+        EXPECT_NEAR(point[d], expected_point_solution[d], 1e-4);
         // TODO 3: раскомментируйте^, почему он находит не то что ожидалось?
         // либо мы набагали в коде, либо в аналитическом поиске правильного ответа на бумажке (проверьте вычисления на бумажке)
         // если бага в коде, то первые подозреваемые - две функции невязки (только там есть содержательный код)
@@ -234,6 +237,8 @@ TEST (CeresSolver, HelloWorld2) {
         // отладьте те функции невязки которые по-хорошему не должны соглашаться на такой ответ - поставьте просто точку остановки чуть выше, там где мы проверяли
         // что невязка найденного решения - нулевая, и найдите где вдруг ваше ожидание большой невязки для этого ответа сталкивается с суровой реальностью баги в коде
         // которая приводит к нулевой невязке
+
+        // Исправил формулу в ResidualToParaboloid
     }
 
     // TODO 4: если любопытно и хватит времени - можете попросить ceres-solver посчитать якобианы в некоторых точках подобно тому как это делалось в конце теста HelloWorld1
@@ -260,7 +265,12 @@ class PointObservationError {
         // Блок параметров - line=[a, b, c] - задает прямую вида ax+by+c=0
         // TODO 5 посчитайте единственную невязку - расстояние от нашей точки-замера до текущего состояния прямой (для извлечения корня, помня про T=Jet, нужно использовать ceres::sqrt):
         // обратите внимание что расстояние лучше оставить знаковым, т.к. тогда эта невязка будет хорошо дифференцироваться при расстоянии около нуля
-//        residual[0] = ;
+
+        T u = (T) line[0];
+        T v = (T) line[1];
+        T w = (T) line[2];
+
+        residual[0] = (u * samplePoint[0] + v * samplePoint[1] + w) / ceres::sqrt(u * u + v * v);
         return true;
     }
 protected:
@@ -299,6 +309,7 @@ void evaluateLineFitting(double sigma, double &fitted_inliers_fraction, double &
     min_y -= sigma * n_points;
     max_y += sigma * n_points;
 
+
     std::uniform_real_distribution<double> uniform_x(min_x, max_x); // генерирует случайное значение x в рамках выбранного куска плоскости
     std::uniform_real_distribution<double> uniform_y(min_y, max_y); // генерирует случайное значение y в рамках выбранного куска плоскости (для порождения выбросов)
     std::normal_distribution<double>       sigma_shift(0.0, sigma); // нормальное распределение с учетом выбранной sigma (будем генерировать случайное смещение точки от прямой)
@@ -348,7 +359,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 +380,40 @@ 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 scale = ideal_line[2] / line_params[2];
+    line_params[0] *= scale; 
+    line_params[1] *= scale; 
+    line_params[2] *= scale;
+
+
     double threshold = 1e-4 * 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-а)
+        threshold *= 40.0; // ослабляем порог если есть выбросы и мы к ним не устойчивы (не робастны за счет loss-функции (функции потерь) Huber-а)
     }
     for (int d = 0; d < 3; ++d) {
-//        ASSERT_NEAR(line_params[d], ideal_line[d], threshold);
+        ASSERT_NEAR(line_params[d], ideal_line[d], threshold);
         // TODO 7 расскоментируйте сверку найденной прямой и эталонной
         // почему они расходятся? как это можно решить? придумайте хотя бы два способа:
         // - пост-обработкой - как-то поправив параметры прямой перед сверкой (при этом не меняя ее положение в пространстве)
+        // - нормируем коэффициенты по параметру c
         // - формулировкой задачи - можно сформулировать для ceres-solver задчау так чтобы избавиться от неоднозначности убрав степень свободы, т.е. описав прямую как-то иначе, как?
+        // - переведем нашу прямую в другую систему координат - будем задавать ее углом отклонения от оси Х и смещением относительно оси Y.
         // TODO 7 поправьте тест так или иначе (хотя бы пост-процессингом)
     }
 
     // Оцениваем качество идеальной прямой
     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); // TODO 8 раскоментируйте, почему эта проверка падает? как поправить?
+    // исключили случаи outliers
+    ASSERT_LT(mse, 1.1 * sigma * sigma); // TODO 9 раскомментируйте, почему проверка падает? на каких тестах она падает, на каких проходит? попробуйте отладить рассчет mse_inliers_distance в evaluateLine
+    // исправил ошибку измерения расстояния в 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);
     }
 }
 
@@ -404,7 +423,7 @@ void evaluateLine(const std::vector<double_2> &points, const double* line,
     size_t inliers = 0;
     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);
+        double dist = std::abs(calcDistanceToLine2D(points[i][0], points[i][1], line));
         if (dist <= 3 * sigma) {
             ++inliers;
             mse_inliers_distance += dist * dist;