diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i
index d6ae409f8..dcacf14d9 100644
--- a/gtsam/gtsam.i
+++ b/gtsam/gtsam.i
@@ -2762,12 +2762,12 @@ virtual class EssentialMatrixFactor : gtsam::NoiseModelFactor {
 
 class SfmTrack {
   SfmTrack();
-  Point3 point3() const;
+  SfmTrack(const gtsam::Point3& pt);
+  const Point3& point3() const;
   size_t number_measurements() const;
-  void set_point3(gtsam::Point3& p_);
   pair<size_t, gtsam::Point2> measurement(size_t idx) const;
   pair<size_t, size_t> siftIndex(size_t idx) const;
-  void add_measurement(const pair<size_t, gtsam::Point2>& m);
+  void add_measurement(size_t idx, const gtsam::Point2& m);
 };
 
 class SfmData {
diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h
index 85b32ff5a..93bd2b2ee 100644
--- a/gtsam/slam/dataset.h
+++ b/gtsam/slam/dataset.h
@@ -217,6 +217,7 @@ typedef std::pair<size_t, size_t> SiftIndex;
 /// Define the structure for the 3D points
 struct SfmTrack {
   SfmTrack(): p(0,0,0) {}
+  SfmTrack(const gtsam::Point3& pt) : p(pt) {}
   Point3 p; ///< 3D position of the point
   float r, g, b; ///< RGB color of the 3D point
   std::vector<SfmMeasurement> measurements; ///< The 2D image projections (id,(u,v))
@@ -226,10 +227,6 @@ struct SfmTrack {
   size_t number_measurements() const {
     return measurements.size();
   }
-  /// Set 3D point
-  void set_point3(const Point3& p_){
-    p = p_;
-  }
   /// Get the measurement (camera index, Point2) at pose index `idx`
   SfmMeasurement measurement(size_t idx) const {
     return measurements[idx];
@@ -239,12 +236,12 @@ struct SfmTrack {
     return siftIndices[idx];
   }
   /// Get 3D point
-  Point3 point3() const {
+  const Point3& point3() const {
     return p;
   }
-  /// Add measurement to track
-  void add_measurement(const pair<size_t, gtsam::Point2>& m) {
-    measurements.push_back(m);
+  /// Add measurement (camera_idx, Point2) to track
+  void add_measurement(size_t idx, const gtsam::Point2& m) {
+    measurements.emplace_back(idx, m);
   }
 };
 
diff --git a/python/gtsam/tests/test_SfmData.py b/python/gtsam/tests/test_SfmData.py
index aa93a69bb..51564fb6f 100644
--- a/python/gtsam/tests/test_SfmData.py
+++ b/python/gtsam/tests/test_SfmData.py
@@ -26,7 +26,8 @@ class TestSfmData(GtsamTestCase):
     def setUp(self):
         """Initialize SfmData and SfmTrack"""
         self.data = gtsam.SfmData()
-        self.tracks = gtsam.SfmTrack()
+        # initialize SfmTrack with 3D point
+        self.tracks = gtsam.SfmTrack(gtsam.Point3(2.5, 3.3, 1.2))
 
     def test_tracks(self):
         """Test functions in SfmTrack"""
@@ -37,17 +38,14 @@ class TestSfmData(GtsamTestCase):
         uv_i1 = gtsam.Point2(12.6, 82)
         # translating point uv_i1 along X-axis
         uv_i2 = gtsam.Point2(24.88, 82)
-        m_i1 = (i1, uv_i1)
-        m_i2 = (i2, uv_i2)
         # add measurements to the track
-        self.tracks.add_measurement(m_i1)
-        self.tracks.add_measurement(m_i2)
+        self.tracks.add_measurement(i1, uv_i1)
+        self.tracks.add_measurement(i2, uv_i2)
         # Number of measurements in the track is 2
         self.assertEqual(self.tracks.number_measurements(), 2)
         # camera_idx in the first measurement of the track corresponds to i1
-        self.assertEqual(self.tracks.measurement(0)[0], i1)
-        # Set arbitrary 3D point corresponding to the track
-        self.tracks.set_point3(gtsam.Point3(2.5, 3.3, 1.2))
+        cam_idx, img_measurement = self.tracks.measurement(0)
+        self.assertEqual(cam_idx, i1)
         np.testing.assert_array_almost_equal(
             gtsam.Point3(2.5,3.3,1.2), 
             self.tracks.point3()
@@ -63,17 +61,17 @@ class TestSfmData(GtsamTestCase):
         # translating along X-axis
         uv_i2 = gtsam.Point2(45.7, 45.64)
         uv_i3 = gtsam.Point2(68.35, 45.64)
-        m_i1, m_i2, m_i3 = (i1, uv_i1),  (i2, uv_i2), (i3, uv_i3)
         # add measurements to the track
-        track2.add_measurement(m_i1)
-        track2.add_measurement(m_i2)
-        track2.add_measurement(m_i3)
+        track2.add_measurement(i1, uv_i1)
+        track2.add_measurement(i2, uv_i2)
+        track2.add_measurement(i3, uv_i3)
         self.data.add_track(self.tracks)
         self.data.add_track(track2)
         # Number of tracks in SfmData is 2
         self.assertEqual(self.data.number_tracks(), 2)
         # camera idx of first measurement of second track corresponds to i1
-        self.assertEqual(self.data.track(1).measurement(0)[0], i1)
+        cam_idx, img_measurement = self.data.track(1).measurement(0)
+        self.assertEqual(cam_idx, i1)
 
 if __name__ == '__main__':
     unittest.main()