From 1ed651b1a2bc537d5c7f249f8d30bdd1e42ff8e5 Mon Sep 17 00:00:00 2001
From: akrishnan86 <akrishnan86@gatech.edu>
Date: Fri, 18 Sep 2020 23:14:07 -0700
Subject: [PATCH] wrap MFAS

---
 gtsam/gtsam.i                  | 21 +++++++++++++++++++++
 gtsam/sfm/MFAS.h               |  2 ++
 python/CMakeLists.txt          |  7 +++++--
 python/gtsam/specializations.h |  1 +
 4 files changed, 29 insertions(+), 2 deletions(-)

diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index 52f5901ee..649d80ae3 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -3105,6 +3105,27 @@ class ShonanAveraging3 {
   pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
 };
 
+#include <gtsam/sfm/MFAS.h>
+
+class KeyPairDoubleMap {
+  KeyPairDoubleMap();
+  KeyPairDoubleMap(const gtsam::KeyPairDoubleMap& other);
+
+  size_t size() const;
+  bool empty() const;
+  void clear();
+  size_t at(pair<size_t, size_t>) const;
+};
+
+class MFAS {
+  MFAS(const KeyVector*& nodes,
+       const gtsam::BinaryMeasurementsUnit3& relativeTranslations,
+       const gtsam::Unit3& projectionDirection);
+
+  KeyPairDoubleMap computeOutlierWeights() const;
+  KeyVector computeOrdering() const;
+};
+
 #include <gtsam/sfm/TranslationRecovery.h>
 class TranslationRecovery {
   TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations,
diff --git a/gtsam/sfm/MFAS.h b/gtsam/sfm/MFAS.h
index 929aa5ff0..67a7df219 100644
--- a/gtsam/sfm/MFAS.h
+++ b/gtsam/sfm/MFAS.h
@@ -108,4 +108,6 @@ class MFAS {
   std::map<KeyPair, double> computeOutlierWeights() const;
 };
 
+typedef std::map<std::pair<Key, Key>, double> KeyPairDoubleMap;
+
 }  // namespace gtsam
diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt
index bec02fb64..00b537340 100644
--- a/python/CMakeLists.txt
+++ b/python/CMakeLists.txt
@@ -37,7 +37,8 @@ set(ignore
     gtsam::Point2Vector
     gtsam::Pose3Vector
     gtsam::KeyVector
-    gtsam::BinaryMeasurementsUnit3)
+    gtsam::BinaryMeasurementsUnit3
+    gtsam::KeyPairDoubleMap)
 
 pybind_wrap(gtsam_py # target
             ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header
@@ -80,7 +81,9 @@ set(ignore
         gtsam::Pose3Vector
         gtsam::KeyVector
         gtsam::FixedLagSmootherKeyTimestampMapValue
-        gtsam::BinaryMeasurementsUnit3)
+        gtsam::BinaryMeasurementsUnit3
+        gtsam::KeyPairDoubleMap)
+        
 pybind_wrap(gtsam_unstable_py # target
         ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
         "gtsam_unstable.cpp" # generated_cpp
diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h
index 3f6b8fa38..cacad874c 100644
--- a/python/gtsam/specializations.h
+++ b/python/gtsam/specializations.h
@@ -12,3 +12,4 @@ py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2>
 py::bind_vector<std::vector<gtsam::BinaryMeasurement<gtsam::Unit3> > >(m_, "BinaryMeasurementsUnit3");
 py::bind_map<gtsam::IndexPairSetMap>(m_, "IndexPairSetMap");
 py::bind_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");
+py::bind_map<gtsam::KeyPairDoubleMap>(m_, "KeyPairDoubleMap");