diff --git a/src/phg/matching/cl/bruteforce_matcher.cl b/src/phg/matching/cl/bruteforce_matcher.cl index e51a34a..a4369f4 100644 --- a/src/phg/matching/cl/bruteforce_matcher.cl +++ b/src/phg/matching/cl/bruteforce_matcher.cl @@ -25,13 +25,17 @@ __kernel void bruteforce_matcher(__global const float* train, // храним два лучших сопоставления для каждого дескриптора-query: __local uint res_train_idx_local[KEYPOINTS_PER_WG * 2]; __local float res_distance2_local[KEYPOINTS_PER_WG * 2]; // храним квадраты чтобы не считать корень до самого последнего момента + __local float dist2_for_reduction[NDIM]; // заполняем текущие лучшие дистанции большими значениями if (dim_id < KEYPOINTS_PER_WG * 2) { res_distance2_local[dim_id] = FLT_MAX; // полагаемся на то что res_distance2_local размера KEYPOINTS_PER_WG*2==4*2 0) { if (dim_id < step) { - // TODO + dist2_for_reduction[dim_id] += dist2_for_reduction[dim_id + step]; } barrier(CLK_LOCAL_MEM_FENCE); step /= 2; @@ -63,17 +67,20 @@ __kernel void bruteforce_matcher(__global const float* train, #define SECOND_BEST_INDEX 1 // пытаемся улучшить самое лучшее сопоставление для локального дескриптора - if (dist2 <= res_distance2_local[query_local_i * 2 + BEST_INDEX]) { + if (dist2 < res_distance2_local[query_local_i * 2 + BEST_INDEX]) { // не забываем что прошлое лучшее сопоставление теперь стало вторым по лучшевизне (на данный момент) res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX] = res_distance2_local[query_local_i * 2 + BEST_INDEX]; res_train_idx_local[query_local_i * 2 + SECOND_BEST_INDEX] = res_train_idx_local[query_local_i * 2 + BEST_INDEX]; - // TODO заменяем нашим (dist2, train_idx) самое лучшее сопоставление для локального дескриптора - } else if (dist2 <= res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX]) { // может мы улучшили хотя бы второе по лучшевизне сопоставление? - // TODO заменяем второе по лучшевизне сопоставление для локального дескриптора + res_distance2_local[query_local_i * 2 + BEST_INDEX] = dist2; + res_train_idx_local[query_local_i * 2 + BEST_INDEX] = train_idx; + } else if (dist2 < res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX]) { // может мы улучшили хотя бы второе по лучшевизне сопоставление? + res_distance2_local[query_local_i * 2 + SECOND_BEST_INDEX] = dist2; + res_train_idx_local[query_local_i * 2 + SECOND_BEST_INDEX] = train_idx; } } } } + barrier(CLK_LOCAL_MEM_FENCE); // итак, мы нашли два лучших сопоставления для наших KEYPOINTS_PER_WG дескрипторов, надо сохрнить эти результаты в глобальную память if (dim_id < KEYPOINTS_PER_WG * 2) { // полагаемся на то что нам надо прогрузить KEYPOINTS_PER_WG*2==4*2 #include "flann_factory.h" +#include + void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector> &matches, std::vector &filtered_matches) { filtered_matches.clear(); - throw std::runtime_error("not implemented yet"); + constexpr float ratio_threshold = 0.75f; + filtered_matches.reserve(matches.size()); + for (const auto &desc_matches : matches) { + if (desc_matches.size() < 2) + continue; + + const cv::DMatch &best = desc_matches[0]; + if (best.distance < ratio_threshold * desc_matches[1].distance) + filtered_matches.push_back(best); + } } @@ -20,7 +31,7 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector filtered_matches.clear(); const size_t total_neighbours = 5; // total number of neighbours to test (including candidate) - const size_t consistent_matches = 3; // minimum number of consistent matches (including candidate) + const size_t consistent_matches = 4; // minimum number of consistent matches (including candidate) const float radius_limit_scale = 2.f; // limit search radius by scaled median const int n_matches = matches.size(); @@ -35,42 +46,58 @@ 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(1); + std::shared_ptr search_params = flannKsTreeSearchParams(std::max(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, (int)total_neighbours - 1); + max_dists2_train[i] = distances2_train.at(i, (int)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 общих элементов + filtered_matches.reserve(matches.size()); + for (int i = 0; i < n_matches; ++i) { + std::unordered_set query_neighbours; + query_neighbours.reserve(total_neighbours); + for (size_t j = 0; j < total_neighbours; ++j) { + if (distances2_query.at(i, j) <= radius2_query) + query_neighbours.insert(indices_query.at(i, j)); + } + + size_t intersection_size = 0; + for (size_t j = 0; j < total_neighbours; ++j) { + if (distances2_train.at(i, j) <= radius2_train && query_neighbours.count(indices_train.at(i, j))) + ++intersection_size; + } + if (intersection_size >= 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 9e9f518..6c2f4b0 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(4); + search_params = flannKsTreeSearchParams(32); } void phg::FlannMatcher::train(const cv::Mat &train_desc) @@ -17,5 +17,29 @@ 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"); + if (!flann_index) { + throw std::runtime_error("FlannMatcher::knnMatch : matcher is not trained"); + } + + if (k != 2) { + throw std::runtime_error("FlannMatcher::knnMatch : only k = 2 supported"); + } + + cv::Mat indices(query_desc.rows, k, CV_32SC1); + cv::Mat dists2(query_desc.rows, k, CV_32FC1); + flann_index->knnSearch(query_desc, indices, dists2, k, *search_params); + + matches.resize(query_desc.rows); + for (int qi = 0; qi < query_desc.rows; ++qi) { + std::vector &dst = matches[qi]; + dst.resize(k); + for (int ki = 0; ki < k; ++ki) { + cv::DMatch match; + match.imgIdx = 0; + match.queryIdx = qi; + match.trainIdx = indices.at(qi, ki); + match.distance = std::sqrt(dists2.at(qi, ki)); + dst[ki] = match; + } + } } diff --git a/src/phg/sfm/ematrix.cpp b/src/phg/sfm/ematrix.cpp index 3bc052b..d34e2f1 100644 --- a/src/phg/sfm/ematrix.cpp +++ b/src/phg/sfm/ematrix.cpp @@ -18,8 +18,10 @@ namespace { copy(Ecv, E); Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); - throw std::runtime_error("not implemented yet"); -// TODO + Eigen::Vector3d s = svd.singularValues(); + double sigma = 0.5 * (s[0] + s[1]); + s << sigma, sigma, 0; + E = svd.matrixU() * s.asDiagonal() * svd.matrixV().transpose(); copy(E, Ecv); } @@ -28,12 +30,11 @@ namespace { cv::Matx33d phg::fmatrix2ematrix(const cv::Matx33d &F, const phg::Calibration &calib0, const phg::Calibration &calib1) { - throw std::runtime_error("not implemented yet"); -// matrix3d E = TODO; -// -// ensureSpectralProperty(E); -// -// return E; + matrix3d E = calib1.K().t() * F * calib0.K(); + + ensureSpectralProperty(E); + + return E; } namespace { @@ -61,21 +62,21 @@ namespace { bool depthTest(const vector2d &m0, const vector2d &m1, const phg::Calibration &calib0, const phg::Calibration &calib1, const matrix34d &P0, const matrix34d &P1) { - throw std::runtime_error("not implemented yet"); -// // скомпенсировать калибровки камер -// vector3d p0 = TODO; -// vector3d p1 = TODO; -// -// vector3d ps[2] = {p0, p1}; -// matrix34d Ps[2] = {P0, P1}; -// -// vector4d X = phg::triangulatePoint(Ps, ps, 2); -// if (X[3] != 0) { -// X /= X[3]; -// } -// -// // точка должна иметь положительную глубину для обеих камер -// return TODO && TODO; + // скомпенсировать калибровки камер + vector3d p0 = calib0.unproject(m0); + vector3d p1 = calib1.unproject(m1); + + vector3d ps[2] = {p0, p1}; + matrix34d Ps[2] = {P0, P1}; + + vector4d X = phg::triangulatePoint(Ps, ps, 2); + if (X[3] != 0) { + X /= X[3]; + } + + // точка должна иметь положительную глубину для обеих камер + vector3d depth0 = P0 * X, depth1 = P1 * X; + return depth0[2] > 0 && depth1[2] > 0; } } @@ -88,80 +89,82 @@ namespace { // первичное разложение существенной матрицы (а из него, взаимное расположение камер) для последующего уточнения методом нелинейной оптимизации void phg::decomposeEMatrix(cv::Matx34d &P0, cv::Matx34d &P1, const cv::Matx33d &Ecv, const std::vector &m0, const std::vector &m1, const Calibration &calib0, const Calibration &calib1) { - throw std::runtime_error("not implemented yet"); -// if (m0.size() != m1.size()) { -// throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()"); -// } -// -// using mat = Eigen::MatrixXd; -// using vec = Eigen::VectorXd; -// -// mat E; -// copy(Ecv, E); -// -// // (см. Hartley & Zisserman p.258) -// -// Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); -// -// mat U = svd.matrixU(); -// vec s = svd.singularValues(); -// mat V = svd.matrixV(); -// -// // U, V must be rotation matrices, not just orthogonal -// if (U.determinant() < 0) U = -U; -// if (V.determinant() < 0) V = -V; -// -// std::cout << "U:\n" << U << std::endl; -// std::cout << "s:\n" << s << std::endl; -// std::cout << "V:\n" << V << std::endl; -// -// -// mat R0 = TODO; -// mat R1 = TODO; -// -// std::cout << "R0:\n" << R0 << std::endl; -// std::cout << "R1:\n" << R1 << std::endl; -// -// vec t0 = TODO; -// vec t1 = TODO; -// -// std::cout << "t0:\n" << t0 << std::endl; -// -// P0 = matrix34d::eye(); -// -// // 4 possible solutions -// matrix34d P10 = composeP(R0, t0); -// matrix34d P11 = composeP(R0, t1); -// matrix34d P12 = composeP(R1, t0); -// matrix34d P13 = composeP(R1, t1); -// matrix34d P1s[4] = {P10, P11, P12, P13}; -// -// // need to select best of 4 solutions: 3d points should be in front of cameras (positive depths) -// int best_count = 0; -// int best_idx = -1; -// for (int i = 0; i < 4; ++i) { -// int count = 0; -// for (int j = 0; j < (int) m0.size(); ++j) { -// if (depthTest(m0[j], m1[j], calib0, calib1, P0, P1s[i])) { -// ++count; -// } -// } -// std::cout << "decomposeEMatrix: count: " << count << std::endl; -// if (count > best_count) { -// best_count = count; -// best_idx = i; -// } -// } -// -// if (best_count == 0) { -// throw std::runtime_error("decomposeEMatrix : can't decompose ematrix"); -// } -// -// P1 = P1s[best_idx]; -// -// std::cout << "best idx: " << best_idx << std::endl; -// std::cout << "P0: \n" << P0 << std::endl; -// std::cout << "P1: \n" << P1 << std::endl; + if (m0.size() != m1.size()) { + throw std::runtime_error("decomposeEMatrix : m0.size() != m1.size()"); + } + + using mat = Eigen::MatrixXd; + using vec = Eigen::VectorXd; + + mat E; + copy(Ecv, E); + + // (см. Hartley & Zisserman p.258) + + Eigen::JacobiSVD svd(E, Eigen::ComputeFullU | Eigen::ComputeFullV); + + mat U = svd.matrixU(); + vec s = svd.singularValues(); + mat V = svd.matrixV(); + + // U, V must be rotation matrices, not just orthogonal + if (U.determinant() < 0) U = -U; + if (V.determinant() < 0) V = -V; + + std::cout << "U:\n" << U << std::endl; + std::cout << "s:\n" << s << std::endl; + std::cout << "V:\n" << V << std::endl; + + + mat W((Eigen::Matrix3d() << 0, -1., 0, 1., 0, 0, 0, 0, 1.).finished()); + mat R0 = U * W * V.transpose(); + mat R1 = U * W.transpose() * V.transpose(); + if (R0.determinant() < 0) R0 = -R0; + if (R1.determinant() < 0) R1 = -R1; + + std::cout << "R0:\n" << R0 << std::endl; + std::cout << "R1:\n" << R1 << std::endl; + + vec t0 = U.col(2); + vec t1 = -U.col(2); + + std::cout << "t0:\n" << t0 << std::endl; + + P0 = matrix34d::eye(); + + // 4 possible solutions + matrix34d P10 = composeP(R0, t0); + matrix34d P11 = composeP(R0, t1); + matrix34d P12 = composeP(R1, t0); + matrix34d P13 = composeP(R1, t1); + matrix34d P1s[4] = {P10, P11, P12, P13}; + + // need to select best of 4 solutions: 3d points should be in front of cameras (positive depths) + int best_count = 0; + int best_idx = -1; + for (int i = 0; i < 4; ++i) { + int count = 0; + for (int j = 0; j < (int) m0.size(); ++j) { + if (depthTest(m0[j], m1[j], calib0, calib1, P0, P1s[i])) { + ++count; + } + } + std::cout << "decomposeEMatrix: count: " << count << std::endl; + if (count > best_count) { + best_count = count; + best_idx = i; + } + } + + if (best_count == 0) { + throw std::runtime_error("decomposeEMatrix : can't decompose ematrix"); + } + + P1 = P1s[best_idx]; + + std::cout << "best idx: " << best_idx << std::endl; + std::cout << "P0: \n" << P0 << std::endl; + std::cout << "P1: \n" << P1 << std::endl; } void phg::decomposeUndistortedPMatrix(cv::Matx33d &R, cv::Vec3d &O, const cv::Matx34d &P) diff --git a/src/phg/sfm/fmatrix.cpp b/src/phg/sfm/fmatrix.cpp index 5012718..2040b22 100644 --- a/src/phg/sfm/fmatrix.cpp +++ b/src/phg/sfm/fmatrix.cpp @@ -25,49 +25,70 @@ namespace { // (см. Hartley & Zisserman p.279) cv::Matx33d estimateFMatrixDLT(const cv::Vec2d *m0, const cv::Vec2d *m1, int count) { - throw std::runtime_error("not implemented yet"); -// int a_rows = TODO; -// int a_cols = TODO; -// -// Eigen::MatrixXd A(a_rows, a_cols); -// -// for (int i_pair = 0; i_pair < count; ++i_pair) { -// -// double x0 = m0[i_pair][0]; -// double y0 = m0[i_pair][1]; -// -// double x1 = m1[i_pair][0]; -// double y1 = m1[i_pair][1]; -// -//// std::cout << "(" << x0 << ", " << y0 << "), (" << x1 << ", " << y1 << ")" << std::endl; -// -// TODO -// } -// -// Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); -// Eigen::VectorXd null_space = TODO -// -// Eigen::MatrixXd F(3, 3); -// F.row(0) << null_space[0], null_space[1], null_space[2]; -// 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); -// -// TODO -// -// cv::Matx33d Fcv; -// copy(F, Fcv); -// -// return Fcv; + int a_rows = count; + int a_cols = 9; + + Eigen::MatrixXd A(a_rows, a_cols); + + for (int i_pair = 0; i_pair < count; ++i_pair) { + + double x0 = m0[i_pair][0]; + double y0 = m0[i_pair][1]; + + double x1 = m1[i_pair][0]; + double y1 = m1[i_pair][1]; + + A.row(i_pair) << x1 * x0, x1 * y0, x1, y1 * x0, y1 * y0, y1, x0, y0, 1.; + + // std::cout << "(" << x0 << ", " << y0 << "), (" << x1 << ", " << y1 << ")" << std::endl; + + } + + Eigen::JacobiSVD svda(A, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::VectorXd null_space = svda.matrixV().col(8); + + Eigen::MatrixXd F(3, 3); + F.row(0) << null_space[0], null_space[1], null_space[2]; + F.row(1) << null_space[3], null_space[4], null_space[5]; + F.row(2) << null_space[6], null_space[7], null_space[8]; + + Eigen::JacobiSVD svdf(F, Eigen::ComputeFullU | Eigen::ComputeFullV); + Eigen::Vector3d s = svdf.singularValues(); + s[2] = 0; + F = svdf.matrixU() * s.asDiagonal() * svdf.matrixV().transpose(); + + + cv::Matx33d Fcv; + copy(F, Fcv); + + return Fcv; } // Нужно создать матрицу преобразования, которая сдвинет переданное множество точек так, что центр масс перейдет в ноль, а Root Mean Square расстояние до него станет sqrt(2) // (см. Hartley & Zisserman p.107 Why is normalization essential?) cv::Matx33d getNormalizeTransform(const std::vector &m) { - throw std::runtime_error("not implemented yet"); + cv::Vec2d center = {0, 0}; + for (const cv::Vec2d &pt : m) + center += pt; + center *= 1. / (double)m.size(); + + double rms = 0; + for (const cv::Vec2d &pt : m) { + cv::Vec2d dist = pt - center; + rms += dist.dot(dist); + } + rms = std::sqrt(rms / (double)m.size()); + + double scale = 1.; + if (rms > 1e-10) + scale = std::sqrt(2.) / rms; + + return { + scale, 0, -scale * center[0], + 0, scale, -scale * center[1], + 0, 0, 1. + }; } cv::Vec2d transformPoint(const cv::Vec2d &pt, const cv::Matx33d &T) @@ -81,6 +102,22 @@ namespace { return cv::Vec2d(tmp[0] / tmp[2], tmp[1] / tmp[2]); } + cv::Matx33d estimateFMatrixNormalized(const std::vector &m0, const std::vector &m1) + { + if (m0.size() != m1.size()) { + throw std::runtime_error("estimateFMatrixNormalized: m0.size() != m1.size()"); + } + + int size = m0.size(); + cv::Matx33d T0 = getNormalizeTransform(m0), T1 = getNormalizeTransform(m1); + std::vector m0_t(size), m1_t(size); + for (int i = 0; i < size; ++i) { + m0_t[i] = transformPoint(m0[i], T0); + m1_t[i] = transformPoint(m1[i], T1); + } + return T1.t() * estimateFMatrixDLT(m0_t.data(), m1_t.data(), size) * T0; + } + cv::Matx33d estimateFMatrixRANSAC(const std::vector &m0, const std::vector &m1, double threshold_px) { if (m0.size() != m1.size()) { @@ -89,76 +126,73 @@ namespace { const int n_matches = m0.size(); - cv::Matx33d TN0 = getNormalizeTransform(m0); - cv::Matx33d TN1 = getNormalizeTransform(m1); + if (n_matches == 8) + return estimateFMatrixNormalized(m0, m1); - std::vector m0_t(n_matches); - std::vector m1_t(n_matches); - for (int i = 0; i < n_matches; ++i) { - m0_t[i] = transformPoint(m0[i], TN0); - m1_t[i] = transformPoint(m1[i], TN1); + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + // будет отличаться от случая с гомографией + const int n_trials = 200000; + + const int n_samples = 8; + uint64_t seed = 1; + + int best_support = 0; + cv::Matx33d best_F; + + std::vector sample; + for (int i_trial = 0; i_trial < n_trials; ++i_trial) { + phg::randomSample(sample, n_matches, n_samples, &seed); + + cv::Vec2d ms0[n_samples]; + cv::Vec2d ms1[n_samples]; + for (int i = 0; i < n_samples; ++i) { + ms0[i] = m0[sample[i]]; + ms1[i] = m1[sample[i]]; + } + + cv::Matx33d F = estimateFMatrixNormalized(std::vector(ms0, ms0 + n_samples), std::vector(ms1, ms1 + n_samples)); + + int support = 0; + for (int i = 0; i < n_matches; ++i) { + if (phg::epipolarTest(m0[i], m1[i], F, threshold_px) && phg::epipolarTest(m1[i], m0[i], F.t(), threshold_px)) + { + ++support; + } + } + + if (support > best_support) { + best_support = support; + best_F = F; + + std::cout << "estimateFMatrixRANSAC : support: " << best_support << "/" << n_matches << std::endl; + infoF(F); + + if (best_support == n_matches) { + break; + } + } + } + + std::cout << "estimateFMatrixRANSAC : best support: " << best_support << "/" << n_matches << std::endl; + + if (best_support == 0) { + throw std::runtime_error("estimateFMatrixRANSAC : failed to estimate fundamental matrix"); } - { -// Проверьте лог: при повторной нормализации должно найтись почти единичное преобразование - getNormalizeTransform(m0_t); - getNormalizeTransform(m1_t); + std::vector inlier_m0, inlier_m1; + inlier_m0.reserve(best_support); + inlier_m1.reserve(best_support); + for (int i = 0; i < n_matches; ++i) { + if (phg::epipolarTest(m0[i], m1[i], best_F, threshold_px) && phg::epipolarTest(m1[i], m0[i], best_F.t(), threshold_px)) + { + inlier_m0.push_back(m0[i]); + inlier_m1.push_back(m1[i]); + } } - throw std::runtime_error("not implemented yet"); -// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters -// // будет отличаться от случая с гомографией -// const int n_trials = TODO; -// -// const int n_samples = TODO; -// uint64_t seed = 1; -// -// int best_support = 0; -// cv::Matx33d best_F; -// -// std::vector sample; -// for (int i_trial = 0; i_trial < n_trials; ++i_trial) { -// phg::randomSample(sample, n_matches, n_samples, &seed); -// -// cv::Vec2d ms0[n_samples]; -// cv::Vec2d ms1[n_samples]; -// for (int i = 0; i < n_samples; ++i) { -// ms0[i] = m0_t[sample[i]]; -// ms1[i] = m1_t[sample[i]]; -// } -// -// cv::Matx33d F = estimateFMatrixDLT(ms0, ms1, n_samples); -// -// // denormalize -// F = TODO -// -// int support = 0; -// for (int i = 0; i < n_matches; ++i) { -// if (phg::epipolarTest(m0[i], m1[i], todo, threshold_px) && phg::epipolarTest(m1[i], m0[i], todo, threshold_px)) -// { -// ++support; -// } -// } -// -// if (support > best_support) { -// best_support = support; -// best_F = F; -// -// std::cout << "estimateFMatrixRANSAC : support: " << best_support << "/" << n_matches << std::endl; -// infoF(F); -// -// if (best_support == n_matches) { -// break; -// } -// } -// } -// -// std::cout << "estimateFMatrixRANSAC : best support: " << best_support << "/" << n_matches << std::endl; -// -// if (best_support == 0) { -// throw std::runtime_error("estimateFMatrixRANSAC : failed to estimate fundamental matrix"); -// } -// -// return best_F; + if (inlier_m0.size() >= n_samples) + best_F = estimateFMatrixNormalized(inlier_m0, inlier_m1); + + return best_F; } } diff --git a/src/phg/sfm/homography.cpp b/src/phg/sfm/homography.cpp index 5cbc780..0cec41f 100644 --- a/src/phg/sfm/homography.cpp +++ b/src/phg/sfm/homography.cpp @@ -2,8 +2,23 @@ #include #include +#include +#include + +#ifndef M_PI +#define M_PI 3.14159265358979323846 +#endif + +#ifndef ENABLE_ORSA_REFINE +#define ENABLE_ORSA_REFINE 1 +#endif namespace { + constexpr double ORSA_precision_px_threshold = 5.; + constexpr double ORSA_NFA_threshold = 1.; + + static_assert(ORSA_precision_px_threshold > 0.0, "ORSA_precision_px_threshold must be > 0"); + static_assert(ORSA_NFA_threshold > 0.0, "ORSA_NFA_threshold must be > 0"); // источник: https://e-maxx.ru/algo/linear_systems_gauss // очень важно при выполнении метода гаусса использовать выбор опорного элемента: об этом можно почитать в источнике кода @@ -84,8 +99,8 @@ namespace { double w1 = ws1[i]; // 8 elements of matrix + free term as needed by gauss routine -// A.push_back({TODO}); -// A.push_back({TODO}); + A.push_back({x0, y0, w0, 0., 0., 0., -x1 * x0, -x1 * y0, x1 * w0}); + A.push_back({0., 0., 0., x0, y0, w0, -y1 * x0, -y1 * y0, y1 * w0}); } int res = gauss(A, H); @@ -136,6 +151,43 @@ namespace { return *state = x; } + struct NormalizationData { + double width = 1., height = 1.; + cv::Matx33d T = cv::Matx33d::eye(), T_inv = cv::Matx33d::eye(); + }; + + struct MatchSet { + std::vector lhs, rhs, lhs_norm, rhs_norm; + NormalizationData lhs_normalization, rhs_normalization; + }; + + struct ModelEval { + double log_nfa = std::numeric_limits::infinity(); + int support = 0; + std::vector inliers; + }; + + uint32_t floatBits(float value) + { + uint32_t bits = 0; + std::memcpy(&bits, &value, sizeof(bits)); + return bits; + } + + void removeDuplicateMatches(const std::vector &points_lhs, const std::vector &points_rhs, std::vector &unique_lhs, std::vector &unique_rhs) + { + unique_lhs.clear(); + unique_rhs.clear(); + std::set> was; + for (int i = 0; i < points_lhs.size(); ++i) { + const auto key = std::make_tuple(floatBits(points_lhs[i].x), floatBits(points_lhs[i].y), floatBits(points_rhs[i].x), floatBits(points_rhs[i].y)); + if (was.insert(key).second) { + unique_lhs.push_back(points_lhs[i]); + unique_rhs.push_back(points_rhs[i]); + } + } + } + void randomSample(std::vector &dst, int max_id, int sample_size, uint64_t *state) { dst.clear(); @@ -156,69 +208,357 @@ namespace { } } + void randomSampleFromPool(std::vector &dst, const std::vector &pool, int sample_size, uint64_t *state) + { + if (pool.size() < sample_size) + throw std::runtime_error("Failed to sample ids"); + + dst.clear(); + dst.reserve(sample_size); + + const int max_attempts = 1000; + + for (int i = 0; i < sample_size; ++i) { + for (int k = 0; k < max_attempts; ++k) { + int v = pool[xorshift64(state) % pool.size()]; + if (std::find(dst.begin(), dst.end(), v) == dst.end()) { + dst.push_back(v); + break; + } + } + if (dst.size() < i + 1) { + throw std::runtime_error("Failed to sample ids"); + } + } + } + + bool transformPoint(const cv::Point2d &pt, const cv::Matx33d &T, cv::Point2d &dst) + { + const double x = pt.x, y = pt.y; + const double z = T(2, 0) * x + T(2, 1) * y + T(2, 2); + if (std::abs(z) < 1e-8 || !std::isfinite(z)) + return false; + + dst.x = (T(0, 0) * x + T(0, 1) * y + T(0, 2)) / z; + dst.y = (T(1, 0) * x + T(1, 1) * y + T(1, 2)) / z; + return std::isfinite(dst.x) && std::isfinite(dst.y); + } + + cv::Mat matFromMatx(const cv::Matx33d &T) + { + cv::Mat out(3, 3, CV_64F); + std::copy(T.val, T.val + 9, out.ptr()); + return out; + } + + bool normalizeHomographyScale(cv::Matx33d &H) + { + double scale = H(2, 2); + if (std::abs(scale) < 1e-8) + scale = cv::norm(matFromMatx(H)); + + if (std::abs(scale) < 1e-8 || !std::isfinite(scale)) + return false; + + H /= scale; + for (const double& val : H.val) { + if (!std::isfinite(val)) + return false; + } + return true; + } + + NormalizationData makeNormalization(const std::vector &points) + { + double max_x = 0., max_y = 0.; + for (const cv::Point2f &pt : points) { + max_x = std::max(max_x, std::ceil((double)pt.x)); + max_y = std::max(max_y, std::ceil((double)pt.y)); + } + + NormalizationData data; + data.width = std::max(1., max_x + 1.); + data.height = std::max(1., max_y + 1.); + const double s = std::sqrt(data.width * data.height); + data.T = cv::Matx33d( + 1./s, 0., -data.width/(2.*s), + 0., 1./s, -data.height/(2.*s), + 0., 0., 1. + ); + data.T_inv = cv::Matx33d( + s, 0., data.width/2., + 0., s, data.height/2., + 0., 0., 1. + ); + return data; + } + + bool satisfiesPositiveJacobian(const cv::Matx33d &H, const cv::Point2d &pt) + { + const double det_h = cv::determinant(matFromMatx(H)); + if (!std::isfinite(det_h) || std::abs(det_h) < 1e-8) + return false; + + const double numerator = H(2, 0) * pt.x + H(2, 1) * pt.y + H(2, 2); + return std::isfinite(numerator) && numerator / det_h > 0.; + } + + bool estimateHomography(const MatchSet &matches, const std::vector &indices, cv::Matx33d &H_norm, cv::Matx33d &H, const bool& demand_unique_solution_and_orientation_on_indices) + { + if (indices.size() < 4) + return false; + + cv::Mat A = cv::Mat::zeros(2 * indices.size(), 9, CV_64F); + for (int i = 0; i < indices.size(); ++i) { + const int idx = indices[i]; + const cv::Point2d &lhs = matches.lhs_norm[idx]; + const cv::Point2d &rhs = matches.rhs_norm[idx]; + + const int row0 = 2 * i; + const int row1 = row0 + 1; + + A.at(row0, 0) = lhs.x; + A.at(row0, 1) = lhs.y; + A.at(row0, 2) = 1.; + A.at(row0, 6) = -rhs.x * lhs.x; + A.at(row0, 7) = -rhs.x * lhs.y; + A.at(row0, 8) = -rhs.x; + + A.at(row1, 3) = lhs.x; + A.at(row1, 4) = lhs.y; + A.at(row1, 5) = 1.; + A.at(row1, 6) = -rhs.y * lhs.x; + A.at(row1, 7) = -rhs.y * lhs.y; + A.at(row1, 8) = -rhs.y; + } + + int svd_flags = cv::SVD::MODIFY_A; + if (A.rows < A.cols) + svd_flags |= cv::SVD::FULL_UV; + + cv::Mat singular_values, vt; + cv::SVD::compute(A, singular_values, cv::noArray(), vt, svd_flags); + + if (demand_unique_solution_and_orientation_on_indices) { + const int n = singular_values.total(); + const double sigma_max = singular_values.at(0), sigma_prev = singular_values.at(n - 2), sigma_last = singular_values.at(n - 1); + if ((sigma_last >= 0.01 * sigma_max) || (sigma_prev < 0.01 * sigma_max && sigma_last >= 0.1 * sigma_prev)) + return false; + } + + cv::Mat h_vec = vt.row(vt.rows - 1).reshape(1, 3); + H_norm = cv::Matx33d( + h_vec.at(0, 0), h_vec.at(0, 1), h_vec.at(0, 2), + h_vec.at(1, 0), h_vec.at(1, 1), h_vec.at(1, 2), + h_vec.at(2, 0), h_vec.at(2, 1), h_vec.at(2, 2) + ); + + H = matches.rhs_normalization.T_inv * H_norm * matches.lhs_normalization.T; + if (!normalizeHomographyScale(H_norm) || !normalizeHomographyScale(H)) + return false; + + cv::Mat H_norm_singular_values; + cv::SVD::compute(matFromMatx(H_norm), H_norm_singular_values, cv::SVD::NO_UV); + const double sigma_max = H_norm_singular_values.at(0, 0), sigma_min = H_norm_singular_values.at(2, 0); + if (sigma_min <= 1e-8 || (sigma_max / sigma_min) > 10.) + return false; + + if (demand_unique_solution_and_orientation_on_indices) { + for (const int& i : indices) { + if (!satisfiesPositiveJacobian(H, matches.lhs[i])) + return false; + } + } + + return true; + } + + bool evaluateHomographyACRansac(const MatchSet &matches, const cv::Matx33d &H_norm, const cv::Matx33d &H, const double& loge0, const std::vector &logc_n, const std::vector &logc_k, const double& max_error2, ModelEval &eval) + { + const int n_samples = 4; + const int n_matches = matches.lhs.size(); + const double logalpha0 = std::log10(M_PI); + + std::vector> residuals; + residuals.reserve(n_matches); + for (int i = 0; i < n_matches; ++i) { + if (!satisfiesPositiveJacobian(H, matches.lhs[i])) { + residuals.push_back({std::numeric_limits::infinity(), i}); + continue; + } + + cv::Point2d proj; + if (!transformPoint(matches.lhs_norm[i], H_norm, proj)) { + residuals.push_back({std::numeric_limits::infinity(), i}); + continue; + } + + const cv::Point2d diff = proj - matches.rhs_norm[i]; + residuals.push_back({diff.dot(diff), i}); + } + std::sort(residuals.begin(), residuals.end(), [](const auto &lhs, const auto &rhs) { + return lhs.first < rhs.first; + }); + + const int first_k = n_samples + 1; + eval = {}; + for (int k = first_k; k <= n_matches; ++k) { + const double error2 = residuals[k - 1].first; + if (!std::isfinite(error2) || error2 > max_error2) + break; + + const double logalpha = std::min(0., logalpha0 + std::log10(error2)); + const double log_nfa = loge0 + logc_n[k] + logc_k[k] + logalpha * (double)(k - n_samples); + if (log_nfa < eval.log_nfa) { + eval.log_nfa = log_nfa; + eval.support = k; + } + } + if (eval.support == 0) + return false; + + eval.inliers.reserve(eval.support); + for (int i = 0; i < eval.support; ++i) + eval.inliers.push_back(residuals[i].second); + + return true; + } + + bool refineHomographyFromInliers(const MatchSet &matches, const std::vector &inliers, const double& loge0, const std::vector &logc_n, const std::vector &logc_k, const double& max_error2, cv::Matx33d &refined_H, ModelEval &eval) + { + cv::Matx33d refined_H_norm; + if (!estimateHomography(matches, inliers, refined_H_norm, refined_H, false)) + return false; + + return evaluateHomographyACRansac(matches, refined_H_norm, refined_H, loge0, logc_n, logc_k, max_error2, eval); + } + cv::Mat estimateHomographyRANSAC(const std::vector &points_lhs, const std::vector &points_rhs) { if (points_lhs.size() != points_rhs.size()) { throw std::runtime_error("findHomography: points_lhs.size() != points_rhs.size()"); } - // TODO Дополнительный балл, если вместо обычной версии будет использована модификация a-contrario RANSAC - // * [1] Automatic Homographic Registration of a Pair of Images, with A Contrario Elimination of Outliers. (Lionel Moisan, Pierre Moulon, Pascal Monasse) - // * [2] Adaptive Structure from Motion with a contrario model estimation. (Pierre Moulon, Pascal Monasse, Renaud Marlet) - // * (простое описание для понимания) - // * [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_samples = 4; + std::vector unique_lhs, unique_rhs; + removeDuplicateMatches(points_lhs, points_rhs, unique_lhs, unique_rhs); + + if (unique_lhs.size() < n_samples) + throw std::runtime_error("findHomography : unique_lhs.size() < n_samples"); + + MatchSet matches; + matches.lhs_normalization = makeNormalization(unique_lhs); + matches.rhs_normalization = makeNormalization(unique_rhs); + + matches.lhs.reserve(unique_lhs.size()); + matches.rhs.reserve(unique_rhs.size()); + matches.lhs_norm.reserve(unique_lhs.size()); + matches.rhs_norm.reserve(unique_rhs.size()); + for (int i = 0; i < unique_lhs.size(); ++i) { + matches.lhs.emplace_back(unique_lhs[i]); + matches.rhs.emplace_back(unique_rhs[i]); + + cv::Point2d lhs_norm, rhs_norm; + transformPoint(matches.lhs.back(), matches.lhs_normalization.T, lhs_norm); + transformPoint(matches.rhs.back(), matches.rhs_normalization.T, rhs_norm); + matches.lhs_norm.push_back(lhs_norm); + matches.rhs_norm.push_back(rhs_norm); + } + + const int n_matches = matches.lhs.size(); + + std::vector all_indices(n_matches); + std::iota(all_indices.begin(), all_indices.end(), 0); + if (n_matches == n_samples) { + cv::Matx33d H, H_norm; + if (!estimateHomography(matches, all_indices, H_norm, H, true)) + throw std::runtime_error("estimateHomographyRANSAC : failed to estimate homography from 4 points"); + + return matFromMatx(H); + } + + const int n_trials_total = 10000; + int n_trials_reserve = n_trials_total / 10; + int n_trials = n_trials_total - n_trials_reserve; + const double loge0 = std::log10((double)std::max(1, n_matches - n_samples)), log_epsilon = std::log10(ORSA_NFA_threshold), max_precision_norm = ORSA_precision_px_threshold * matches.rhs_normalization.T(0, 0); + const double max_error2 = max_precision_norm * max_precision_norm; + + std::vector logc_n(n_matches + 1, 0.); + std::vector logc_k(n_matches + 1, 0.); + for (int k = 0; k <= n_matches; ++k) { + logc_n[k] = (std::lgamma(n_matches + 1.) - std::lgamma(k + 1.) - std::lgamma(n_matches - k + 1.)) / std::log(10.); + if (k >= n_samples) + logc_k[k] = (std::lgamma(k + 1.) - std::lgamma(n_samples + 1.) - std::lgamma(k - n_samples + 1.)) / std::log(10.); + } + + uint64_t seed = 1; + std::vector sampling_pool = all_indices, sample, best_inliers; + int best_support = 0; + double best_log_nfa = std::numeric_limits::infinity(); + cv::Matx33d best_H = cv::Matx33d::eye(); + for (int i_trial = 0; i_trial < n_trials; ++i_trial) { + randomSampleFromPool(sample, sampling_pool, n_samples, &seed); + + cv::Matx33d H_norm; + cv::Matx33d H; + if (!estimateHomography(matches, sample, H_norm, H, true)) + continue; + + ModelEval cand; + if (!evaluateHomographyACRansac(matches, H_norm, H, loge0, logc_n, logc_k, max_error2, cand)) + continue; + + const bool cand_better = (cand.log_nfa < best_log_nfa || (std::abs(cand.log_nfa - best_log_nfa) < 1e-8 && cand.support > best_support)); + if (cand_better) { + best_log_nfa = cand.log_nfa; + best_support = cand.support; + best_H = H; + best_inliers = cand.inliers; + } + + if (cand_better && cand.log_nfa < log_epsilon) { + sampling_pool = best_inliers; + if (n_trials_reserve > 0) { + n_trials = (i_trial + 1) + n_trials_reserve; + n_trials_reserve = 0; + } + } else if (i_trial + 1 == n_trials && n_trials_reserve > 0 && !best_inliers.empty()) { + sampling_pool = best_inliers; + n_trials = (i_trial + 1) + n_trials_reserve; + n_trials_reserve = 0; + } + } + if (best_log_nfa >= log_epsilon || best_inliers.empty()) { + throw std::runtime_error("estimateHomographyRANSAC : failed to estimate homography"); + } + +#if ENABLE_ORSA_REFINE + std::vector current_inliers = best_inliers; + cv::Matx33d current_H = best_H; + bool refined_successfully = false; + for (int i = 0; i < 10; ++i) { // it just safer than while (true) + cv::Matx33d refined_H; + ModelEval refined; + if (!refineHomographyFromInliers(matches, current_inliers, loge0, logc_n, logc_k, max_error2, refined_H, refined)) + break; + + current_H = refined_H; + refined_successfully = true; + if (refined.inliers == current_inliers) + break; + + current_inliers.swap(refined.inliers); + } + if (refined_successfully) + best_H = current_H; +#else + cv::Matx33d refined_H; + ModelEval refined; + if (refineHomographyFromInliers(matches, best_inliers, loge0, logc_n, logc_k, max_error2, refined_H, refined)) + best_H = refined_H; +#endif + + return matFromMatx(best_H); } } @@ -238,7 +578,15 @@ 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"); + cv::Mat Td; + T.convertTo(Td, CV_64F); + + const double x = pt.x, y = pt.y; + const double z = Td.at(2, 0) * x + Td.at(2, 1) * y + Td.at(2, 2); + if (std::abs(z) < 1e-8) + throw std::runtime_error("transformPoint: unacceptably small scale"); + + return {(Td.at(0, 0) * x + Td.at(0, 1) * y + Td.at(0, 2)) / z, (Td.at(1, 0) * x + Td.at(1, 1) * y + Td.at(1, 2)) / z}; } 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 8d76939..0971c18 100644 --- a/src/phg/sfm/panorama_stitcher.cpp +++ b/src/phg/sfm/panorama_stitcher.cpp @@ -23,7 +23,27 @@ cv::Mat phg::stitchPanorama(const std::vector &imgs, { // здесь надо посчитать вектор Hs // при этом можно обойтись n_images - 1 вызовами функтора homography_builder - throw std::runtime_error("not implemented yet"); + std::vector already_calculated(n_images, false); + std::vector parent_Hs(n_images); + auto calc_Hs = [&](auto&& self, const int& imgs_idx) -> const cv::Mat& { + if (already_calculated[imgs_idx]) + return Hs[imgs_idx]; + if (parent[imgs_idx] < 0) { + Hs[imgs_idx] = cv::Mat::eye(3, 3, CV_64FC1); + already_calculated[imgs_idx] = true; + return Hs[imgs_idx]; + } + + if (parent_Hs[imgs_idx].empty()) + parent_Hs[imgs_idx] = homography_builder(imgs[imgs_idx], imgs[parent[imgs_idx]]); + + Hs[imgs_idx] = self(self, parent[imgs_idx]) * parent_Hs[imgs_idx]; + already_calculated[imgs_idx] = true; + return Hs[imgs_idx]; + }; + for (int i = 0; i < n_images; ++i) { + calc_Hs(calc_Hs, i); + } } bbox2 bbox; diff --git a/src/phg/sfm/resection.cpp b/src/phg/sfm/resection.cpp index d2cf643..585c07c 100644 --- a/src/phg/sfm/resection.cpp +++ b/src/phg/sfm/resection.cpp @@ -49,95 +49,110 @@ namespace { // (см. Hartley & Zisserman p.178) cv::Matx34d estimateCameraMatrixDLT(const cv::Vec3d *Xs, const cv::Vec3d *xs, int count) { - throw std::runtime_error("not implemented yet"); -// using mat = Eigen::MatrixXd; -// using vec = Eigen::VectorXd; -// -// mat A(TODO); -// -// for (int i = 0; i < count; ++i) { -// -// double x = xs[i][0]; -// double y = xs[i][1]; -// double w = xs[i][2]; -// -// double X = Xs[i][0]; -// double Y = Xs[i][1]; -// double Z = Xs[i][2]; -// double W = 1.0; -// -// TODO -// } -// -// matrix34d result; -// TODO -// -// return canonicalizeP(result); + using mat = Eigen::MatrixXd; + using vec = Eigen::VectorXd; + + mat A(count << 1, 12); + + for (int i = 0; i < count; ++i) { + + double x = xs[i][0]; + double y = xs[i][1]; + double w = xs[i][2]; + + double X = Xs[i][0]; + double Y = Xs[i][1]; + double Z = Xs[i][2]; + double W = 1.0; + + A.row(i << 1) << 0, 0, 0, 0, -w * X, -w * Y, -w * Z, -w * W, y * X, y * Y, y * Z, y * W; + A.row((i << 1) + 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::ComputeFullV); + Eigen::VectorXd p = svd.matrixV().col(11); + for (int i = 0; i < 12; ++i) + result(i / 4, i % 4) = p[i]; + + return canonicalizeP(result); } // По трехмерным точкам и их проекциям на изображении определяем положение камеры cv::Matx34d estimateCameraMatrixRANSAC(const phg::Calibration &calib, const std::vector &X, const std::vector &x) { - throw std::runtime_error("not implemented yet"); -// if (X.size() != x.size()) { -// throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()"); -// } -// -// const int n_points = X.size(); -// -// // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters -// // будет отличаться от случая с гомографией -// const int n_trials = TODO; -// -// const double threshold_px = 3; -// -// const int n_samples = TODO; -// uint64_t seed = 1; -// -// int best_support = 0; -// cv::Matx34d best_P; -// -// std::vector sample; -// for (int i_trial = 0; i_trial < n_trials; ++i_trial) { -// phg::randomSample(sample, n_points, n_samples, &seed); -// -// cv::Vec3d ms0[n_samples]; -// cv::Vec3d ms1[n_samples]; -// for (int i = 0; i < n_samples; ++i) { -// ms0[i] = TODO; -// ms1[i] = TODO; -// } -// -// cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples); -// -// int support = 0; -// for (int i = 0; i < n_points; ++i) { -// cv::Vec2d px = TODO спроецировать 3Д точку в пиксель с использованием P и calib; -// if (cv::norm(px - x[i]) < threshold_px) { -// ++support; -// } -// } -// -// if (support > best_support) { -// best_support = support; -// best_P = P; -// -// std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl; -// -// if (best_support == n_points) { -// break; -// } -// } -// } -// -// std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl; -// -// if (best_support == 0) { -// throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix"); -// } -// -// return best_P; + if (X.size() != x.size()) { + throw std::runtime_error("estimateCameraMatrixRANSAC: X.size() != x.size()"); + } + + const int n_points = X.size(); + + // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters + // будет отличаться от случая с гомографией + const int n_trials = 50000; + + const double threshold_px = 3; + + const int n_samples = 6; + uint64_t seed = 1; + + int best_support = 0; + cv::Matx34d best_P; + + std::vector sample; + + std::vector x_unproj(n_points); + for (int i = 0; i < n_points; ++i) + x_unproj[i] = calib.unproject(x[i]); + + if (n_points == n_samples) + return estimateCameraMatrixDLT(X.data(), x_unproj.data(), n_points); + + for (int i_trial = 0; i_trial < n_trials; ++i_trial) { + phg::randomSample(sample, n_points, n_samples, &seed); + + cv::Vec3d ms0[n_samples]; + cv::Vec3d ms1[n_samples]; + for (int i = 0; i < n_samples; ++i) { + ms0[i] = X[sample[i]]; + ms1[i] = x_unproj[sample[i]]; + } + + cv::Matx34d P = estimateCameraMatrixDLT(ms0, ms1, n_samples); + + int support = 0; + for (int i = 0; i < n_points; ++i) { + cv::Vec3d x_cam = P * cv::Vec4d(X[i][0], X[i][1], X[i][2], 1.); + if (std::abs(x_cam[2]) < 1e-10) + continue; + + cv::Vec3d x_pix_h = calib.project(x_cam); + cv::Vec2d px = {x_pix_h[0] / x_pix_h[2], x_pix_h[1] / x_pix_h[2]}; + if (cv::norm(px - x[i]) < threshold_px) { + ++support; + } + } + + if (support > best_support) { + best_support = support; + best_P = P; + + std::cout << "estimateCameraMatrixRANSAC : support: " << best_support << "/" << n_points << std::endl; + + if (best_support == n_points) { + break; + } + } + } + + std::cout << "estimateCameraMatrixRANSAC : best support: " << best_support << "/" << n_points << std::endl; + + if (best_support == 0) { + throw std::runtime_error("estimateCameraMatrixRANSAC : failed to estimate camera matrix"); + } + + return best_P; } diff --git a/src/phg/sfm/sfm_utils.cpp b/src/phg/sfm/sfm_utils.cpp index d2d2e29..72e9916 100644 --- a/src/phg/sfm/sfm_utils.cpp +++ b/src/phg/sfm/sfm_utils.cpp @@ -41,5 +41,11 @@ 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) { - throw std::runtime_error("not implemented yet"); + cv::Vec3d line = F * cv::Vec3d(pt0[0], pt0[1], 1.); + double norm = std::sqrt(line[0] * line[0] + line[1] * line[1]); + if (norm < 1e-10) + return false; + + double dist = std::abs(line.dot(cv::Vec3d(pt1[0], pt1[1], 1.))) / norm; + return dist < t; } diff --git a/src/phg/sfm/triangulation.cpp b/src/phg/sfm/triangulation.cpp index 8dd11e6..49449b4 100644 --- a/src/phg/sfm/triangulation.cpp +++ b/src/phg/sfm/triangulation.cpp @@ -12,5 +12,15 @@ cv::Vec4d phg::triangulatePoint(const cv::Matx34d *Ps, const cv::Vec3d *ms, int { // составление однородной системы + SVD // без подвохов - throw std::runtime_error("not implemented yet"); + Eigen::MatrixXd A(count << 1, 4); + for (int i = 0; i < count; ++i) { + for (int j = 0; j < 4; ++j) { + A(i << 1, j) = ms[i][0] * Ps[i](2, j) - ms[i][2] * Ps[i](0, j); + A((i << 1) + 1, j) = ms[i][1] * Ps[i](2, j) - ms[i][2] * Ps[i](1, j); + } + } + + Eigen::JacobiSVD svd(A, Eigen::ComputeFullV); + Eigen::VectorXd X = svd.matrixV().col(3); + return {X[0], X[1], X[2], X[3]}; } diff --git a/src/phg/sift/sift.cpp b/src/phg/sift/sift.cpp index 7204771..b340b46 100755 --- a/src/phg/sift/sift.cpp +++ b/src/phg/sift/sift.cpp @@ -16,6 +16,9 @@ // 3) https://github.com/opencv/opencv/blob/1834eed8098aa2c595f4d1099eeaa0992ce8b321/modules/features2d/src/sift.dispatch.cpp // 4) https://github.com/opencv/opencv/blob/1834eed8098aa2c595f4d1099eeaa0992ce8b321/modules/features2d/src/sift.simd.hpp +#define ENABLE_EXACT_NFEATURES_RECOVERY 1 +#define ENABLE_INCREMENTAL_OCTAVE_GAUSSIAN_BLUR 0 + namespace { cv::Mat upsample2x(const cv::Mat& src) @@ -108,16 +111,22 @@ 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 получали удвоение базового блюра; - // // вычтем sigma0 чтобы размыть ровно до нужной суммарной сигмы - // TODO sigma_layer = ... (вычитаем как в sigma base); - // cv::GaussianBlur(oct.layers[0], oct.layers[i], cv::Size(), sigma_layer, sigma_layer); + double sigma_layer = sigma0 * std::pow(2.0, (double)i / s); +#if ENABLE_INCREMENTAL_OCTAVE_GAUSSIAN_BLUR + double prev_sigma_layer = sigma0 * std::pow(2.0, (double)(i - 1) / s); + sigma_layer = std::sqrt(sigma_layer * sigma_layer - prev_sigma_layer * prev_sigma_layer); + cv::GaussianBlur(oct.layers[i - 1], oct.layers[i], cv::Size(), sigma_layer, sigma_layer); +#else + // вычтем sigma0 чтобы размыть ровно до нужной суммарной сигмы + sigma_layer = std::sqrt(sigma_layer * sigma_layer - sigma0 * sigma0); + cv::GaussianBlur(oct.layers[0], oct.layers[i], cv::Size(), sigma_layer, sigma_layer); +#endif } // подготавливаем базовый слой для следующей октавы if (o + 1 < n_octaves) { // используется в opencv, формула для пересчета ключевых точек: pt_upscaled = 2^o * pt_downscaled - // TODO cv::resize(даунскейлим текущий слой в два раза, без интерполяции, просто сабсепмлинг); + base = downsample2x(oct.layers[s]); // можно использовать и downsample2x_avg(oct.layers[s]), это позволяет потом заапскейлить слои обратно до оригинального разрешения без сдвига // но потребуется везде изменить формулу для пересчета ключевых точек: pt_upscaled = (pt_downscaled + 0.5) * 2^o - 0.5 @@ -138,7 +147,9 @@ 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 = nL.at(yi, xi) + pL.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,21 +307,21 @@ std::vector phg::findScaleSpaceExtrema(const std::vector (r + 1.f) * (r + 1.f) / r) + break; } // скейлим координаты точек обратно до родных размеров картинки @@ -379,39 +413,42 @@ 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 * n_bins / 360.f; + 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; + } + + float weighted_mag = mag * weight; + histogram[bin0] += weighted_mag * (1.f - frac); + histogram[bin1] += weighted_mag * frac; } } @@ -450,20 +487,24 @@ 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 = 0.f; + float denom = 2.f * (left + right - 2.f * center); + if (std::abs(denom) > 1e-7f) { + offset = (left - right) / denom; + } + 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 +615,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 +650,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] += weighted_mag * wx * wy * wo; } } } @@ -620,9 +661,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; } } } @@ -712,6 +752,75 @@ void phg::SIFT::detectAndCompute(const cv::Mat& img, const cv::Mat& mask, std::v savePyramid("pyramidDoG/03_dog_octave", dog, true); kpts = findScaleSpaceExtrema(dog, p, verbose_level); + +#if ENABLE_EXACT_NFEATURES_RECOVERY + // Почему раньше могли вернуть меньше чем nfeatures: + // 1) в computeOrientations() часть точек может отброситься по условию в строке 408 (если окно вышло за границы слоя), + // 2) в computeDescriptors() часть точек может отброситься по условию в строке 564 (патч дескриптора вышел за границы слоя), + // 3) а у нас пайплайн был такой: + // - находим space-scale экстремумы; + // - в selectTopKeypoints() берём топ-nfeatures из них; + // - делаем computeOrientations(), после чего у нас уже может стать меньше чем nfeatures точек (но могло стать и больше, + // если пиков ориентаций было взято несколько для одного и того же кандидата); + // - делаем selectTopKeypoints() - опять имеем <= nfeatures точек; + // - делаем computeDescriptors(), после которого у нас также может стать меньше чем nfeatures точек. + // + // Получаем, что если заранее жестко отрезать до nfeatures, "резервных" кандидатов не остается и финальный набор недобирается. + // + // Что делаем: не отрезаем :) + // Но гоняем computeOrientations() и computeDescriptors() батчами, начиная с самых сильных, чтобы не просесть по производительности. + // Если после очередного батча все еще недобор, добираем следующую порцию. + // Так мы обычно считаем почти столько же, сколько раньше, но при необходимости можем добрать до ровно nfeatures + // (если в принципе хватит валидных точек после всех фильтраций). + + std::vector extrema_kpts = std::move(kpts); + if (p.nfeatures > 0) { + std::vector ranked_idx(extrema_kpts.size()); + std::iota(ranked_idx.begin(), ranked_idx.end(), 0); + std::sort(ranked_idx.begin(), ranked_idx.end(), [&extrema_kpts](const int& a, const int& b) { + float ra = std::abs(extrema_kpts[a].response); + float rb = std::abs(extrema_kpts[b].response); + if (ra != rb) + return ra > rb; + return a < b; + }); + kpts.clear(); + desc.release(); + + size_t processed = 0; + size_t batch_size = std::max(32, p.nfeatures); + while (kpts.size() < p.nfeatures && processed < ranked_idx.size()) { + size_t next_processed = std::min(ranked_idx.size(), processed + batch_size); + std::vector batch_extrema; + batch_extrema.reserve(next_processed - processed); + for (size_t i = processed; i < next_processed; ++i) { + batch_extrema.push_back(extrema_kpts[ranked_idx[i]]); + } + + std::vector batch_oriented = computeOrientations(batch_extrema, octaves, p, verbose_level); + cv::Mat batch_desc; + std::vector batch_valid_kpts; + std::tie(batch_desc, batch_valid_kpts) = computeDescriptors(batch_oriented, octaves, p, verbose_level); + if (!batch_desc.empty()) { + desc.push_back(batch_desc); + } + + kpts.insert(kpts.end(), batch_valid_kpts.begin(), batch_valid_kpts.end()); + processed = next_processed; + if (kpts.size() < p.nfeatures) { + batch_size = std::max(32, 2 * (p.nfeatures - kpts.size())); + } + } + + if (kpts.size() > p.nfeatures) { + kpts.resize(p.nfeatures); + desc = desc.rowRange(0, p.nfeatures).clone(); + } + } else { + kpts = computeOrientations(extrema_kpts, octaves, p, verbose_level); + std::tie(desc, kpts) = computeDescriptors(kpts, octaves, p, verbose_level); + } +#else // ориентация ключевых точек это довольно дорогая операция // в случае если пользователь просит малое количество лучших точек (например, 1000, а без порога нашлось 20000), // то по производительности очень оправдано сразу их здесь и выбрать, чтобы не тащить до самого конца где все равно отбросим @@ -721,6 +830,7 @@ void phg::SIFT::detectAndCompute(const cv::Mat& img, const cv::Mat& mask, std::v // после подсчета ориентаций количество могло возрасти (и скорее всего возросло) // нужно снова выбрать лучшие точки чтобы уложиться в бюджет kpts = selectTopKeypoints(kpts, p, verbose_level); +#endif if (verbose_level >= 2) { cv::Mat kpts_img; @@ -728,10 +838,10 @@ void phg::SIFT::detectAndCompute(const cv::Mat& img, const cv::Mat& mask, std::v saveImg("04_keypoints.jpg", kpts_img); } +#if !ENABLE_EXACT_NFEATURES_RECOVERY std::tie(desc, kpts) = computeDescriptors(kpts, octaves, p, verbose_level); +#endif - // TODO всегда ли мы получаем ровно столько точек сколько запросили в параметре nfeatures? в каких случаях это не так и в какую сторону? - // как подкрутить алгоритм, чтобы всегда выдавать ровно запрошенное количество точек (когда это в принципе возможно) но не сильно просесть в производительности? } void phg::SIFT::saveImg(const std::string& name, const cv::Mat& img) const diff --git a/tests/test_sfm.cpp b/tests/test_sfm.cpp index 4229b86..48cc158 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 {