From ad3a473cee6a94d05d712558dd25986c498d7b80 Mon Sep 17 00:00:00 2001 From: kerelMblsh Date: Thu, 4 Jun 2026 02:48:39 +0300 Subject: [PATCH 1/2] task04 done --- src/phg/core/calibration.cpp | 9 +++ tests/test_ceres_solver.cpp | 47 ++++++++++----- tests/test_sfm_ba.cpp | 113 ++++++++++++++++++++++++++++++++--- 3 files changed, 146 insertions(+), 23 deletions(-) diff --git a/src/phg/core/calibration.cpp b/src/phg/core/calibration.cpp index 887476b3..81aee43d 100644 --- a/src/phg/core/calibration.cpp +++ b/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); } diff --git a/tests/test_ceres_solver.cpp b/tests/test_ceres_solver.cpp index 898e2cdb..b8027a5a 100644 --- a/tests/test_ceres_solver.cpp +++ b/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 (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 uniform_x(min_x, max_x); // генерирует случайное значение x в рамках выбранного куска плоскости std::uniform_real_distribution uniform_y(min_y, max_y); // генерирует случайное значение y в рамках выбранного куска плоскости (для порождения выбросов) std::normal_distribution 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 &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; diff --git a/tests/test_sfm_ba.cpp b/tests/test_sfm_ba.cpp index 0490c45e..f8b08563 100644 --- a/tests/test_sfm_ba.cpp +++ b/tests/test_sfm_ba.cpp @@ -22,10 +22,10 @@ #include // 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 25 // сколько фотографий обрабатывать (можно выставить меньше чтобы ускорить экспериментирование, или в случае если весь датасет не выравнивается) #define INTRINSICS_CALIBRATION_MIN_IMGS 5 // начиная со скольки камер начинать оптимизировать внутренние параметры камеры (фокальную длину и т.п.) - из соображений что "пока камер мало - наблюдений может быть недостаточно чтобы не сойтись к ложной внутренней модели камеры" #define ENABLE_INSTRINSICS_K1_K2 1 // TODO учитывать ли радиальную дисторсию - коэффициенты k1, k2 попробуйте с ним и и без saharov32, заметна ли разница? @@ -40,17 +40,17 @@ // Datasets: // достаточно чтобы у вас работало на этом датасете, тестирование на Travis CI тоже ведется на нем -#define DATASET_DIR "saharov32" -#define DATASET_DOWNSCALE 1 // картинки уже уменьшены в 4 раза (оригинальные вы можете скачать по ссылке из saharov32/LINK.txt) -#define DATASET_F (1585.5 / DATASET_DOWNSCALE) +//#define DATASET_DIR "saharov32" +//#define DATASET_DOWNSCALE 1 // картинки уже уменьшены в 4 раза (оригинальные вы можете скачать по ссылке из saharov32/LINK.txt) +//#define DATASET_F (1585.5 / DATASET_DOWNSCALE) // но если любопытно - для экспериментов предлагаются еще дополнительные датасеты // скачайте их фотографии в папку 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 - не вышло, я не разобрался в чем с ним проблема, может быть слишком мало точек, может критерии фильтрации выкидышей для него слишком строги @@ -58,6 +58,7 @@ //#define DATASET_DOWNSCALE 1 //#define DATASET_F (1520.4 / DATASET_DOWNSCALE) // see temple47/README.txt about K-matrix (i.e. focal length = K11 from templeR_par.txt) + // Специальный датасет прямо с Марса! /* #define DATASET_DIR "perseverance25" @@ -168,6 +169,7 @@ TEST (SFM, ReconstructNViews) { phg::Calibration calib(imgs[0].cols, imgs[0].rows); calib.f_ = DATASET_F; + // сверяем что все картинки одинакового размера (мы ведь предполагаем что их снимала одна и та же камера с одними и те же интринсиками) for (const auto &img : imgs) { rassert(img.cols == imgs[0].cols && img.rows == imgs[0].rows, 34125412512512); @@ -265,6 +267,7 @@ TEST (SFM, ReconstructNViews) { continue; } + vector3d X3d{X(0) / X(3), X(1) / X(3), X(2) / X(3)}; tie_points.push_back(X3d); @@ -339,6 +342,7 @@ TEST (SFM, ReconstructNViews) { tie_points.push_back({X(0) / X(3), X(1) / X(3), X(2) / X(3)}); + Track track; track.img_kpt_pairs.push_back({i_camera, match.queryIdx}); track.img_kpt_pairs.push_back({i_camera_prev, match.trainIdx}); @@ -386,24 +390,61 @@ class ReprojectionError { // translation[3] - сдвиг в локальную систему координат камеры + T translation[3] = { + camera_extrinsics[0], + camera_extrinsics[1], + camera_extrinsics[2] + }; + // rotation[3] - angle-axis rotation, поворачиваем точку point->p (чтобы перейти в локальную систему координат камеры) // подробнее см. https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation // (P.S. у камеры всмысле вращения три степени свободы) + T rotation[3] = { + camera_extrinsics[3], + camera_extrinsics[4], + camera_extrinsics[5] + }; + + T point_local[3]; + T point_in_camera[3] = { + point_global[0] - translation[0], + point_global[1] - translation[1], + point_global[2] - translation[2] + }; + ceres::AngleAxisRotatePoint(rotation, point_in_camera, point_local); + // Проецируем точку на фокальную плоскость матрицы (т.е. плоскость Z=фокальная длина) + T x = point_local[0] / point_local[2]; + T y = point_local[1] / point_local[2]; + #if ENABLE_INSTRINSICS_K1_K2 // k1, k2 - коэффициенты радиального искажения (radial distortion) + T r_2 = x * x + y * y; + T dist = T(1.f) + camera_intrinsics[0] * r_2 + camera_intrinsics[1] * r_2 * r_2; + 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 (они должны совпадать) } @@ -436,7 +477,11 @@ void runBA(std::vector &tie_points, // внутренние калибровочные параметры камеры: [5] = {k1, k2, f, cx, cy} // TODO: преобразуйте calib в блок параметров камеры (ее внутренних характеристик) для оптимизации в BA - double camera_intrinsics[5]; + 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); @@ -479,6 +524,7 @@ void runBA(std::vector &tie_points, std::vector cameras_inliers(ncameras, 0); std::vector cameras_nprojections(ncameras, 0); + std::vector reprojection_residuals; std::vector reprojection_residuals_for_deletion; @@ -548,6 +594,7 @@ void runBA(std::vector &tie_points, } } + { // Полностью фиксируем положение первой камеры (чтобы не уползло облако точек) size_t camera_id = 0; @@ -577,6 +624,11 @@ void runBA(std::vector &tie_points, 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()); @@ -619,7 +671,12 @@ void runBA(std::vector &tie_points, size_t n_old_outliers = 0; size_t n_new_outliers = 0; + double min_angle = std::cos(CV_PI * 2.5f / 180.f); + + std::vector rays; + size_t next_loss_k = 0; + for (size_t i = 0; i < tie_points.size(); ++i) { Track &track = tracks[i]; bool should_be_disabled = false; @@ -638,6 +695,7 @@ void runBA(std::vector &tie_points, matrix3d R; vector3d camera_origin; phg::decomposeUndistortedPMatrix(R, camera_origin, cameras[camera_id]); + if (ENABLE_OUTLIERS_FILTRATION_NEGATIVE_Z && ENABLE_BA) { vector3d track_in_camera = R * (track_point - camera_origin); double z = track_in_camera[2]; @@ -651,6 +709,42 @@ void runBA(std::vector &tie_points, if (ENABLE_OUTLIERS_FILTRATION_COLINEAR && ENABLE_BA) { // TODO выполните проверку случая когда два луча почти параллельны, чтобы не было странных точек улетающих на бесконечность (например чтобы угол был хотя бы 2.5 градуса) // should_be_disabled = true; + + vector3d ray1 = track_point - camera_origin; + double norm1 = cv::norm(ray1); + + if (norm1 < 1e-10) should_be_disabled = true; + else { + ray1 /= norm1; + + for (size_t cj = 0; cj < track.img_kpt_pairs.size(); ++cj) { + int other_camera_id = track.img_kpt_pairs[cj].first; + if (other_camera_id == camera_id) continue; + + matrix3d R2; + vector3d camera_origin2; + + phg::decomposeUndistortedPMatrix( + R2, + camera_origin2, + cameras[other_camera_id]); + + vector3d ray2 = track_point - camera_origin2; + double norm2 = cv::norm(ray2); + + if (norm2 < 1e-10) { + should_be_disabled = true; + break; + } + + ray2 /= norm2; + + if (ray1.dot(ray2) > min_angle) { + should_be_disabled = true; + break; + } + } + } } { @@ -693,6 +787,7 @@ void runBA(std::vector &tie_points, ASSERT_GT(ninls, 0.15 * nproj); } + for (auto ptr : reprojection_residuals_for_deletion) { delete ptr; // т.к. мы не отдали указатель в ceres-solver - мы ответственны за его lifetime - надо самим деаллоцировать } From 0f866c08295622f38bdf6c06edded6d873d9abc5 Mon Sep 17 00:00:00 2001 From: kerelMblsh Date: Thu, 4 Jun 2026 05:36:36 +0300 Subject: [PATCH 2/2] changed dataset --- tests/test_sfm_ba.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/tests/test_sfm_ba.cpp b/tests/test_sfm_ba.cpp index f8b08563..bc12cd38 100644 --- a/tests/test_sfm_ba.cpp +++ b/tests/test_sfm_ba.cpp @@ -40,17 +40,17 @@ // Datasets: // достаточно чтобы у вас работало на этом датасете, тестирование на Travis CI тоже ведется на нем -//#define DATASET_DIR "saharov32" -//#define DATASET_DOWNSCALE 1 // картинки уже уменьшены в 4 раза (оригинальные вы можете скачать по ссылке из saharov32/LINK.txt) -//#define DATASET_F (1585.5 / DATASET_DOWNSCALE) +#define DATASET_DIR "saharov32" +#define DATASET_DOWNSCALE 1 // картинки уже уменьшены в 4 раза (оригинальные вы можете скачать по ссылке из saharov32/LINK.txt) +#define DATASET_F (1585.5 / DATASET_DOWNSCALE) // но если любопытно - для экспериментов предлагаются еще дополнительные датасеты // скачайте их фотографии в папку 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 - не вышло, я не разобрался в чем с ним проблема, может быть слишком мало точек, может критерии фильтрации выкидышей для него слишком строги