diff --git a/gtsam/gtsam.i b/gtsam/gtsam.i index 75f11b449..f52b6cedd 100644 --- a/gtsam/gtsam.i +++ b/gtsam/gtsam.i @@ -2961,6 +2961,13 @@ class BinaryMeasurement { typedef gtsam::BinaryMeasurement BinaryMeasurementUnit3; typedef gtsam::BinaryMeasurement BinaryMeasurementRot3; +class BinaryMeasurementsUnit3 { + BinaryMeasurementsUnit3(); + size_t size() const; + gtsam::BinaryMeasurement at(size_t idx) const; + void push_back(const gtsam::BinaryMeasurement& measurement); +}; + #include // TODO(frank): copy/pasta below until we have integer template paremeters in wrap! @@ -3082,6 +3089,16 @@ class ShonanAveraging3 { pair run(const gtsam::Values& initial, size_t min_p, size_t max_p) const; }; +#include +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 //************************************************************************* diff --git a/gtsam/slam/dataset.h b/gtsam/slam/dataset.h index 2e03c92d8..91ceaa5fd 100644 --- a/gtsam/slam/dataset.h +++ b/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>; #ifdef GTSAM_ALLOW_DEPRECATED_SINCE_V41 inline boost::optional parseVertex(std::istream &is, const std::string &tag) { diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 07e2d51ac..bec02fb64 100644 --- a/python/CMakeLists.txt +++ b/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 diff --git a/python/gtsam/specializations.h b/python/gtsam/specializations.h index dbb95c9d6..3f6b8fa38 100644 --- a/python/gtsam/specializations.h +++ b/python/gtsam/specializations.h @@ -9,5 +9,6 @@ py::bind_vector >(m_, "Pose3Vector"); py::bind_vector > > >(m_, "BetweenFactorPose3s"); py::bind_vector > > >(m_, "BetweenFactorPose2s"); +py::bind_vector > >(m_, "BinaryMeasurementsUnit3"); py::bind_map(m_, "IndexPairSetMap"); -py::bind_vector(m_, "IndexPairVector"); \ No newline at end of file +py::bind_vector(m_, "IndexPairVector"); diff --git a/python/gtsam/tests/test_TranslationRecovery.py b/python/gtsam/tests/test_TranslationRecovery.py new file mode 100644 index 000000000..ec6d6a83f --- /dev/null +++ b/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() +