def test_optimize(self):
"""Do trivial test with three optimizer variants."""
fg = NonlinearFactorGraph()
model = gtsam.noiseModel.Unit.Create(2)
fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
optimal_values = Values()
optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, fg.error(optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
initial_values = Values()
initial_values.insert(KEY1, x0)
self.assertEqual(9.0, fg.error(initial_values), 1e-3)
# optimize parameters
ordering = Ordering()
ordering.push_back(KEY1)
# Gauss-Newton
gnParams = GaussNewtonParams()
gnParams.setOrdering(ordering)
actual1 = GaussNewtonOptimizer(fg, initial_values, gnParams).optimize()
self.assertAlmostEqual(0, fg.error(actual1))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setOrdering(ordering)
actual2 = LevenbergMarquardtOptimizer(fg, initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Levenberg-Marquardt
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
lmParams.setIterativeParams(cgParams)
actual2 = LevenbergMarquardtOptimizer(fg, initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, fg.error(actual2))
# Dogleg
dlParams = DoglegParams()
dlParams.setOrdering(ordering)
actual3 = DoglegOptimizer(fg, initial_values, dlParams).optimize()
self.assertAlmostEqual(0, fg.error(actual3))
# Graduated Non-Convexity (GNC)
gncParams = GncLMParams()
actual4 = GncLMOptimizer(fg, initial_values, gncParams).optimize()
self.assertAlmostEqual(0, fg.error(actual4))
def main():
"""
Camera observations of landmarks (i.e. pixel coordinates) will be stored as Point2 (x, y).
Each variable in the system (poses and landmarks) must be identified with a unique key.
We can either use simple integer keys (1, 2, 3, ...) or symbols (X1, X2, L1).
Here we will use Symbols
In GTSAM, measurement functions are represented as 'factors'. Several common factors
have been provided with the library for solving robotics/SLAM/Bundle Adjustment problems.
Here we will use Projection factors to model the camera's landmark observations.
Also, we will initialize the robot at some location using a Prior factor.
When the factors are created, we will add them to a Factor Graph. As the factors we are using
are nonlinear factors, we will need a Nonlinear Factor Graph.
Finally, once all of the factors have been added to our factor graph, we will want to
solve/optimize to graph to find the best (Maximum A Posteriori) set of variable values.
GTSAM includes several nonlinear optimizers to perform this step. Here we will use a
trust-region method known as Powell's Degleg
The nonlinear solvers within GTSAM are iterative solvers, meaning they linearize the
nonlinear functions around an initial linearization point, then solve the linear system
to update the linearization point. This happens repeatedly until the solver converges
to a consistent set of variable values. This requires us to specify an initial guess
for each variable, held in a Values container.
"""
# Define the camera calibration parameters
K = Cal3_S2(50.0, 50.0, 0.0, 50.0, 50.0)
# Define the camera observation noise model
measurement_noise = gtsam.noiseModel.Isotropic.Sigma(
2, 1.0) # one pixel in u and v
# Create the set of ground-truth landmarks
points = SFMdata.createPoints()
# Create the set of ground-truth poses
poses = SFMdata.createPoses(K)
# Create a factor graph
graph = NonlinearFactorGraph()
# Add a prior on pose x1. This indirectly specifies where the origin is.
# 0.3 rad std on roll,pitch,yaw and 0.1m on x,y,z
pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
factor = PriorFactorPose3(X(0), poses[0], pose_noise)
graph.push_back(factor)
# Simulated measurements from each camera pose, adding them to the factor graph
for i, pose in enumerate(poses):
camera = PinholeCameraCal3_S2(pose, K)
for j, point in enumerate(points):
measurement = camera.project(point)
factor = GenericProjectionFactorCal3_S2(measurement,
measurement_noise, X(i),
L(j), K)
graph.push_back(factor)
# Because the structure-from-motion problem has a scale ambiguity, the problem is still under-constrained
# Here we add a prior on the position of the first landmark. This fixes the scale by indicating the distance
# between the first camera and the first landmark. All other landmark positions are interpreted using this scale.
point_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
factor = PriorFactorPoint3(L(0), points[0], point_noise)
graph.push_back(factor)
graph.print_('Factor Graph:\n')
# Create the data structure to hold the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initial_estimate = Values()
for i, pose in enumerate(poses):
transformed_pose = pose.retract(0.1 * np.random.randn(6, 1))
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = point + 0.1 * np.random.randn(3)
initial_estimate.insert(L(j), transformed_point)
initial_estimate.print_('Initial Estimates:\n')
# Optimize the graph and print results
params = gtsam.DoglegParams()
params.setVerbosity('TERMINATION')
optimizer = DoglegOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
result.print_('Final results:\n')
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
marginals = Marginals(graph, result)
plot.plot_3d_points(1, result, marginals=marginals)
plot.plot_trajectory(1, result, marginals=marginals, scale=8)
plot.set_axes_equal(1)
plt.show()
class TestScenario(GtsamTestCase):
"""Do trivial test with three optimizer variants."""
def setUp(self):
"""Set up the optimization problem and ordering"""
# create graph
self.fg = NonlinearFactorGraph()
model = gtsam.noiseModel.Unit.Create(2)
self.fg.add(PriorFactorPoint2(KEY1, Point2(0, 0), model))
# test error at minimum
xstar = Point2(0, 0)
self.optimal_values = Values()
self.optimal_values.insert(KEY1, xstar)
self.assertEqual(0.0, self.fg.error(self.optimal_values), 0.0)
# test error at initial = [(1-cos(3))^2 + (sin(3))^2]*50 =
x0 = Point2(3, 3)
self.initial_values = Values()
self.initial_values.insert(KEY1, x0)
self.assertEqual(9.0, self.fg.error(self.initial_values), 1e-3)
# optimize parameters
self.ordering = Ordering()
self.ordering.push_back(KEY1)
def test_gauss_newton(self):
gnParams = GaussNewtonParams()
gnParams.setOrdering(self.ordering)
actual = GaussNewtonOptimizer(self.fg, self.initial_values,
gnParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_levenberg_marquardt(self):
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setOrdering(self.ordering)
actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_levenberg_marquardt_pcg(self):
lmParams = LevenbergMarquardtParams.CeresDefaults()
lmParams.setLinearSolverType("ITERATIVE")
cgParams = PCGSolverParameters()
cgParams.setPreconditionerParams(DummyPreconditionerParameters())
lmParams.setIterativeParams(cgParams)
actual = LevenbergMarquardtOptimizer(self.fg, self.initial_values,
lmParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_dogleg(self):
dlParams = DoglegParams()
dlParams.setOrdering(self.ordering)
actual = DoglegOptimizer(self.fg, self.initial_values,
dlParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_graduated_non_convexity(self):
gncParams = GncLMParams()
actual = GncLMOptimizer(self.fg, self.initial_values,
gncParams).optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
def test_iteration_hook(self):
# set up iteration hook to track some testable values
iteration_count = 0
final_error = 0
final_values = None
def iteration_hook(iter, error_before, error_after):
nonlocal iteration_count, final_error, final_values
iteration_count = iter
final_error = error_after
final_values = optimizer.values()
# optimize
params = LevenbergMarquardtParams.CeresDefaults()
params.setOrdering(self.ordering)
params.iterationHook = iteration_hook
optimizer = LevenbergMarquardtOptimizer(self.fg, self.initial_values,
params)
actual = optimizer.optimize()
self.assertAlmostEqual(0, self.fg.error(actual))
self.gtsamAssertEquals(final_values, actual)
self.assertEqual(self.fg.error(actual), final_error)
self.assertEqual(optimizer.iterations(), iteration_count)
initial_estimate.insert(X(i), transformed_pose)
for j, point in enumerate(points):
transformed_point = point + 0.1*np.random.randn(3)
points_noise.append(transformed_point)
initial_estimate.insert(L(j), transformed_point)
# initial_estimate.print('Initial Estimates:\n')
# Optimize the graph and print results
params = gtsam.LevenbergMarquardtParams()
params.setVerbosity('TERMINATION')
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
# result.print('Final results:\n')
print(sems)
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
# rs_poses = []
# rs_points = []
# for i, pose in enumerate(poses):
# rs_poses.append(result.atPose3(X(i)))
# for j, point in enumerate(points_noise):
# rs_points.append(result.atPoint3(L(j)))
#
# ### VISUALIZATION ###
# from SlamUtils.visualization import getVisualizationBB, getVisualizationPL, getKeyframeGTSAM
#
# vis = o3d.visualization.Visualizer()
# vis.create_window(window_name='Trajectory', width=1024, height=768)