gtsam-develop 4.3a0.dev202505270244__cp313-cp313-macosx_11_0_arm64.whl → 4.3a0.dev202505281327__cp313-cp313-macosx_11_0_arm64.whl
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
Potentially problematic release.
This version of gtsam-develop might be problematic. Click here for more details.
- gtsam/.dylibs/{libgtsam.4.3a0.dev202505270244.dylib → libgtsam.4.3a0.dev202505281327.dylib} +0 -0
- gtsam/.dylibs/{libgtsam_unstable.4.3a0.dev202505270244.dylib → libgtsam_unstable.4.3a0.dev202505281327.dylib} +0 -0
- gtsam/__init__.pyi +13 -13
- gtsam/gtsam/__init__.pyi +161 -0
- gtsam/gtsam.cpython-313-darwin.so +0 -0
- gtsam/tests/test_HybridValues.py +210 -14
- {gtsam_develop-4.3a0.dev202505270244.dist-info → gtsam_develop-4.3a0.dev202505281327.dist-info}/METADATA +1 -1
- {gtsam_develop-4.3a0.dev202505270244.dist-info → gtsam_develop-4.3a0.dev202505281327.dist-info}/RECORD +11 -11
- gtsam_unstable/gtsam_unstable.cpython-313-darwin.so +0 -0
- {gtsam_develop-4.3a0.dev202505270244.dist-info → gtsam_develop-4.3a0.dev202505281327.dist-info}/WHEEL +0 -0
- {gtsam_develop-4.3a0.dev202505270244.dist-info → gtsam_develop-4.3a0.dev202505281327.dist-info}/top_level.txt +0 -0
|
Binary file
|
|
Binary file
|
gtsam/__init__.pyi
CHANGED
|
@@ -2,28 +2,28 @@
|
|
|
2
2
|
Module definition file for GTSAM
|
|
3
3
|
"""
|
|
4
4
|
from __future__ import annotations
|
|
5
|
-
from builtins import dict as MatchIndicesMap
|
|
6
5
|
from builtins import dict as KeyPairDoubleMap
|
|
6
|
+
from builtins import dict as MatchIndicesMap
|
|
7
7
|
from builtins import dict as IndexPairSetMap
|
|
8
|
-
from builtins import list as
|
|
8
|
+
from builtins import list as Point2Vector
|
|
9
9
|
from builtins import list as KeyVector
|
|
10
|
-
from builtins import list as
|
|
11
|
-
from builtins import list as Pose3Pairs
|
|
12
|
-
from builtins import list as SfmTracks
|
|
10
|
+
from builtins import list as BinaryMeasurementsPoint3
|
|
13
11
|
from builtins import list as Point3Pairs
|
|
12
|
+
from builtins import list as Pose3Vector
|
|
13
|
+
from builtins import list as BinaryMeasurementsRot3
|
|
14
|
+
from builtins import list as Rot3Vector
|
|
14
15
|
from builtins import list as Point2Pairs
|
|
15
|
-
from builtins import list as
|
|
16
|
-
from builtins import list as
|
|
17
|
-
from builtins import list as
|
|
16
|
+
from builtins import list as SfmTracks
|
|
17
|
+
from builtins import list as IndexPairVector
|
|
18
|
+
from builtins import list as Pose2Pairs
|
|
18
19
|
from builtins import list as KeypointsVector
|
|
19
|
-
from builtins import list as
|
|
20
|
-
from builtins import list as
|
|
20
|
+
from builtins import list as BinaryMeasurementsUnit3
|
|
21
|
+
from builtins import list as Pose3Pairs
|
|
21
22
|
from builtins import list as BetweenFactorPose3s
|
|
22
|
-
from builtins import list as Rot3Vector
|
|
23
23
|
from builtins import list as BetweenFactorPose2s
|
|
24
|
-
from builtins import list as Pose3Vector
|
|
25
|
-
from builtins import list as IndexPairVector
|
|
26
24
|
from builtins import list as SfmTrack2dVector
|
|
25
|
+
from builtins import list as SfmMeasurementVector
|
|
26
|
+
from builtins import list as SfmCameras
|
|
27
27
|
from gtsam.gtsam import AHRSFactor
|
|
28
28
|
from gtsam.gtsam import AcceleratingScenario
|
|
29
29
|
from gtsam.gtsam import BarometricFactor
|
gtsam/gtsam/__init__.pyi
CHANGED
|
@@ -6199,6 +6199,9 @@ class HybridValues:
|
|
|
6199
6199
|
@typing.overload
|
|
6200
6200
|
def __init__(self, cv: VectorValues, dv: DiscreteValues) -> None:
|
|
6201
6201
|
...
|
|
6202
|
+
@typing.overload
|
|
6203
|
+
def __init__(self, cv: VectorValues, dv: DiscreteValues, v: Values) -> None:
|
|
6204
|
+
...
|
|
6202
6205
|
def __repr__(self, s: str = 'HybridValues', keyFormatter: typing.Callable[[int], str] = ...) -> str:
|
|
6203
6206
|
...
|
|
6204
6207
|
def at(self, j: int) -> numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]:
|
|
@@ -6221,6 +6224,22 @@ class HybridValues:
|
|
|
6221
6224
|
"""
|
|
6222
6225
|
equals required byTestablefor unit testing
|
|
6223
6226
|
"""
|
|
6227
|
+
def exists(self, j: int) -> bool:
|
|
6228
|
+
"""
|
|
6229
|
+
Check whether a variable with keyjexists.
|
|
6230
|
+
"""
|
|
6231
|
+
def existsDiscrete(self, j: int) -> bool:
|
|
6232
|
+
"""
|
|
6233
|
+
Check whether a variable with keyjexists inDiscreteValues.
|
|
6234
|
+
"""
|
|
6235
|
+
def existsNonlinear(self, j: int) -> bool:
|
|
6236
|
+
"""
|
|
6237
|
+
Check whether a variable with keyjexists in values.
|
|
6238
|
+
"""
|
|
6239
|
+
def existsVector(self, j: int) -> bool:
|
|
6240
|
+
"""
|
|
6241
|
+
Check whether a variable with keyjexists inVectorValues.
|
|
6242
|
+
"""
|
|
6224
6243
|
@typing.overload
|
|
6225
6244
|
def insert(self, j: int, value: int) -> None:
|
|
6226
6245
|
"""
|
|
@@ -6254,6 +6273,13 @@ class HybridValues:
|
|
|
6254
6273
|
Throws an invalid_argument exception if any keys to be inserted are already used.
|
|
6255
6274
|
"""
|
|
6256
6275
|
@typing.overload
|
|
6276
|
+
def insert(self, values: Values) -> None:
|
|
6277
|
+
"""
|
|
6278
|
+
Insert all values fromvalues.
|
|
6279
|
+
|
|
6280
|
+
Throws an invalid_argument exception if any keys to be inserted are already used.
|
|
6281
|
+
"""
|
|
6282
|
+
@typing.overload
|
|
6257
6283
|
def insert(self, values: HybridValues) -> None:
|
|
6258
6284
|
"""
|
|
6259
6285
|
Insert all values fromvalues.
|
|
@@ -6261,6 +6287,123 @@ class HybridValues:
|
|
|
6261
6287
|
Throws an invalid_argument exception if any keys to be inserted are already used.
|
|
6262
6288
|
"""
|
|
6263
6289
|
@typing.overload
|
|
6290
|
+
def insertNonlinear(self, j: int, vector: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
|
|
6291
|
+
...
|
|
6292
|
+
@typing.overload
|
|
6293
|
+
def insertNonlinear(self, j: int, matrix: numpy.ndarray[tuple[M, N], numpy.dtype[numpy.float64]]) -> None:
|
|
6294
|
+
...
|
|
6295
|
+
@typing.overload
|
|
6296
|
+
def insertNonlinear(self, j: int, point2: numpy.ndarray[tuple[typing.Literal[2], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
|
|
6297
|
+
...
|
|
6298
|
+
@typing.overload
|
|
6299
|
+
def insertNonlinear(self, j: int, point3: numpy.ndarray[tuple[typing.Literal[3], typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
|
|
6300
|
+
...
|
|
6301
|
+
@typing.overload
|
|
6302
|
+
def insertNonlinear(self, j: int, rot2: Rot2) -> None:
|
|
6303
|
+
...
|
|
6304
|
+
@typing.overload
|
|
6305
|
+
def insertNonlinear(self, j: int, pose2: Pose2) -> None:
|
|
6306
|
+
...
|
|
6307
|
+
@typing.overload
|
|
6308
|
+
def insertNonlinear(self, j: int, R: SO3) -> None:
|
|
6309
|
+
...
|
|
6310
|
+
@typing.overload
|
|
6311
|
+
def insertNonlinear(self, j: int, Q: SO4) -> None:
|
|
6312
|
+
...
|
|
6313
|
+
@typing.overload
|
|
6314
|
+
def insertNonlinear(self, j: int, P: SOn) -> None:
|
|
6315
|
+
...
|
|
6316
|
+
@typing.overload
|
|
6317
|
+
def insertNonlinear(self, j: int, rot3: Rot3) -> None:
|
|
6318
|
+
...
|
|
6319
|
+
@typing.overload
|
|
6320
|
+
def insertNonlinear(self, j: int, pose3: Pose3) -> None:
|
|
6321
|
+
...
|
|
6322
|
+
@typing.overload
|
|
6323
|
+
def insertNonlinear(self, j: int, similarity2: Similarity2) -> None:
|
|
6324
|
+
...
|
|
6325
|
+
@typing.overload
|
|
6326
|
+
def insertNonlinear(self, j: int, similarity3: Similarity3) -> None:
|
|
6327
|
+
...
|
|
6328
|
+
@typing.overload
|
|
6329
|
+
def insertNonlinear(self, j: int, unit3: Unit3) -> None:
|
|
6330
|
+
...
|
|
6331
|
+
@typing.overload
|
|
6332
|
+
def insertNonlinear(self, j: int, cal3bundler: Cal3Bundler) -> None:
|
|
6333
|
+
...
|
|
6334
|
+
@typing.overload
|
|
6335
|
+
def insertNonlinear(self, j: int, cal3f: Cal3f) -> None:
|
|
6336
|
+
...
|
|
6337
|
+
@typing.overload
|
|
6338
|
+
def insertNonlinear(self, j: int, cal3_s2: Cal3_S2) -> None:
|
|
6339
|
+
...
|
|
6340
|
+
@typing.overload
|
|
6341
|
+
def insertNonlinear(self, j: int, cal3ds2: Cal3DS2) -> None:
|
|
6342
|
+
...
|
|
6343
|
+
@typing.overload
|
|
6344
|
+
def insertNonlinear(self, j: int, cal3fisheye: Cal3Fisheye) -> None:
|
|
6345
|
+
...
|
|
6346
|
+
@typing.overload
|
|
6347
|
+
def insertNonlinear(self, j: int, cal3unified: Cal3Unified) -> None:
|
|
6348
|
+
...
|
|
6349
|
+
@typing.overload
|
|
6350
|
+
def insertNonlinear(self, j: int, E: EssentialMatrix) -> None:
|
|
6351
|
+
...
|
|
6352
|
+
@typing.overload
|
|
6353
|
+
def insertNonlinear(self, j: int, F: FundamentalMatrix) -> None:
|
|
6354
|
+
...
|
|
6355
|
+
@typing.overload
|
|
6356
|
+
def insertNonlinear(self, j: int, F: SimpleFundamentalMatrix) -> None:
|
|
6357
|
+
...
|
|
6358
|
+
@typing.overload
|
|
6359
|
+
def insertNonlinear(self, j: int, plane: OrientedPlane3) -> None:
|
|
6360
|
+
...
|
|
6361
|
+
@typing.overload
|
|
6362
|
+
def insertNonlinear(self, j: int, camera: PinholeCameraCal3Bundler) -> None:
|
|
6363
|
+
...
|
|
6364
|
+
@typing.overload
|
|
6365
|
+
def insertNonlinear(self, j: int, camera: PinholeCameraCal3f) -> None:
|
|
6366
|
+
...
|
|
6367
|
+
@typing.overload
|
|
6368
|
+
def insertNonlinear(self, j: int, camera: PinholeCameraCal3_S2) -> None:
|
|
6369
|
+
...
|
|
6370
|
+
@typing.overload
|
|
6371
|
+
def insertNonlinear(self, j: int, camera: PinholeCameraCal3DS2) -> None:
|
|
6372
|
+
...
|
|
6373
|
+
@typing.overload
|
|
6374
|
+
def insertNonlinear(self, j: int, camera: PinholeCameraCal3Fisheye) -> None:
|
|
6375
|
+
...
|
|
6376
|
+
@typing.overload
|
|
6377
|
+
def insertNonlinear(self, j: int, camera: PinholeCameraCal3Unified) -> None:
|
|
6378
|
+
...
|
|
6379
|
+
@typing.overload
|
|
6380
|
+
def insertNonlinear(self, j: int, camera: PinholePoseCal3Bundler) -> None:
|
|
6381
|
+
...
|
|
6382
|
+
@typing.overload
|
|
6383
|
+
def insertNonlinear(self, j: int, camera: ...) -> None:
|
|
6384
|
+
...
|
|
6385
|
+
@typing.overload
|
|
6386
|
+
def insertNonlinear(self, j: int, camera: PinholePoseCal3_S2) -> None:
|
|
6387
|
+
...
|
|
6388
|
+
@typing.overload
|
|
6389
|
+
def insertNonlinear(self, j: int, camera: PinholePoseCal3DS2) -> None:
|
|
6390
|
+
...
|
|
6391
|
+
@typing.overload
|
|
6392
|
+
def insertNonlinear(self, j: int, camera: PinholePoseCal3Fisheye) -> None:
|
|
6393
|
+
...
|
|
6394
|
+
@typing.overload
|
|
6395
|
+
def insertNonlinear(self, j: int, camera: PinholePoseCal3Unified) -> None:
|
|
6396
|
+
...
|
|
6397
|
+
@typing.overload
|
|
6398
|
+
def insertNonlinear(self, j: int, constant_bias: imuBias.ConstantBias) -> None:
|
|
6399
|
+
...
|
|
6400
|
+
@typing.overload
|
|
6401
|
+
def insertNonlinear(self, j: int, nav_state: NavState) -> None:
|
|
6402
|
+
...
|
|
6403
|
+
@typing.overload
|
|
6404
|
+
def insertNonlinear(self, j: int, c: float) -> None:
|
|
6405
|
+
...
|
|
6406
|
+
@typing.overload
|
|
6264
6407
|
def insert_or_assign(self, j: int, value: numpy.ndarray[tuple[M, typing.Literal[1]], numpy.dtype[numpy.float64]]) -> None:
|
|
6265
6408
|
"""
|
|
6266
6409
|
insert_or_assign(), similar toValues.h
|
|
@@ -6270,10 +6413,21 @@ class HybridValues:
|
|
|
6270
6413
|
"""
|
|
6271
6414
|
insert_or_assign(), similar toValues.h
|
|
6272
6415
|
"""
|
|
6416
|
+
def nonlinear(self) -> Values:
|
|
6417
|
+
"""
|
|
6418
|
+
Return the nonlinear values.
|
|
6419
|
+
"""
|
|
6273
6420
|
def print(self, s: str = 'HybridValues', keyFormatter: typing.Callable[[int], str] = ...) -> None:
|
|
6274
6421
|
"""
|
|
6275
6422
|
print required byTestablefor unit testing
|
|
6276
6423
|
"""
|
|
6424
|
+
def retract(self, delta: VectorValues) -> HybridValues:
|
|
6425
|
+
"""
|
|
6426
|
+
Add a delta config to current config and returns a new config.
|
|
6427
|
+
|
|
6428
|
+
Args:
|
|
6429
|
+
delta: The delta to be added.
|
|
6430
|
+
"""
|
|
6277
6431
|
@typing.overload
|
|
6278
6432
|
def update(self, values: VectorValues) -> None:
|
|
6279
6433
|
"""
|
|
@@ -6289,6 +6443,13 @@ class HybridValues:
|
|
|
6289
6443
|
Throws std::out_of_range if any keys in values are not present in this object.
|
|
6290
6444
|
"""
|
|
6291
6445
|
@typing.overload
|
|
6446
|
+
def update(self, values: Values) -> None:
|
|
6447
|
+
"""
|
|
6448
|
+
For all key/value pairs invalues, replace nonlinear values with corresponding keys in this object with those invalues.
|
|
6449
|
+
|
|
6450
|
+
Throws std::out_of_range if any keys in values are not present in this object.
|
|
6451
|
+
"""
|
|
6452
|
+
@typing.overload
|
|
6292
6453
|
def update(self, values: HybridValues) -> None:
|
|
6293
6454
|
"""
|
|
6294
6455
|
For all key/value pairs invalues, replace all values with corresponding keys in this object with those invalues.
|
|
Binary file
|
gtsam/tests/test_HybridValues.py
CHANGED
|
@@ -6,16 +6,15 @@ All Rights Reserved
|
|
|
6
6
|
See LICENSE for the license information
|
|
7
7
|
|
|
8
8
|
Unit tests for Hybrid Values.
|
|
9
|
-
Author: Shangjie Xue
|
|
9
|
+
Author: Shangjie Xue, Varun Agrawal, Frank Dellaert
|
|
10
10
|
"""
|
|
11
|
-
# pylint: disable=invalid-name, no-name-in-module, no-member
|
|
12
11
|
|
|
13
|
-
|
|
12
|
+
# pylint: disable=invalid-name, no-name-in-module, no-member
|
|
14
13
|
|
|
15
14
|
import unittest
|
|
16
15
|
|
|
17
16
|
import numpy as np
|
|
18
|
-
from gtsam.symbol_shorthand import
|
|
17
|
+
from gtsam.symbol_shorthand import D, V, M
|
|
19
18
|
from gtsam.utils.test_case import GtsamTestCase
|
|
20
19
|
|
|
21
20
|
import gtsam
|
|
@@ -24,19 +23,216 @@ import gtsam
|
|
|
24
23
|
class TestHybridValues(GtsamTestCase):
|
|
25
24
|
"""Unit tests for HybridValues."""
|
|
26
25
|
|
|
27
|
-
def
|
|
28
|
-
"""
|
|
26
|
+
def setUp(self):
|
|
27
|
+
"""Set up common objects for tests."""
|
|
28
|
+
self.vector_values = gtsam.VectorValues()
|
|
29
|
+
self.vector_values.insert(V(0), np.array([1.0, 2.0]))
|
|
30
|
+
self.vector_values.insert(V(1), np.array([3.0]))
|
|
31
|
+
|
|
32
|
+
self.discrete_values = gtsam.DiscreteValues()
|
|
33
|
+
self.discrete_values[D(0)] = 1
|
|
34
|
+
self.discrete_values[D(1)] = 0
|
|
35
|
+
|
|
36
|
+
self.nonlinear_values = gtsam.Values()
|
|
37
|
+
self.nonlinear_values.insert(M(5), gtsam.Pose2(1, 2, 0.3))
|
|
38
|
+
|
|
39
|
+
def test_constructors(self):
|
|
40
|
+
"""Test various constructors."""
|
|
41
|
+
hv_empty = gtsam.HybridValues()
|
|
42
|
+
self.assertEqual(hv_empty.continuous().size(), 0)
|
|
43
|
+
self.assertEqual(len(hv_empty.discrete()), 0)
|
|
44
|
+
self.assertEqual(hv_empty.nonlinear().size(), 0)
|
|
45
|
+
|
|
46
|
+
hv_vd = gtsam.HybridValues(self.vector_values, self.discrete_values)
|
|
47
|
+
self.assertEqual(hv_vd.continuous().size(), 2)
|
|
48
|
+
self.assertEqual(len(hv_vd.discrete()), 2)
|
|
49
|
+
self.assertEqual(hv_vd.nonlinear().size(), 0)
|
|
50
|
+
self.assertTrue(hv_vd.continuous().equals(self.vector_values, 1e-9))
|
|
51
|
+
# DiscreteValues comparison needs to be element-wise or via string
|
|
52
|
+
self.assertEqual(hv_vd.discrete()[D(0)], self.discrete_values[D(0)])
|
|
53
|
+
|
|
54
|
+
hv_all = gtsam.HybridValues(
|
|
55
|
+
self.vector_values, self.discrete_values, self.nonlinear_values
|
|
56
|
+
)
|
|
57
|
+
self.assertEqual(hv_all.continuous().size(), 2)
|
|
58
|
+
self.assertEqual(len(hv_all.discrete()), 2)
|
|
59
|
+
self.assertEqual(hv_all.nonlinear().size(), 1)
|
|
60
|
+
self.assertTrue(hv_all.nonlinear().equals(self.nonlinear_values, 1e-9))
|
|
61
|
+
|
|
62
|
+
def test_accessors(self):
|
|
63
|
+
"""Test accessing underlying containers."""
|
|
64
|
+
hv_all = gtsam.HybridValues(
|
|
65
|
+
self.vector_values, self.discrete_values, self.nonlinear_values
|
|
66
|
+
)
|
|
67
|
+
|
|
68
|
+
self.assertTrue(hv_all.continuous().equals(self.vector_values, 1e-9))
|
|
69
|
+
# Compare DiscreteValues content
|
|
70
|
+
retrieved_dv = hv_all.discrete()
|
|
71
|
+
self.assertEqual(len(retrieved_dv), len(self.discrete_values))
|
|
72
|
+
for k, v in self.discrete_values.items():
|
|
73
|
+
self.assertEqual(retrieved_dv[k], v)
|
|
74
|
+
self.assertTrue(hv_all.nonlinear().equals(self.nonlinear_values, 1e-9))
|
|
75
|
+
|
|
76
|
+
# Test at methods
|
|
77
|
+
self.gtsamAssertEquals(hv_all.at(V(0)), self.vector_values.at(V(0)), 1e-9)
|
|
78
|
+
self.assertEqual(hv_all.atDiscrete(D(0)), self.discrete_values[D(0)])
|
|
79
|
+
# For nonlinear, access via nonlinear().atTYPE()
|
|
80
|
+
self.assertTrue(
|
|
81
|
+
hv_all.nonlinear()
|
|
82
|
+
.atPose2(M(5))
|
|
83
|
+
.equals(self.nonlinear_values.atPose2(M(5)), 1e-9)
|
|
84
|
+
)
|
|
85
|
+
|
|
86
|
+
def test_exists(self):
|
|
87
|
+
"""Test existence checks."""
|
|
88
|
+
hv_all = gtsam.HybridValues(
|
|
89
|
+
self.vector_values, self.discrete_values, self.nonlinear_values
|
|
90
|
+
)
|
|
91
|
+
|
|
92
|
+
self.assertTrue(hv_all.existsVector(V(0)))
|
|
93
|
+
self.assertFalse(hv_all.existsVector(D(0))) # Key for discrete
|
|
94
|
+
self.assertFalse(hv_all.existsVector(M(5))) # Key for nonlinear
|
|
95
|
+
|
|
96
|
+
self.assertTrue(hv_all.existsDiscrete(D(1)))
|
|
97
|
+
self.assertFalse(hv_all.existsDiscrete(V(0))) # Key for vector
|
|
98
|
+
|
|
99
|
+
self.assertTrue(hv_all.existsNonlinear(M(5)))
|
|
100
|
+
self.assertFalse(hv_all.existsNonlinear(V(0))) # Key for vector
|
|
101
|
+
|
|
102
|
+
# General exists (checks nonlinear, then vector, then discrete)
|
|
103
|
+
self.assertTrue(hv_all.exists(V(0))) # Vector
|
|
104
|
+
self.assertTrue(hv_all.exists(D(0))) # Discrete
|
|
105
|
+
self.assertTrue(hv_all.exists(M(5))) # Nonlinear
|
|
106
|
+
self.assertFalse(hv_all.exists(D(7))) # Non-existent key
|
|
107
|
+
|
|
108
|
+
def test_equals(self):
|
|
109
|
+
"""Test equals method."""
|
|
110
|
+
hv1 = gtsam.HybridValues(
|
|
111
|
+
self.vector_values, self.discrete_values, self.nonlinear_values
|
|
112
|
+
)
|
|
113
|
+
hv2 = gtsam.HybridValues(
|
|
114
|
+
self.vector_values, self.discrete_values, self.nonlinear_values
|
|
115
|
+
)
|
|
116
|
+
|
|
117
|
+
self.assertTrue(hv1.equals(hv2, 1e-9))
|
|
118
|
+
|
|
119
|
+
def test_insert_individual(self):
|
|
120
|
+
"""Test inserting individual values."""
|
|
121
|
+
hv = gtsam.HybridValues()
|
|
122
|
+
hv.insert(V(10), np.array([1.0]))
|
|
123
|
+
hv.insert(D(10), 1)
|
|
124
|
+
hv.insertNonlinear(M(11), gtsam.Pose2())
|
|
125
|
+
|
|
126
|
+
self.assertTrue(hv.existsVector(V(10)))
|
|
127
|
+
self.gtsamAssertEquals(hv.at(V(10)), np.array([1.0]))
|
|
128
|
+
self.assertTrue(hv.existsDiscrete(D(10)))
|
|
129
|
+
self.assertEqual(hv.atDiscrete(D(10)), 1)
|
|
130
|
+
|
|
131
|
+
def test_insert_containers(self):
|
|
132
|
+
"""Test inserting from other Values containers."""
|
|
133
|
+
hv = gtsam.HybridValues()
|
|
134
|
+
|
|
135
|
+
hv.insert(self.vector_values)
|
|
136
|
+
self.assertEqual(hv.continuous().size(), 2)
|
|
137
|
+
self.assertTrue(hv.continuous().equals(self.vector_values, 1e-9))
|
|
138
|
+
|
|
139
|
+
hv.insert(self.discrete_values)
|
|
140
|
+
self.assertEqual(len(hv.discrete()), 2)
|
|
141
|
+
# Check discrete values equality
|
|
142
|
+
retrieved_dv = hv.discrete()
|
|
143
|
+
for k, v in self.discrete_values.items():
|
|
144
|
+
self.assertEqual(retrieved_dv[k], v)
|
|
145
|
+
|
|
146
|
+
hv.insert(self.nonlinear_values)
|
|
147
|
+
self.assertEqual(hv.nonlinear().size(), 1)
|
|
148
|
+
self.assertTrue(hv.nonlinear().equals(self.nonlinear_values, 1e-9))
|
|
149
|
+
|
|
150
|
+
hv_copy = gtsam.HybridValues()
|
|
151
|
+
hv_copy.insert(hv) # Test insert(HybridValues)
|
|
152
|
+
self.assertTrue(hv_copy.equals(hv, 1e-9))
|
|
153
|
+
|
|
154
|
+
def test_insert_or_assign(self):
|
|
155
|
+
"""Test insert_or_assign method."""
|
|
156
|
+
hv = gtsam.HybridValues()
|
|
157
|
+
hv.insert(V(0), np.array([1.0]))
|
|
158
|
+
hv.insert(D(0), 0)
|
|
159
|
+
|
|
160
|
+
# Test insert_or_assign for vector
|
|
161
|
+
hv.insert_or_assign(V(0), np.array([2.0])) # Update existing
|
|
162
|
+
self.gtsamAssertEquals(hv.at(V(0)), np.array([2.0]))
|
|
163
|
+
hv.insert_or_assign(V(1), np.array([3.0])) # Insert new
|
|
164
|
+
self.gtsamAssertEquals(hv.at(V(1)), np.array([3.0]))
|
|
165
|
+
|
|
166
|
+
# Test insert_or_assign for discrete
|
|
167
|
+
hv.insert_or_assign(D(0), 1) # Update existing
|
|
168
|
+
self.assertEqual(hv.atDiscrete(D(0)), 1)
|
|
169
|
+
hv.insert_or_assign(D(1), 2) # Insert new
|
|
170
|
+
self.assertEqual(hv.atDiscrete(D(1)), 2)
|
|
171
|
+
|
|
172
|
+
def test_update(self):
|
|
173
|
+
"""Test update methods."""
|
|
174
|
+
hv = gtsam.HybridValues(
|
|
175
|
+
self.vector_values, self.discrete_values, self.nonlinear_values
|
|
176
|
+
)
|
|
177
|
+
|
|
178
|
+
# Update VectorValues
|
|
179
|
+
vv_update = gtsam.VectorValues()
|
|
180
|
+
vv_update.insert(V(0), np.array([10.0, 20.0])) # Existing key
|
|
181
|
+
hv.update(vv_update)
|
|
182
|
+
self.gtsamAssertEquals(hv.at(V(0)), np.array([10.0, 20.0]))
|
|
183
|
+
self.assertEqual(hv.continuous().size(), 2) # X0, X1
|
|
184
|
+
|
|
185
|
+
# Update DiscreteValues
|
|
186
|
+
dv_update = gtsam.DiscreteValues()
|
|
187
|
+
dv_update[D(0)] = 5 # Existing key
|
|
188
|
+
hv.update(dv_update)
|
|
189
|
+
self.assertEqual(hv.atDiscrete(D(0)), 5)
|
|
190
|
+
self.assertEqual(len(hv.discrete()), 2) # C0, C1
|
|
191
|
+
|
|
192
|
+
# Update NonlinearValues
|
|
193
|
+
nv_update = gtsam.Values()
|
|
194
|
+
nv_update.insert(M(5), gtsam.Pose2(3, 4, 0.5))
|
|
195
|
+
hv.update(nv_update)
|
|
196
|
+
self.assertTrue(
|
|
197
|
+
hv.nonlinear().atPose2(M(5)).equals(gtsam.Pose2(3, 4, 0.5), 1e-9)
|
|
198
|
+
)
|
|
199
|
+
self.assertEqual(hv.nonlinear().size(), 1)
|
|
200
|
+
|
|
201
|
+
# Update HybridValues (only continuous and discrete parts for now)
|
|
202
|
+
hv_update_source = gtsam.HybridValues()
|
|
203
|
+
vv_source = gtsam.VectorValues()
|
|
204
|
+
vv_source.insert(V(0), np.array([-1.0, -2.0]))
|
|
205
|
+
dv_source = gtsam.DiscreteValues()
|
|
206
|
+
dv_source[D(0)] = 100
|
|
207
|
+
hv_update_source.insert(vv_source)
|
|
208
|
+
hv_update_source.insert(dv_source)
|
|
209
|
+
|
|
210
|
+
hv.update(hv_update_source)
|
|
211
|
+
self.gtsamAssertEquals(hv.at(V(0)), np.array([-1.0, -2.0]))
|
|
212
|
+
self.assertEqual(hv.atDiscrete(D(0)), 100)
|
|
213
|
+
|
|
214
|
+
def test_retract(self):
|
|
215
|
+
"""Test retract method."""
|
|
216
|
+
hv = gtsam.HybridValues(
|
|
217
|
+
self.vector_values, self.discrete_values, self.nonlinear_values
|
|
218
|
+
)
|
|
219
|
+
|
|
220
|
+
delta = gtsam.VectorValues()
|
|
221
|
+
deltaM5 = np.array([0.5, 0.5, -1.0])
|
|
222
|
+
delta.insert(M(5), deltaM5)
|
|
223
|
+
|
|
224
|
+
hv_retracted = hv.retract(delta)
|
|
29
225
|
|
|
30
|
-
|
|
31
|
-
|
|
32
|
-
hv1.insert(C(0), 2)
|
|
226
|
+
# Original should be unchanged
|
|
227
|
+
self.assertTrue(hv.continuous().equals(self.vector_values, 1e-9))
|
|
33
228
|
|
|
34
|
-
|
|
35
|
-
|
|
36
|
-
|
|
229
|
+
# Check retracted values
|
|
230
|
+
expected_M5 = hv.nonlinear().atPose2(M(5)).retract(deltaM5)
|
|
231
|
+
self.gtsamAssertEquals(hv_retracted.nonlinear().atPose2(M(5)), expected_M5)
|
|
37
232
|
|
|
38
|
-
|
|
39
|
-
self.assertEqual(
|
|
233
|
+
# Check that discrete and continuous parts are copied
|
|
234
|
+
self.assertEqual(len(hv_retracted.discrete()), len(self.discrete_values))
|
|
235
|
+
self.assertEqual(hv_retracted.continuous().size(), self.vector_values.size())
|
|
40
236
|
|
|
41
237
|
|
|
42
238
|
if __name__ == "__main__":
|
|
@@ -1,13 +1,13 @@
|
|
|
1
1
|
gtsam/symbol_shorthand.py,sha256=HlTV-Z5aB5cXWT5AsbKOeWZIHoXQsH2W1S90ET-tidA,236
|
|
2
|
-
gtsam/gtsam.cpython-313-darwin.so,sha256=
|
|
3
|
-
gtsam/__init__.pyi,sha256=
|
|
2
|
+
gtsam/gtsam.cpython-313-darwin.so,sha256=3hXZPKcsCnu1ODx0Xe6Qk6aDGkSrtRYNVbFqokWIDos,17955920
|
|
3
|
+
gtsam/__init__.pyi,sha256=XVlF_ll1DIXDAbO9HbsCUKgly97t9UKxQBvtzGYDUa4,42970
|
|
4
4
|
gtsam/gtsfm.py,sha256=Udlkb6o5iUk69uxBkb88-W1GLfu1g8iSuZlLu-RRU0o,202
|
|
5
5
|
gtsam/__init__.py,sha256=6G-WPnb_FMQEJXNDDAmrKrQPau7evtd5svhTmMqhYSI,2011
|
|
6
6
|
gtsam/noiseModel.py,sha256=ybfIHa4sLXe78_k-Dib8nTaw34BRXqEDVtS2B9dzSO0,217
|
|
7
7
|
gtsam/imuBias.py,sha256=tz3bOCFl29iMycbGFoF-ud6kRsJYjA7DJ2RJoKPFRU8,209
|
|
8
8
|
gtsam/gtsam/utilities.pyi,sha256=-yn7_jeJbTa_3KJGgqcr_NDD70UhDZ8tYX2M2lPLmSQ,3384
|
|
9
9
|
gtsam/gtsam/gtsfm.pyi,sha256=Onoh4CRaSYIQ0FID0vBkZeV7ESIhc2fh4rJGgaPY-ug,702
|
|
10
|
-
gtsam/gtsam/__init__.pyi,sha256
|
|
10
|
+
gtsam/gtsam/__init__.pyi,sha256=YeFsDuMRJjrYvv7hdtBMrloa_GgOUghAs_nsLq_w70c,716998
|
|
11
11
|
gtsam/gtsam/so3.pyi,sha256=UoDGISMgcStVTAtc_y6CmHXCtPJrJ7ju70z3ZH1OLR4,3939
|
|
12
12
|
gtsam/gtsam/imuBias.pyi,sha256=gD6w5kd9xpO4uBEpfNym_3LjW67MV7HPQRYOOoDPysQ,2478
|
|
13
13
|
gtsam/gtsam/symbol_shorthand.pyi,sha256=iaQLf_-hch_4YTYbSWTLINKtIL9Mg7W7HKOMuCXp8Ok,991
|
|
@@ -25,7 +25,7 @@ gtsam/tests/IMUKittiExampleGPS.py,sha256=xWAuLEBQX2rhAMDIBouCDOr13WPZZYLk8wRXbWJ
|
|
|
25
25
|
gtsam/tests/test_VisualISAMExample.py,sha256=x_pISckn6Nd7FpM3t2DdxqZo4whMVXZkBamI0xi-2qM,4254
|
|
26
26
|
gtsam/tests/FixedLagSmootherExample.py,sha256=1i8TzMAzn2jgCxYDbxW7nDLsLvcFDpyd9tO0EiRn8w8,3185
|
|
27
27
|
gtsam/tests/test_HybridFactorGraph.py,sha256=_tJld82_p_AxkqMQdeL11PEA5xTk-p21Fj1RLPvhljE,13093
|
|
28
|
-
gtsam/tests/test_HybridValues.py,sha256=
|
|
28
|
+
gtsam/tests/test_HybridValues.py,sha256=DtgMnPFoifEuce0hS-gOozLFqfheVfwPO8ehQhpigtc,9027
|
|
29
29
|
gtsam/tests/SFMExample_bal.py,sha256=qLqJOuz4bmG2KQj44mO9pA5sR_TxQJDGV9EJj55E1VQ,4529
|
|
30
30
|
gtsam/tests/test_GaussianBayesNet.py,sha256=tF5CDF5giL9gfHOgQ4dwY1u4JQmg4mxh8jCDQ-3-8Wk,2430
|
|
31
31
|
gtsam/tests/test_SFMExample.py,sha256=qxBV3s4KZDD4ao4Yh03mdptFILbu3n57gXMABc6uvxQ,2784
|
|
@@ -128,13 +128,13 @@ gtsam/utils/__init__.py,sha256=_ID7pb13SDZedga5KdBqpPug4PW3eU3THdVF_3jrakQ,678
|
|
|
128
128
|
gtsam/utils/test_case.py,sha256=3wIqAW5_smes95XUJgUjD4v3FXACYSVzQ1l6MMdwSkA,1848
|
|
129
129
|
gtsam/utils/logging_optimizer.py,sha256=tRnzQKV4eT5djS0Ojy5J7OGu7oVRjZw7Jhjrx_VQVTU,4417
|
|
130
130
|
gtsam/.dylibs/libboost_thread.dylib,sha256=aT1DCt1Kvw1A2w2mRHIJhZRFrYmoDaI5xYM6wEt86L8,137200
|
|
131
|
-
gtsam/.dylibs/
|
|
131
|
+
gtsam/.dylibs/libgtsam.4.3a0.dev202505281327.dylib,sha256=n2GyhM7MZA-aromghixcyd57XIcD2j9SjDPlGfbbLwE,5666800
|
|
132
132
|
gtsam/.dylibs/libboost_regex.dylib,sha256=XWw3H3ehJjFuFVSSDGo2lyKOIsoTQ-0Aaf1ekhsQJfY,356464
|
|
133
|
-
gtsam/.dylibs/libgtsam.4.3a0.dev202505270244.dylib,sha256=wcdkBwNV0Vj065usNIA15_KK5vC3hcR2XyjjhfcoKGs,5634720
|
|
134
133
|
gtsam/.dylibs/libboost_serialization.dylib,sha256=7OW78djID14u2YB_F_TuXomOIsEpt8I7RnCAGuGmwCc,418288
|
|
135
134
|
gtsam/.dylibs/libboost_timer.dylib,sha256=leVXIyCdydip4vwIm-Ghrt6UGy_Q-yhL2f8ITyfRku0,81520
|
|
136
135
|
gtsam/.dylibs/libboost_filesystem.dylib,sha256=WUuuO1JZADriLciFi63Ky4dmqt1KA7_V6DcNdzCANSo,197328
|
|
137
136
|
gtsam/.dylibs/libcephes-gtsam.1.0.0.dylib,sha256=pNZtgCIaV7_dC0WPXobOc8sNvd_0WQo1AdYxUsqZ1D0,176352
|
|
137
|
+
gtsam/.dylibs/libgtsam_unstable.4.3a0.dev202505281327.dylib,sha256=-bouyNqzO8DKp0E7ifcLLoFF85cwByJYF41-Jo9-juM,1623104
|
|
138
138
|
gtsam/.dylibs/libmetis-gtsam.dylib,sha256=Od5V_NOZKhxu5cEw1wMfGxk3l0_2LEhV-GkEwnqyx4Q,449504
|
|
139
139
|
gtsam/.dylibs/libboost_atomic.dylib,sha256=AbG9FmLd7biQVvDL9F7_sLOEgmhgj5bwxX98c5jnVVc,104160
|
|
140
140
|
gtsam/.dylibs/libboost_date_time.dylib,sha256=zkeDyIl7aGu1K5avCbDjtJURdIPT8llHnNZ-G7N-_Cc,35008
|
|
@@ -249,12 +249,12 @@ gtsam/Data/Balbianello/BalbianelloMedium-4.key.gz,sha256=P6tu3JmA0NKlcFYdgYczTOH
|
|
|
249
249
|
gtsam/Data/Balbianello/BalbianelloMedium-1.key.gz,sha256=RgT7tVXXOwvDug20TW-9xtoeO3dSSPMF1n9DaKxdrIw,332878
|
|
250
250
|
gtsam/Data/Balbianello/BalbianelloMedium-3.key.gz,sha256=yNMcyqwZCOj9FG-6qXQ9xhJjpM135cTBJYlOUBG0rnQ,296236
|
|
251
251
|
gtsam/Data/Balbianello/BalbianelloMedium-5.key.gz,sha256=4veDrxRdLH8k1DIhgj1984MitZ7nAWoXDWTWyXuu7Lg,241361
|
|
252
|
-
gtsam_develop-4.3a0.
|
|
253
|
-
gtsam_develop-4.3a0.
|
|
254
|
-
gtsam_develop-4.3a0.
|
|
255
|
-
gtsam_develop-4.3a0.
|
|
252
|
+
gtsam_develop-4.3a0.dev202505281327.dist-info/RECORD,,
|
|
253
|
+
gtsam_develop-4.3a0.dev202505281327.dist-info/WHEEL,sha256=oqGJCpG61FZJmvyZ3C_0aCv-2mdfcY9e3fXvyUNmWfM,136
|
|
254
|
+
gtsam_develop-4.3a0.dev202505281327.dist-info/top_level.txt,sha256=DOnqfd8DN2HpG5-V5t32TjFOB_vcYuyOWyRsgeoANEo,30
|
|
255
|
+
gtsam_develop-4.3a0.dev202505281327.dist-info/METADATA,sha256=5Re4njWXaXjOYmHOEK9NHnfc-Fm_x7RkEF9yZGBHpfU,7767
|
|
256
256
|
gtsam_unstable/__init__.py,sha256=FPc_oO5PFQZbrfpgugzQuI6LJfP1fzq82UQf_nuyGtk,30
|
|
257
|
-
gtsam_unstable/gtsam_unstable.cpython-313-darwin.so,sha256=
|
|
257
|
+
gtsam_unstable/gtsam_unstable.cpython-313-darwin.so,sha256=JHP4Q7Bg-vWt2lf7lBf8uu-Jruyjgxzo_vTNXLLleQo,2050208
|
|
258
258
|
gtsam_unstable/tests/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
259
259
|
gtsam_unstable/tests/test_ProjectionFactorRollingShutter.py,sha256=t2l62uWoXfjrM8oH6ogV7M20WjTYKZ4CSferdurMIY0,2156
|
|
260
260
|
gtsam_unstable/examples/__init__.py,sha256=47DEQpj8HBSa-_TImW-5JCeuQeRkm5NMpJWZG3hSuFU,0
|
|
Binary file
|
|
File without changes
|
|
File without changes
|