Task02 Денис Ловцев ИТМО

src/phg/matching/descriptor_matcher.cpp

@@ -1,14 +1,20 @@
 #include "descriptor_matcher.h"
-
+#include <iostream>
 #include <opencv2/flann/miniflann.hpp>
 #include "flann_factory.h"
-
+using namespace std;
+#include <unordered_set>
 void phg::DescriptorMatcher::filterMatchesRatioTest(const std::vector<std::vector<cv::DMatch>> &matches,
                                                     std::vector<cv::DMatch> &filtered_matches)
 {
     filtered_matches.clear();
 
-    throw std::runtime_error("not implemented yet");
+    filtered_matches.reserve(matches.size());
+    for(auto &match : matches) {
+        if(match[0].distance  < 0.7 * match[1].distance){
+            filtered_matches.push_back(match[0]);
+        }
+    }
 }
 
 
@@ -35,42 +41,61 @@ void phg::DescriptorMatcher::filterMatchesClusters(const std::vector<cv::DMatch>
         points_query.at<cv::Point2f>(i) = keypoints_query[matches[i].queryIdx].pt;
         points_train.at<cv::Point2f>(i) = keypoints_train[matches[i].trainIdx].pt;
     }
-//
-//    // размерность всего 2, так что точное KD-дерево
-//    std::shared_ptr<cv::flann::IndexParams> index_params = flannKdTreeIndexParams(TODO);
-//    std::shared_ptr<cv::flann::SearchParams> search_params = flannKsTreeSearchParams(TODO);
-//
-//    std::shared_ptr<cv::flann::Index> index_query = flannKdTreeIndex(points_query, index_params);
-//    std::shared_ptr<cv::flann::Index> 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<double> max_dists2_query(n_matches);
-//        std::vector<double> max_dists2_train(n_matches);
-//        for (int i = 0; i < n_matches; ++i) {
-//            max_dists2_query[i] = distances2_query.at<float>(i, total_neighbours - 1);
-//            max_dists2_train[i] = distances2_train.at<float>(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<cv::flann::IndexParams> index_params = flannKdTreeIndexParams(4);
+   std::shared_ptr<cv::flann::SearchParams> search_params = flannKsTreeSearchParams(32);
+
+   std::shared_ptr<cv::flann::Index> index_query = flannKdTreeIndex(points_query, index_params);
+   std::shared_ptr<cv::flann::Index> 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<double> max_dists2_query(n_matches);
+       std::vector<double> max_dists2_train(n_matches);
+       for (int i = 0; i < n_matches; ++i) {
+           max_dists2_query[i] = distances2_query.at<float>(i, total_neighbours - 1);
+           max_dists2_train[i] = distances2_train.at<float>(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
+   filtered_matches.reserve(n_matches);
+   for(int i=0; i < n_matches; i++ ){
+    unordered_set<int> closest_points;
+    int points_counter =0;
+    for(int j=0; j<total_neighbours; j++){
+        if(distances2_query.at<float>(i,j)<= radius2_query){
+            closest_points.insert(indices_query.at<int>(i,j));
+        }
+    }
+    for(int j=0; j<total_neighbours; j++){
+        if((distances2_train.at<float>(i,j)<= radius2_train) && (closest_points.find(indices_train.at<int>(i,j))) !=closest_points.end()){
+            points_counter++;
+        }
+    }
+    if (points_counter>= consistent_matches){
+        filtered_matches.push_back(matches[i]);
+    }
+
+   }
 }

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,16 @@ void phg::FlannMatcher::train(const cv::Mat &train_desc)
 
 void phg::FlannMatcher::knnMatch(const cv::Mat &query_desc, std::vector<std::vector<cv::DMatch>> &matches, int k) const
 {
-    throw std::runtime_error("not implemented yet");
+    cv::Mat indices(query_desc.rows, k, CV_32SC1);
+    cv::Mat dists(query_desc.rows, k, CV_32FC1);
+    flann_index->knnSearch(query_desc, indices, dists,k, *search_params);
+    matches.resize(query_desc.rows);
+    for(int qd_idx = 0; qd_idx< query_desc.rows; qd_idx++){
+       std::vector<cv::DMatch> &d_matches = matches[qd_idx];
+        d_matches.resize(k);
+       for (int i =0; i< k; i++){
+            cv::DMatch match(qd_idx, indices.at<int>(qd_idx, i),0,std::sqrt(dists.at<float>(qd_idx, i)));
+            d_matches[i] = match;
+       }
+    }
 }

src/phg/sfm/homography.cpp

@@ -84,8 +84,17 @@ 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, 1, 0, 0, 0,
+                -x0 * x1, -y0 * x1,
+                x1
+            });
+
+            A.push_back({
+                0, 0, 0, x0, y0, 1,
+                -x0 * y1, -y0 * y1,
+                y1
+            });
         }
 
         int res = gauss(A, H);
@@ -168,57 +177,57 @@ 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<int> 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();
+
+       // https://en.wikipedia.org/wiki/Random_sample_consensus#Parameters
+       const int n_trials = 2000;
+
+       const int n_samples = 4;
+       uint64_t seed = 1;
+       const double reprojection_error_threshold_px = 2;
+
+       int best_support = 0;
+       cv::Mat best_H;
+
+       std::vector<int> 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 +247,15 @@ cv::Mat phg::findHomographyCV(const std::vector<cv::Point2f> &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<double>(2, 0) * x + Td.at<double>(2, 1) * y + Td.at<double>(2, 2);
+    if (std::abs(z) < 1e-8)
+        throw std::runtime_error("transformPoint: unacceptably small scale");
+
+    return {(Td.at<double>(0, 0) * x + Td.at<double>(0, 1) * y + Td.at<double>(0, 2)) / z, (Td.at<double>(1, 0) * x + Td.at<double>(1, 1) * y + Td.at<double>(1, 2)) / z};
 }
 
 cv::Point2d phg::transformPointCV(const cv::Point2d &pt, const cv::Mat &T) {

src/phg/sfm/panorama_stitcher.cpp

@@ -4,6 +4,8 @@
 #include <libutils/bbox2.h>
 #include <iostream>
 
+#include <queue>
+
 /*
  * imgs - список картинок
  * parent - список индексов, каждый индекс указывает, к какой картинке должна быть приклеена текущая картинка
@@ -20,11 +22,32 @@ cv::Mat phg::stitchPanorama(const std::vector<cv::Mat> &imgs,
 
     // вектор гомографий, для каждой картинки описывает преобразование до корня
     std::vector<cv::Mat> Hs(n_images);
-    {
-        // здесь надо посчитать вектор Hs
-        // при этом можно обойтись n_images - 1 вызовами функтора homography_builder
-        throw std::runtime_error("not implemented yet");
+{
+    int root;
+    std::vector<std::vector<int>> children(n_images);
+    for (int i = 0; i < n_images; ++i) {
+        if (parent[i] != -1) {
+            children[parent[i]].push_back(i);
+        }else{
+            root = i;
+        }
     }
+
+    Hs[root] = cv::Mat::eye(3, 3, CV_64F);
+    std::queue<int> q;
+    q.push(root);
+
+    while (!q.empty()) {
+        int current = q.front();
+        q.pop();
+
+        for (int child : children[current]) {
+            cv::Mat H_child_to_current = homography_builder(imgs[child], imgs[current]);
+            Hs[child] = H_child_to_current * Hs[current];
+            q.push(child);
+        }
+    }
+}
 
     bbox2<double, cv::Point2d> bbox;
     for (int i = 0; i < n_images; ++i) {