12345qiupeng
/
gtsam
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

fix remaining python tests

release/4.3a0
Varun Agrawal 2023-06-15 16:34:37 -04:00
parent 5b588f2ea7
commit 31adb3ed45
13 changed files with 49 additions and 43 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
python/CMakeLists.txt
View File

@ -52,6 +52,7 @@ set(ignore
gtsam::IndexPairVector
gtsam::BetweenFactorPose2s
gtsam::BetweenFactorPose3s
gtsam::FixedLagSmootherKeyTimestampMap
gtsam::FixedLagSmootherKeyTimestampMapValue
gtsam::Point2Vector
gtsam::Point2Pairs

7
python/gtsam/examples/FixedLagSmootherExample.py
View File

@ -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]

2
python/gtsam/preamble/base.h
View File

@ -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<gtsam::Matrix>); // JacobianVector

3
python/gtsam/preamble/custom.h
View File

@ -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<gtsam::Matrix>); // JacobianVector

2
python/gtsam/preamble/geometry.h
View File

@ -11,8 +11,6 @@
* mutations on Python side will not be reflected on C++.
*/
#include <pybind11/stl.h>
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3_S2>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Bundler>>);
PYBIND11_MAKE_OPAQUE(gtsam::CameraSet<gtsam::PinholeCamera<gtsam::Cal3Unified>>);

8
python/gtsam/preamble/hybrid.h
View File

@ -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 <pybind11/stl.h>
// NOTE: Needed since we are including pybind11/stl.h.
#ifdef GTSAM_ALLOCATOR_TBB
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key, tbb::tbb_allocator<gtsam::Key>>);
#else
PYBIND11_MAKE_OPAQUE(std::vector<gtsam::Key>);
#endif

1
python/gtsam/specializations/base.h
View File

@ -11,4 +11,3 @@
* and saves one copy operation.
*/
py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

3
python/gtsam/specializations/custom.h
View File

@ -10,3 +10,6 @@
* with `PYBIND11_MAKE_OPAQUE` this allows the types to be modified with Python,
* and saves one copy operation.
*/
// Added so that CustomFactor can pass the JacobianVector to the C++ side
py::bind_vector<std::vector<gtsam::Matrix> >(m_, "JacobianVector");

2
python/gtsam/specializations/inference.h
View File

@ -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<std::map<char, double>>(m_, "__MapCharDouble");

9
python/gtsam/tests/test_FixedLagSmootherExample.py
View File

@ -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] *

17
python/gtsam/tests/test_HybridBayesNet.py
View File

@ -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.
@ -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)

30
python/gtsam/tests/test_custom_factor.py
View File

@ -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

3
python/gtsam/utils/plot.py
View File

@ -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):