1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
|
#!/usr/bin/python3
# autopkgtest smoke test for python3-gtsam
# Exercises factor graph construction and optimisation through the Python API.
import sys
import math
import numpy as np
import gtsam
# ---------------------------------------------------------------------------
# 1. Basic import / version check
# ---------------------------------------------------------------------------
print(f"gtsam module loaded from: {gtsam.__file__}")
# ---------------------------------------------------------------------------
# 2. Tiny 2-D odometry problem (mirrors OdometryExample)
# ---------------------------------------------------------------------------
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph = gtsam.NonlinearFactorGraph()
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
graph.add(gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2, 0, 0), ODOMETRY_NOISE))
initial = gtsam.Values()
initial.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initial.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initial.insert(3, gtsam.Pose2(4.1, 0.1, 0.1))
result = gtsam.LevenbergMarquardtOptimizer(graph, initial).optimize()
x3 = result.atPose2(3)
assert math.isclose(x3.x(), 4.0, abs_tol=0.1), f"x3.x() unexpected: {x3.x()}"
assert math.isclose(x3.y(), 0.0, abs_tol=0.1), f"x3.y() unexpected: {x3.y()}"
print(f"smoke-python PASSED x3=({x3.x():.4f}, {x3.y():.4f}, {x3.theta():.4f})")
sys.exit(0)
|
