Added flag for minimal group size.

spt/triangulator.cpp

@@ -102,9 +102,10 @@ void CameraInternal::update_projection_matrix()
 // =================================================================================================
 // =================================================================================================
 
-TriangulatorInternal::TriangulatorInternal(float min_score)
+TriangulatorInternal::TriangulatorInternal(float min_score, size_t min_group_size)
 {
     this->min_score = min_score;
+    this->min_group_size = min_group_size;
 }
 
 // =================================================================================================
@@ -443,20 +444,13 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
     stime = std::chrono::high_resolution_clock::now();
 
     // Drop low scoring poses
-    std::vector<size_t> drop_indices;
-    for (size_t i = 0; i < all_scored_poses.size(); ++i)
+    size_t num_poses = all_scored_poses.size();
+    for (size_t i = num_poses; i > 0; --i)
     {
-        if (all_scored_poses[i].second < min_score)
+        if (all_scored_poses[i - 1].second < min_score)
         {
-            drop_indices.push_back(i);
-        }
-    }
-    if (!drop_indices.empty())
-    {
-        for (size_t i = drop_indices.size(); i > 0; --i)
-        {
-            all_scored_poses.erase(all_scored_poses.begin() + drop_indices[i - 1]);
-            all_pairs.erase(all_pairs.begin() + drop_indices[i - 1]);
+            all_scored_poses.erase(all_scored_poses.begin() + i - 1);
+            all_pairs.erase(all_pairs.begin() + i - 1);
         }
     }
 
@@ -464,6 +458,16 @@ std::vector<std::vector<std::array<float, 4>>> TriangulatorInternal::triangulate
     std::vector<std::tuple<cv::Point3d, cv::Mat, std::vector<int>>> groups;
     groups = calc_grouping(all_pairs, all_scored_poses, min_score);
 
+    // Drop groups with too few matches
+    size_t num_groups = groups.size();
+    for (size_t i = num_groups; i > 0; --i)
+    {
+        if (std::get<2>(groups[i - 1]).size() < this->min_group_size)
+        {
+            groups.erase(groups.begin() + i - 1);
+        }
+    }
+
     elapsed = std::chrono::high_resolution_clock::now() - stime;
     grouping_time += elapsed.count();
     stime = std::chrono::high_resolution_clock::now();