diff --git a/src/phg/core/calibration.cpp b/src/phg/core/calibration.cpp index 887476b3..d761fb70 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 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); } diff --git a/src/phg/sfm/fmatrix.cpp b/src/phg/sfm/fmatrix.cpp index dcfee2fb..f4e41cad 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/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; diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index a0374c5a..5ab60fc3 100644 --- a/src/phg/sfm/resection.cpp +++ b/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; diff --git a/tests/test_sfm_ba.cpp b/tests/test_sfm_ba.cpp index 0490c45e..d14f380e 100644 --- a/tests/test_sfm_ba.cpp +++ b/tests/test_sfm_ba.cpp @@ -1,3 +1,4 @@ +#include #include #include @@ -22,10 +23,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 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 &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 &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 &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; + }); } {