From 6e45be3d98ecc9bbd52ebd7be256e021cf01d7a3 Mon Sep 17 00:00:00 2001
From: senselessdev1 <johnlambert@hayoungs-mbp.lan>
Date: Sat, 16 Jul 2022 16:09:21 -0400
Subject: [PATCH] fix all wrapper issues

---
 gtsam/sfm/DsfTrackGenerator.h                | 16 ++++++++------
 gtsam/sfm/sfm.i                              | 15 ++++++-------
 python/gtsam/tests/test_DsfTrackGenerator.py | 23 +++++++++++++++++++-
 3 files changed, 38 insertions(+), 16 deletions(-)

diff --git a/gtsam/sfm/DsfTrackGenerator.h b/gtsam/sfm/DsfTrackGenerator.h
index 619ca40ca..6483bae97 100644
--- a/gtsam/sfm/DsfTrackGenerator.h
+++ b/gtsam/sfm/DsfTrackGenerator.h
@@ -21,6 +21,7 @@
 #include <Eigen/Core>
 
 #include <gtsam/base/DSFMap.h>
+#include <gtsam/geometry/Point2.h>
 
 #include <algorithm>
 #include <iostream>
@@ -34,9 +35,10 @@ typedef DSFMap<IndexPair> DSFMapIndexPair;
 typedef std::pair<size_t, size_t> ImagePair;
 typedef Eigen::MatrixX2i CorrespondenceIndices; // N x 2 array
 
-
+//struct Keypoints;
 using KeypointCoordinates = Eigen::MatrixX2d;
 
+
 struct Keypoints
 {
   KeypointCoordinates coordinates;
@@ -47,6 +49,10 @@ struct Keypoints
   Keypoints(const gtsam::KeypointCoordinates& coordinates): coordinates(coordinates) {}; // boost::none
 };
 
+using KeypointsList = std::vector<Keypoints>;
+using KeypointsVector = std::vector<Keypoints>; // TODO(johnwlambert): prefer KeypointsSet?
+using MatchIndicesMap = std::map<ImagePair, CorrespondenceIndices>;
+
 
 // @param camera index
 // @param 2d measurement
@@ -54,9 +60,9 @@ struct Keypoints
 struct NamedSfmMeasurement
 {
   size_t i;
-  Point2 uv;
+  gtsam::Point2 uv;
 
-  NamedSfmMeasurement(size_t i, Point2 uv) : i(i), uv(uv) {}
+  NamedSfmMeasurement(size_t i, gtsam::Point2 uv) : i(i), uv(uv) {}
 };
 
 
@@ -87,10 +93,6 @@ class SfmTrack2d
   }
 };
 
-using MatchIndicesMap = std::map<ImagePair, CorrespondenceIndices>;
-using KeypointsList = std::vector<Keypoints>;
-using KeypointsVector = std::vector<Keypoints>; // TODO(johnwlambert): prefer KeypointsSet?
-
 
 class DsfTrackGenerator
 {
diff --git a/gtsam/sfm/sfm.i b/gtsam/sfm/sfm.i
index 89fc2b281..0374d99b3 100644
--- a/gtsam/sfm/sfm.i
+++ b/gtsam/sfm/sfm.i
@@ -4,14 +4,6 @@
 
 namespace gtsam {
 
-#include <gtsam/sfm/GtsfmData.h>
-class GtsfmData
-{
-  GtsfmData(const int numberImages);
-  void write_points(std::vector<gtsam::GtsfmImage>& images, const std::string save_dir);
-  void write_images(std::vector<gtsam::GtsfmImage>& images, const std::string save_dir);
-};
-
 #include <gtsam/sfm/DsfTrackGenerator.h>
 
 class MatchIndicesMap {
@@ -43,6 +35,13 @@ class Keypoints
 }; // check if this should be a method
 
 
+class NamedSfmMeasurement
+{
+  size_t i;
+  gtsam::Point2 uv;
+  NamedSfmMeasurement(size_t i, gtsam::Point2 uv);
+};
+
 class SfmTrack2d
 {
   void addMeasurement(const gtsam::NamedSfmMeasurement &m);
diff --git a/python/gtsam/tests/test_DsfTrackGenerator.py b/python/gtsam/tests/test_DsfTrackGenerator.py
index 51665df7a..953167dca 100644
--- a/python/gtsam/tests/test_DsfTrackGenerator.py
+++ b/python/gtsam/tests/test_DsfTrackGenerator.py
@@ -32,4 +32,25 @@ class TestDsfTrackGenerator(GtsamTestCase):
         matches_dict[(1, 2)] = np.array([[2, 0], [1, 1]])
 
         tracks = DsfTrackGenerator().generate_tracks_from_pairwise_matches(matches_dict, keypoints_list)
-        print(tracks[0])
+
+        assert len(tracks) == 3
+
+        # Verify track 0.
+        assert np.allclose(tracks[0].measurements()[0].uv, np.array([10.0, 20.0]))
+        assert np.allclose(tracks[0].measurements()[1].uv, np.array([50.0, 60.0]))
+        assert tracks[0].measurements()[0].i, 0
+        assert tracks[0].measurements()[1].i, 1
+
+        # Verify track 1.
+        assert np.allclose(tracks[1].measurements()[0].uv, np.array([30.0, 40.0]))
+        assert np.allclose(tracks[1].measurements()[1].uv, np.array([70.0, 80.0]))
+        assert np.allclose(tracks[1].measurements()[2].uv, np.array([130.0, 140.0]))
+        assert tracks[1].measurements()[0].i, 0
+        assert tracks[1].measurements()[1].i, 1
+        assert tracks[1].measurements()[2].i, 2
+
+        # Verify track 2.
+        assert np.allclose(tracks[2].measurements()[0].uv, np.array([90.0, 100.0]))
+        assert np.allclose(tracks[2].measurements()[1].uv, np.array([110.0, 120.0]))
+        assert tracks[2].measurements()[0].i, 1
+        assert tracks[2].measurements()[1].i, 2