diff --git a/.github/scripts/linux/install_ceres_solver.sh b/.github/scripts/linux/install_ceres_solver.sh old mode 100644 new mode 100755 index b6594501..b8f7549e --- a/.github/scripts/linux/install_ceres_solver.sh +++ b/.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 diff --git a/.gitignore b/.gitignore index 81ef2b99..f6117d26 100755 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,6 @@ .idea build cmake-build* +data/debug/ data/src/test_sfm/saharov/*.JPG +.vscode/ diff --git a/src/phg/core/calibration.cpp b/src/phg/core/calibration.cpp index 887476b3..0dc1deef 100644 --- a/src/phg/core/calibration.cpp +++ b/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); } diff --git a/tests/test_ceres_solver.cpp b/tests/test_ceres_solver.cpp index 898e2cdb..ea005760 100644 --- a/tests/test_ceres_solver.cpp +++ b/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 (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 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 &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; } diff --git a/tests/test_sfm_ba.cpp b/tests/test_sfm_ba.cpp index 0490c45e..71bb719b 100644 --- a/tests/test_sfm_ba.cpp +++ b/tests/test_sfm_ba.cpp @@ -21,11 +21,10 @@ #include #include -// TODO включите Bundle Adjustment (но из любопытства посмотрите как ведет себя реконструкция без BA например для saharov32 без BA) -#define ENABLE_BA 0 +#define ENABLE_BA 1 -// TODO когда заработает при малом количестве фотографий - увеличьте это ограничение до 100 чтобы попробовать обработать все фотографии (если же успешно будут отрабаывать только N фотографий - отправьте PR выставив здесь это N) -#define NIMGS_LIMIT 10 // сколько фотографий обрабатывать (можно выставить меньше чтобы ускорить экспериментирование, или в случае если весь датасет не выравнивается) +// когда заработает при малом количестве фотографий - увеличьте это ограничение до 100 чтобы попробовать обработать все фотографии (если же успешно будут отрабаывать только N фотографий - отправьте PR выставив здесь это N) +#define NIMGS_LIMIT 100 // сколько фотографий обрабатывать (можно выставить меньше чтобы ускорить экспериментирование, или в случае если весь датасет не выравнивается) #define INTRINSICS_CALIBRATION_MIN_IMGS 5 // начиная со скольки камер начинать оптимизировать внутренние параметры камеры (фокальную длину и т.п.) - из соображений что "пока камер мало - наблюдений может быть недостаточно чтобы не сойтись к ложной внутренней модели камеры" #define ENABLE_INSTRINSICS_K1_K2 1 // TODO учитывать ли радиальную дисторсию - коэффициенты k1, k2 попробуйте с ним и и без saharov32, заметна ли разница? @@ -48,9 +47,9 @@ // скачайте их фотографии в папку data/src/datasets/DATASETNAME/ по ссылке из файла LINK.txt в папке датасета: // saharov32 и herzjesu25 - приятные датасеты, вероятно их оба получится выравнять целиком -//#define DATASET_DIR "herzjesu25" -//#define DATASET_DOWNSCALE 2 // для ускорения SIFT -//#define DATASET_F (2761.5 / DATASET_DOWNSCALE) // see herzjesu25/K.txt +// #define DATASET_DIR "herzjesu25" +// #define DATASET_DOWNSCALE 2 // для ускорения SIFT +// #define DATASET_F (2761.5 / DATASET_DOWNSCALE) // see herzjesu25/K.txt // TODO почему фокальная длина меняется от того что мы уменьшаем картинку? почему именно в такой пропорции? может надо домножать? или делить на downscale^2 ? // но temple47 - не вышло, я не разобрался в чем с ним проблема, может быть слишком мало точек, может критерии фильтрации выкидышей для него слишком строги @@ -374,7 +373,7 @@ TEST (SFM, ReconstructNViews) { class ReprojectionError { public: - ReprojectionError(double x, double y) : observed_x(x), observed_y(y) + ReprojectionError(double x, double y) : observed{x, y} {} template @@ -382,34 +381,51 @@ 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 - // translation[3] - сдвиг в локальную систему координат камеры + T pts[3], pts_swap[3]; + for (int d = 0; d < 3; ++d) { + pts[d] = point_global[d] - camera_extrinsics[d]; + } // rotation[3] - angle-axis rotation, поворачиваем точку point->p (чтобы перейти в локальную систему координат камеры) // подробнее см. https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation // (P.S. у камеры всмысле вращения три степени свободы) + ceres::AngleAxisRotatePoint(camera_extrinsics + 3, pts, pts_swap); + std::swap(pts, pts_swap); // Проецируем точку на фокальную плоскость матрицы (т.е. плоскость Z=фокальная длина) - + pts[0] /= pts[2]; + pts[1] /= pts[2]; #if ENABLE_INSTRINSICS_K1_K2 // k1, k2 - коэффициенты радиального искажения (radial distortion) -#endif + T k1 = camera_intrinsics[0]; + T k2 = camera_intrinsics[1]; + T r_2 = pts[0] * pts[0] + pts[1] * pts[1]; + T rad = 1.0 + k1 * r_2 + k2 * r_2 * r_2; + pts[0] *= rad; + pts[1] *= rad; +#endif // Домножаем на f, тем самым переводя в пиксели + T f = camera_intrinsics[2]; + pts[0] *= f; + pts[1] *= f; // Из координат когда точка (0, 0) - центр оптической оси // Переходим в координаты когда точка (0, 0) - левый верхний угол картинки // cx, cy - координаты центра оптической оси (обычно это центр картинки, но часто он чуть смещен) + const T* c = camera_intrinsics + 3; + pts[0] += c[0]; + pts[1] += c[1]; // Теперь по спроецированным координатам не забудьте посчитать невязку репроекции + residuals[0] = pts[0] - observed[0]; + residuals[1] = pts[1] - observed[1]; return true; - // TODO сверьте эту функцию с вашей реализацией проекции в src/phg/core/calibration.cpp (они должны совпадать) } protected: - double observed_x; - double observed_y; + double observed[2]; }; void printCamera(double* camera_intrinsics) @@ -435,8 +451,14 @@ void runBA(std::vector &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]; + // преобразуйте calib в блок параметров камеры (ее внутренних характеристик) для оптимизации в BA + double camera_intrinsics[5] = { + calib.k1_, + calib.k2_, + calib.f_, + calib.cx_ + calib.width_ * 0.5, + calib.cy_ + calib.height_ * 0.5 + }; std::cout << "Before BA "; printCamera(camera_intrinsics); @@ -575,8 +597,12 @@ void runBA(std::vector &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] - calib.width_ * 0.5; + calib.cy_ = camera_intrinsics[4] - calib.height_ * 0.5; ASSERT_NEAR(calib.f_ , DATASET_F, 0.2 * DATASET_F); ASSERT_NEAR(calib.cx_, 0.0, 0.3 * calib.width()); @@ -649,8 +675,21 @@ void runBA(std::vector &tie_points, } if (ENABLE_OUTLIERS_FILTRATION_COLINEAR && ENABLE_BA) { - // TODO выполните проверку случая когда два луча почти параллельны, чтобы не было странных точек улетающих на бесконечность (например чтобы угол был хотя бы 2.5 градуса) - // should_be_disabled = true; + // выполните проверку случая когда два луча почти параллельны, чтобы не было странных точек улетающих на бесконечность (например чтобы угол был хотя бы 2.5 градуса) + for (int pi = 0; pi < ci; ++pi) { + int prev_id = track.img_kpt_pairs[pi].first; + + matrix3d R_prev; + vector3d prev_origin; + phg::decomposeUndistortedPMatrix(R_prev, prev_origin, cameras[prev_id]); + + vector3d v1 = (track_point - camera_origin); + vector3d v2 = (track_point - prev_origin); + if (v1.dot(v2) / std::sqrt(v1.dot(v1) * v2.dot(v2)) > 0.9999) { + should_be_disabled = true; + break; + } + } } {