diff --git a/gtsam/nonlinear/ISAM2Result.h b/gtsam/nonlinear/ISAM2Result.h index b249af5c5..be91f17e2 100644 --- a/gtsam/nonlinear/ISAM2Result.h +++ b/gtsam/nonlinear/ISAM2Result.h @@ -175,6 +175,7 @@ struct ISAM2Result { /** Getters and Setters */ size_t getVariablesRelinearized() const { return variablesRelinearized; } size_t getVariablesReeliminated() const { return variablesReeliminated; } + FactorIndices getNewFactorsIndices() const { return newFactorsIndices; } size_t getCliques() const { return cliques; } double getErrorBefore() const { return errorBefore ? *errorBefore : std::nan(""); } double getErrorAfter() const { return errorAfter ? *errorAfter : std::nan(""); } diff --git a/gtsam/nonlinear/nonlinear.i b/gtsam/nonlinear/nonlinear.i index 800208c33..326b84d16 100644 --- a/gtsam/nonlinear/nonlinear.i +++ b/gtsam/nonlinear/nonlinear.i @@ -682,6 +682,7 @@ class ISAM2Result { /** Getters and Setters for all properties */ size_t getVariablesRelinearized() const; size_t getVariablesReeliminated() const; + FactorIndices getNewFactorsIndices() const; size_t getCliques() const; double getErrorBefore() const; double getErrorAfter() const; 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_VisualISAMExample.py b/python/gtsam/tests/test_VisualISAMExample.py index 6eb05eeee..ebc77e2e3 100644 --- a/python/gtsam/tests/test_VisualISAMExample.py +++ b/python/gtsam/tests/test_VisualISAMExample.py @@ -10,9 +10,6 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python) """ import unittest -import numpy as np - -import gtsam import gtsam.utils.visual_data_generator as generator import gtsam.utils.visual_isam as visual_isam from gtsam import symbol @@ -20,8 +17,9 @@ from gtsam.utils.test_case import GtsamTestCase class TestVisualISAMExample(GtsamTestCase): - + """Test class for ISAM2 with visual landmarks.""" def test_VisualISAMExample(self): + """Test to see if ISAM works as expected for a simple visual SLAM example.""" # Data Options options = generator.Options() options.triangle = False @@ -39,19 +37,22 @@ class TestVisualISAMExample(GtsamTestCase): data, truth = generator.generate_data(options) # Initialize iSAM with the first pose and points - isam, result, nextPose = visual_isam.initialize(data, truth, isamOptions) + isam, result, nextPose = visual_isam.initialize( + data, truth, isamOptions) # Main loop for iSAM: stepping through all poses for currentPose in range(nextPose, options.nrCameras): - isam, result = visual_isam.step(data, isam, result, truth, currentPose) + isam, result = visual_isam.step(data, isam, result, truth, + currentPose) - for i in range(len(truth.cameras)): + for i, _ in enumerate(truth.cameras): pose_i = result.atPose3(symbol('x', i)) self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5) - for j in range(len(truth.points)): + for j, _ in enumerate(truth.points): point_j = result.atPoint3(symbol('l', j)) self.gtsamAssertEquals(point_j, truth.points[j], 1e-5) + if __name__ == "__main__": unittest.main() 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()