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 setUp(self):
model = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
# We consider a small graph:
# symbolic FG
# x2 0 1
# / | \ 1 2
# / | \ 2 3
# x3 | x1 2 0
# \ | / 0 3
# \ | /
# x0
#
p0 = Point3(0, 0, 0)
self.R0 = Rot3.Expmap(np.array([0.0, 0.0, 0.0]))
p1 = Point3(1, 2, 0)
self.R1 = Rot3.Expmap(np.array([0.0, 0.0, 1.570796]))
p2 = Point3(0, 2, 0)
self.R2 = Rot3.Expmap(np.array([0.0, 0.0, 3.141593]))
p3 = Point3(-1, 1, 0)
self.R3 = Rot3.Expmap(np.array([0.0, 0.0, 4.712389]))
pose0 = Pose3(self.R0, p0)
pose1 = Pose3(self.R1, p1)
pose2 = Pose3(self.R2, p2)
pose3 = Pose3(self.R3, p3)
g = NonlinearFactorGraph()
g.add(gtsam.BetweenFactorPose3(x0, x1, pose0.between(pose1), model))
g.add(gtsam.BetweenFactorPose3(x1, x2, pose1.between(pose2), model))
g.add(gtsam.BetweenFactorPose3(x2, x3, pose2.between(pose3), model))
g.add(gtsam.BetweenFactorPose3(x2, x0, pose2.between(pose0), model))
g.add(gtsam.BetweenFactorPose3(x0, x3, pose0.between(pose3), model))
g.add(gtsam.PriorFactorPose3(x0, pose0, model))
self.graph = g
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)
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
camera = get_camera(radius)
pose_0 = camera.pose()
delta_t = 1.0 / 18 # makes for 10 degrees per step
angular_velocity = math.radians(180) # rad/sec
scenario = get_scenario(radius, pose_0, angular_velocity, delta_t)
PARAMS, BIAS_COVARIANCE, DELTA = preintegration_parameters()
# Create a factor graph
graph = NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = Values()
# Add a prior on pose x0. This indirectly specifies where the origin is.
# 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1, 0.3, 0.3, 0.3]))
graph.push_back(PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = B(0)
biasnoise = gtsam.noiseModel.Isotropic.Sigma(6, 0.1)
biasprior = PriorFactorConstantBias(biasKey, gtsam.imuBias.ConstantBias(),
biasnoise)
graph.push_back(biasprior)
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
velnoise = gtsam.noiseModel.Isotropic.Sigma(3, 0.1)
# Calculate with correct initial velocity
n_velocity = vector3(0, angular_velocity * radius, 0)
velprior = PriorFactorVector(V(0), n_velocity, velnoise)
graph.push_back(velprior)
initialEstimate.insert(V(0), n_velocity)
accum = gtsam.PreintegratedImuMeasurements(PARAMS)
# Simulate poses and imu measurements, adding them to the factor graph
for i in range(80):
t = i * delta_t # simulation time
if i == 0: # First time add two poses
pose_1 = scenario.pose(delta_t)
initialEstimate.insert(X(0), pose_0.compose(DELTA))
initialEstimate.insert(X(1), pose_1.compose(DELTA))
elif i >= 2: # Add more poses as necessary
pose_i = scenario.pose(t)
initialEstimate.insert(X(i), pose_i.compose(DELTA))
if i > 0:
# Add Bias variables periodically
if i % 5 == 0:
biasKey += 1
factor = BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias.ConstantBias(),
BIAS_COVARIANCE)
graph.add(factor)
initialEstimate.insert(biasKey, gtsam.imuBias.ConstantBias())
# Predict acceleration and gyro measurements in (actual) body frame
nRb = scenario.rotation(t).matrix()
bRn = np.transpose(nRb)
measuredAcc = scenario.acceleration_b(t) - np.dot(bRn, n_gravity)
measuredOmega = scenario.omega_b(t)
accum.integrateMeasurement(measuredAcc, measuredOmega, delta_t)
# Add Imu Factor
imufac = ImuFactor(X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
graph.add(imufac)
# insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity)
accum.resetIntegration()
# Incremental solution
isam.update(graph, initialEstimate)
result = isam.calculateEstimate()
plot.plot_incremental_trajectory(0,
result,
start=i,
scale=3,
time_interval=0.01)
# reset
graph = NonlinearFactorGraph()
initialEstimate.clear()
plt.show()