Merge pull request #513 from borglab/feature/wrap_translation_averaging

Wrap translation averaging

gtsam/gtsam.i

@@ -2961,6 +2961,13 @@ class BinaryMeasurement {
 typedef gtsam::BinaryMeasurement<gtsam::Unit3> BinaryMeasurementUnit3;
 typedef gtsam::BinaryMeasurement<gtsam::Rot3> BinaryMeasurementRot3;
 
+class BinaryMeasurementsUnit3 {
+  BinaryMeasurementsUnit3();
+  size_t size() const;
+  gtsam::BinaryMeasurement<gtsam::Unit3> at(size_t idx) const;
+  void push_back(const gtsam::BinaryMeasurement<gtsam::Unit3>& measurement);
+};
+
 #include <gtsam/sfm/ShonanAveraging.h>
 
 // TODO(frank): copy/pasta below until we have integer template paremeters in wrap!
@@ -3082,6 +3089,16 @@ class ShonanAveraging3 {
   pair<gtsam::Values, double> run(const gtsam::Values& initial, size_t min_p, size_t max_p) const;
 };
 
+#include <gtsam/sfm/TranslationRecovery.h>
+class TranslationRecovery {
+  TranslationRecovery(const gtsam::BinaryMeasurementsUnit3 &relativeTranslations,
+                      const gtsam::LevenbergMarquardtParams &lmParams);
+  TranslationRecovery(
+      const gtsam::BinaryMeasurementsUnit3 & relativeTranslations);  // default LevenbergMarquardtParams
+  gtsam::Values run(const double scale) const;
+  gtsam::Values run() const;    // default scale = 1.0
+};
+
 //*************************************************************************
 // Navigation
 //*************************************************************************

gtsam/slam/dataset.h

@@ -362,6 +362,7 @@ parse3DFactors(const std::string &filename,
                const noiseModel::Diagonal::shared_ptr &model = nullptr,
                size_t maxIndex = 0);
 
+using BinaryMeasurementsUnit3 = std::vector<BinaryMeasurement<Unit3>>;
 #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41
 inline boost::optional<IndexedPose> parseVertex(std::istream &is,
                                                 const std::string &tag) {

python/CMakeLists.txt

@@ -36,7 +36,8 @@ set(ignore
     gtsam::BetweenFactorPose3s
     gtsam::Point2Vector
     gtsam::Pose3Vector
-    gtsam::KeyVector)
+    gtsam::KeyVector
+    gtsam::BinaryMeasurementsUnit3)
 
 pybind_wrap(gtsam_py # target
             ${PROJECT_SOURCE_DIR}/gtsam/gtsam.i # interface_header
@@ -78,7 +79,8 @@ set(ignore
         gtsam::Point2Vector
         gtsam::Pose3Vector
         gtsam::KeyVector
-        gtsam::FixedLagSmootherKeyTimestampMapValue)
+        gtsam::FixedLagSmootherKeyTimestampMapValue
+        gtsam::BinaryMeasurementsUnit3)
 pybind_wrap(gtsam_unstable_py # target
         ${PROJECT_SOURCE_DIR}/gtsam_unstable/gtsam_unstable.i # interface_header
         "gtsam_unstable.cpp" # generated_cpp

python/gtsam/specializations.h

@@ -9,5 +9,6 @@ py::bind_vector<std::vector<gtsam::Point2, Eigen::aligned_allocator<gtsam::Point
 py::bind_vector<std::vector<gtsam::Pose3> >(m_, "Pose3Vector");
 py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose3> > > >(m_, "BetweenFactorPose3s");
 py::bind_vector<std::vector<boost::shared_ptr<gtsam::BetweenFactor<gtsam::Pose2> > > >(m_, "BetweenFactorPose2s");
+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_vector<gtsam::IndexPairVector>(m_, "IndexPairVector");

python/gtsam/tests/test_TranslationRecovery.py

@@ -0,0 +1,58 @@
+from __future__ import print_function
+
+import numpy as np
+import unittest
+
+import gtsam
+
+""" Returns example pose values of 3 points A, B and C in the world frame """
+def ExampleValues():
+    T = []
+    T.append(gtsam.Point3(np.array([3.14, 1.59, 2.65])))
+    T.append(gtsam.Point3(np.array([-1.0590e+00, -3.6017e-02, -1.5720e+00])))
+    T.append(gtsam.Point3(np.array([8.5034e+00, 6.7499e+00, -3.6383e+00])))
+    
+    data = gtsam.Values()
+    for i in range(len(T)):
+        data.insert(i, gtsam.Pose3(gtsam.Rot3(), T[i]))
+    return data
+
+""" Returns binary measurements for the points in the given edges."""
+def SimulateMeasurements(gt_poses, graph_edges):
+    measurements = gtsam.BinaryMeasurementsUnit3()
+    for edge in graph_edges:
+        Ta = gt_poses.atPose3(edge[0]).translation()
+        Tb = gt_poses.atPose3(edge[1]).translation()
+        measurements.append(gtsam.BinaryMeasurementUnit3( \
+            edge[0], edge[1], gtsam.Unit3(Tb - Ta), \
+            gtsam.noiseModel.Isotropic.Sigma(3, 0.01)))
+    return measurements
+
+""" Tests for the translation recovery class """
+class TestTranslationRecovery(unittest.TestCase):
+    """Test selected Translation Recovery methods."""
+
+    def test_constructor(self):
+        """Construct from binary measurements."""
+        algorithm = gtsam.TranslationRecovery(gtsam.BinaryMeasurementsUnit3())
+        self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
+
+    def test_run(self):
+        gt_poses = ExampleValues()
+        measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]])
+        algorithm = gtsam.TranslationRecovery(measurements)
+        scale = 2.0
+        result = algorithm.run(scale)
+
+        w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(0).translation()
+        w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(0).translation()
+        w_aTc_expected = w_aTc*scale/np.linalg.norm(w_aTb)
+        w_aTb_expected = w_aTb*scale/np.linalg.norm(w_aTb)
+
+        np.testing.assert_array_almost_equal(result.atPoint3(0), np.array([0,0,0]), 6, "Origin result is incorrect.")
+        np.testing.assert_array_almost_equal(result.atPoint3(1), w_aTb_expected, 6, "Point B result is incorrect.")
+        np.testing.assert_array_almost_equal(result.atPoint3(2), w_aTc_expected, 6, "Point C result is incorrect.")
+
+if __name__ == "__main__":
+    unittest.main()
+