diff --git a/.github/workflows/cmake.yml b/.github/workflows/cmake.yml index df5ebd07..044b52fc 100755 --- a/.github/workflows/cmake.yml +++ b/.github/workflows/cmake.yml @@ -131,3 +131,7 @@ jobs: run: | $env:OPENCV_IO_ENABLE_OPENEXR = "1" .\build\${{ env.BUILD_TYPE }}\test_mesh_min_cut.exe + # - name: Run test_depth_maps_pm (Windows) + # if: runner.os == 'Windows' + # shell: pwsh + # run: .\build\${{ env.BUILD_TYPE }}\test_depth_maps_pm.exe diff --git a/.gitignore b/.gitignore index 81ef2b99..4865a837 100755 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,12 @@ **/*_cl.h .idea -build +.DS_store +build* cmake-build* data/src/test_sfm/saharov/*.JPG +opencv-4.11.0 +.github/scripts/macos/4.11.0.zip +2.2.0.zip +ceres-solver-2.2.0 +eigen-3.4.0 +eigen-3.4.0.tar.gz \ No newline at end of file diff --git a/Q&A2.md b/Q&A2.md new file mode 100644 index 00000000..73ed7f11 --- /dev/null +++ b/Q&A2.md @@ -0,0 +1,35 @@ +# Перечислите идеи и коротко обозначьте мысли которые у вас возникали по мере выполнения задания, в частности попробуйте ответить на вопросы: + +1) Зачем фильтровать матчи, если потом мы запускаем устойчивый к выбросам RANSAC и отфильтровываем шумные сопоставления? \ +\ +Число запусков в алгоритме RANSAC (см homography.cpp:182) зависит от w (переменная singlePointInlierProbability) -- вероятности того, что метч является инлаером. Чем больше эта вероятность, тем меньше нужно итераций RANSAC. Фильтрация метчей перед запускам RANSAC нужна чтобы увеличить эту вероятность. + +2) Cluster filtering довольно хорошо работает и без Ratio test. Однако, если оставить только Cluster filtering, некоторые тесты начнут падать. Почему так происходит? В каких случаях наоборот, не хватает Ratio test и необходима дополнительная фильтрация? +- Cluster filtering слабо себя показывает в случае если на картинке присутсвуют регулярные структуры, за которые цепляется дескриптор => образуются множественные очень близкие друг к другу кластера, которые могут найти метчи из другого кластера. В этом случае cluster filtering такие матчи посчитает верными, что может быть ошибочно. +- Ratio test наоборот не учитывает пространственные окрестности, из-за чего может сметчить цветочки (элементы) из двух разных клумб (окрестностей). +3) С какой проблемой можно столкнуться при приравнивании единице элемента H33 матрицы гомографии? Как ее решить? \ +\ +В случае если матрица полученной системы численно нестабильна (это может произойти если в одной из нормировок H_33~0, остальные значения конечны и не бесконечно малы => при нормировке на H_33 получим огромные значения оставшихся элементов), тогда +- будет проблематично обращать такую матрицу, чтобы получить преобразованные точки +- флотовые вычисления вносят дополнительную ошибку когда складываем числа разного порядка + +4) Какой подвох таится в попытке склеивать большие панорамы и ортофото методом, реализованным в данной домашке? (Для интуиции можно посмотреть на результат склейки, когда за корень взята какая-нибудь другая картинка) \ +\ +Заметно, что чем ближе пара склеенных фоток к корню, тем лучше склейка. Следовательно от выбора корня зависит общая невязка -- лучше выбирать корень таким образом, чтобы корень оказался в наиболее важной части композиции, тогда, согласно наблюдению, в зонах интереса склейка будет менее резкой. + +5) Как можно автоматически построить граф для построения панорамы, чтобы на вход метод принимал только список картинок? \ +(Мысли именно насчет однострочной панорамы) +- провести метчинг для каждой пары картинок +- для каждой пары картинок вычислить MSE заметченных дескрипторов +- для каждой картинки находим по две картинки с минимумом MSE -- это окрестность картинки +- далее находим две картинки с максимальными минимумами -- это краевые картинки +- начиная с любой из этих двух картинок используя минимальный из ее минимумов начинаем собирать граф-путь + +6) Если с вашей реализацией SIFT пройти тесты не получилось, напишите (если пробовали дебажить), где, как вам кажется, проблема и как вы пробовали ее решать. \ +\ +Изначально была проблема с тайм-лимитами, пофиксил поправив настройки FLANN. + +7) Если есть, фидбек по заданию: какая часть больше всего понравилась, где-то слишком сложно/просто (что именно), где-то слишком мало ссылок и тд. +\ +\ +Подбор гиперпарматеров в алгоритме FLANN оказался весьма неприятным -- приходилось искать компромисс между тайм-лимитами и скором, при этом у фейлов низкая воспроизводимость, что усложняло дебаг. \ No newline at end of file diff --git a/Q&A3.md b/Q&A3.md new file mode 100644 index 00000000..d625df48 --- /dev/null +++ b/Q&A3.md @@ -0,0 +1,10 @@ +# Перечислите идеи и коротко обозначьте мысли которые у вас возникали по мере выполнения задания, в частности попробуйте ответить на вопросы: + +1) Мы можем получить облако точек и взаимную ориентацию по двум камерам. Почему для выравнивания трёх камер мы использовали резекцию, а не посчитали E матрицу для второй пары камер и не разложили ее? \ + При работе с двумя камерами, мы получаем матрицу E с точностью до масштаба. При добавлении последующих камер масштабы могут не метэчиться. +2) Как реализовать выравнивание если мы все же хотим использовать Е матрицу? +- Фиттить первоначально полученную матрицу P_n таким образом, чтобы 3х-мерные точки, полученные из первых двух камер при триангуляции не перемещались, но описанный процесс сильно похож на резекцию. Наверное потому что без иных вводных и критериев правильности облака точек сложно придумать иную концепцию. +- С другой стороны можно попробовать повторить нахождение матрицы E для следующей пары (одна из камер уже была в наборе, ее берем за основную с единичной P-матрицей), а потом RANSAC-ом связывать два облака, фиттируя преобразование масштаба для P-матрицы новой камеры. Проблема появялется при росте числа камер: если связывать новую пару только с предыдущей -- невязка будет накапливаться, если связывать все облака -- большая стоимость одной итерации RANSAC. + +3) Если есть, фидбек по заданию: какая часть больше всего понравилась, где-то слишком сложно/просто (что именно), где-то слишком мало ссылок и тд. \ +Как и в прошлом задании -- подбор параметров RANSAC для прохождения тестов. Хотелось бы больше интуиции, ведь по сути так как для разных тестов необходимо разное количество итераций RANSAC (причем по порядкам), из формулы числа итераций следует, что вероятности инлаеров сильно отличаются, возможно стоит добавить оценку этих параметров отдельным подзаданием. diff --git a/Q&A4.md b/Q&A4.md new file mode 100644 index 00000000..f4c64d95 --- /dev/null +++ b/Q&A4.md @@ -0,0 +1,38 @@ +# Перечислите идеи и коротко обозначьте мысли которые у вас возникали по мере выполнения задания, в частности попробуйте ответить на вопросы: + +1) test_ceres_solver/FitLine: почему найденная прямая и эталонная - не совпадают? Как это исправить пост-обработкой? Как это исправить формулировкой задачи?\ +\ +Прямые геометрически совпадают (одна прямая задается семейством наборов коэффициентов {a, b, c}, определенных с точностью до масштаба), коэффициенты же могут отличаться масштабом. \ +На постобработке можно исправить, поделив коэффициенты на некий скейл (такой, чтобы c_0==c_ideal, тут можно любой из коэффициентов приравнять, но в решении c имеет наибольший модуль и вычисления получаются более стабильными?)\ +В формулировку задачи можно добавить ограничение на величину одного из коэффициентов или на длину вектора (a, b) + +2) BA: представьте что вы написали преобразование phg::Calibration -> блок параметров и обратное блок параметров -> phg::Calibration. Как проверить простым образом что эти преобразования сделаны корректно? Что должно быть в логе про процент inliers до/после BA если runBA() вызывать всегда два раза пордяд? Иначе говоря - что следует из того что в идеале runBA() должна быть (мне очень нравится это слово) - [идемпотентна](https://ru.wikipedia.org/wiki/%D0%98%D0%B4%D0%B5%D0%BC%D0%BF%D0%BE%D1%82%D0%B5%D0%BD%D1%82%D0%BD%D0%BE%D1%81%D1%82%D1%8C)?\ +\ +Написать темплейт-функцию, считающую, например, проекцию точки в фокальную плоскость и сделать ассерт на равенство проекций. \ +Идемпотентность означает, что BA(BA(X)) = BA(X) => результат (процент inliers после однократного/двухкратного BA) должен совпадать. + + +3) Какое максимальное число кадров у вас получилось хорошо выравнять для каждого из датасетов? (проверьте хотя бы saharov32 и herzjesu25) Не забудьте приложить скриншоты. \ +\ +saharov32 -- 32/32 ![Alt Text](saharov32.png) +herzjesu25 -- 22/25 ![Alt Text](herzjesu22.png) + +4) Если бы вычисления в double были абсолютно точны - можно ли было бы назвать вычисления в Calibration::project/unproject строго зеркальными? \ +\ +Нет, т.к. мы полностью теряем информацию о координате z исходной точки. Кстати, в задании 3 при условии обратимости матрицы K project/unproject были взаимно обратными. Мне кажется стоит в задании 3 функционал проецирования однородных координат также перенести в project. + +5) Почему фокальная длина меняется от того что мы уменьшаем картинку? Почему именно f/downscale? \ +\ +$x_p = x_r \dfrac{f}{z} \Leftrightarrow x_r = \dfrac{x_p}{f} * z$ . \ +При даунсемпле картинки в s раз $x_p^{'} = \dfrac{x_p}{s} \Rightarrow $ \ +$x_r^{'} = \dfrac{x_p^{'}}{f} * z = \dfrac{x_p / s} {f} * z = \dfrac{x_p}{(f^{'} * s)} * z$ \ +Так как $x_r^{'} = x_r \Rightarrow f^{'} * s = f \Rightarrow f^{'} = \dfrac{f}{s}$ + +6) Имеет ли право BA двигать точку отсчета системы координат (т.е. добавить константу ко всем координатам)? Как это повлияет на суммарную Loss? \\ +В целом может, но зачем, это же не изменит координат векторов, не повлияет на Loss. + +7) Каким образом можно гарантировать чтобы при сравнении нескольких последовательно построенных облаков точек одного и того же датасета (созданных по мере добавления фотографии за фотографией) в MeshLab - облака не были хаотично смещены/отмасштабированы/повернуты друг от друга? \\ +Не оптимизировать параметры уже добавленных камер -- использовать систему координат 0-й камеры как якорную. Таким образом при сравнении в качестве осей брать оси 0-й камеры, в качестве масштаба брать среднее расстояние до триангулированных метчей первых двух камер. А еще лучше -- расстояние между первыми двумя камерами + +100) Если есть - фидбек/идеи по улучшению задания. +Сделать более плавный переход от 3 задания к 4-му (как минимум project/unproject). \ No newline at end of file diff --git a/data/debug/test_matching/.gitignore b/data/debug/test_matching/.gitignore new file mode 100755 index 00000000..86d0cb27 --- /dev/null +++ b/data/debug/test_matching/.gitignore @@ -0,0 +1,4 @@ +# Ignore everything in this directory +* +# Except this file +!.gitignore \ No newline at end of file diff --git a/data/highlighted_results/test_depth_maps_pm/Herzjesu_two_cameras00.png b/data/highlighted_results/test_depth_maps_pm/Herzjesu_two_cameras00.png new file mode 100644 index 00000000..25db24c7 Binary files /dev/null and b/data/highlighted_results/test_depth_maps_pm/Herzjesu_two_cameras00.png differ diff --git a/data/highlighted_results/test_depth_maps_pm/Saharov_all_cameras00.png b/data/highlighted_results/test_depth_maps_pm/Saharov_all_cameras00.png new file mode 100644 index 00000000..f96a9ecc Binary files /dev/null and b/data/highlighted_results/test_depth_maps_pm/Saharov_all_cameras00.png differ diff --git a/data/highlighted_results/test_depth_maps_pm/Saharov_all_cameras01.png b/data/highlighted_results/test_depth_maps_pm/Saharov_all_cameras01.png new file mode 100644 index 00000000..44f63df9 Binary files /dev/null and b/data/highlighted_results/test_depth_maps_pm/Saharov_all_cameras01.png differ diff --git a/data/highlighted_results/test_depth_maps_pm/Saharov_two_cameras00.png b/data/highlighted_results/test_depth_maps_pm/Saharov_two_cameras00.png new file mode 100644 index 00000000..0f99bc96 Binary files /dev/null and b/data/highlighted_results/test_depth_maps_pm/Saharov_two_cameras00.png differ diff --git a/data/highlighted_results/test_depth_maps_pm/herzjesu_all_cameras00.png b/data/highlighted_results/test_depth_maps_pm/herzjesu_all_cameras00.png new file mode 100644 index 00000000..2e4ad8f5 Binary files /dev/null and b/data/highlighted_results/test_depth_maps_pm/herzjesu_all_cameras00.png differ diff --git a/herzjesu22.png b/herzjesu22.png new file mode 100644 index 00000000..5c4ee2a4 Binary files /dev/null and b/herzjesu22.png differ diff --git a/saharov32.png b/saharov32.png new file mode 100644 index 00000000..b5b927b6 Binary files /dev/null and b/saharov32.png differ diff --git a/src/phg/core/calibration.cpp b/src/phg/core/calibration.cpp index f70ddc25..37fdf73c 100644 --- a/src/phg/core/calibration.cpp +++ b/src/phg/core/calibration.cpp @@ -32,11 +32,20 @@ int phg::Calibration::height() const { cv::Vec3d phg::Calibration::project(const cv::Vec3d &point) const { + // return K() * point; // original from task03 double x = point[0] / point[2]; double y = point[1] / point[2]; double r2 = x * x + y * y; double r4 = r2 * r2; + // from task04 + // // 11: добавьте учет радиальных искажений (k1_, k2_) (после деления на Z, но до умножения на f) + // double r_squared = x * x + y * y; + // double r_fourth = r_squared * r_squared; + // double radial_coef = 1. + k1_ * r_squared + k2_ * r_fourth; + + // x *= radial_coef; + // y *= radial_coef; x = x * (1.0 + k1_ * r2 + k2_ * r4); y = y * (1.0 + k1_ * r2 + k2_ * r4); @@ -52,17 +61,38 @@ cv::Vec3d phg::Calibration::project(const cv::Vec3d &point) const cv::Vec3d phg::Calibration::unproject(const cv::Vec2d &pixel) const { + // K().inv() * vector3d(pixel[0], pixel[1], 1.0); double x = pixel[0] - cx_ - width_ * 0.5; double y = pixel[1] - cy_ - height_ * 0.5; x /= f_; y /= f_; - double r2 = x * x + y * y; - double r4 = r2 * r2; - - x = x / (1.0 + k1_ * r2 + k2_ * r4); - y = y / (1.0 + k1_ * r2 + k2_ * r4); - - return cv::Vec3d(x, y, 1.0); + // from task05 -- probably wrong + // double r2 = x * x + y * y; + // double r4 = r2 * r2; + + // x = x / (1.0 + k1_ * r2 + k2_ * r4); + // y = y / (1.0 + k1_ * r2 + k2_ * r4); + + // return cv::Vec3d(x, y, 1.0); + + // from task04 + // 12: добавьте учет радиальных искажений, когда реализуете - подумайте: почему строго говоря это - не симметричная формула формуле из project? (но лишь приближение) + // потому что мы имеем дело с функцией 4 степени относительно x и y. + double x_u = x, y_u = y; + // while (1) { + for (size_t i = 0; i < 1000; i++) { + double r_squared = x_u * x_u + y_u * y_u; + double r_fourth = r_squared * r_squared; + double dr = 1 + k1_ * r_squared + k2_ * r_fourth; + x_u = x / dr; + y_u = y / dr; + + if (std::abs(dr * x_u - x) + std::abs(dr * y_u - y) < 1e-10) { + break; + } + } + + return cv::Vec3d(x_u, y_u, 1.0); } diff --git a/src/phg/matching/descriptor_matcher.cpp b/src/phg/matching/descriptor_matcher.cpp index f4bcd871..1cf96387 100644 --- a/src/phg/matching/descriptor_matcher.cpp +++ b/src/phg/matching/descriptor_matcher.cpp @@ -7,8 +7,11 @@ void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector &filtered_matches) { filtered_matches.clear(); - - throw std::runtime_error("not implemented yet"); + for (const auto match: matches) { + if (match.size() > 1 && match[0].distance / match[1].distance < 0.5f) { + filtered_matches.push_back(match[0]); + } + } } @@ -35,42 +38,59 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector points_query.at(i) = keypoints_query[matches[i].queryIdx].pt; points_train.at(i) = keypoints_train[matches[i].trainIdx].pt; } -// -// // размерность всего 2, так что точное KD-дерево -// std::shared_ptr index_params = flannKdTreeIndexParams(TODO); -// std::shared_ptr search_params = flannKsTreeSearchParams(TODO); -// -// std::shared_ptr index_query = flannKdTreeIndex(points_query, index_params); -// std::shared_ptr index_train = flannKdTreeIndex(points_train, index_params); -// -// // для каждой точки найти total neighbors ближайших соседей -// cv::Mat indices_query(n_matches, total_neighbours, CV_32SC1); -// cv::Mat distances2_query(n_matches, total_neighbours, CV_32FC1); -// cv::Mat indices_train(n_matches, total_neighbours, CV_32SC1); -// cv::Mat distances2_train(n_matches, total_neighbours, CV_32FC1); -// -// index_query->knnSearch(points_query, indices_query, distances2_query, total_neighbours, *search_params); -// index_train->knnSearch(points_train, indices_train, distances2_train, total_neighbours, *search_params); -// -// // оценить радиус поиска для каждой картинки -// // NB: radius2_query, radius2_train: квадраты радиуса! -// float radius2_query, radius2_train; -// { -// std::vector max_dists2_query(n_matches); -// std::vector max_dists2_train(n_matches); -// for (int i = 0; i < n_matches; ++i) { -// max_dists2_query[i] = distances2_query.at(i, total_neighbours - 1); -// max_dists2_train[i] = distances2_train.at(i, total_neighbours - 1); -// } -// -// int median_pos = n_matches / 2; -// std::nth_element(max_dists2_query.begin(), max_dists2_query.begin() + median_pos, max_dists2_query.end()); -// std::nth_element(max_dists2_train.begin(), max_dists2_train.begin() + median_pos, max_dists2_train.end()); -// -// radius2_query = max_dists2_query[median_pos] * radius_limit_scale * radius_limit_scale; -// radius2_train = max_dists2_train[median_pos] * radius_limit_scale * radius_limit_scale; -// } -// -// метч остается, если левое и правое множества первых total_neighbors соседей в радиусах поиска(radius2_query, radius2_train) имеют как минимум consistent_matches общих элементов -// // TODO заполнить filtered_matches + + // размерность всего 2, так что точное KD-дерево + std::shared_ptr index_params = flannKdTreeIndexParams(4); // 1 + std::shared_ptr search_params = flannKsTreeSearchParams(32); // n_matches + + std::shared_ptr index_query = flannKdTreeIndex(points_query, index_params); + std::shared_ptr index_train = flannKdTreeIndex(points_train, index_params); + + // для каждой точки найти total neighbors ближайших соседей + cv::Mat indices_query(n_matches, total_neighbours, CV_32SC1); + cv::Mat distances2_query(n_matches, total_neighbours, CV_32FC1); + cv::Mat indices_train(n_matches, total_neighbours, CV_32SC1); + cv::Mat distances2_train(n_matches, total_neighbours, CV_32FC1); + + index_query->knnSearch(points_query, indices_query, distances2_query, total_neighbours, *search_params); + index_train->knnSearch(points_train, indices_train, distances2_train, total_neighbours, *search_params); + + // оценить радиус поиска для каждой картинки + // NB: radius2_query, radius2_train: квадраты радиуса! + float radius2_query, radius2_train; + { + std::vector max_dists2_query(n_matches); + std::vector max_dists2_train(n_matches); + for (int i = 0; i < n_matches; ++i) { + max_dists2_query[i] = distances2_query.at(i, total_neighbours - 1); + max_dists2_train[i] = distances2_train.at(i, total_neighbours - 1); + } + + int median_pos = n_matches / 2; + std::nth_element(max_dists2_query.begin(), max_dists2_query.begin() + median_pos, max_dists2_query.end()); + std::nth_element(max_dists2_train.begin(), max_dists2_train.begin() + median_pos, max_dists2_train.end()); + + radius2_query = max_dists2_query[median_pos] * radius_limit_scale * radius_limit_scale; + radius2_train = max_dists2_train[median_pos] * radius_limit_scale * radius_limit_scale; + } + + // метч остается, если левое и правое множества первых total_neighbors соседей в радиусах поиска(radius2_query, radius2_train) имеют как минимум consistent_matches общих элементов + for (size_t i = 0; i < n_matches; i++) { + std::set set2train; + size_t n_consistent_matches = 0; + for (size_t j = 0; j < total_neighbours; j++) { + if (distances2_train.at(i, j) <= radius2_train) { + set2train.insert(indices_train.at(i, j)); + } + } + for (size_t j = 0; j < total_neighbours; j++) { + if (distances2_query.at(i, j) <= radius2_query && set2train.count(indices_query.at(i, j))) { + n_consistent_matches++; + } + } + + if (n_consistent_matches >= consistent_matches) { + filtered_matches.push_back(matches[i]); + } + } } diff --git a/src/phg/matching/flann_matcher.cpp b/src/phg/matching/flann_matcher.cpp index 9e9f5180..358d2336 100644 --- a/src/phg/matching/flann_matcher.cpp +++ b/src/phg/matching/flann_matcher.cpp @@ -6,8 +6,8 @@ phg::FlannMatcher::FlannMatcher() { // параметры для приближенного поиска -// index_params = flannKdTreeIndexParams(TODO); -// search_params = flannKsTreeSearchParams(TODO); + index_params = flannKdTreeIndexParams(3); // рекомендуется выставлять 5, но когда включил свой SIFT пришлось уменьшить до 3, чтобы прозоходились тесты + search_params = flannKsTreeSearchParams(35); // в документации OpenCV используют 50, но 50 не очень стабильно проходит тесты по тайм-лимитам } void phg::FlannMatcher::train(const cv::Mat &train_desc) @@ -17,5 +17,11 @@ void phg::FlannMatcher::train(const cv::Mat &train_desc) void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, std::vector> &matches, int k) const { - throw std::runtime_error("not implemented yet"); + std::vector outIndices(k); + std::vector outDists(k); + + for (size_t i = 0; i < query_desc.size().height; i++) { + flann_index->knnSearch(query_desc.row(i), outIndices, outDists, k, *search_params); + matches.push_back({cv::DMatch(i, outIndices[0], std::sqrt(outDists[0])), cv::DMatch(i, outIndices[1], std::sqrt(outDists[1]))}); + } } diff --git a/src/phg/mvs/depth_maps/pm_depth_maps.cpp b/src/phg/mvs/depth_maps/pm_depth_maps.cpp index 179fb404..24c06038 100644 --- a/src/phg/mvs/depth_maps/pm_depth_maps.cpp +++ b/src/phg/mvs/depth_maps/pm_depth_maps.cpp @@ -46,14 +46,21 @@ vector3d project(const vector3d& global_point, const phg::Calibration& calibrati return pixel_with_depth; } -// TODO 101 реализуйте unproject (вам поможет тест на идемпотентность project -> unproject в test_depth_maps_pm) +// TODODONE 101 реализуйте unproject (вам поможет тест на идемпотентность project -> unproject в test_depth_maps_pm) vector3d unproject(const vector3d& pixel, const phg::Calibration& calibration, const matrix34d& PtoWorld) { double depth = pixel[2]; // на самом деле это не глубина, это координата по оси +Z (вдоль которой смотрит камера в ее локальной системе координат) - vector3d local_point = calibration.unproject(vector2d(pixel[0], pixel[1])) * depth; - vector3d global_point = PtoWorld * homogenize(local_point); + // from task06 + // vector3d local_point = calibration.unproject(vector2d(pixel[0], pixel[1])) * depth; + + // vector3d global_point = PtoWorld * homogenize(local_point); + + vector3d local_point = calibration.unproject({pixel[0], pixel[1]}) * depth; // TODODONE 102 пустите луч pixel из calibration а затем возьмите ан нем точку у которой по оси +Z координата=depth + + vector4d local_point_ext = {local_point[0], local_point[1], local_point[2], 1.0}; + vector3d global_point = PtoWorld * local_point_ext; // TODODONE 103 переведите точку из локальной системы в глобальную return global_point; } @@ -162,7 +169,7 @@ void PMDepthMapsBuilder::buildDepthMap(unsigned int camera_key, cv::Mat& depth_m // в первую очередь нам надо заполнить случайными гипотезами, этим займется refinement refinement(); - + printf("First refinement done!\n"); for (iter = 1; iter <= NITERATIONS; ++iter) { propagation(); refinement(); @@ -195,13 +202,15 @@ void PMDepthMapsBuilder::refinement() n0 = normal_map.at(j, i); // 2) случайной пертурбации текущей гипотезы (мутация и уточнение того что уже смогли найти) - dp = r.nextf(d0 * 0.5f, d0 * 1.5); // TODO 104: сделайте так чтобы отклонение было тем меньше, чем номер итерации ближе к NITERATIONS, улучшило ли это результат? - np = cv::normalize(n0 + randomNormalObservedFromCamera(cameras_RtoWorld[ref_cam], r) * 0.5); // TODO 105: сделайте так чтобы отклонение было тем меньше, чем номер итерации ближе к NITERATIONS, улучшило ли это результат? + dp = r.nextf(d0 * (0.5f + 0.5f * iter / (NITERATIONS + 1.f)), d0 * (1.5f - 0.5f * iter / (NITERATIONS + 1.f))); // TODODONE 104: сделайте так чтобы отклонение было тем меньше, чем номер итерации ближе к NITERATIONS, улучшило ли это результат? + np = cv::normalize(n0 + randomNormalObservedFromCamera(cameras_RtoWorld[ref_cam], r) * 0.5 * (1.f - iter / (NITERATIONS + 1.f))); // TODODONE 105: сделайте так чтобы отклонение было тем меньше, чем номер итерации ближе к NITERATIONS, улучшило ли это результат? dp = std::max(ref_depth_min, std::min(ref_depth_max, dp)); // 3) новой случайной гипотезы из фрустума поиска (новые идеи, вечный поиск во всем пространстве) - // TODO 106: создайте случайную гипотезу dr+nr, вам поможет: + // TODODONE 106: создайте случайную гипотезу dr+nr, вам поможет: + dr = r.nextf(ref_depth_min, ref_depth_max); + nr = cv::normalize(randomNormalObservedFromCamera(cameras_RtoWorld[ref_cam], r)); // - r.nextf(...) // - ref_depth_min, ref_depth_max // - randomNormalObservedFromCamera - поможет создать нормаль которая гарантированно смотрит на нас @@ -285,6 +294,69 @@ void PMDepthMapsBuilder::tryToPropagateDonor(ptrdiff_t ni, ptrdiff_t nj, int che hypos_cost.push_back(cost); } + +void PMDepthMapsBuilder::tryToPropagateDonor3dots(ptrdiff_t ni0, ptrdiff_t nj0, + ptrdiff_t ni1, ptrdiff_t nj1, + ptrdiff_t ni2, ptrdiff_t nj2, + int chessboard_pattern_step, std::vector& hypos_depth, std::vector& hypos_normal, std::vector& hypos_cost) +{ + // rassert-ы или любой другой способ явной фиксации инвариантов со встроенной их проверкой в runtime - + // это очень приятный способ ускорить отладку и гарантировать что ожидания в голове сойдутся с реальностью в коде, + // а если разойдутся - то узнать об этом в самом первом сломавшемся предположении + // (в данном случае мы явно проверяем что нигде не промахнулись и все соседи - другого шахматного цвета) + // пусть лучше эта проверка упадет, мы сразу это заметим и отладим, чем бага будет тихо портить результаты + // а мы это может быть даже не заметим + // rassert((ni0 + nj0) % 2 != chessboard_pattern_step, 2391249129510120); + // rassert((ni1 + nj1) % 2 != chessboard_pattern_step, 2391249129510121); + // rassert((ni2 + nj2) % 2 != chessboard_pattern_step, 2391249129510122); + + std::vector depths; + std::vector costs; + std::vector> costs_indexed; + std::vector ns; + size_t cur_idx = 0; + if (!(ni0 < 0 || ni0 >= width || nj0 < 0 || nj0 >= height)) { + depths.push_back(depth_map.at(nj0, ni0)); + costs.push_back(cost_map.at(nj0, ni0)); + costs_indexed.push_back(std::make_pair(cost_map.at(nj0, ni0), cur_idx)); + ns.push_back(normal_map.at(nj0, ni0)); + cur_idx++; + } + + if (!(ni1 < 0 || ni1 >= width || nj1 < 0 || nj1 >= height)) { + depths.push_back(depth_map.at(nj1, ni1)); + costs.push_back(cost_map.at(nj1, ni1)); + costs_indexed.push_back(std::make_pair(cost_map.at(nj1, ni1), cur_idx)); + ns.push_back(normal_map.at(nj1, ni1)); + cur_idx++; + } + + + if (!(ni2 < 0 || ni2 >= width || nj2 < 0 || nj2 >= height)) { + depths.push_back(depth_map.at(nj2, ni2)); + costs.push_back(cost_map.at(nj2, ni2)); + costs_indexed.push_back(std::make_pair(cost_map.at(nj2, ni2), cur_idx)); + ns.push_back(normal_map.at(nj2, ni2)); + cur_idx++; + } + + std::sort(costs_indexed.begin(), costs_indexed.end()); + + if (costs_indexed.size() == 0) { + return; + } + size_t best_idx = costs_indexed[0].second; + if (depths[best_idx] == NO_DEPTH) { + return; + } + + hypos_depth.push_back(depths[best_idx]); + hypos_normal.push_back(ns[best_idx]); + hypos_cost.push_back(costs[best_idx]); + +} + + void PMDepthMapsBuilder::propagation() { timer t; @@ -298,46 +370,101 @@ void PMDepthMapsBuilder::propagation() std::vector hypos_normal; std::vector hypos_cost; - /* 4 прямых соседа A, 8 соседей B через диагональ, 4 соседа C вдалеке (условный рисунок для PROPAGATION_STEP=5): - * (удобно подсвечивать через Ctrl+F) - * center - * | - * v - * o o o o o C o o o o o - * o o o o o o o o o o o - * o o o o o o o v o o o - * o o o o B o B o v o o - * o o o B o A o B o o o - * C o o o A . A o o o C <- center - * o o o B o A o B o o o - * o o o o B o B o v o o - * o o o o o o o v o o o - * o o o o o o o o o o o - * o o o o o C o o o o o - */ - tryToPropagateDonor(i - 1, j + 0, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 0, j - 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 1, j + 0, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 0, j + 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - - tryToPropagateDonor(i - 2, j - 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i - 1, j - 2, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 1, j - 2, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 2, j - 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 2, j + 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 1, j + 2, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i - 1, j + 2, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i - 2, j + 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - - // в таких случаях очень приятно использовать множественный курсор (чтобы скопировав четыре строки выше, затем просто колесиком мышки сделать четыре каретки для того чтобы дважды вставить *PROPAGATION_STEP): - tryToPropagateDonor(i - 1 * PROPAGATION_STEP, j + 0 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 0 * PROPAGATION_STEP, j - 1 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 1 * PROPAGATION_STEP, j + 0 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - tryToPropagateDonor(i + 0 * PROPAGATION_STEP, j + 1 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); - - // TODO 201 переделайте чтобы было как в ACMH: - // TODO 202 - паттерн донорства - // TODO 203 - логика про "берем 8 лучших по их личной оценке - по их личному cost" и только их примеряем уже на себя для рассчета cost в нашей точке + if (USE_ACMH_POPAGATION_SCHEME) { + // TODODONE 201 переделайте чтобы было как в ACMH: + // TODODONE 202 - паттерн донорства + tryToPropagateDonor3dots( + i + 1, j + 0, + i + 2, j + 1, + i + 2, j - 1, + chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost + ); + tryToPropagateDonor3dots( + i - 1, j + 0, + i - 2, j + 1, + i - 2, j - 1, + chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost + ); + tryToPropagateDonor3dots( + i + 0, j + 1, + i + 1, j + 2, + i - 1, j + 2, + chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost + ); + tryToPropagateDonor3dots( + i + 0, j - 1, + i + 1, j - 2, + i - 1, j - 2, + chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost + ); + + + + tryToPropagateDonor3dots( + i - 1 * PROPAGATION_STEP, j + 0 * PROPAGATION_STEP, + i - 1 * (PROPAGATION_STEP - 1), j + 0 * (PROPAGATION_STEP - 1), + i - 1 * (PROPAGATION_STEP - 2), j + 0 * (PROPAGATION_STEP - 2), + chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost + ); + tryToPropagateDonor3dots( + i + 0 * PROPAGATION_STEP, j - 1 * PROPAGATION_STEP, + i + 0 * (PROPAGATION_STEP - 1), j - 1 * (PROPAGATION_STEP - 1), + i + 0 * (PROPAGATION_STEP - 2), j - 1 * (PROPAGATION_STEP - 2), + chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost + ); + tryToPropagateDonor3dots( + i + 1 * PROPAGATION_STEP, j + 0 * PROPAGATION_STEP, + i + 1 * (PROPAGATION_STEP - 1), j + 0 * (PROPAGATION_STEP - 1), + i + 1 * (PROPAGATION_STEP - 2), j + 0 * (PROPAGATION_STEP - 2), + chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost + ); + tryToPropagateDonor3dots( + i + 0 * PROPAGATION_STEP, j + 1 * PROPAGATION_STEP, + i + 0 * (PROPAGATION_STEP - 1), j + 1 * (PROPAGATION_STEP - 1), + i + 0 * (PROPAGATION_STEP - 2), j + 1 * (PROPAGATION_STEP - 2), + chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost + ); + } else { + /* 4 прямых соседа A, 8 соседей B через диагональ, 4 соседа C вдалеке (условный рисунок для PROPAGATION_STEP=5): + * (удобно подсвечивать через Ctrl+F) + * center + * | + * v + * o o o o o C o o o o o + * o o o o o o o o o o o + * o o o o o o o v o o o + * o o o o B o B o v o o + * o o o B o A o B o o o + * C o o o A . A o o o C <- center + * o o o B o A o B o o o + * o o o o B o B o v o o + * o o o o o o o v o o o + * o o o o o o o o o o o + * o o o o o C o o o o o + */ + tryToPropagateDonor(i - 1, j + 0, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 0, j - 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 1, j + 0, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 0, j + 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + + tryToPropagateDonor(i - 2, j - 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i - 1, j - 2, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 1, j - 2, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 2, j - 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 2, j + 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 1, j + 2, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i - 1, j + 2, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i - 2, j + 1, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + + // в таких случаях очень приятно использовать множественный курсор (чтобы скопировав четыре строки выше, затем просто колесиком мышки сделать четыре каретки для того чтобы дважды вставить *PROPAGATION_STEP): + tryToPropagateDonor(i - 1 * PROPAGATION_STEP, j + 0 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 0 * PROPAGATION_STEP, j - 1 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 1 * PROPAGATION_STEP, j + 0 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + tryToPropagateDonor(i + 0 * PROPAGATION_STEP, j + 1 * PROPAGATION_STEP, chessboard_pattern_step, hypos_depth, hypos_normal, hypos_cost); + } + + + // TODO 301 - сделайте вместо наивного переноса depth+normal в наш пиксель - логику про "пересекли луч из нашего пикселя с плоскостью которую задает донор-сосед" и оценку cost в нашей точке тогда можно провести для более // релевантной точки-пересечения @@ -348,6 +475,34 @@ void PMDepthMapsBuilder::propagation() best_cost = NO_COST; } + if (ESTIMATE_BEST_SELF_COST_N) { + // TODODONE 203 - логика про "берем 8 лучших по их личной оценке - по их личному cost" и только их примеряем уже на себя для рассчета cost в нашей точке + // rassert(hypos_cost.size() >= ESTIMATE_BEST_SELF_COST_N, 126731264127481); + if (hypos_cost.size() > ESTIMATE_BEST_SELF_COST_N) { + printf("%lu\n", hypos_cost.size()); + } + std::vector> costs_indexed; + for (size_t i = 0; i < hypos_cost.size(); i++) { + costs_indexed.push_back(std::make_pair(hypos_cost[i], i)); + } + std::sort(costs_indexed.begin(), costs_indexed.end()); + std::vector hypos_depth_tmp; + std::vector hypos_normal_tmp; + std::vector hypos_cost_tmp; + + for (size_t i = 0; i < std::min(ESTIMATE_BEST_SELF_COST_N, costs_indexed.size()); i++) { + size_t cur_best_idx = costs_indexed[i].second; + + hypos_depth_tmp.push_back(hypos_depth[cur_best_idx]); + hypos_cost_tmp.push_back(hypos_cost[cur_best_idx]); + hypos_normal_tmp.push_back(hypos_normal[cur_best_idx]); + + hypos_depth = hypos_depth_tmp; + hypos_cost = hypos_cost_tmp; + hypos_normal = hypos_normal_tmp; + } + } + for (size_t hi = 0; hi < hypos_depth.size(); ++hi) { // эту гипотезу мы сейчас рассматриваем как очередного кандидата float d = hypos_depth[hi]; @@ -431,18 +586,45 @@ float PMDepthMapsBuilder::estimateCost(ptrdiff_t i, ptrdiff_t j, double d, const double x = neighb_proj[0]; double y = neighb_proj[1]; - // TODO 205: замените этот наивный вариант nearest neighbor сэмплирования текстуры на билинейную интерполяцию (учтите что центр пикселя - .5 после запятой) - ptrdiff_t u = x; - ptrdiff_t v = y; - - // TODO 108: добавьте проверку "попали ли мы в камеру номер neighb_cam?" если не попали - возвращаем NO_COST + // TODODONE 108: добавьте проверку "попали ли мы в камеру номер neighb_cam?" если не попали - возвращаем NO_COST + if (x < 0 || x > calibration.width() || y < 0 || y > calibration.height()) { // не height - 1 и width - 1, тк пиксели в данной нотации имеют ширину + return NO_COST; + } - float intensity = cameras_imgs_grey[neighb_cam].at(v, u) / 255.0f; - patch1.push_back(intensity); + // TODODONE 205: замените этот наивный вариант nearest neighbor сэмплирования текстуры на билинейную интерполяцию (учтите что центр пикселя - .5 после запятой) + // тк центры в (p.x + 0.5, p.y + 0.5) + if (USE_BILINEAR_INTERPOLATION) { + x -= 0.5; // смещаем для того чтобы проще округлять к центрам пикселей + y -= 0.5; + + ptrdiff_t u0 = std::floor(x) < 0 ? std::floor(x) + 1 : std::floor(x); + ptrdiff_t v0 = std::floor(y) < 0 ? std::floor(y) + 1 : std::floor(y); + ptrdiff_t ud = u0 < (width - 1) ? 1 : 0; + ptrdiff_t vd = v0 < (height - 1) ? 1 : 0; + float du_lambda0 = 1.f - (x - u0); + float dv_lambda0 = 1.f - (y - v0); + float du_lambda1 = 1. - du_lambda0; + float dv_lambda1 = 1. - dv_lambda0; + + float intensity = dv_lambda0 * ( + du_lambda0 * cameras_imgs_grey[neighb_cam].at(v0, u0) + + du_lambda1 * cameras_imgs_grey[neighb_cam].at(v0, u0 + ud) + ) + dv_lambda1 * ( + du_lambda0 * cameras_imgs_grey[neighb_cam].at(v0 + vd, u0) + + du_lambda1 * cameras_imgs_grey[neighb_cam].at(v0 + vd, u0 + ud) + ); + intensity /= 255.; + patch1.push_back(intensity); + } else { + ptrdiff_t u = x; + ptrdiff_t v = y; + float intensity = cameras_imgs_grey[neighb_cam].at(v, u) / 255.0f; + patch1.push_back(intensity); + } } } - // TODO 109: реализуйте ZNCC https://en.wikipedia.org/wiki/Cross-correlation#Zero-normalized_cross-correlation_(ZNCC) + // TODODONE 109: реализуйте ZNCC https://en.wikipedia.org/wiki/Cross-correlation#Zero-normalized_cross-correlation_(ZNCC) // или слайд #25 в лекции 5 про SGM и Cost-функции - https://my.compscicenter.ru/attachments/classes/slides_w2n8WNLY/photogrammetry_lecture_090321.pdf rassert(patch0.size() == patch1.size(), 12489185129326); size_t n = patch0.size(); @@ -459,7 +641,34 @@ float PMDepthMapsBuilder::estimateCost(ptrdiff_t i, ptrdiff_t j, double d, const mean0 /= n; mean1 /= n; // ... - float zncc = 0.0f; + + float nom = 0.0, denom1 = 0.0, denom2 = 0.0; + for (size_t k = 0; k < n; ++k) { + float a = patch0[k]; + float b = patch1[k]; + + nom += (a - mean0) * (b - mean1); + denom1 += (a - mean0) * (a - mean0); + denom2 += (b - mean1) * (b - mean1); + } + + + float zncc = 0; + if (USE_ADVANCED_UNCERTANTY) { + float denom = std::sqrt(denom1 * denom2); + if (denom < 1e-9) { // в scipy.stats.pearsonr в случае если одна из выборок близка к среднему выдают ворнинги + if (std::abs(nom) < 1e-6) { + return 0.f; + } else { + return NO_COST; + } + } + zncc = nom / denom; + } else { + if (std::abs(nom) > 0. && std::abs(denom1) > 0. && std::abs(denom2) > 0.) { + zncc = nom / std::sqrt(denom1 * denom2); + } + } // ZNCC в диапазоне [-1; 1], 1: идеальное совпадение, -1: ничего общего rassert(zncc == zncc, 23141241210380); // проверяем что не nan @@ -485,10 +694,33 @@ float PMDepthMapsBuilder::avgCost(std::vector& costs) float cost_sum = best_cost; float cost_w = 1.0f; - - // TODO 110 реализуйте какое-то "усреднение cost-ов по всем соседям", с ограничением что участвуют только COSTS_BEST_K_LIMIT лучших - // TODO 111 добавьте к этому усреднению еще одно ограничение: если cost больше чем best_cost*COSTS_K_RATIO - то такой cost подозрительно плохой и мы его не хотим учитывать (вероятно occlusion) - // TODO 112 а что если в пикселе occlusion, но best_cost - большой и поэтому отсечение по best_cost*COSTS_K_RATIO не срабатывает? можно ли это отсечение как-то выправить для такого случая? + if (USE_BIG_COST_FILTERING) { + // TODODONE 112 а что если в пикселе occlusion, но best_cost - большой и поэтому отсечение по best_cost*COSTS_K_RATIO не срабатывает? можно ли это отсечение как-то выправить для такого случая? + // не дает улучшений + if (best_cost > GOOD_COST) { + return NO_COST; + } + } + + // TODODONE 110 реализуйте какое-то "усреднение cost-ов по всем соседям", с ограничением что участвуют только COSTS_BEST_K_LIMIT лучших + for (size_t i = 1; i < COSTS_BEST_K_LIMIT && i < costs.size(); i++) { + // TODODONE 111 добавьте к этому усреднению еще одно ограничение: если cost больше чем best_cost*COSTS_K_RATIO - то такой cost подозрительно плохой и мы его не хотим учитывать (вероятно occlusion) + if (costs[i] > best_cost * COSTS_K_RATIO) { + break; + } + if (USE_BIG_COST_FILTERING) { + // TODODONE 112 а что если в пикселе occlusion, но best_cost - большой и поэтому отсечение по best_cost*COSTS_K_RATIO не срабатывает? можно ли это отсечение как-то выправить для такого случая? + // не дает улучшений + if (costs[i] > GOOD_COST) { + break; + } + } + if (costs[i] < 0) { + printf("SHIT %lf\n", costs[i]); + } + cost_sum += costs[i]; + cost_w += 1; + } // TODO 207 а что если добавить какой-нибудь бонус в случае если больше чем Х камер засчиталось? улучшается/ухудшается ли от этого что-то на herzjezu25? а при большем числе фотографий float avg_cost = cost_sum / cost_w; diff --git a/src/phg/mvs/depth_maps/pm_depth_maps.h b/src/phg/mvs/depth_maps/pm_depth_maps.h index 18976cb0..7a8cac05 100644 --- a/src/phg/mvs/depth_maps/pm_depth_maps.h +++ b/src/phg/mvs/depth_maps/pm_depth_maps.h @@ -73,6 +73,10 @@ class PMDepthMapsBuilder { float estimateCost(ptrdiff_t i, ptrdiff_t j, double d, const vector3d& global_normal, size_t neighb_cam); float avgCost(std::vector& costs); void tryToPropagateDonor(ptrdiff_t ni, ptrdiff_t nj, int chessboard_pattern_step, std::vector& hypos_depth, std::vector& hypos_normal, std::vector& hypos_cost); + void tryToPropagateDonor3dots(ptrdiff_t ni0, ptrdiff_t nj0, + ptrdiff_t ni1, ptrdiff_t nj1, + ptrdiff_t ni2, ptrdiff_t nj2, + int chessboard_pattern_step, std::vector& hypos_depth, std::vector& hypos_normal, std::vector& hypos_cost); void printCurrentStats(); void debugCurrentPoints(const std::string& label); diff --git a/src/phg/mvs/depth_maps/pm_depth_maps_defines.h b/src/phg/mvs/depth_maps/pm_depth_maps_defines.h index 87f62399..db084ddd 100644 --- a/src/phg/mvs/depth_maps/pm_depth_maps_defines.h +++ b/src/phg/mvs/depth_maps/pm_depth_maps_defines.h @@ -4,6 +4,11 @@ #define NO_DEPTH 0.0f #define NO_COST 1.0f #define GOOD_COST 0.2f +#define USE_BILINEAR_INTERPOLATION 1 +#define USE_ACMH_POPAGATION_SCHEME 1 +#define ESTIMATE_BEST_SELF_COST_N 0ul +#define USE_ADVANCED_UNCERTANTY 1 +#define USE_BIG_COST_FILTERING 1 #define NITERATIONS 5 diff --git a/src/phg/mvs/model_min_cut/min_cut_cgal_structs.cpp b/src/phg/mvs/model_min_cut/min_cut_cgal_structs.cpp index e3363529..2dba47a7 100644 --- a/src/phg/mvs/model_min_cut/min_cut_cgal_structs.cpp +++ b/src/phg/mvs/model_min_cut/min_cut_cgal_structs.cpp @@ -10,6 +10,7 @@ vertex_info_t::vertex_info_t(unsigned int camera_id, const cv::Vec3b& color) : color(color) { camera_ids.push_back(camera_id); + radius = 0.; } void vertex_info_t::merge(const vertex_info_t& that) @@ -32,4 +33,6 @@ void vertex_info_t::merge(const vertex_info_t& that) for (int i = 1; i < camera_ids.size(); ++i) { rassert(camera_ids[i - 1] < camera_ids[i], 23781274121024); } + + radius = std::max(that.radius, radius); } diff --git a/src/phg/mvs/model_min_cut/min_cut_cgal_structs.h b/src/phg/mvs/model_min_cut/min_cut_cgal_structs.h index 9264d80f..3161d244 100644 --- a/src/phg/mvs/model_min_cut/min_cut_cgal_structs.h +++ b/src/phg/mvs/model_min_cut/min_cut_cgal_structs.h @@ -11,6 +11,7 @@ struct vertex_info_t { std::vector camera_ids; cv::Vec3b color; size_t vertex_on_surface_id; + double radius; vertex_info_t() : color(0, 0, 255) // red color, BGR convention (OpenCV compatible) diff --git a/src/phg/mvs/model_min_cut/min_cut_defines.h b/src/phg/mvs/model_min_cut/min_cut_defines.h index 3572d0b8..3502ae78 100644 --- a/src/phg/mvs/model_min_cut/min_cut_defines.h +++ b/src/phg/mvs/model_min_cut/min_cut_defines.h @@ -2,3 +2,7 @@ #define LAMBDA_OUT 1.0 #define LAMBDA_IN 1.0 + +#define SIGMA_RADIUS_COEFF 1.0 +#define N_SIGMAS_EXTEND_DIST 3.0 +#define SET_CAPACITIES_FOR_EXT_POINTS 1 diff --git a/src/phg/mvs/model_min_cut/min_cut_model_builder.cpp b/src/phg/mvs/model_min_cut/min_cut_model_builder.cpp index f5e84c3a..19ee185a 100644 --- a/src/phg/mvs/model_min_cut/min_cut_model_builder.cpp +++ b/src/phg/mvs/model_min_cut/min_cut_model_builder.cpp @@ -58,10 +58,16 @@ void MinCutModelBuilder::appendToTriangulation( // проверяем насколько ближайшая точка далеко vector3d np = from_cgal_point(nearest_vertex->point()); // TODO 2001 appendToTriangulation(): реализуйте нормальную проверку объединять ли точку с уже добавленной ранее (с учетом r и MERGE_THRESHOLD_RADIUS_KOEF) - to_merge = false; + // if (r < MERGE_THRESHOLD_RADIUS_KOEF) { + if (std::sqrt(phg::norm2(np - p)) < r * MERGE_THRESHOLD_RADIUS_KOEF) { + to_merge = true; + } else { + to_merge = false; + } } vertex_info_t p_info(camera_id, color); + p_info.radius = std::max(r, p_info.radius); if (to_merge) { nearest_vertex->info().merge(p_info); } else { @@ -328,6 +334,7 @@ void MinCutModelBuilder::buildMesh(std::vector& mesh_faces, std::vect const vector3d ray_from_camera = cv::normalize(point0 - camera_center); const vector3d ray_to_camera = cv::normalize(camera_center - point0); const double distance_to_camera = phg::norm(camera_center - point0); + double sigma = SIGMA_RADIUS_COEFF * vi->info().radius; { // хотим найти ячейку триангуляции лежащую внутри поверхности (т.е. сразу за этим лучем видимости camera_center->point0): @@ -340,10 +347,79 @@ void MinCutModelBuilder::buildMesh(std::vector& mesh_faces, std::vect const cgal_facet_t intersected_facet = chooseIntersectedFacet(proxy->triangulation, point0, point0 + ray_from_camera, cur_facets, false); rassert(intersected_facet != cgal_facet_t(), 2378213120305); - // это ячейка триангуляции лежащая под поверхностью (т.е. сразу за вершиной) - const cell_handle_t cell_after_point = intersected_facet.first; - // добавляем пропускной способности из этой ячейки (из этого тетрагедрончика) к стоку - cell_after_point->info().t_capacity += LAMBDA_IN; + vector3d point0_extended = point0 + ray_from_camera * sigma * N_SIGMAS_EXTEND_DIST; + cell_handle_t cell_after_point_extended = intersected_facet.first; + // go backwards from camera up to point0 + ray_from_camera * sigma * N_SIGMAS_EXTEND_DIST + { + std::vector cur_facets_backwards = facets_around_point0; // это актуальные на данный момент треугольники-кандидаты для пересечения с лучем + double prev_distance = 0.0; + size_t steps = 0; + double prev_distance_to_camera = phg::norm(point0 - camera_center); + double prev_capacity = 0.; + + while (cur_facets_backwards.size() > 0 && prev_distance <= sigma * N_SIGMAS_EXTEND_DIST) { + const cgal_facet_t intersected_facet = chooseIntersectedFacet(proxy->triangulation, point0, point0_extended, cur_facets_backwards, false); + rassert(intersected_facet != cgal_facet_t() || steps == 0, + 238192412849030373); // всегда должно находится пересечение (иначе это означает что мы потерялись по пути, вместо того чтобы однажды добраться до ячейки содержащей камеру) + if (intersected_facet == cgal_facet_t() && steps == 0) { + // единственное исключение - это когда отрезок из вершины до камеры не пересекает ни одного треугольника (т.е. когда камера находится в смежной с вершиной ячейке) + break; + } + rassert(!proxy->triangulation.is_infinite(intersected_facet), + 23892141031273); // если треугольник бесконечный - с ним сложно работать, чтобы такого не случалось - мы добавили фиктивные точки - создали bounding box в insertBoundingBoxVertices() + ++steps; + + // отзеркаливаем треугольник (грань ячейки), т.к. нам нужно обновить пропускную способность ребра по направлению от камеры к точке, а перешагивали по треугольникам мы по направлению от точки к камере + const cgal_facet_t mirrored_intersected_facet = proxy->triangulation.mirror_facet(intersected_facet); + + // находим две ячейки (находящиеся по разные стороны от только что пересеченного треугольника): + // следующая всмысле шагания ячейка (та что ближе к камере) + const cell_handle_t next_cell = mirrored_intersected_facet.first; + const int next_cell_facet_subindex = mirrored_intersected_facet.second; + rassert(next_cell_facet_subindex >= 0 && next_cell_facet_subindex < 4, 23812948124029273); + // предыдущая всмысле шагания ячейка (та что ближе к точке) + const cell_handle_t prev_cell = next_cell->neighbor(next_cell_facet_subindex); + + // посчитаем какой путь мы уже прошли от точки, для этого надо найти расстояние от точки до места пересечения луча и треугольника (т.е. плоскости на которой он лежит, т.к. мы уже знаем что треугольник мы пересекаем лучем) + plane_t facet_plane(intersected_facet); + double distance_from_surface = facet_plane.distanceToIntersection(point0, ray_from_camera); + double distance_to_camera2 = facet_plane.distanceToIntersection(camera_center, ray_from_camera); + + if (distance_from_surface < 0.0) { + // плоскость и луч почти параллельны, вычисления ненадежны, расстояние до пересечения может быть странным (например монотонность может сломаться) + // в таком случае оставим предыдущую оценку пройденного пути + distance_from_surface = prev_distance; + } else { + rassert(distance_from_surface > prev_distance * 0.99, 2378924712421029373); // дополнительная проверка на разумность происходящего, мы удаляемся от точки - приближаемся к камере + // rassert(distance_from_surface < distance_to_camera * 1.01, 23871297312033573); // проверяем что chooseIntersectedFacet справился со своей задачей "остановиться когда мы дойдем до ячейки содержащей камеру" + rassert(distance_to_camera2 > prev_distance_to_camera * 0.99, 23582375723573); // проверяем что новая плоскость дальше от камеры чем предыдущая + } + prev_distance_to_camera = distance_to_camera2; + prev_distance = distance_from_surface; + + // увеличиваем пропускную способность на треугольнике-ребре (в направлении от края к точке) + double capacity = std::max(0.0, std::min(LAMBDA_OUT * (1.0 - std::exp(-(distance_from_surface * distance_from_surface) / (2.0 * sigma * sigma))), LAMBDA_OUT)); + rassert(capacity > prev_capacity * 0.99, 456845678347573); + prev_capacity = capacity; + + if (SET_CAPACITIES_FOR_EXT_POINTS) { + next_cell->info().facets_capacities[next_cell_facet_subindex] += capacity; + } + if (prev_distance <= sigma * N_SIGMAS_EXTEND_DIST) { + cell_after_point_extended = prev_cell; + } + } + } + + // cell_handle_t cell_after_point_extended = proxy->triangulation.locate(to_cgal_point(point0_extended)); + if (!proxy->triangulation.is_infinite(cell_after_point_extended)) { + cell_after_point_extended->info().t_capacity += LAMBDA_IN; + } else { + // это ячейка триангуляции лежащая под поверхностью (т.е. сразу за вершиной) + const cell_handle_t cell_after_point = intersected_facet.first; + // добавляем пропускной способности из этой ячейки (из этого тетрагедрончика) к стоку + cell_after_point->info().t_capacity += LAMBDA_IN; + } } // шагаем от точки до камеры выставляя веса на треугольниках (они же ребра в графе) которые пересекаются по мере трассировки луча @@ -388,7 +464,11 @@ void MinCutModelBuilder::buildMesh(std::vector& mesh_faces, std::vect prev_distance = distance_from_surface; // увеличиваем пропускную способность на треугольнике-ребре (в направлении от камеры к точке) - next_cell->info().facets_capacities[next_cell_facet_subindex] += LAMBDA_OUT; + // next_cell->info().facets_capacities[next_cell_facet_subindex] += LAMBDA_OUT; + double capacity = std::max(0.0, std::min(LAMBDA_OUT * (1.0 - std::exp(-(distance_from_surface * distance_from_surface) / (2.0 * sigma * sigma))), LAMBDA_OUT)); + // double capacity = (LAMBDA_OUT / distance_to_camera); + + next_cell->info().facets_capacities[next_cell_facet_subindex] += capacity; if (cur_facets.size() == 0) { // если на будущее у нас нет кандидатов-треугольников, значит мы закончили наш путь и следующая ячейка содержит нашу камеру @@ -477,11 +557,17 @@ void MinCutModelBuilder::buildMesh(std::vector& mesh_faces, std::vect // TODO 2002 добавьте проверку - не опирается ли треугольник на одну из фиктивных вершин (лежащих на гранях вспомогательного bounding box), можете для этого использовать bb_min и bb_max, или добавьте явный флаг в каждую вершину // иначе говоря сделайте так чтобы такие треугольники не добавлялись в результирующую модель эти большие красные треугольники - + bool lay_on_bb = false; for (int v_index = 1; v_index <= 3; ++v_index) { auto vi = ci->vertex((i + v_index) % 4); size_t& surface_vertex_id = vi->info().vertex_on_surface_id; - if (surface_vertex_id == VERTEX_NOT_ON_SURFACE_RESULT) { + auto pp = from_cgal_point(vi->point()); + for (int check_i = 0; check_i < 3; ++check_i) { + if (pp[check_i] <= bb_min[check_i] || pp[check_i] >= bb_max[check_i]) { + lay_on_bb = true; + } + } + if (surface_vertex_id == VERTEX_NOT_ON_SURFACE_RESULT && !lay_on_bb) { surface_vertex_id = mesh_nvertices++; } face[v_index - 1] = surface_vertex_id; @@ -490,8 +576,18 @@ void MinCutModelBuilder::buildMesh(std::vector& mesh_faces, std::vect // TODO 2003 некоторые треугольники выглядят темными в результирующей модели, проблема уходит если выключить в MeshLab освещение (кнопка желтой лампочка - Light on/off) которое учитывает нормаль, которая строится с учетом // порядка вершин треугольника (по часовой стрелке или против) иначе говоря оказывается что порядок обхода вершин в треугольнике не всегда корректен подумайте чем это вызывано и поправьте (лучше всего это делать посматривая на // картинку 'Figure 44.1' в документации https://doc.cgal.org/latest/Triangulation_3/index.html ) + if (!lay_on_bb) { + vector3d p0 = from_cgal_point(ci->vertex(i % 4)->point()); + vector3d p1 = from_cgal_point(ci->vertex((i + 1) % 4)->point()); + vector3d p2 = from_cgal_point(ci->vertex((i + 2) % 4)->point()); + vector3d p3 = from_cgal_point(ci->vertex((i + 3) % 4)->point()); + vector3d normal = (p2 - p1).cross(p3 - p1); + if ((p1 - p0).dot(normal) < 0) { + std::swap(face[0], face[1]); + } - mesh_faces.push_back(face); + mesh_faces.push_back(face); + } } } diff --git a/src/phg/sfm/ematrix.cpp b/src/phg/sfm/ematrix.cpp index e13f9843..cd1fb05c 100644 --- a/src/phg/sfm/ematrix.cpp +++ b/src/phg/sfm/ematrix.cpp @@ -17,19 +17,21 @@ namespace { copy(Ecv, E); Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); - - Eigen::MatrixXd U = svd.matrixU(); - Eigen::VectorXd s = svd.singularValues(); - Eigen::MatrixXd V = svd.matrixV(); - - Eigen::MatrixXd S = Eigen::MatrixXd(3, 3); - S.setZero(); - - S(0, 0) = 1.0; - S(1, 1) = 1.0; - S(2, 2) = 0.0; - - E = U * S * V.transpose(); + auto singular_values_vector = svd.singularValues(); + double sigma12 = (singular_values_vector[0] + singular_values_vector[1]) / 2.; + singular_values_vector[0] = sigma12; + singular_values_vector[1] = sigma12; + singular_values_vector[2] = double(0.); + E = svd.matrixU() * singular_values_vector.asDiagonal() * svd.matrixV().transpose(); + + // course version + // Eigen::MatrixXd S = Eigen::MatrixXd(3, 3); + // S.setZero(); + + // S(0, 0) = 1.0; + // S(1, 1) = 1.0; + // S(2, 2) = 0.0; + // E = U * S * V.transpose(); copy(E, Ecv); } @@ -38,6 +40,7 @@ namespace { cv::Matx33d phg::fmatrix2ematrix(const cv::Matx33d &F, const phg::Calibration &calib0, const phg::Calibration &calib1) { + // maybe odd // TODO check that correct conversion (transpose ok) // TODO add separate test for fmatrix (check 1) reprojection errors 2) f matrix singular value property) @@ -76,6 +79,9 @@ namespace { double getDepth(const vector2d &m0, const vector2d &m1, const phg::Calibration &calib0, const phg::Calibration &calib1, const matrix34d &P0, const matrix34d &P1) { + // скомпенсировать калибровки камер + // vector3d p0 = calib0.K().t() * vector3d(m0[0], m0[1], 1.); // analog to unproject? + // vector3d p1 = calib1.K().t() * vector3d(m1[0], m1[1], 1.); vector3d p0 = calib0.unproject(m0); vector3d p1 = calib1.unproject(m1); @@ -87,7 +93,9 @@ namespace { X /= X[3]; } - return p0.dot(P0 * X) > 0 && p1.dot(P1 * X) > 0; + // return p0.dot(P0 * X) > 0 && p1.dot(P1 * X) > 0; // course version + // точка должна иметь положительную глубину для обеих камер + return ((P0 * X)[2] > 0) && ((P1 * X)[2] > 0); } } @@ -106,7 +114,7 @@ void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d & using mat = Eigen::MatrixXd; using vec = Eigen::VectorXd; - + mat E; copy(Ecv, E); @@ -126,20 +134,15 @@ void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d & std::cout << "U:\n" << U << std::endl; std::cout << "s:\n" << s << std::endl; std::cout << "V:\n" << V << std::endl; - } + } mat W(3, 3); - W << 0, -1, 0, 1, 0, 0, 0, 0, 1; + W << 0., -1., 0., 1., 0., 0., 0., 0., 1.; mat Z(3, 3); - Z << 0, 1, 0, -1, 0, 0, 0, 0, 0; - - if (verbose) { - std::cout << "W:\n" << W << std::endl; - std::cout << "Z:\n" << Z << std::endl; - } + Z << 0., 1., 0., -1., 0., 0., 0., 0., 0.; - mat R0 = U * W * V.transpose(); + mat R0 = U * W * V.transpose(); // the same thing as for U and V? => positive determinant : det ALWAYS > 0 mat R1 = U * W.transpose() * V.transpose(); if (verbose) { @@ -148,7 +151,7 @@ void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d & } vec t0 = U.col(2); - vec t1 = -t0; + vec t1 = -U.col(2); if (verbose) { std::cout << "t0:\n" << t0 << std::endl; diff --git a/src/phg/sfm/fmatrix.cpp b/src/phg/sfm/fmatrix.cpp index dcfee2fb..3032372f 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/src/phg/sfm/fmatrix.cpp @@ -29,17 +29,37 @@ namespace { Eigen::MatrixXd A(a_rows, a_cols); + double avg_x1 = 0, avg_y1 = 0, avg_x2 = 0, avg_y2 = 0; + double std_x1 = 0, std_y1 = 0, std_x2 = 0, std_y2 = 0; for (int i_pair = 0; i_pair < count; ++i_pair) { double x0 = m0[i_pair][0]; double y0 = m0[i_pair][1]; + avg_x1 += x0; + avg_y1 += y0; + std_x1 += x0 * x0; + std_y1 += y0 * y0; double x1 = m1[i_pair][0]; double y1 = m1[i_pair][1]; + avg_x2 += x1; + avg_y2 += y1; + std_x2 += x1 * x1; + std_y2 += y1 * y1; -// std::cout << "(" << x0 << ", " << y0 << "), (" << x1 << ", " << y1 << ")" << std::endl; + // std::cout << "(" << x0 << ", " << y0 << "), (" << x1 << ", " << y1 << ")" << std::endl; - A.row(i_pair) << x1*x0, x1*y0, x1, y1*x0, y1*y0, y1, x0, y0, 1.0; + A(i_pair, 0) = x1 * x0; + A(i_pair, 1) = x1 * y0; + A(i_pair, 2) = x1; + + A(i_pair, 3) = y1 * x0; + A(i_pair, 4) = y1 * y0; + A(i_pair, 5) = y1; + + A(i_pair, 6) = x0; + A(i_pair, 7) = y0; + A(i_pair, 8) = 1.; } Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); @@ -50,19 +70,13 @@ namespace { F.row(1) << null_space[3], null_space[4], null_space[5]; F.row(2) << null_space[6], null_space[7], null_space[8]; + // Поправить F так, чтобы соблюдалось свойство фундаментальной матрицы (последнее сингулярное значение = 0) Eigen::JacobiSVD svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV); - Eigen::MatrixXd U = svdf.matrixU(); - Eigen::VectorXd s = svdf.singularValues(); - Eigen::MatrixXd V = svdf.matrixV(); - Eigen::MatrixXd S = Eigen::MatrixXd(3, 3); - S.setZero(); - S(0, 0) = s[0]; - S(1, 1) = s[1]; - S(2, 2) = 0.0; - - F = U * S * V.transpose(); + auto singular_values_vector = svdf.singularValues(); + singular_values_vector[2] = double(0.); + F = svdf.matrixU() * singular_values_vector.asDiagonal() * svdf.matrixV().transpose(); cv::Matx33d Fcv; copy(F, Fcv); @@ -73,40 +87,21 @@ namespace { // get transform matrix so that after transform centroid of points will be zero and Root Mean Square distance from centroid will be sqrt(2) cv::Matx33d getNormalizeTransform(const std::vector &m, bool verbose=true) { - cv::Vec2d centroid(0.0, 0.0); - - if (m.empty()) { - throw std::runtime_error("Can't normalize transform"); - } - - for (int i = 0; i < (int) m.size(); ++i) { - centroid += m[i]; + cv::Vec2d avg(0, 0); + for (size_t i = 0; i < m.size(); i++) { + avg += m[i]; } - - centroid /= (double) m.size(); - + avg = cv::Vec2d(avg[0] / m.size(), avg[1] / m.size()); + double rms = 0; - for (int i = 0; i < (int) m.size(); ++i) { - cv::Vec2d d = m[i] - centroid; - rms += d[0] * d[0] + d[1] * d[1]; - } - rms = std::sqrt(rms / (2 * m.size())); + for (size_t i = 0; i < m.size(); i++) { + rms += (m[i][0] - avg[0]) * (m[i][0] - avg[0]) + (m[i][1] - avg[1]) * (m[i][1] - avg[1]); - if (rms == 0) { - throw std::runtime_error("Can't normalize transform"); } + + double s = std::sqrt(2 * m.size() / (rms + 1e-6)); - double s = std::sqrt(2) / rms; - - if (verbose) { - std::cout << "NORMALIZE TRANSFORM: centroid = " << centroid << ", scale = " << s << std::endl; - } - - cv::Matx33d T(s, 0.0, -s*centroid[0], - 0.0, s, -s*centroid[1], - 0.0, 0.0, 1.0); - - return T; + return {s, 0., -avg[0] * s, 0., s, -avg[1] * s, 0., 0., 1.}; } cv::Vec2d transformPoint(const cv::Vec2d &pt, const cv::Matx33d &T) @@ -139,16 +134,19 @@ namespace { } { -// check log: centroid should become close to zero, scale close to 1 - getNormalizeTransform(m0_t, verbose); - getNormalizeTransform(m1_t, verbose); +// Проверьте лог: при повторной нормализации должно найтись почти единичное преобразование + // std::cout << getNormalizeTransform(m0_t) << "\n"; + // std::cout << getNormalizeTransform(m1_t) << "\n"; } - // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters // будет отличаться от случая с гомографией - const int n_trials = 10000; + const int n_points_fit = 8; + const double singlePointInlierProbability = 0.287, successProbability = 0.99; // maybe singlePointInlierProbability should be set to lower value + // const int n_trials = (int) (log(1 - successProbability) / log(1 - pow(singlePointInlierProbability, n_points_fit))); + const int n_trials = 100000; // works + // const int n_trials = 10000; // from task04 - const int n_samples = 8; + const int n_samples = n_points_fit; uint64_t seed = 1; int best_support = 0; @@ -167,12 +165,12 @@ namespace { cv::Matx33d F = estimateFMatrixDLT(ms0, ms1, n_samples); - // denormalize + // denormalize TODO F = TN1.t() * F * TN0; + int support = 0; for (int i = 0; i < n_matches; ++i) { - // todo todo todo todo todo todo if (phg::epipolarTest(m0[i], m1[i], F, threshold_px) && phg::epipolarTest(m1[i], m0[i], F.t(), threshold_px)) { ++support; diff --git a/src/phg/sfm/homography.cpp b/src/phg/sfm/homography.cpp index 5cbc780c..af544ead 100644 --- a/src/phg/sfm/homography.cpp +++ b/src/phg/sfm/homography.cpp @@ -84,8 +84,13 @@ namespace { double w1 = ws1[i]; // 8 elements of matrix + free term as needed by gauss routine -// A.push_back({TODO}); -// A.push_back({TODO}); + // in fact 9 elements + 1 free term (extended matrix form) + // A.push_back({0, 0, 0, -w1 * x0, -w1 * y0, -w1 * w0, y1 * x0, y1 * y0, y1 * w0, 0}); + // A.push_back({w1 * x0, w1 * y0, w1 * w0, 0, 0, 0, -x1 * x0, -x1 * y0, -x1 * w0, 0}); + + // set h_9 equals 1 and move Ae_9 to the free term + A.push_back({0, 0, 0, -w1 * x0, -w1 * y0, -w1 * w0, y1 * x0, y1 * y0, -(y1 * w0)}); + A.push_back({w1 * x0, w1 * y0, w1 * w0, 0, 0, 0, -x1 * x0, -x1 * y0, -(-(x1 * w0))}); } int res = gauss(A, H); @@ -168,57 +173,59 @@ namespace { // * (простое описание для понимания) // * [3] http://ikrisoft.blogspot.com/2015/01/ransac-with-contrario-approach.html -// const int n_matches = points_lhs.size(); -// -// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters -// const int n_trials = TODO; -// -// const int n_samples = TODO; -// uint64_t seed = 1; -// const double reprojection_error_threshold_px = 2; -// -// int best_support = 0; -// cv::Mat best_H; -// -// std::vector sample; -// for (int i_trial = 0; i_trial < n_trials; ++i_trial) { -// randomSample(sample, n_matches, n_samples, &seed); -// -// cv::Mat H = estimateHomography4Points(points_lhs[sample[0]], points_lhs[sample[1]], points_lhs[sample[2]], points_lhs[sample[3]], -// points_rhs[sample[0]], points_rhs[sample[1]], points_rhs[sample[2]], points_rhs[sample[3]]); -// -// int support = 0; -// for (int i_point = 0; i_point < n_matches; ++i_point) { -// try { -// cv::Point2d proj = phg::transformPoint(points_lhs[i_point], H); -// if (cv::norm(proj - cv::Point2d(points_rhs[i_point])) < reprojection_error_threshold_px) { -// ++support; -// } -// } catch (const std::exception &e) -// { -// std::cerr << e.what() << std::endl; -// } -// } -// -// if (support > best_support) { -// best_support = support; -// best_H = H; -// -// std::cout << "estimateHomographyRANSAC : support: " << best_support << "/" << n_matches << std::endl; -// -// if (best_support == n_matches) { -// break; -// } -// } -// } -// -// std::cout << "estimateHomographyRANSAC : best support: " << best_support << "/" << n_matches << std::endl; -// -// if (best_support == 0) { -// throw std::runtime_error("estimateHomographyRANSAC : failed to estimate homography"); -// } -// -// return best_H; + const int n_matches = points_lhs.size(); + const int n_points = 4; + + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + const double singlePointInlierProbability = 0.5, successProbability = 0.99; + const int n_trials = (int) (log(1 - successProbability) / log(1 - pow(singlePointInlierProbability, n_points))); + + const int n_samples = n_points; + uint64_t seed = 1; + const double reprojection_error_threshold_px = 2; + + int best_support = 0; + cv::Mat best_H; + + std::vector sample; + for (int i_trial = 0; i_trial < n_trials; ++i_trial) { + randomSample(sample, n_matches, n_samples, &seed); + + cv::Mat H = estimateHomography4Points(points_lhs[sample[0]], points_lhs[sample[1]], points_lhs[sample[2]], points_lhs[sample[3]], + points_rhs[sample[0]], points_rhs[sample[1]], points_rhs[sample[2]], points_rhs[sample[3]]); + + int support = 0; + for (int i_point = 0; i_point < n_matches; ++i_point) { + try { + cv::Point2d proj = phg::transformPoint(points_lhs[i_point], H); + if (cv::norm(proj - cv::Point2d(points_rhs[i_point])) < reprojection_error_threshold_px) { + ++support; + } + } catch (const std::exception &e) + { + std::cerr << e.what() << std::endl; + } + } + + if (support > best_support) { + best_support = support; + best_H = H; + + std::cout << "estimateHomographyRANSAC : support: " << best_support << "/" << n_matches << std::endl; + + if (best_support == n_matches) { + break; + } + } + } + + std::cout << "estimateHomographyRANSAC : best support: " << best_support << "/" << n_matches << std::endl; + + if (best_support == 0) { + throw std::runtime_error("estimateHomographyRANSAC : failed to estimate homography"); + } + + return best_H; } } @@ -238,7 +245,9 @@ cv::Mat phg::findHomographyCV(const std::vector &points_lhs, const // таким преобразованием внутри занимается функции cv::perspectiveTransform и cv::warpPerspective cv::Point2d phg::transformPoint(const cv::Point2d &pt, const cv::Mat &T) { - throw std::runtime_error("not implemented yet"); + double w = T.at(2, 0) * pt.x + T.at(2, 1) * pt.y + T.at(2, 2); + return {(T.at(0, 0) * pt.x + T.at(0, 1) * pt.y + T.at(0, 2)) / w, + (T.at(1, 0) * pt.x + T.at(1, 1) * pt.y + T.at(1, 2)) / w}; } cv::Point2d phg::transformPointCV(const cv::Point2d &pt, const cv::Mat &T) { diff --git a/src/phg/sfm/panorama_stitcher.cpp b/src/phg/sfm/panorama_stitcher.cpp index 8d76939b..4b4002b4 100644 --- a/src/phg/sfm/panorama_stitcher.cpp +++ b/src/phg/sfm/panorama_stitcher.cpp @@ -21,9 +21,36 @@ cv::Mat phg::stitchPanorama(const std::vector &imgs, // вектор гомографий, для каждой картинки описывает преобразование до корня std::vector Hs(n_images); { + size_t root_idx = 0; + std::vector visited(n_images, false); // здесь надо посчитать вектор Hs // при этом можно обойтись n_images - 1 вызовами функтора homography_builder - throw std::runtime_error("not implemented yet"); + for (size_t img_num = 0; img_num < n_images; img_num++) { + if (parent[img_num] == -1) { + root_idx = img_num; + } + } + + Hs[root_idx] = cv::Mat::eye(3, 3, CV_64FC1); + visited[root_idx] = true; + + std::function calc_recurse = [&] (size_t cur_idx) { + if (visited[cur_idx]) { + return; + } + const size_t parent_idx = parent[cur_idx]; + if (!visited[parent_idx]) { + calc_recurse(parent_idx); + } + Hs[cur_idx] = homography_builder(imgs[cur_idx], imgs[parent_idx]) * Hs[parent_idx]; + visited[cur_idx] = true; + }; + + for (size_t img_num = 0; img_num < n_images; img_num++) { + if (img_num != root_idx && !visited[img_num]) { + calc_recurse(img_num); + } + } } bbox2 bbox; diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index a0374c5a..2f63de4b 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -64,17 +64,18 @@ namespace { double Z = Xs[i][2]; double W = 1.0; - A.row(i * 2 + 0) << 0, 0, 0, 0, -w*X, -w*Y, -w*Z, -w*W, y*X, y*Y, y*Z, y*W; - A.row(i * 2 + 1) << w*X, w*Y, w*Z, w*W, 0, 0, 0, 0, -x*X, -x*Y, -x*Z, -x*W; + A.row(i * 2) << 0., 0., 0., 0., -w * X, -w * Y, -w * Z, -w * W, y * X, y * Y, y * Z, y * W; + A.row(i * 2 + 1) << w * X, w * Y, w * Z, w * W, 0., 0., 0., 0., -x * X, -x * Y, -x * Z, -x * W; } + matrix34d result; + Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::VectorXd null_space = svd.matrixV().col(11); - - matrix34d result; - for (int i = 0; i < 12; ++i) { - result(i / 4, i % 4) = null_space(i); - } + + result(0, 0) = null_space[0]; result(0, 1) = null_space[1]; result(0, 2) = null_space[2]; result(0, 3) = null_space[3]; + result(1, 0) = null_space[4]; result(1, 1) = null_space[5]; result(1, 2) = null_space[6]; result(1, 3) = null_space[7]; + result(2, 0) = null_space[8]; result(2, 1) = null_space[9]; result(2, 2) = null_space[10]; result(2, 3) = null_space[11]; return canonicalizeP(result); } @@ -90,11 +91,15 @@ namespace { // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters // будет отличаться от случая с гомографией - const int n_trials = 10000; + const int n_points_fit = 6; + const double singlePointInlierProbability = 0.1893, successProbability = 0.99; // maybe singlePointInlierProbability should be set to lower value + // const int n_trials = (int) (log(1 - successProbability) / log(1 - pow(singlePointInlierProbability, n_points_fit))); + const int n_trials = 100000; // from task03 + // const int n_trials = 10000; // from task04 const double threshold_px = 3; - const int n_samples = 6; + const int n_samples = n_points_fit; uint64_t seed = 1; int best_support = 0; @@ -115,11 +120,11 @@ namespace { int support = 0; for (int i = 0; i < n_points; ++i) { - cv::Vec3d pt = calib.project(P * cv::Vec4d(X[i][0], X[i][1], X[i][2], 1.0)); - if (pt[2] == 0) { + cv::Vec3d px_uniqform = calib.project(P * cv::Vec4d(X[i][0], X[i][1], X[i][2], 1.)); // спроецировать 3Д точку в пиксель с использованием P и calib; + if (px_uniqform[2] == 0) { continue; } - cv::Vec2d px = {pt[0] / pt[2], pt[1] / pt[2]}; + cv::Vec2d px = {px_uniqform[0] / px_uniqform[2], px_uniqform[1] / px_uniqform[2]}; if (cv::norm(px - x[i]) < threshold_px) { ++support; } diff --git a/src/phg/sfm/sfm_utils.cpp b/src/phg/sfm/sfm_utils.cpp index d4a4b0b1..72bcdbfa 100644 --- a/src/phg/sfm/sfm_utils.cpp +++ b/src/phg/sfm/sfm_utils.cpp @@ -41,8 +41,17 @@ void phg::randomSample(std::vector &dst, int max_id, int sample_size, uint6 bool phg::epipolarTest(const cv::Vec2d &pt0, const cv::Vec2d &pt1, const cv::Matx33d &F, double t) { - cv::Vec3d l1 = F * cv::Vec3d(pt0[0], pt0[1], 1.0); - double s1 = l1[0] * l1[0] + l1[1] * l1[1]; - double d1 = l1[0] * pt1[0] + l1[1] * pt1[1] + l1[2]; - return d1*d1 < t*t * s1; + // from task04 + // cv::Vec3d l1 = F * cv::Vec3d(pt0[0], pt0[1], 1.0); + // double s1 = l1[0] * l1[0] + l1[1] * l1[1]; + // double d1 = l1[0] * pt1[0] + l1[1] * pt1[1] + l1[2]; + // return d1*d1 < t*t * s1; + + cv::Vec3d pt0ext(pt0[0], pt0[1], 1.), pt1ext(pt1[0], pt1[1], 1.); + + cv::Vec3d epipolarLine = F * pt0ext; + double inner_product = pt1ext.ddot(epipolarLine); + double epipolarLineLength = std::sqrt(epipolarLine[0] * epipolarLine[0] + epipolarLine[1] * epipolarLine[1]); + + return std::abs(inner_product) < t * epipolarLineLength; } diff --git a/src/phg/sfm/triangulation.cpp b/src/phg/sfm/triangulation.cpp index 7492480e..04214242 100644 --- a/src/phg/sfm/triangulation.cpp +++ b/src/phg/sfm/triangulation.cpp @@ -8,38 +8,67 @@ // (см. Hartley & Zisserman p.312) cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int count) { - using mat = Eigen::MatrixXd; - using vec = Eigen::VectorXd; + // from task04 + // using mat = Eigen::MatrixXd; + // using vec = Eigen::VectorXd; - mat A(2 * count, 4); + // mat A(2 * count, 4); - for (int i = 0; i < count; ++i) { + // for (int i = 0; i < count; ++i) { - const matrix34d &P = Ps[i]; - const vector3d &m = ms[i]; + // const matrix34d &P = Ps[i]; + // const vector3d &m = ms[i]; - double x = m[0]; - double y = m[1]; - double z = m[2]; + // double x = m[0]; + // double y = m[1]; + // double z = m[2]; - auto p0 = P.row(0); - auto p1 = P.row(1); - auto p2 = P.row(2); + // auto p0 = P.row(0); + // auto p1 = P.row(1); + // auto p2 = P.row(2); - auto row0 = x * p2 - z * p0; - auto row1 = y * p2 - z * p1; + // auto row0 = x * p2 - z * p0; + // auto row1 = y * p2 - z * p1; - A.row(i * 2 + 0) << row0(0), row0(1), row0(2), row0(3); - A.row(i * 2 + 1) << row1(0), row1(1), row1(2), row1(3); - } + // A.row(i * 2 + 0) << row0(0), row0(1), row0(2), row0(3); + // A.row(i * 2 + 1) << row1(0), row1(1), row1(2), row1(3); + // } - Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); - Eigen::VectorXd null_space = svd.matrixV().col(3); + // Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + // Eigen::VectorXd null_space = svd.matrixV().col(3); + + // vector4d result; + // for (int i = 0; i < 4; ++i) { + // result(i) = null_space(i); + // } + + // return result; + + // составление однородной системы + SVD + // без подвохов + Eigen::MatrixXd A(4, 4); + A.row(0) << ms[0][0] * Ps[0](2, 0) - Ps[0](0, 0), + ms[0][0] * Ps[0](2, 1) - Ps[0](0, 1), + ms[0][0] * Ps[0](2, 2) - Ps[0](0, 2), + ms[0][0] * Ps[0](2, 3) - Ps[0](0, 3); - vector4d result; - for (int i = 0; i < 4; ++i) { - result(i) = null_space(i); - } + A.row(1) << ms[0][1] * Ps[0](2, 0) - Ps[0](1, 0), + ms[0][1] * Ps[0](2, 1) - Ps[0](1, 1), + ms[0][1] * Ps[0](2, 2) - Ps[0](1, 2), + ms[0][1] * Ps[0](2, 3) - Ps[0](1, 3); + + A.row(2) << ms[1][0] * Ps[1](2, 0) - Ps[1](0, 0), + ms[1][0] * Ps[1](2, 1) - Ps[1](0, 1), + ms[1][0] * Ps[1](2, 2) - Ps[1](0, 2), + ms[1][0] * Ps[1](2, 3) - Ps[1](0, 3); + + A.row(3) << ms[1][1] * Ps[1](2, 0) - Ps[1](1, 0), + ms[1][1] * Ps[1](2, 1) - Ps[1](1, 1), + ms[1][1] * Ps[1](2, 2) - Ps[1](1, 2), + ms[1][1] * Ps[1](2, 3) - Ps[1](1, 3); + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + auto null_space = svd.matrixV().col(3); - return result; + return {null_space[0], null_space[1], null_space[2], null_space[3]}; } diff --git a/src/phg/sift/sift.cpp b/src/phg/sift/sift.cpp index 72047711..a7ad7cc4 100755 --- a/src/phg/sift/sift.cpp +++ b/src/phg/sift/sift.cpp @@ -108,14 +108,18 @@ std::vector phg::buildOctaves(const cv::Mat& img, const phg:: // можно подумать, как сделать эффективнее - для построения n+1 слоя доблюревать уже поблюренный n-ый слой, так чтобы в итоге получилась такая же сигма // это будет немного быстрее, тк нужно более маленькое ядро свертки на каждый шаг for (int i = 1; i < n_layers; i++) { - // TODO double sigma_layer = sigma0 * корень из двух нужной степени, чтобы при i==s получали удвоение базового блюра; + double sigma_layer = sigma0 * std::pow((double) 2, (double) i / s); + sigma_layer = std::sqrt(sigma_layer * sigma_layer - sigma0 * sigma0); + cv::GaussianBlur(oct.layers[0], oct.layers[i], cv::Size(), sigma_layer, sigma_layer); + // double sigma_layer = sigma0 * корень из двух нужной степени, чтобы при i==s получали удвоение базового блюра; // // вычтем sigma0 чтобы размыть ровно до нужной суммарной сигмы - // TODO sigma_layer = ... (вычитаем как в sigma base); + // sigma_layer = ... (вычитаем как в sigma base); // cv::GaussianBlur(oct.layers[0], oct.layers[i], cv::Size(), sigma_layer, sigma_layer); } // подготавливаем базовый слой для следующей октавы if (o + 1 < n_octaves) { + cv::resize(oct.layers[s], base, cv::Size(), 0.5f, 0.5f, cv::INTER_NEAREST); // используется в opencv, формула для пересчета ключевых точек: pt_upscaled = 2^o * pt_downscaled // TODO cv::resize(даунскейлим текущий слой в два раза, без интерполяции, просто сабсепмлинг); @@ -137,8 +141,10 @@ std::vector phg::buildDoG(const std::vector phg::findScaleSpaceExtrema(const std::vector phg::findScaleSpaceExtrema(const std::vector(yi, xi + 1) + cL.at(yi, xi - 1) - 2.f * resp_center; -// float dyy = TODO; -// float dss = TODO; -// -// float dxy = (cL.at(yi + 1, xi + 1) - cL.at(yi + 1, xi - 1) - cL.at(yi - 1, xi + 1) + cL.at(yi - 1, xi - 1)) * 0.25f; -// float dxs = TODO; -// float dys = TODO; + dxx = cL.at(yi, xi + 1) + cL.at(yi, xi - 1) - 2.f * resp_center; + dyy = cL.at(yi + 1, xi) + cL.at(yi - 1, xi) - 2.f * resp_center; + dss = pL.at(yi, xi) + nL.at(yi, xi) - 2.f * resp_center; + + dxy = (cL.at(yi + 1, xi + 1) - cL.at(yi + 1, xi - 1) - cL.at(yi - 1, xi + 1) + cL.at(yi - 1, xi - 1)) * 0.25f; + dxs = (nL.at(yi, xi + 1) - nL.at(yi, xi - 1) - pL.at(yi, xi + 1) + pL.at(yi, xi - 1)) * 0.25f; + dys = (nL.at(yi + 1, xi) - nL.at(yi - 1, xi) - pL.at(yi + 1, xi) + pL.at(yi - 1, xi)) * 0.25f; cv::Matx33f H(dxx, dxy, dxs, dxy, dyy, dys, dxs, dys, dss); @@ -273,10 +302,10 @@ std::vector phg::findScaleSpaceExtrema(const std::vector phg::findScaleSpaceExtrema(const std::vector ((double) r + 1) * ((double) r + 1) / r) + break; } // скейлим координаты точек обратно до родных размеров картинки @@ -379,39 +408,39 @@ std::vector phg::computeOrientations(const std::vector(py, px + 1) - img.at(py, px - 1); -// float gy = img.at(py + 1, px) - img.at(py - 1, px); -// -// float mag = TODO; -// float angle = std::atan2(TODO); // [-pi, pi] -// -// float angle_deg = angle * 180.f / (float) CV_PI; -// if (angle_deg < 0.f) angle_deg += 360.f; -// -// // гауссово взвешивание голоса точки с затуханием к краям -// float weight = std::exp(-(TODO) / (2.f * sigma_win * sigma_win)); -// if (!params.enable_orientation_gaussian_weighting) { -// weight = 1.f; -// } -// -// // голосуем в гистограмме направлений. находим два ближайших бина и гладко распределяем голос между ними -// // в таком случае, голос попавший близко к границе между бинами, проголосует поровну за оба бина -// float bin = TODO; -// if (bin >= n_bins) bin -= n_bins; -// int bin0 = (int) bin; -// int bin1 = (bin0 + 1) % n_bins; -// -// float frac = bin - bin0; -// if (!params.enable_orientation_bin_interpolation) { -// frac = 0.f; -// } -// -// histogram[bin0] += TODO; -// histogram[bin1] += TODO; + int px = xi + dx; + int py = yi + dy; + + // градиент + float gx = img.at(py, px + 1) - img.at(py, px - 1); + float gy = img.at(py + 1, px) - img.at(py - 1, px); + + float mag = std::sqrt(gx * gx + gy * gy); + float angle = std::atan2(gy, gx); // [-pi, pi] + + float angle_deg = angle * 180.f / (float) CV_PI; + if (angle_deg < 0.f) angle_deg += 360.f; + + // гауссово взвешивание голоса точки с затуханием к краям + float weight = std::exp(-(dx * dx + dy * dy) / (2.f * sigma_win * sigma_win)); + if (!params.enable_orientation_gaussian_weighting) { + weight = 1.f; + } + + // голосуем в гистограмме направлений. находим два ближайших бина и гладко распределяем голос между ними + // в таком случае, голос попавший близко к границе между бинами, проголосует поровну за оба бина + float bin = angle_deg / 360.f * n_bins; + if (bin >= n_bins) bin -= n_bins; + int bin0 = (int) bin; + int bin1 = (bin0 + 1) % n_bins; + + float frac = bin - bin0; + if (!params.enable_orientation_bin_interpolation) { + frac = 0.f; + } + + histogram[bin0] += weight * mag * (1.f - frac); + histogram[bin1] += weight * mag * frac; } } @@ -450,20 +479,20 @@ std::vector phg::computeOrientations(const std::vector a = (left + right - 2 * center) / 2 // f(1) - f(-1) = 2b -> b = (right - left) / 2 -// float offset = TODO; -// if (!params.enable_orientation_subpixel_localization) { -// offset = 0.f; -// } -// -// float bin_real = i + offset; -// if (bin_real < 0.f) bin_real += n_bins; -// if (bin_real >= n_bins) bin_real -= n_bins; -// -// float angle = bin_real * 360.f / n_bins; -// -// cv::KeyPoint new_kp = kp; -// new_kp.angle = angle; -// oriented_kpts.push_back(new_kp); + float offset = (left - right) / (left + right - 2.f * center) / 2.f; + if (!params.enable_orientation_subpixel_localization) { + offset = 0.f; + } + + float bin_real = i + offset; + if (bin_real < 0.f) bin_real += n_bins; + if (bin_real >= n_bins) bin_real -= n_bins; + + float angle = bin_real * 360.f / n_bins; + + cv::KeyPoint new_kp = kp; + new_kp.angle = angle; + oriented_kpts.push_back(new_kp); } } } @@ -574,11 +603,11 @@ std::pair> phg::computeDescriptors(const std: bin_o -= n_orient_bins; // семплы вблизи края патча взвешиваем с меньшим весом -// float weight = std::exp(-(TODO) / (2.f * sigma_desc * sigma_desc)); -// if (!params.enable_descriptor_gaussian_weighting) { -// weight = 1.f; -// } -// float weighted_mag = mag * weight; + float weight = std::exp(-(rot_x * rot_x + rot_y * rot_y) / (2.f * sigma_desc * sigma_desc)); + if (!params.enable_descriptor_gaussian_weighting) { + weight = 1.f; + } + float weighted_mag = mag * weight; if (params.enable_descriptor_bin_interpolation) { // размажем вклад weighted_mag по пространственным бинам и по бинам гистограммок трилинейной интерполяцией @@ -609,8 +638,8 @@ std::pair> phg::computeDescriptors(const std: io += n_orient_bins; float wo = (dio == 0) ? (1.f - fo) : fo; -// int idx = TODO; -// desc[idx] += TODO; + int idx = (iy * n_spatial_bins + ix) * n_orient_bins + io; + desc[idx] += wy * wx * wo * weighted_mag; } } } @@ -620,9 +649,8 @@ std::pair> phg::computeDescriptors(const std: int io_nearest = (int)std::round(bin_o) % n_orient_bins; if (ix_nearest >= 0 && ix_nearest < n_spatial_bins && iy_nearest >= 0 && iy_nearest < n_spatial_bins) { - // TODO uncomment -// int idx = (iy_nearest * n_spatial_bins + ix_nearest) * n_orient_bins + io_nearest; -// desc[idx] += weighted_mag; + int idx = (iy_nearest * n_spatial_bins + ix_nearest) * n_orient_bins + io_nearest; + desc[idx] += weighted_mag; } } } diff --git a/tests/test_ceres_solver.cpp b/tests/test_ceres_solver.cpp index 898e2cdb..f802eea1 100644 --- a/tests/test_ceres_solver.cpp +++ b/tests/test_ceres_solver.cpp @@ -66,8 +66,10 @@ TEST (CeresSolver, HelloWorld1) { std::cout << "x: " << initial_x << " -> " << cur_x << std::endl; 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 - + // 1: почему результирующая производная не ноль? мы ведь должны были сойтись в минимуме функции 0.5*(10-x)^2 + // Потому что мы вычисляем функцию в CostFunctor1 не в виде 0.5*(10-x)^2, а в виде 10 - x => f'=-1, а класс jet позволяет вычислять табличные производные, а не разностные. + // Судя по всему ceres выводит якобиан не общего лосса, а конкретно нашей функции потерь: dL/dx = dL/dF * dF/dx, + // где L -- TrivialLoss, F -- CostFunctor1, y = L(F(x, r)) ASSERT_NEAR(cur_x, 10.0, 1e-6); } @@ -132,7 +134,9 @@ 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]; + T dz = queryPoint[2] - center[2]; + // z = a*dx*dx + b*dy*dy + c0 + residual[0] = ceres::abs(a*dx*dx + b*dy*dy + center[2] - queryPoint[2]); return true; } protected: @@ -158,10 +162,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]; @@ -225,8 +228,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); + // 3: раскомментируйте, почему он находит не то что ожидалось? // либо мы набагали в коде, либо в аналитическом поиске правильного ответа на бумажке (проверьте вычисления на бумажке) // если бага в коде, то первые подозреваемые - две функции невязки (только там есть содержательный код) // заметьте что у найденного ответа ошибка только по одной из осей @@ -234,6 +237,8 @@ TEST (CeresSolver, HelloWorld2) { // отладьте те функции невязки которые по-хорошему не должны соглашаться на такой ответ - поставьте просто точку остановки чуть выше, там где мы проверяли // что невязка найденного решения - нулевая, и найдите где вдруг ваше ожидание большой невязки для этого ответа сталкивается с суровой реальностью баги в коде // которая приводит к нулевой невязке + + // ошибка была в функции невязки параболоида -- по сути исходной функцией невязки мы тянули точку к (x_1, y_1, 0) -- эта точка принадлежит параболоиду, но не принадлежит прямой } // TODO 4: если любопытно и хватит времени - можете попросить ceres-solver посчитать якобианы в некоторых точках подобно тому как это делалось в конце теста HelloWorld1 @@ -258,9 +263,9 @@ 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): + // 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 +353,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) { @@ -372,29 +376,45 @@ void evaluateLineFitting(double sigma, double &fitted_inliers_fraction, double & 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 *= 100.0; // ослабляем порог если есть выбросы и мы к ним не устойчивы (не робастны за счет loss-функции (функции потерь) Huber-а) } + double scale = ideal_line[2] / line_params[2]; + double normed_line_params[3] = {line_params[0] * scale, line_params[1] * scale, line_params[2] * scale}; + std::cout << "Normed line: (a=" << normed_line_params[0] << ", b=" << normed_line_params[1] << ", c=" << normed_line_params[2] << ")" << std::endl; + for (int d = 0; d < 3; ++d) { -// ASSERT_NEAR(line_params[d], ideal_line[d], threshold); - // TODO 7 расскоментируйте сверку найденной прямой и эталонной + ASSERT_NEAR(normed_line_params[d], ideal_line[d], threshold); + // ASSERT_NEAR(line_params[d], ideal_line[d], threshold); // почему они расходятся? как это можно решить? придумайте хотя бы два способа: // - пост-обработкой - как-то поправив параметры прямой перед сверкой (при этом не меняя ее положение в пространстве) + // 7 поправьте тест так или иначе (хотя бы пост-процессингом) + + // по сути скейл зашит в отношении param_ideal / param_our, домножаем на это выражение все координаты => но приходится увеличивать трешолд. + // Далее у нас есть степень свободы в выборе коэффициента, на который будем нормировать + // Эмперически лучше всего подошел коэффициент c, что возможно объяснимо тем, что он обладает наибольшим модулем // - формулировкой задачи - можно сформулировать для ceres-solver задчау так чтобы избавиться от неоднозначности убрав степень свободы, т.е. описав прямую как-то иначе, как? - // TODO 7 поправьте тест так или иначе (хотя бы пост-процессингом) + // задание прямой двумя параметрами + // 1) cos(a) * x + sin(a) * y = b -- требует нормировки; + // 2) 0.5 * x + b * y + c = 0 -- не требует нормировки, но необходимо знать один из коэффициентов идеальной прямой. } // Оцениваем качество идеальной прямой 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 - + std::cout << inliers_fraction << outliers_fraction << "\n"; + ASSERT_GT(inliers_fraction, 0.99 * (1. - outliers_fraction)); // 8 раскоментируйте, почему эта проверка падает? как поправить? + // 0.99 * max_possible_inliers_fraction = 0.99 * (1. - outliers_fraction) + ASSERT_LT(mse, 1.1 * sigma * sigma); // 9 раскомментируйте, почему проверка падает? на каких тестах она падает, на каких проходит? попробуйте отладить рассчет mse_inliers_distance в evaluateLine + // abs in 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); + // 10 раскоментируйте обе проверки, почему они падают? в каких тестах? поправьте (в т.ч. подобно тому как было с ослаблением порога выше) + ASSERT_GT(inliers_fraction, 0.99 * (1. - outliers_fraction)); + // 0.99 * max_possible_inliers_fraction = 0.99 * (1. - outliers_fraction) + ASSERT_LT(mse, 1.1 * sigma * sigma); + // abs in evaluateLine } } @@ -405,7 +425,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_matching.cpp b/tests/test_matching.cpp index 4de5b716..6a0e4661 100644 --- a/tests/test_matching.cpp +++ b/tests/test_matching.cpp @@ -19,8 +19,8 @@ // TODO enable both toggles for testing custom detector & matcher -#define ENABLE_MY_DESCRIPTOR 0 -#define ENABLE_MY_MATCHING 0 +#define ENABLE_MY_DESCRIPTOR 1 +#define ENABLE_MY_MATCHING 1 #define ENABLE_GPU_BRUTEFORCE_MATCHER 0 // TODO disable for local testing but do not commit @@ -143,6 +143,7 @@ namespace { phg::DescriptorMatcher::filterMatchesClusters(good_matches, keypoints1, keypoints2, tmp); std::swap(tmp, good_matches); } + #else { std::vector tmp; @@ -156,6 +157,7 @@ namespace { points1.push_back(keypoints1[match.queryIdx].pt); points2.push_back(keypoints2[match.trainIdx].pt); } + #if ENABLE_MY_MATCHING cv::Mat H = phg::findHomography(points1, points2); #else @@ -560,7 +562,7 @@ TEST (MATCHING, SimpleMatching) { EXPECT_LT(time_my, 1.5 * time_cv); EXPECT_LT(time_my, 0.1 * time_bruteforce); -#if ENABLE_GPU_BRUTEFORCE_MATCHER +#if ENABLE_GPU_BRUTEFORCE_MATCHER && !SERVER_TESTING EXPECT_LT(time_bruteforce_gpu, time_bruteforce); #endif @@ -839,4 +841,4 @@ TEST (STITCHING, Orthophoto) { std::cout << "n stable ortho kpts: : " << score << std::endl; EXPECT_GT(score, 7500); #endif -} \ No newline at end of file +} diff --git a/tests/test_mesh_min_cut.cpp b/tests/test_mesh_min_cut.cpp index 38d29a0a..5b33cf82 100644 --- a/tests/test_mesh_min_cut.cpp +++ b/tests/test_mesh_min_cut.cpp @@ -32,7 +32,7 @@ // #define DATASET_DOWNSCALE 8 //________________________________________________________________________________ -#define CAMERAS_LIMIT 5 +#define CAMERAS_LIMIT 32 void checkFloat32ImageReadWrite() { diff --git a/tests/test_sfm.cpp b/tests/test_sfm.cpp index 4229b86b..48cc1588 100644 --- a/tests/test_sfm.cpp +++ b/tests/test_sfm.cpp @@ -18,7 +18,7 @@ #include "utils/test_utils.h" -#define ENABLE_MY_SFM 0 +#define ENABLE_MY_SFM 1 namespace { diff --git a/tests/test_sfm_ba.cpp b/tests/test_sfm_ba.cpp index 0490c45e..0ec56d45 100644 --- a/tests/test_sfm_ba.cpp +++ b/tests/test_sfm_ba.cpp @@ -21,11 +21,12 @@ #include #include -// TODO включите Bundle Adjustment (но из любопытства посмотрите как ведет себя реконструкция без BA например для saharov32 без BA) -#define ENABLE_BA 0 +// включите Bundle Adjustment (но из любопытства посмотрите как ведет себя реконструкция без BA например для saharov32 без BA) +#define ENABLE_BA 1 -// TODO когда заработает при малом количестве фотографий - увеличьте это ограничение до 100 чтобы попробовать обработать все фотографии (если же успешно будут отрабаывать только N фотографий - отправьте PR выставив здесь это N) -#define NIMGS_LIMIT 10 // сколько фотографий обрабатывать (можно выставить меньше чтобы ускорить экспериментирование, или в случае если весь датасет не выравнивается) +// когда заработает при малом количестве фотографий - увеличьте это ограничение до 100 чтобы попробовать обработать все фотографии (если же успешно будут отрабаывать только N фотографий - отправьте PR выставив здесь это N) +// sakharov OK, herzjesu 22 -- works, 23-fails +#define NIMGS_LIMIT 100 // сколько фотографий обрабатывать (можно выставить меньше чтобы ускорить экспериментирование, или в случае если весь датасет не выравнивается) #define INTRINSICS_CALIBRATION_MIN_IMGS 5 // начиная со скольки камер начинать оптимизировать внутренние параметры камеры (фокальную длину и т.п.) - из соображений что "пока камер мало - наблюдений может быть недостаточно чтобы не сойтись к ложной внутренней модели камеры" #define ENABLE_INSTRINSICS_K1_K2 1 // TODO учитывать ли радиальную дисторсию - коэффициенты k1, k2 попробуйте с ним и и без saharov32, заметна ли разница? @@ -51,8 +52,10 @@ //#define DATASET_DIR "herzjesu25" //#define DATASET_DOWNSCALE 2 // для ускорения SIFT //#define DATASET_F (2761.5 / DATASET_DOWNSCALE) // see herzjesu25/K.txt -// TODO почему фокальная длина меняется от того что мы уменьшаем картинку? почему именно в такой пропорции? может надо домножать? или делить на downscale^2 ? - +// почему фокальная длина меняется от того что мы уменьшаем картинку? почему именно в такой пропорции? может надо домножать? или делить на downscale^2 ? +// x_p = x_r * f / z <=> x_r = x_p / f * z . +// При даунсемпле картинки в s раз x_p1 = x_p / s => x_r1 = x_p1 / f * z = x_p / s / f * z = x_p / (f1 * s) * z +// Так как x_r1 = x_r => f1 * s = f => f1 = f / s // но temple47 - не вышло, я не разобрался в чем с ним проблема, может быть слишком мало точек, может критерии фильтрации выкидышей для него слишком строги //#define DATASET_DIR "temple47" //#define DATASET_DOWNSCALE 1 @@ -382,30 +385,50 @@ 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 чтобы ceres-solver мог под него подставить как Jet (очень рекомендую посмотреть Jet.h - как класная статья из википедии!), так и double // translation[3] - сдвиг в локальную систему координат камеры + T pt_T[3] = { + point_global[0] - camera_extrinsics[0], + point_global[1] - camera_extrinsics[1], + point_global[2] - camera_extrinsics[2] + }; + T pt_R[3]; // rotation[3] - angle-axis rotation, поворачиваем точку point->p (чтобы перейти в локальную систему координат камеры) + ceres::AngleAxisRotatePoint(camera_extrinsics + 3, pt_T, pt_R); // подробнее см. https://en.wikipedia.org/wiki/Axis%E2%80%93angle_representation // (P.S. у камеры всмысле вращения три степени свободы) // Проецируем точку на фокальную плоскость матрицы (т.е. плоскость Z=фокальная длина) - + // вроде что бы проекция была в фокальной плоскости нужно в этом месте домножать на фокусное расстояние, но это требуется ниже + T pt_P[2] = { + pt_R[0] / pt_R[2], + pt_R[1] / pt_R[2] + }; #if ENABLE_INSTRINSICS_K1_K2 // k1, k2 - коэффициенты радиального искажения (radial distortion) + T r_squared = pt_P[0] * pt_P[0] + pt_P[1] * pt_P[1]; + T r_fourth = r_squared * r_squared; + T radial_coef = (1. + camera_intrinsics[0] * r_squared + camera_intrinsics[1] * r_fourth); + pt_P[0] *= radial_coef; + pt_P[1] *= radial_coef; #endif // Домножаем на f, тем самым переводя в пиксели - + pt_P[0] *= camera_intrinsics[2]; + pt_P[1] *= camera_intrinsics[2]; // Из координат когда точка (0, 0) - центр оптической оси // Переходим в координаты когда точка (0, 0) - левый верхний угол картинки // cx, cy - координаты центра оптической оси (обычно это центр картинки, но часто он чуть смещен) - + pt_P[0] += camera_intrinsics[3]; + pt_P[1] += camera_intrinsics[4]; // Теперь по спроецированным координатам не забудьте посчитать невязку репроекции + residuals[0] = pt_P[0] - observed_x; + residuals[1] = pt_P[1] - observed_y; return true; - // TODO сверьте эту функцию с вашей реализацией проекции в src/phg/core/calibration.cpp (они должны совпадать) + // сверьте эту функцию с вашей реализацией проекции в src/phg/core/calibration.cpp (они должны совпадать) } protected: double observed_x; @@ -435,8 +458,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 +604,13 @@ 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 +683,21 @@ void runBA(std::vector &tie_points, } if (ENABLE_OUTLIERS_FILTRATION_COLINEAR && ENABLE_BA) { - // TODO выполните проверку случая когда два луча почти параллельны, чтобы не было странных точек улетающих на бесконечность (например чтобы угол был хотя бы 2.5 градуса) - // should_be_disabled = true; + // выполните проверку случая когда два луча почти параллельны, чтобы не было странных точек улетающих на бесконечность (например чтобы угол был хотя бы 2.5 градуса) + double min_angle_degrees = 2.5; + double min_angle_rad = (min_angle_degrees * CV_PI / 180.0); + double max_cos = std::cos(min_angle_rad); + vector3d track_in_current_camera = cv::normalize(track_point - camera_origin); + + int second_camera_id = track.img_kpt_pairs[ci].second; + double* second_camera_extrinsics = cameras_extrinsics.data() + CAMERA_EXTRINSICS_NPARAMS * second_camera_id; + matrix3d second_R; vector3d second_camera_origin; + phg::decomposeUndistortedPMatrix(second_R, second_camera_origin, cameras[second_camera_id]); + vector3d track_in_second_camera = cv::normalize(track_point - second_camera_origin); + + if (std::abs(track_in_current_camera.ddot(track_in_second_camera)) > max_cos) { + should_be_disabled = true; + } } { diff --git a/tests/test_sift.cpp b/tests/test_sift.cpp index cf3bd7df..7433f9bb 100755 --- a/tests/test_sift.cpp +++ b/tests/test_sift.cpp @@ -28,7 +28,7 @@ // TODO ENABLE ME // TODO ENABLE ME // TODO ENABLE ME -#define ENABLE_MY_SIFT_TESTING 0 +#define ENABLE_MY_SIFT_TESTING 1 #define DENY_CREATE_REF_DATA 1