def __init__(self):
# Create graph container and add factors to it
self.graph = gt.NonlinearFactorGraph()
# Create initial estimate for camera poses and landmarks
self.initialEstimate = gt.Values()
sigmas = np.array([
5 * np.pi / 180, 5 * np.pi / 180, 5 * np.pi / 180, 0.05, 0.05, 0.05
])
self.covariance = gt.noiseModel.Diagonal.Sigmas(sigmas)
self.graph.add(gt.NonlinearEqualityPose3(X(0), gt.Pose3(np.eye(4))))
self.result = None
self.marginals = None
def test_Pose3SLAMExample(self) -> None:
# Create a hexagon of poses
hexagon = circlePose3(numPoses=6, radius=1.0)
p0 = hexagon.atPose3(0)
p1 = hexagon.atPose3(1)
# create a Pose graph with one equality constraint and one measurement
fg = gtsam.NonlinearFactorGraph()
fg.add(gtsam.NonlinearEqualityPose3(0, p0))
delta = p0.between(p1)
covariance = gtsam.noiseModel.Diagonal.Sigmas(
np.array([
0.05, 0.05, 0.05,
np.deg2rad(5.),
np.deg2rad(5.),
np.deg2rad(5.)
]))
fg.add(gtsam.BetweenFactorPose3(0, 1, delta, covariance))
fg.add(gtsam.BetweenFactorPose3(1, 2, delta, covariance))
fg.add(gtsam.BetweenFactorPose3(2, 3, delta, covariance))
fg.add(gtsam.BetweenFactorPose3(3, 4, delta, covariance))
fg.add(gtsam.BetweenFactorPose3(4, 5, delta, covariance))
fg.add(gtsam.BetweenFactorPose3(5, 0, delta, covariance))
# Create initial config
initial = gtsam.Values()
s = 0.10
initial.insert(0, p0)
initial.insert(1,
hexagon.atPose3(1).retract(s * np.random.randn(6, 1)))
initial.insert(2,
hexagon.atPose3(2).retract(s * np.random.randn(6, 1)))
initial.insert(3,
hexagon.atPose3(3).retract(s * np.random.randn(6, 1)))
initial.insert(4,
hexagon.atPose3(4).retract(s * np.random.randn(6, 1)))
initial.insert(5,
hexagon.atPose3(5).retract(s * np.random.randn(6, 1)))
# optimize
optimizer = gtsam.LevenbergMarquardtOptimizer(fg, initial)
result = optimizer.optimizeSafely()
pose_1 = result.atPose3(1)
self.gtsamAssertEquals(pose_1, p1, 1e-4)
def test_StereoVOExample(self):
## Assumptions
# - For simplicity this example is in the camera's coordinate frame
# - X: right, Y: down, Z: forward
# - Pose x1 is at the origin, Pose 2 is 1 meter forward (along Z-axis)
# - x1 is fixed with a constraint, x2 is initialized with noisy values
# - No noise on measurements
## Create keys for variables
x1 = symbol('x', 1)
x2 = symbol('x', 2)
l1 = symbol('l', 1)
l2 = symbol('l', 2)
l3 = symbol('l', 3)
## Create graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()
## add a constraint on the starting pose
first_pose = gtsam.Pose3()
graph.add(gtsam.NonlinearEqualityPose3(x1, first_pose))
## Create realistic calibration and measurement noise model
# format: fx fy skew cx cy baseline
K = gtsam.Cal3_S2Stereo(1000, 1000, 0, 320, 240, 0.2)
stereo_model = gtsam.noiseModel.Diagonal.Sigmas(
np.array([1.0, 1.0, 1.0]))
## Add measurements
# pose 1
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(520, 480, 440),
stereo_model, x1, l1, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(120, 80, 440),
stereo_model, x1, l2, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 280, 140),
stereo_model, x1, l3, K))
#pose 2
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(570, 520, 490),
stereo_model, x2, l1, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(70, 20, 490),
stereo_model, x2, l2, K))
graph.add(
gtsam.GenericStereoFactor3D(gtsam.StereoPoint2(320, 270, 115),
stereo_model, x2, l3, K))
## Create initial estimate for camera poses and landmarks
initialEstimate = gtsam.Values()
initialEstimate.insert(x1, first_pose)
# noisy estimate for pose 2
initialEstimate.insert(
x2, gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1, -.1, 1.1)))
expected_l1 = gtsam.Point3(1, 1, 5)
initialEstimate.insert(l1, expected_l1)
initialEstimate.insert(l2, gtsam.Point3(-1, 1, 5))
initialEstimate.insert(l3, gtsam.Point3(0, -.5, 5))
## optimize
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimize()
## check equality for the first pose and point
pose_x1 = result.atPose3(x1)
self.gtsamAssertEquals(pose_x1, first_pose, 1e-4)
point_l1 = result.atPoint3(l1)
self.gtsamAssertEquals(point_l1, expected_l1, 1e-4)
def initialize(data, truth, options):
# Initialize iSAM
params = gtsam.ISAM2Params()
if options.alwaysRelinearize:
params.relinearizeSkip = 1
isam = gtsam.ISAM2(params=params)
# Add constraints/priors
# TODO: should not be from ground truth!
newFactors = gtsam.NonlinearFactorGraph()
initialEstimates = gtsam.Values()
for i in range(2):
ii = symbol('x', i)
if i == 0:
if options.hardConstraint: # add hard constraint
newFactors.add(
gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose()))
else:
newFactors.add(
gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(),
data.noiseModels.posePrior))
initialEstimates.insert(ii, truth.cameras[i].pose())
nextPoseIndex = 2
# Add visual measurement factors from two first poses and initialize
# observed landmarks
for i in range(2):
ii = symbol('x', i)
for k in range(len(data.Z[i])):
j = data.J[i][k]
jj = symbol('l', j)
newFactors.add(
gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], data.noiseModels.measurement, ii, jj,
data.K))
# TODO: initial estimates should not be from ground truth!
if not initialEstimates.exists(jj):
initialEstimates.insert(jj, truth.points[j])
if options.pointPriors: # add point priors
newFactors.add(
gtsam.PriorFactorPoint3(jj, truth.points[j],
data.noiseModels.pointPrior))
# Add odometry between frames 0 and 1
newFactors.add(
gtsam.BetweenFactorPose3(symbol('x', 0), symbol('x', 1),
data.odometry[1], data.noiseModels.odometry))
# Update ISAM
if options.batchInitialization: # Do a full optimize for first two poses
batchOptimizer = gtsam.LevenbergMarquardtOptimizer(
newFactors, initialEstimates)
fullyOptimized = batchOptimizer.optimize()
isam.update(newFactors, fullyOptimized)
else:
isam.update(newFactors, initialEstimates)
# figure(1)tic
# t=toc plot(frame_i,t,'r.') tic
result = isam.calculateEstimate()
# t=toc plot(frame_i,t,'g.')
return isam, result, nextPoseIndex