diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt index 2557da237..684cf31a1 100644 --- a/python/CMakeLists.txt +++ b/python/CMakeLists.txt @@ -52,6 +52,7 @@ set(ignore gtsam::IndexPairVector gtsam::BetweenFactorPose2s gtsam::BetweenFactorPose3s + gtsam::FixedLagSmootherKeyTimestampMap gtsam::FixedLagSmootherKeyTimestampMapValue gtsam::Point2Vector gtsam::Point2Pairs diff --git a/python/gtsam/examples/FixedLagSmootherExample.py b/python/gtsam/examples/FixedLagSmootherExample.py index 99af0edcf..c56ebbe07 100644 --- a/python/gtsam/examples/FixedLagSmootherExample.py +++ b/python/gtsam/examples/FixedLagSmootherExample.py @@ -16,6 +16,7 @@ import numpy as np import gtsam import gtsam_unstable + def BatchFixedLagSmootherExample(): """ Runs a batch fixed smoother on an agent with two odometry @@ -31,7 +32,7 @@ def BatchFixedLagSmootherExample(): # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() + new_timestamps = {} # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) @@ -39,7 +40,7 @@ def BatchFixedLagSmootherExample(): X1 = 0 new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) - new_timestamps.insert((X1, 0.0)) + new_timestamps[X1] = 0.0 delta_time = 0.25 time = 0.25 @@ -49,7 +50,7 @@ def BatchFixedLagSmootherExample(): current_key = int(1000 * time) # assign current key to the current timestamp - new_timestamps.insert((current_key, time)) + new_timestamps[current_key] = time # Add a guess for this pose to the new values # Assume that the robot moves at 2 m/s. Position is time[s] * 2[m/s] diff --git a/python/gtsam/preamble/base.h b/python/gtsam/preamble/base.h index b1ed2ba14..d07a75f6f 100644 --- a/python/gtsam/preamble/base.h +++ b/python/gtsam/preamble/base.h @@ -10,5 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ - -PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/custom.h b/python/gtsam/preamble/custom.h index d07a75f6f..d7bc6360f 100644 --- a/python/gtsam/preamble/custom.h +++ b/python/gtsam/preamble/custom.h @@ -10,3 +10,6 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ + +// Added so that CustomFactor can pass the JacobianVector to the C++ side +PYBIND11_MAKE_OPAQUE(std::vector); // JacobianVector diff --git a/python/gtsam/preamble/geometry.h b/python/gtsam/preamble/geometry.h index 5c9df8ef8..40b841abc 100644 --- a/python/gtsam/preamble/geometry.h +++ b/python/gtsam/preamble/geometry.h @@ -11,8 +11,6 @@ * mutations on Python side will not be reflected on C++. */ -#include - PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); PYBIND11_MAKE_OPAQUE(gtsam::CameraSet>); diff --git a/python/gtsam/preamble/hybrid.h b/python/gtsam/preamble/hybrid.h index 90a062d66..d07a75f6f 100644 --- a/python/gtsam/preamble/hybrid.h +++ b/python/gtsam/preamble/hybrid.h @@ -10,11 +10,3 @@ * Without this they will be automatically converted to a Python object, and all * mutations on Python side will not be reflected on C++. */ -#include - -// NOTE: Needed since we are including pybind11/stl.h. -#ifdef GTSAM_ALLOCATOR_TBB -PYBIND11_MAKE_OPAQUE(std::vector>); -#else -PYBIND11_MAKE_OPAQUE(std::vector); -#endif diff --git a/python/gtsam/specializations/base.h b/python/gtsam/specializations/base.h index 995753da1..22fe3beff 100644 --- a/python/gtsam/specializations/base.h +++ b/python/gtsam/specializations/base.h @@ -11,4 +11,3 @@ * and saves one copy operation. */ -py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/custom.h b/python/gtsam/specializations/custom.h index d46ccdc66..cffb5115f 100644 --- a/python/gtsam/specializations/custom.h +++ b/python/gtsam/specializations/custom.h @@ -9,4 +9,7 @@ * interface, but without the `` copying mechanism. Combined * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. - */ \ No newline at end of file + */ + +// Added so that CustomFactor can pass the JacobianVector to the C++ side +py::bind_vector >(m_, "JacobianVector"); diff --git a/python/gtsam/specializations/inference.h b/python/gtsam/specializations/inference.h index 9e23444ea..da8842eaf 100644 --- a/python/gtsam/specializations/inference.h +++ b/python/gtsam/specializations/inference.h @@ -10,5 +10,3 @@ * with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python, * and saves one copy operation. */ - -py::bind_map>(m_, "__MapCharDouble"); diff --git a/python/gtsam/tests/test_FixedLagSmootherExample.py b/python/gtsam/tests/test_FixedLagSmootherExample.py index ddd2d49e4..412abe987 100644 --- a/python/gtsam/tests/test_FixedLagSmootherExample.py +++ b/python/gtsam/tests/test_FixedLagSmootherExample.py @@ -11,10 +11,11 @@ Author: Frank Dellaert & Duy Nguyen Ta (Python) import unittest import numpy as np +from gtsam.utils.test_case import GtsamTestCase import gtsam import gtsam_unstable -from gtsam.utils.test_case import GtsamTestCase + class TestFixedLagSmootherExample(GtsamTestCase): ''' @@ -36,7 +37,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): # that will be sent to the smoothers new_factors = gtsam.NonlinearFactorGraph() new_values = gtsam.Values() - new_timestamps = gtsam.FixedLagSmootherKeyTimestampMap() + new_timestamps = {} # Create a prior on the first pose, placing it at the origin prior_mean = gtsam.Pose2(0, 0, 0) @@ -47,7 +48,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): gtsam.PriorFactorPose2( X1, prior_mean, prior_noise)) new_values.insert(X1, prior_mean) - new_timestamps.insert((X1, 0.0)) + new_timestamps[X1] = 0.0 delta_time = 0.25 time = 0.25 @@ -77,7 +78,7 @@ class TestFixedLagSmootherExample(GtsamTestCase): current_key = int(1000 * time) # assign current key to the current timestamp - new_timestamps.insert((current_key, time)) + new_timestamps[current_key] = time # Add a guess for this pose to the new values # Assume that the robot moves at 2 m/s. Position is time[s] * diff --git a/python/gtsam/tests/test_HybridBayesNet.py b/python/gtsam/tests/test_HybridBayesNet.py index 01e1c5a5d..bc685dec6 100644 --- a/python/gtsam/tests/test_HybridBayesNet.py +++ b/python/gtsam/tests/test_HybridBayesNet.py @@ -17,8 +17,9 @@ import numpy as np from gtsam.symbol_shorthand import A, X from gtsam.utils.test_case import GtsamTestCase -from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues, GaussianConditional, - GaussianMixture, HybridBayesNet, HybridValues, noiseModel, VectorValues) +from gtsam import (DiscreteConditional, DiscreteKeys, DiscreteValues, + GaussianConditional, GaussianMixture, HybridBayesNet, + HybridValues, VectorValues, noiseModel) class TestHybridBayesNet(GtsamTestCase): @@ -31,8 +32,8 @@ class TestHybridBayesNet(GtsamTestCase): # Create the continuous conditional I_1x1 = np.eye(1) - conditional = GaussianConditional.FromMeanAndStddev(X(0), 2 * I_1x1, X(1), [-4], - 5.0) + conditional = GaussianConditional.FromMeanAndStddev( + X(0), 2 * I_1x1, X(1), [-4], 5.0) # Create the noise models model0 = noiseModel.Diagonal.Sigmas([2.0]) @@ -47,8 +48,9 @@ class TestHybridBayesNet(GtsamTestCase): # Create hybrid Bayes net. bayesNet = HybridBayesNet() bayesNet.push_back(conditional) - bayesNet.push_back(GaussianMixture( - [X(1)], [], discrete_keys, [conditional0, conditional1])) + bayesNet.push_back( + GaussianMixture([X(1)], [], discrete_keys, + [conditional0, conditional1])) bayesNet.push_back(DiscreteConditional(Asia, "99/1")) # Create values at which to evaluate. @@ -76,7 +78,7 @@ class TestHybridBayesNet(GtsamTestCase): self.check_invariance(bayesNet.at(0).asGaussian(), continuous) self.check_invariance(bayesNet.at(0).asGaussian(), values) self.check_invariance(bayesNet.at(0), values) - + self.check_invariance(bayesNet.at(1), values) self.check_invariance(bayesNet.at(2).asDiscrete(), discrete) @@ -89,7 +91,8 @@ class TestHybridBayesNet(GtsamTestCase): self.assertTrue(probability >= 0.0) logProb = conditional.logProbability(values) self.assertAlmostEqual(probability, np.exp(logProb)) - expected = conditional.logNormalizationConstant() - conditional.error(values) + expected = conditional.logNormalizationConstant() - \ + conditional.error(values) self.assertAlmostEqual(logProb, expected) diff --git a/python/gtsam/tests/test_custom_factor.py b/python/gtsam/tests/test_custom_factor.py index a3bb00384..d19706c49 100644 --- a/python/gtsam/tests/test_custom_factor.py +++ b/python/gtsam/tests/test_custom_factor.py @@ -11,17 +11,20 @@ Author: Fan Jiang import unittest from typing import List -import gtsam import numpy as np -from gtsam import CustomFactor, JacobianFactor, Pose2, Values from gtsam.utils.test_case import GtsamTestCase +import gtsam +from gtsam import CustomFactor, Pose2, Values + class TestCustomFactor(GtsamTestCase): + def test_new(self): """Test the creation of a new CustomFactor""" - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """Minimal error function stub""" return np.array([1, 0, 0]), H @@ -31,7 +34,8 @@ class TestCustomFactor(GtsamTestCase): def test_new_keylist(self): """Test the creation of a new CustomFactor""" - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """Minimal error function stub""" return np.array([1, 0, 0]) @@ -42,7 +46,8 @@ class TestCustomFactor(GtsamTestCase): """Test if calling the factor works (only error)""" expected_pose = Pose2(1, 1, 0) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]) -> np.ndarray: + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]) -> np.ndarray: """Minimal error function with no Jacobian""" key0 = this.keys()[0] error = -v.atPose2(key0).localCoordinates(expected_pose) @@ -64,7 +69,8 @@ class TestCustomFactor(GtsamTestCase): expected = Pose2(2, 2, np.pi / 2) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """ the custom error function. One can freely use variables captured from the outside scope. Or the variables can be acquired by indexing `v`. @@ -103,7 +109,8 @@ class TestCustomFactor(GtsamTestCase): gT1 = Pose2(1, 2, np.pi / 2) gT2 = Pose2(-1, 4, np.pi) - def error_func(this: CustomFactor, v: gtsam.Values, _: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + _: List[np.ndarray]): """Minimal error function stub""" return np.array([1, 0, 0]) @@ -124,7 +131,8 @@ class TestCustomFactor(GtsamTestCase): expected = Pose2(2, 2, np.pi / 2) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated @@ -137,7 +145,8 @@ class TestCustomFactor(GtsamTestCase): gT1, gT2 = v.atPose2(key0), v.atPose2(key1) error = expected.localCoordinates(gT1.between(gT2)) - self.assertTrue(H is None) # Should be true if we only request the error + self.assertTrue( + H is None) # Should be true if we only request the error if H is not None: result = gT1.between(gT2) @@ -164,7 +173,8 @@ class TestCustomFactor(GtsamTestCase): expected = Pose2(2, 2, np.pi / 2) - def error_func(this: CustomFactor, v: gtsam.Values, H: List[np.ndarray]): + def error_func(this: CustomFactor, v: gtsam.Values, + H: List[np.ndarray]): """ Error function that mimics a BetweenFactor :param this: reference to the current CustomFactor being evaluated diff --git a/python/gtsam/utils/plot.py b/python/gtsam/utils/plot.py index 880c436e8..fe067e787 100644 --- a/python/gtsam/utils/plot.py +++ b/python/gtsam/utils/plot.py @@ -12,7 +12,6 @@ from mpl_toolkits.mplot3d import Axes3D # pylint: disable=unused-import import gtsam from gtsam import Marginals, Point2, Point3, Pose2, Pose3, Values - # For translation between a scaling of the uncertainty ellipse and the # percentage of inliers see discussion in # [PR 1067](https://github.com/borglab/gtsam/pull/1067) @@ -557,7 +556,7 @@ def plot_incremental_trajectory(fignum: int, axes = fig.axes[0] poses = gtsam.utilities.allPose3s(values) - keys = gtsam.KeyVector(poses.keys()) + keys = poses.keys() for key in keys[start:]: if values.exists(key):