diff --git a/gtsam/sam/sam.i b/gtsam/sam/sam.i index 7ba401f1e..90c319ede 100644 --- a/gtsam/sam/sam.i +++ b/gtsam/sam/sam.i @@ -20,6 +20,8 @@ virtual class RangeFactor : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; + + const double measured() const; }; // between points: @@ -54,6 +56,9 @@ virtual class RangeFactorWithTransform : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; + + // Use `double` instead of template since that is all we need. + const double measured() const; }; typedef gtsam::RangeFactorWithTransform @@ -73,6 +78,8 @@ virtual class BearingFactor : gtsam::NoiseModelFactor { // enabling serialization functionality void serialize() const; + + const BEARING& measured() const; }; typedef gtsam::BearingFactor diff --git a/python/gtsam/tests/test_sam.py b/python/gtsam/tests/test_sam.py new file mode 100644 index 000000000..e01b9c1d1 --- /dev/null +++ b/python/gtsam/tests/test_sam.py @@ -0,0 +1,55 @@ +""" +GTSAM Copyright 2010-2019, Georgia Tech Research Corporation, +Atlanta, Georgia 30332-0415 +All Rights Reserved + +See LICENSE for the license information + +Basis unit tests. +Author: Frank Dellaert & Varun Agrawal (Python) +""" +import unittest + +import gtsam +from gtsam.utils.test_case import GtsamTestCase + + +class TestSam(GtsamTestCase): + """ + Tests python binding for classes/functions in `sam.i`. + """ + def test_RangeFactor2D(self): + """ + Test that `measured` works as expected for RangeFactor2D. + """ + measurement = 10.0 + factor = gtsam.RangeFactor2D(1, 2, measurement, + gtsam.noiseModel.Isotropic.Sigma(1, 1)) + self.assertEqual(measurement, factor.measured()) + + def test_BearingFactor2D(self): + """ + Test that `measured` works as expected for BearingFactor2D. + """ + measurement = gtsam.Rot2(.3) + factor = gtsam.BearingFactor2D(1, 2, measurement, + gtsam.noiseModel.Isotropic.Sigma(1, 1)) + self.gtsamAssertEquals(measurement, factor.measured()) + + def test_BearingRangeFactor2D(self): + """ + Test that `measured` works as expected for BearingRangeFactor2D. + """ + range_measurement = 10.0 + bearing_measurement = gtsam.Rot2(0.3) + factor = gtsam.BearingRangeFactor2D( + 1, 2, bearing_measurement, range_measurement, + gtsam.noiseModel.Isotropic.Sigma(2, 1)) + measurement = factor.measured() + + self.assertEqual(range_measurement, measurement.range()) + self.gtsamAssertEquals(bearing_measurement, measurement.bearing()) + + +if __name__ == "__main__": + unittest.main()