Merge pull request #154 from swift-nav/seth/ransac_metric_group_count

Minor ransac fixes. Changes default GP consensus metric to a feature count

Author: seth-swiftnav

include/albatross/src/models/ransac.hpp

@@ -80,15 +80,13 @@ ransac(const RansacFunctions<FitType> &ransac_functions,
         random_without_replacement(groups, random_sample_size, gen);
     const auto fit = ransac_functions.fitter(candidate_groups);
 
-    std::vector<FoldName> candidate_consensus;
+    // Any group that's part of the candidate set is automatically an inlier.
+    std::vector<FoldName> candidate_consensus = candidate_groups;
+
     // Find which of the other groups agree with the reference model
     // which gives us a consensus (set of inliers).
     for (const auto &possible_inlier : groups) {
-      if (contains_group(candidate_groups, possible_inlier)) {
-        // Any group that's part of the candidate set is automatically an
-        // inlier.
-        candidate_consensus.emplace_back(possible_inlier);
-      } else {
+      if (!contains_group(candidate_groups, possible_inlier)) {
         double metric_value =
             ransac_functions.inlier_metric(possible_inlier, fit);
         if (metric_value < inlier_threshold) {
@@ -99,7 +97,7 @@ ransac(const RansacFunctions<FitType> &ransac_functions,
 
     // If there is enough agreement, consider this random set of inliers
     // as a candidate model.
-    if (candidate_consensus.size() + random_sample_size >= min_consensus_size) {
+    if (candidate_consensus.size() >= min_consensus_size) {
       double consensus_metric_value =
           ransac_functions.consensus_metric(candidate_consensus);
       if (consensus_metric_value < best_consensus_metric) {

include/albatross/src/models/ransac_gp.hpp

@@ -66,6 +66,7 @@ get_gp_ransac_inlier_metric(const RegressionDataset<FeatureType> &dataset,
   };
 }
 
+/*
 template <typename ModelType, typename FeatureType>
 inline typename RansacFunctions<
     FitAndIndices<ModelType, FeatureType>>::ConsensusMetric
@@ -77,6 +78,17 @@ get_gp_ransac_model_entropy_metric(const FoldIndexer &indexer,
     return differential_entropy(consensus_cov);
   };
 }
+ */
+
+template <typename ModelType, typename FeatureType>
+inline typename RansacFunctions<
+    FitAndIndices<ModelType, FeatureType>>::ConsensusMetric
+get_gp_ransac_feature_count_consensus_metric(const FoldIndexer &indexer) {
+  return [&, indexer](const std::vector<FoldName> &groups) {
+    auto inds = indices_from_names(indexer, groups);
+    return (-1.0 * static_cast<double>(inds.size()));
+  };
+}
 
 template <typename ModelType, typename FeatureType, typename InlierMetric>
 inline RansacFunctions<FitAndIndices<ModelType, FeatureType>>
@@ -97,12 +109,12 @@ get_gp_ransac_functions(const ModelType &model,
       get_gp_ransac_inlier_metric<ModelType, FeatureType, InlierMetric>(
           dataset, indexer, full_cov, model, inlier_metric);
 
-  const auto consensus_metric_from_gorup =
-      get_gp_ransac_model_entropy_metric<ModelType, FeatureType>(indexer,
-                                                                 full_cov);
+  const auto consensus_metric_from_group =
+      get_gp_ransac_feature_count_consensus_metric<ModelType, FeatureType>(
+          indexer);
 
   return RansacFunctions<FitAndIndices<ModelType, FeatureType>>(
-      fitter, inlier_metric_from_group, consensus_metric_from_gorup);
+      fitter, inlier_metric_from_group, consensus_metric_from_group);
 };
 
 template <typename InlierMetric, typename IndexingFunction>