def test_run(self):
gt_poses = ExampleValues()
measurements = SimulateMeasurements(gt_poses, [[0, 1], [0, 2], [1, 2]])
# Set verbosity to Silent for tests
lmParams = gtsam.LevenbergMarquardtParams()
lmParams.setVerbosityLM("silent")
algorithm = gtsam.TranslationRecovery(measurements, lmParams)
scale = 2.0
result = algorithm.run(scale)
w_aTc = gt_poses.atPose3(2).translation() - gt_poses.atPose3(
0).translation()
w_aTb = gt_poses.atPose3(1).translation() - gt_poses.atPose3(
0).translation()
w_aTc_expected = w_aTc * scale / np.linalg.norm(w_aTb)
w_aTb_expected = w_aTb * scale / np.linalg.norm(w_aTb)
np.testing.assert_array_almost_equal(result.atPoint3(0),
np.array([0, 0, 0]), 6,
"Origin result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(1),
w_aTb_expected, 6,
"Point B result is incorrect.")
np.testing.assert_array_almost_equal(result.atPoint3(2),
w_aTc_expected, 6,
"Point C result is incorrect.")
def test_constructor(self):
"""Construct from binary measurements."""
algorithm = gtsam.TranslationRecovery()
self.assertIsInstance(algorithm, gtsam.TranslationRecovery)
algorithm_params = gtsam.TranslationRecovery(
gtsam.LevenbergMarquardtParams())
self.assertIsInstance(algorithm_params, gtsam.TranslationRecovery)
def main():
"""Main runner."""
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first point, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose3() # prior at origin
graph.add(gtsam.PriorFactorPose3(1, priorMean, PRIOR_NOISE))
# Add GPS factors
gps = gtsam.Point3(lat0, lon0, h0)
graph.add(gtsam.GPSFactor(1, gps, GPS_NOISE))
print("\nFactor Graph:\n{}".format(graph))
# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
initial = gtsam.Values()
initial.insert(1, gtsam.Pose3())
print("\nInitial Estimate:\n{}".format(initial))
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
def optimize(self, graph: gtsam.NonlinearFactorGraph,
initial: gtsam.Values):
"""Optimize using Levenberg-Marquardt optimization."""
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
return result
def estimate_global(self):
np.random.seed(2)
n0 = 0.000001
n03 = np.array([n0, n0, n0])
nNoiseFactor3 = np.array(
[0.2, 0.2,
0.2]) # TODO: something about floats and major row? check cython
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first pose, setting it to the origin
priorMean = self.odom_global[0]
priorNoise = gtsam.noiseModel_Diagonal.Sigmas(n03)
graph.add(gtsam.PriorFactorPose2(self.X(1), priorMean, priorNoise))
# Add odometry factors
odometryNoise = gtsam.noiseModel_Diagonal.Sigmas(nNoiseFactor3)
for i, pose in enumerate(self.odom_relative):
graph.add(
gtsam.BetweenFactorPose2(self.X(i + 1), self.X(i + 2), pose,
odometryNoise))
# Add visual factors
visualNoise = gtsam.noiseModel_Diagonal.Sigmas(nNoiseFactor3)
for i, pose in enumerate(self.visual_relative):
graph.add(
gtsam.BetweenFactorPose2(self.X(i + 1), self.X(i + 2), pose,
visualNoise))
# set initial guess to odometry estimates
initialEstimate = gtsam.Values()
for i, pose in enumerate(self.odom_global):
initialEstimate.insert(self.X(i + 1), pose)
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
# gtsam_quadrics.setMaxIterations(params, iter)
# gtsam_quadrics.setRelativeErrorTol(params, 1e-8)
# gtsam_quadrics.setAbsoluteErrorTol(params, 1e-8)
# graph.print_('graph')
# initialEstimate.print_('initialEstimate ')
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate,
params)
# parameters = gtsam.GaussNewtonParams()
# parameters.relativeErrorTol = 1e-8
# parameters.maxIterations = 300
# optimizer = gtsam.GaussNewtonOptimizer(graph, initialEstimate, parameters)
result = optimizer.optimize()
# result.print_('result ')
# self.draw_trajectories([self.odom_global, self.visual_global], ['b', 'r'], 2)
return self.unwrap_results(result)
def optimize(self, verbosity=0):
params = gtsam.LevenbergMarquardtParams()
params.setVerbosity(["SILENT", "TERMINATION"][verbosity])
params.setAbsoluteErrorTol(1e-10)
params.setRelativeErrorTol(1e-10)
optimizer = gtsam.LevenbergMarquardtOptimizer(self.gtsam_graph, self.values, params)
result_values = optimizer.optimize()
return {name: gtsam2sophus(result_values.atPose3(var)) for name, var in self.vars.items()}
def build_graph():
print("build_graph !!!")
# Create noise models
ODOMETRY_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.2, 0.2,
0.1]))
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
graph.add(gtsam.PriorFactorPose2(1, priorMean, PRIOR_NOISE))
# Add odometry factors
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
odometry1 = gtsam.Pose2(edge_list[0].position.x, edge_list[0].position.y,
edge_list[0].position.z)
odometry2 = gtsam.Pose2(edge_list[1].position.x, edge_list[1].position.y,
edge_list[1].position.z)
# For simplicity, we will use the same noise model for each odometry factor
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry1, ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry2, ODOMETRY_NOISE))
print("\nFactor Graph:\n{}".format(graph))
# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
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))
print("\nInitial Estimate:\n{}".format(initial))
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
# 5. Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for i in range(1, 4):
print("X{} covariance:\n{}\n".format(i,
marginals.marginalCovariance(i)))
fig = plt.figure(0)
for i in range(1, 4):
gtsam_plot.plot_pose2(0, result.atPose2(i), 0.5,
marginals.marginalCovariance(i))
plt.axis('equal')
plt.show()
def __optimize_factor_graph(self, graph: NonlinearFactorGraph,
initial_values: Values) -> Values:
"""Optimize the factor graph."""
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("ERROR")
if self._max_iterations:
params.setMaxIterations(self._max_iterations)
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial_values, params)
result_values = lm.optimize()
return result_values
def optimize(self, graph_to_optimize=None):
# Optimization
params = gtsam.LevenbergMarquardtParams()
if graph_to_optimize is None:
optimizer = gtsam.LevenbergMarquardtOptimizer(
self.graph, self.initial, params)
else:
optimizer = gtsam.LevenbergMarquardtOptimizer(
graph_to_optimize, self.prop_initial, params)
self.result = optimizer.optimize()
self.initial = self.result
self.resultsFlag = True
def test_lm_simple_printing(self):
"""Make sure we are properly terminating LM"""
def hook(_, error):
print(error)
params = gtsam.LevenbergMarquardtParams()
actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook,
self.graph, self.initial)
self.check(actual)
actual = optimize_using(gtsam.LevenbergMarquardtOptimizer, hook,
self.graph, self.initial, params)
self.check(actual)
actual = gtsam_optimize(
gtsam.LevenbergMarquardtOptimizer(self.graph, self.initial,
params), params, hook)
def optimize_old(self,
graph_to_optimize=None
): #TODO: Old one; Turn back on if ISAM2 refuses to work.
# Optimization
params = gtsam.LevenbergMarquardtParams()
if graph_to_optimize is None:
optimizer = gtsam.LevenbergMarquardtOptimizer(
self.graph, self.initial, params)
else:
optimizer = gtsam.LevenbergMarquardtOptimizer(
graph_to_optimize, self.prop_initial, params)
self.result = optimizer.optimize()
self.initial = self.result
self.resultsFlag = True
def test_optimization(self):
"""Tests if a factor graph with a CustomFactor can be properly optimized"""
gT1 = Pose2(1, 2, np.pi / 2)
gT2 = Pose2(-1, 4, np.pi)
expected = Pose2(2, 2, np.pi / 2)
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
:param v: Values object
:param H: list of references to the Jacobian arrays
:return: the non-linear error
"""
key0 = this.keys()[0]
key1 = this.keys()[1]
gT1, gT2 = v.atPose2(key0), v.atPose2(key1)
error = expected.localCoordinates(gT1.between(gT2))
if H is not None:
result = gT1.between(gT2)
H[0] = -result.inverse().AdjointMap()
H[1] = np.eye(3)
return error
noise_model = gtsam.noiseModel.Unit.Create(3)
cf = CustomFactor(noise_model, [0, 1], error_func)
fg = gtsam.NonlinearFactorGraph()
fg.add(cf)
fg.add(gtsam.PriorFactorPose2(0, gT1, noise_model))
v = Values()
v.insert(0, Pose2(0, 0, 0))
v.insert(1, Pose2(0, 0, 0))
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(fg, v, params)
result = optimizer.optimize()
self.gtsamAssertEquals(result.atPose2(0), gT1, tol=1e-5)
self.gtsamAssertEquals(result.atPose2(1), gT2, tol=1e-5)
def optimize(graph, initial_estimate, calibration):
# create optimizer parameters
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY") # SILENT = 0, SUMMARY, TERMINATION, LAMBDA, TRYLAMBDA, TRYCONFIG, DAMPED, TRYDELTA : VALUES, ERROR
params.setMaxIterations(100)
params.setlambdaInitial(1e-5)
params.setlambdaUpperBound(1e10)
params.setlambdaLowerBound(1e-8)
params.setRelativeErrorTol(1e-5)
params.setAbsoluteErrorTol(1e-5)
# create optimizer
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
# run optimizer
print('starting optimization')
estimate = optimizer.optimize()
print('optimization finished')
return estimate
def compute_control(self, start_state, nominal_states):
# build graph
graph = gtsam.NonlinearFactorGraph()
x0_key = self.state_key(0)
graph.add(gtd.PriorFactorVector5(x0_key, start_state,
self.prior_noise))
for i in range(self.N):
x_curr_key = self.state_key(i)
x_next_key = self.state_key(i + 1)
graph.add(
gtd.CPDynamicsFactor(x_curr_key, x_next_key,
self.transition_noise, self.dt))
graph.add(gtd.CPStateCostFactor(x_next_key, self.cost_noise))
# build init values
init_values = gtsam.Values()
for i in range(self.N + 1):
x_key = self.state_key(i)
init_values.insert(x_key, np.array(nominal_states[i]))
# optimize
params = gtsam.LevenbergMarquardtParams()
params.setMaxIterations(20)
# params.setVerbosityLM("SUMMARY")
params.setLinearSolverType("MULTIFRONTAL_QR")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, init_values,
params)
results = optimizer.optimize()
# print("error:", graph.error(results))
# get results
states = []
for i in range(self.N + 1):
x_key = self.state_key(i)
states.append(results.atVector(x_key))
dx_c = states[1][-1] - states[0][-1]
u_star = states[0][-1] + 1.1 * dx_c / (20 * self.dt)
return u_star, states
def setUp(self):
"""Set up a small Karcher mean optimization example."""
# Grabbed from KarcherMeanFactor unit tests.
R = Rot3.Expmap(np.array([0.1, 0, 0]))
rotations = {R, R.inverse()} # mean is the identity
self.expected = Rot3()
graph = gtsam.NonlinearFactorGraph()
for R in rotations:
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
initial = gtsam.Values()
initial.insert(KEY, R)
self.params = gtsam.GaussNewtonParams()
self.optimizer = gtsam.GaussNewtonOptimizer(graph, initial,
self.params)
self.lmparams = gtsam.LevenbergMarquardtParams()
self.lmoptimizer = gtsam.LevenbergMarquardtOptimizer(
graph, initial, self.lmparams)
# setup output capture
self.capturedOutput = StringIO()
sys.stdout = self.capturedOutput
def run(self, initial_data: GtsfmData) -> GtsfmData:
"""Run the bundle adjustment by forming factor graph and optimizing using Levenberg–Marquardt optimization.
Args:
initial_data: initialized cameras, tracks w/ their 3d landmark from triangulation.
Results:
optimized camera poses, 3D point w/ tracks, and error metrics.
"""
logger.info(
f"Input: {initial_data.number_tracks()} tracks on {len(initial_data.get_valid_camera_indices())} cameras\n"
)
# noise model for measurements -- one pixel in u and v
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
IMG_MEASUREMENT_DIM, 1.0)
# Create a factor graph
graph = gtsam.NonlinearFactorGraph()
# Add measurements to the factor graph
for j in range(initial_data.number_tracks()):
track = initial_data.get_track(j) # SfmTrack
# retrieve the SfmMeasurement objects
for m_idx in range(track.number_measurements()):
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
graph.add(
GeneralSFMFactorCal3Bundler(uv, measurement_noise, C(i),
P(j)))
# get all the valid camera indices, which need to be added to the graph.
valid_camera_indices = initial_data.get_valid_camera_indices()
# Add a prior on first pose. This indirectly specifies where the origin is.
graph.push_back(
gtsam.PriorFactorPinholeCameraCal3Bundler(
C(valid_camera_indices[0]),
initial_data.get_camera(valid_camera_indices[0]),
gtsam.noiseModel.Isotropic.Sigma(PINHOLE_CAM_CAL3BUNDLER_DOF,
0.1),
))
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
gtsam.PriorFactorPoint3(
P(0),
initial_data.get_track(0).point3(),
gtsam.noiseModel.Isotropic.Sigma(POINT3_DOF, 0.1)))
# Create initial estimate
initial = gtsam.Values()
# add each PinholeCameraCal3Bundler
for i in valid_camera_indices:
camera = initial_data.get_camera(i)
initial.insert(C(i), camera)
# add each SfmTrack
for j in range(initial_data.number_tracks()):
track = initial_data.get_track(j)
initial.insert(P(j), track.point3())
# Optimize the graph and print results
try:
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("ERROR")
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result_values = lm.optimize()
except Exception:
logger.exception("LM Optimization failed")
# as we did not perform the bundle adjustment, we skip computing the total reprojection error
return GtsfmData(initial_data.number_images())
final_error = graph.error(result_values)
# Error drops from ~2764.22 to ~0.046
logger.info(f"initial error: {graph.error(initial):.2f}")
logger.info(f"final error: {final_error:.2f}")
# construct the results
optimized_data = values_to_gtsfm_data(result_values, initial_data)
metrics_dict = {}
metrics_dict["before_filtering"] = optimized_data.aggregate_metrics()
logger.info("[Result] Number of tracks before filtering: %d",
metrics_dict["before_filtering"]["number_tracks"])
# filter the largest errors
filtered_result = optimized_data.filter_landmarks(
self.output_reproj_error_thresh)
metrics_dict["after_filtering"] = filtered_result.aggregate_metrics()
io_utils.save_json_file(
os.path.join(METRICS_PATH, "bundle_adjustment_metrics.json"),
metrics_dict)
logger.info("[Result] Number of tracks after filtering: %d",
metrics_dict["after_filtering"]["number_tracks"])
logger.info(
"[Result] Mean track length %.3f",
metrics_dict["after_filtering"]["3d_track_lengths"]["mean"])
logger.info(
"[Result] Median track length %.3f",
metrics_dict["after_filtering"]["3d_track_lengths"]["median"])
filtered_result.log_scene_reprojection_error_stats()
return filtered_result
def batch_factorgraph_example():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create the keys for the poses.
X1 = X(1)
X2 = X(2)
X3 = X(3)
pose_variables = [X1, X2, X3]
# Create keys for the landmarks.
L1 = L(1)
L2 = L(2)
landmark_variables = [L1, L2]
# Add a prior on pose X1 at the origin.
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1, 0.1]))
graph.add(
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), prior_noise))
# Add odometry factors between X1,X2 and X2,X3, respectively
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.1, 0.1,
0.1]))
graph.add(
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
# Add Range-Bearing measurements to two different landmarks L1 and L2
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
np.sqrt(4.0 + 4.0), measurement_noise))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Create an optimizer.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
params)
# Solve the MAP problem.
result = optimizer.optimize()
# Calculate marginal covariances for all variables.
marginals = gtsam.Marginals(graph, result)
# Extract marginals
pose_marginals = []
for var in pose_variables:
pose_marginals.append(
MultivariateNormalParameters(result.atPose2(var),
marginals.marginalCovariance(var)))
landmark_marginals = []
for var in landmark_variables:
landmark_marginals.append(
MultivariateNormalParameters(result.atPoint2(var),
marginals.marginalCovariance(var)))
# You can extract the joint marginals like this.
joint_all = marginals.jointMarginalCovariance(
gtsam.KeyVector(pose_variables + landmark_variables))
print("Joint covariance over all variables:")
print(joint_all.fullMatrix())
# Plot the marginals.
plot_result(pose_marginals, landmark_marginals)
def run(args):
""" Run LM optimization with BAL input data and report resulting error """
input_file = gtsam.findExampleDataFile(args.input_file)
# Load the SfM data from file
scene_data = readBal(input_file)
logging.info(
f"read {scene_data.number_tracks()} tracks on {scene_data.number_cameras()} cameras\n"
)
# Create a factor graph
graph = gtsam.NonlinearFactorGraph()
# We share *one* noiseModel between all projection factors
noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Add measurements to the factor graph
j = 0
for t_idx in range(scene_data.number_tracks()):
track = scene_data.track(t_idx) # SfmTrack
# retrieve the SfmMeasurement objects
for m_idx in range(track.number_measurements()):
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, C(i), P(j)))
j += 1
# Add a prior on pose x1. This indirectly specifies where the origin is.
graph.push_back(
gtsam.PriorFactorPinholeCameraCal3Bundler(
C(0), scene_data.camera(0),
gtsam.noiseModel.Isotropic.Sigma(9, 0.1)))
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
gtsam.PriorFactorPoint3(P(0),
scene_data.track(0).point3(),
gtsam.noiseModel.Isotropic.Sigma(3, 0.1)))
# Create initial estimate
initial = gtsam.Values()
i = 0
# add each PinholeCameraCal3Bundler
for cam_idx in range(scene_data.number_cameras()):
camera = scene_data.camera(cam_idx)
initial.insert(C(i), camera)
i += 1
j = 0
# add each SfmTrack
for t_idx in range(scene_data.number_tracks()):
track = scene_data.track(t_idx)
initial.insert(P(j), track.point3())
j += 1
# Optimize the graph and print results
try:
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("ERROR")
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = lm.optimize()
except Exception as e:
logging.exception("LM Optimization failed")
return
# Error drops from ~2764.22 to ~0.046
logging.info(f"final error: {graph.error(result)}")
def run(self, T=12, compute_covariances=False, verbose=True):
graph = gtsam.NonlinearFactorGraph()
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
# T = 12
num_poses = T # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
# simulate the loop
i = 0 # state index
initial_state_i = self.scenario.navState(0)
initial.insert(X(i), initial_state_i.pose())
initial.insert(V(i), initial_state_i.velocity())
# add prior on beginning
self.addPrior(0, graph)
gtNavStates = []
predictedNavStates = []
integrationTime = []
for k, t in enumerate(np.arange(0, T, self.dt)):
# get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
start = time()
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
integrationTime.append(time() - start)
# Plot IMU many times
if k % 10 == 0 and not DISABLE_VISUAL:
self.plotImu(t, measuredOmega, measuredAcc)
if (k + 1) % int(1 / self.dt) == 0:
# Plot every second
gtNavState = self.plotGroundTruthPose(t, scale=0.3)
gtNavStates.append(gtNavState)
plt.title("Ground Truth + Estimated Trajectory")
# create IMU factor every second
factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1),
BIAS_KEY, pim)
graph.push_back(factor)
if verbose:
print(factor)
print(pim.predict(initial_state_i, self.actualBias))
pim.resetIntegration()
rotationNoise = gtsam.Rot3.Expmap(np.random.randn(3) * 0.1 * 1)
translationNoise = gtsam.Point3(*np.random.randn(3) * 1 * 1)
poseNoise = gtsam.Pose3(rotationNoise, translationNoise)
actual_state_i = self.scenario.navState(t + self.dt)
if not DISABLE_VISUAL:
print("Actual state at {0}:\n{1}".format(
t + self.dt, actual_state_i))
noisy_state_i = gtsam.NavState(
actual_state_i.pose().compose(poseNoise),
actual_state_i.velocity() + np.random.randn(3) * 0.1)
initial.insert(X(i + 1), noisy_state_i.pose())
initial.insert(V(i + 1), noisy_state_i.velocity())
i += 1
# add priors on end
self.addPrior(num_poses - 1, graph)
initial.print_("Initial values:")
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
result.print_("Optimized values:")
if compute_covariances:
# Calculate and print marginal covariances
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n",
marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
# Plot resulting poses
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
predictedNavStates.append(pose_i)
if not DISABLE_VISUAL:
plot_pose3(POSES_FIG, pose_i, 1)
i += 1
# plt.title("Estimated Trajectory")
if not DISABLE_VISUAL:
gtsam.utils.plot.set_axes_equal(POSES_FIG)
print("Bias Values", result.atConstantBias(BIAS_KEY))
ATE = []
# import ipdb; ipdb.set_trace()
for gt, pred in zip(gtNavStates, predictedNavStates[1:]):
delta = gt.inverse().compose(pred)
ATE.append(np.linalg.norm(delta.Logmap(delta))**2)
print("ATE={}".format(np.sqrt(np.mean(ATE))))
print("Run time={}".format(np.median(integrationTime)))
plt.ioff()
plt.show()
def run(slam_request=DEFAULT_REQUEST):
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
unknowns = slam_request.symbols
# Add a prior on pose X1 at the origin. A prior factor consists of a mean
# and a noise model
for prior in slam_request.priors:
graph.add(prior)
# Add odometry factors between X1,X2 and X2,X3, respectively
for factor in slam_request.between_pose_factors:
graph.add(factor)
for factor in slam_request.bearing_range_factors:
graph.add(factor)
# Print graph
print("Factor Graph:\n{}".format(graph))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
for variable, estimate in slam_request.initial_estimates.iteritems():
initial_estimate.insert(variable, estimate)
# Print
print("Initial Estimate:\n{}".format(initial_estimate))
# Optimize using Levenberg-Marquardt optimization. The optimizer
# accepts an optional set of configuration parameters, controlling
# things like convergence criteria, the type of linear system solver
# to use, and the amount of information displayed during optimization.
# Here we will use the default set of parameters. See the
# documentation for the full set of parameters.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
output_estimations = {}
reverse_lookup = dict(
(value, key) for (key, value) in unknowns.iteritems())
for variable, initial_estimate in slam_request.initial_estimates.iteritems(
):
if isinstance(initial_estimate, gtsam.Pose2):
pose = result.atPose2(variable)
estimation = [pose.x(), pose.y(), pose.theta()]
elif isinstance(initial_estimate, gtsam.Point2):
point = result.atPoint2(variable)
estimation = [point.x(), point.y()]
else:
raise Exception("Unsupported type: " + type(variable))
output_estimations[reverse_lookup[variable]] = estimation
# Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
covariance_dict = {}
for (string, variable) in unknowns.iteritems():
covariance = marginals.marginalCovariance(variable)
print("{} covariance:\n{}\n".format(string, covariance))
covariance_dict[string] = covariance
return PlanarSlamOutput(
result=output_estimations,
covariance=covariance_dict,
)
def run(self):
graph = gtsam.NonlinearFactorGraph()
# initialize data structure for pre-integrated IMU measurements
pim = gtsam.PreintegratedImuMeasurements(self.params, self.actualBias)
H9 = gtsam.OptionalJacobian9()
T = 12
num_poses = T + 1 # assumes 1 factor per second
initial = gtsam.Values()
initial.insert(BIAS_KEY, self.actualBias)
for i in range(num_poses):
state_i = self.scenario.navState(float(i))
initial.insert(X(i), state_i.pose())
initial.insert(V(i), state_i.velocity())
# simulate the loop
i = 0 # state index
actual_state_i = self.scenario.navState(0)
for k, t in enumerate(np.arange(0, T, self.dt)):
# get measurements and add them to PIM
measuredOmega = self.runner.measuredAngularVelocity(t)
measuredAcc = self.runner.measuredSpecificForce(t)
pim.integrateMeasurement(measuredAcc, measuredOmega, self.dt)
# Plot IMU many times
if k % 10 == 0:
self.plotImu(t, measuredOmega, measuredAcc)
# Plot every second
if k % int(1 / self.dt) == 0:
self.plotGroundTruthPose(t)
# create IMU factor every second
if (k + 1) % int(1 / self.dt) == 0:
factor = gtsam.ImuFactor(X(i), V(i), X(i + 1), V(i + 1),
BIAS_KEY, pim)
graph.push_back(factor)
if True:
print(factor)
H2 = gtsam.OptionalJacobian96()
print(pim.predict(actual_state_i, self.actualBias, H9, H2))
pim.resetIntegration()
actual_state_i = self.scenario.navState(t + self.dt)
i += 1
# add priors on beginning and end
self.addPrior(0, graph)
self.addPrior(num_poses - 1, graph)
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
# Calculate and print marginal covariances
marginals = gtsam.Marginals(graph, result)
print("Covariance on bias:\n", marginals.marginalCovariance(BIAS_KEY))
for i in range(num_poses):
print("Covariance on pose {}:\n{}\n".format(
i, marginals.marginalCovariance(X(i))))
print("Covariance on vel {}:\n{}\n".format(
i, marginals.marginalCovariance(V(i))))
# Plot resulting poses
i = 0
while result.exists(X(i)):
pose_i = result.atPose3(X(i))
plotPose3(POSES_FIG, pose_i, 0.1)
i += 1
print(result.atConstantBias(BIAS_KEY))
plt.ioff()
plt.show()
graph.add(gt.BetweenFactorPose2(8, 2, gt.Pose2(2.9161, 0.0, 0.0), model))
graph.add(gt.BetweenFactorPose2(2, 6, gt.Pose2(-0.3506, 0.0, 0.0), model))
graph.add(gt.BetweenFactorPose2(6, 7, gt.Pose2(-2.1365, 0.0, 0.0), model))
graph.add(gt.BetweenFactorPose2(1, 8, gt.Pose2(0.3261, 0.0, 0.0), model))
graph.add(gt.BetweenFactorPose2(8, 6, gt.Pose2(2.5653, 0.0, 0.0), model))
graph.add(gt.BetweenFactorPose2(5, 6, gt.Pose2(0.8577, 0.0, 0.0), model))
graph.add(gt.BetweenFactorPose2(4, 5, gt.Pose2(0.4074, 0.0, 0.0), model))
graph.add(gt.BetweenFactorPose2(3, 4, gt.Pose2(-0.2153, 0.0, 0.0), model))
graph.add(gt.BetweenFactorPose2(3, 6, gt.Pose2(1.0458, 0.0, 0.0), model))
graph.print("\nFactor Graph\n")
initialEstimate = gt.Values()
initialEstimate.insert(1, gt.Pose2(0.0, 0.0, 0.0))
initialEstimate.insert(2, gt.Pose2(3.2456, 0.0, 0.0))
initialEstimate.insert(3, gt.Pose2(1.8443, 0.0, 0.0))
initialEstimate.insert(4, gt.Pose2(1.6266, 0.0, 0.0))
initialEstimate.insert(5, gt.Pose2(2.034, 0.0, 0.0))
initialEstimate.insert(6, gt.Pose2(2.8917, 0.0, 0.0))
initialEstimate.insert(7, gt.Pose2(0.7579, 0.0, 0.0))
initialEstimate.insert(8, gt.Pose2(0.3261, 0.0, 0.0))
initialEstimate.print("\nInitial Estimate:\n") # print
parameters = gt.LevenbergMarquardtParams()
parameters.relativeErrorTol = 1e-5
parameters.maxIterations = 100
optimizer = gt.LevenbergMarquardtOptimizer(graph, initialEstimate, parameters)
result = optimizer.optimize()
result.print("Final Result:\n")
def run(args: argparse.Namespace) -> None:
""" Run LM optimization with BAL input data and report resulting error """
input_file = args.input_file
# Load the SfM data from file
scene_data = SfmData.FromBalFile(input_file)
logging.info("read %d tracks on %d cameras\n", scene_data.numberTracks(),
scene_data.numberCameras())
# Create a factor graph
graph = gtsam.NonlinearFactorGraph()
# We share *one* noiseModel between all projection factors
noise = gtsam.noiseModel.Isotropic.Sigma(2, 1.0) # one pixel in u and v
# Add measurements to the factor graph
for j in range(scene_data.numberTracks()):
track = scene_data.track(j) # SfmTrack
# retrieve the SfmMeasurement objects
for m_idx in range(track.numberMeasurements()):
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
graph.add(GeneralSFMFactorCal3Bundler(uv, noise, i, P(j)))
# Add a prior on pose x1. This indirectly specifies where the origin is.
graph.push_back(
PriorFactorPinholeCameraCal3Bundler(
0, scene_data.camera(0), gtsam.noiseModel.Isotropic.Sigma(9, 0.1)))
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
PriorFactorPoint3(P(0),
scene_data.track(0).point3(),
gtsam.noiseModel.Isotropic.Sigma(3, 0.1)))
# Create initial estimate
initial = gtsam.Values()
i = 0
# add each PinholeCameraCal3Bundler
for i in range(scene_data.numberCameras()):
camera = scene_data.camera(i)
initial.insert(i, camera)
i += 1
# add each SfmTrack
for j in range(scene_data.numberTracks()):
track = scene_data.track(j)
initial.insert(P(j), track.point3())
# Optimize the graph and print results
try:
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("ERROR")
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = lm.optimize()
except RuntimeError:
logging.exception("LM Optimization failed")
return
# Error drops from ~2764.22 to ~0.046
logging.info("initial error: %f", graph.error(initial))
logging.info("final error: %f", graph.error(result))
plot_scene(scene_data, result)
graph = gtsam.NonlinearFactorGraph()
# Add a prior on the first pose, setting it to the origin
# A prior factor consists of a mean and a noise model (covariance matrix)
priorMean = gtsam.Pose2(0.0, 0.0, 0.0) # prior at origin
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.3, 0.3, 0.1]))
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
# Add odometry factors
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
# For simplicity, we will use the same noise model for each odometry factor
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
# Create odometry (Between) factors between consecutive poses
graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
graph.add(gtsam.BetweenFactorPose2(2, 3, odometry, odometryNoise))
graph.print("\nFactor Graph:\n")
# Create the data structure to hold the initialEstimate estimate to the solution
# For illustrative purposes, these have been deliberately set to incorrect values
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))
initial.print("\nInitial Estimate:\n")
# optimize using Levenberg-Marquardt optimization
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result = optimizer.optimize()
result.print("\nFinal Result:\n")
def main():
"""Main runner"""
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
X1 = X(1)
X2 = X(2)
X3 = X(3)
L1 = L(4)
L2 = L(5)
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
# Add odometry factors between X1,X2 and X2,X3, respectively
graph.add(
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
ODOMETRY_NOISE))
# Add Range-Bearing measurements to two different landmarks L1 and L2
graph.add(
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
MEASUREMENT_NOISE))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
MEASUREMENT_NOISE))
# Print graph
print("Factor Graph:\n{}".format(graph))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Print
print("Initial Estimate:\n{}".format(initial_estimate))
# Optimize using Levenberg-Marquardt optimization. The optimizer
# accepts an optional set of configuration parameters, controlling
# things like convergence criteria, the type of linear system solver
# to use, and the amount of information displayed during optimization.
# Here we will use the default set of parameters. See the
# documentation for the full set of parameters.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
# Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"),
(L2, "L2")]:
print("{} covariance:\n{}\n".format(s,
marginals.marginalCovariance(key)))
for i, imu_measurement in enumerate(imu_measurements):
if i in preintegration_steps_set:
predicted_nav_state = current_preintegrated_IMU.predict(
params.initial_state, params.IMU_bias)
initial_values.insert(symbol('x', i), predicted_nav_state.pose())
initial_values.insert(symbol('v', i),
predicted_nav_state.velocity())
current_preintegrated_IMU.integrateMeasurement(imu_measurements[i, :3],
imu_measurements[i, 3:],
params.dt)
############################### Optimize the factor graph ####################################
# Use the Levenberg-Marquardt algorithm
optimization_params = gtsam.LevenbergMarquardtParams()
optimization_params.setVerbosityLM("SUMMARY")
optimizer = gtsam.LevenbergMarquardtOptimizer(factor_graph, initial_values,
optimization_params)
optimization_result = optimizer.optimize()
############################### Plot the solution ####################################
for i in preintegration_steps:
# Ground truth pose
plot_pose3(1, gtsam.Pose3(true_poses[i]), 0.3)
# Estimated pose
plot_pose3(1, optimization_result.atPose3(symbol('x', i)), 0.1)
ax = plt.gca()
ax.set_xlim3d(-5, 5)
def run(self, initial_data: SfmData) -> SfmResult:
"""Run the bundle adjustment by forming factor graph and optimizing using Levenberg–Marquardt optimization.
Args:
initial_data: initialized cameras, tracks w/ their 3d landmark from triangulation.
Results:
optimized camera poses, 3D point w/ tracks, and error metrics.
"""
logger.info(
f"Input: {initial_data.number_tracks()} tracks on {initial_data.number_cameras()} cameras\n"
)
# noise model for measurements -- one pixel in u and v
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
IMG_MEASUREMENT_DIM, 1.0)
# Create a factor graph
graph = gtsam.NonlinearFactorGraph()
# Add measurements to the factor graph
for j in range(initial_data.number_tracks()):
track = initial_data.track(j) # SfmTrack
# retrieve the SfmMeasurement objects
for m_idx in range(track.number_measurements()):
# i represents the camera index, and uv is the 2d measurement
i, uv = track.measurement(m_idx)
# note use of shorthand symbols C and P
graph.add(
GeneralSFMFactorCal3Bundler(uv, measurement_noise, C(i),
P(j)))
# Add a prior on pose x1. This indirectly specifies where the origin is.
graph.push_back(
gtsam.PriorFactorPinholeCameraCal3Bundler(
C(0),
initial_data.camera(0),
gtsam.noiseModel.Isotropic.Sigma(PINHOLE_CAM_CAL3BUNDLER_DOF,
0.1),
))
# Also add a prior on the position of the first landmark to fix the scale
graph.push_back(
gtsam.PriorFactorPoint3(
P(0),
initial_data.track(0).point3(),
gtsam.noiseModel.Isotropic.Sigma(POINT3_DOF, 0.1),
))
# Create initial estimate
initial = gtsam.Values()
i = 0
# add each PinholeCameraCal3Bundler
for cam_idx in range(initial_data.number_cameras()):
camera = initial_data.camera(cam_idx)
initial.insert(C(i), camera)
i += 1
j = 0
# add each SfmTrack
for t_idx in range(initial_data.number_tracks()):
track = initial_data.track(t_idx)
initial.insert(P(j), track.point3())
j += 1
# Optimize the graph and print results
try:
params = gtsam.LevenbergMarquardtParams()
params.setVerbosityLM("ERROR")
lm = gtsam.LevenbergMarquardtOptimizer(graph, initial, params)
result_values = lm.optimize()
except Exception as e:
logger.exception("LM Optimization failed")
return SfmResult(SfmData(), float("Nan"))
final_error = graph.error(result_values)
# Error drops from ~2764.22 to ~0.046
logger.info(f"initial error: {graph.error(initial):.2f}")
logger.info(f"final error: {final_error:.2f}")
# construct the results
optimized_data = values_to_sfm_data(result_values, initial_data)
sfm_result = SfmResult(optimized_data, final_error)
return sfm_result
