def run(args: Namespace) -> None:
"""Run LAGO on input data stored in g2o file."""
g2oFile = gtsam.findExampleDataFile(
"noisyToyGraph.txt") if args.input is None else args.input
graph = gtsam.NonlinearFactorGraph()
graph, initial = gtsam.readG2o(g2oFile)
# Add prior on the pose having index (key) = 0
priorModel = gtsam.noiseModel.Diagonal.Variances(Point3(1e-6, 1e-6, 1e-8))
graph.add(PriorFactorPose2(0, Pose2(), priorModel))
print(graph)
print("Computing LAGO estimate")
estimateLago: Values = gtsam.lago.initialize(graph)
print("done!")
if args.output is None:
estimateLago.print("estimateLago")
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel = gtsam.NonlinearFactorGraph()
graphNoKernel, initial2 = gtsam.readG2o(g2oFile)
gtsam.writeG2o(graphNoKernel, estimateLago, outputFile)
print("Done! ")
def test_insertProjectionFactors(self):
"""Test insertProjectionFactors."""
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2())
self.assertEqual(graph.size(), 2)
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, [0, 1], np.asarray([[20, 30], [20, 30]]),
gtsam.noiseModel.Isotropic.Sigma(2, 0.1), gtsam.Cal3_S2(),
gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(1, 0, 0)))
self.assertEqual(graph.size(), 2)
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 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()
def check(actual):
# Check that optimizing yields the identity
self.gtsamAssertEquals(actual.atRot3(KEY), self.expected, tol=1e-6)
# Check that logging output prints out 3 lines (exact intermediate values differ by OS)
self.assertEqual(self.capturedOutput.getvalue().count('\n'), 3)
# reset stdout catcher
self.capturedOutput.truncate(0)
self.check = check
self.graph = gtsam.NonlinearFactorGraph()
for R in rotations:
self.graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
self.initial = gtsam.Values()
self.initial.insert(KEY, R)
# setup output capture
self.capturedOutput = StringIO()
sys.stdout = self.capturedOutput
def init_smoother(request):
"""
Runs a batch fixed smoother on an agent with two odometry
sensors that is simply moving along the x axis in constant
speed.
"""
global SMOOTHER_BATCH
# Define a batch fixed lag smoother, which uses
# Levenberg-Marquardt to perform the nonlinear optimization
lag = request.lag
smoother_batch = gtsam_unstable.BatchFixedLagSmoother(lag)
new_factors = gtsam.NonlinearFactorGraph()
new_values = gtsam.Values()
new_timestamps = gtsam_unstable.FixedLagSmootherKeyTimestampMap()
prior_mean = request.prior_mean
prior_noise = request.prior_noise
X1 = 0
new_factors.push_back(gtsam.PriorFactorPose2(X1, prior_mean, prior_noise))
new_values.insert(X1, prior_mean)
new_timestamps.insert(_timestamp_key_value(X1, 0.0))
SMOOTHER_BATCH = smoother_batch
SMOOTHER_BATCH.update(new_factors, new_values, new_timestamps)
return X1
def test_Pose2SLAMExample(self):
# Assumptions
# - All values are axis aligned
# - Robot poses are facing along the X axis (horizontal, to the right in images)
# - We have full odometry for measurements
# - The robot is on a grid, moving 2 meters each step
# Create graph container and add factors to it
graph = gtsam.NonlinearFactorGraph()
# Add prior
# gaussian for prior
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]))
# add directly to graph
graph.add(gtsam.PriorFactorPose2(1, priorMean, priorNoise))
# Add odometry
# general noisemodel for odometry
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.2, 0.2, 0.1]))
graph.add(
gtsam.BetweenFactorPose2(1, 2, gtsam.Pose2(2.0, 0.0, 0.0),
odometryNoise))
graph.add(
gtsam.BetweenFactorPose2(2, 3, gtsam.Pose2(2.0, 0.0, pi / 2),
odometryNoise))
graph.add(
gtsam.BetweenFactorPose2(3, 4, gtsam.Pose2(2.0, 0.0, pi / 2),
odometryNoise))
graph.add(
gtsam.BetweenFactorPose2(4, 5, gtsam.Pose2(2.0, 0.0, pi / 2),
odometryNoise))
# Add pose constraint
model = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.2, 0.2, 0.1]))
graph.add(
gtsam.BetweenFactorPose2(5, 2, gtsam.Pose2(2.0, 0.0, pi / 2),
model))
# Initialize to noisy points
initialEstimate = gtsam.Values()
initialEstimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
initialEstimate.insert(2, gtsam.Pose2(2.3, 0.1, -0.2))
initialEstimate.insert(3, gtsam.Pose2(4.1, 0.1, pi / 2))
initialEstimate.insert(4, gtsam.Pose2(4.0, 2.0, pi))
initialEstimate.insert(5, gtsam.Pose2(2.1, 2.1, -pi / 2))
# Optimize using Levenberg-Marquardt optimization with an ordering from
# colamd
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimizeSafely()
# Plot Covariance Ellipses
marginals = gtsam.Marginals(graph, result)
P = marginals.marginalCovariance(1)
pose_1 = result.atPose2(1)
self.gtsamAssertEquals(pose_1, gtsam.Pose2(), 1e-4)
def __init__(self, initial_state, covariance, alphas, beta):
self._initial_state = gtsam.Pose2(initial_state[0], initial_state[1], initial_state[2])
self._prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([covariance, covariance, covariance]))
# self.observation_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([beta[0] ** 2, np.deg2rad(beta[1]) ** 2]))
#self.observation_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([beta[0] ** 2, np.deg2rad(beta[1]) ** 2]))
self.observation_noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array(([np.deg2rad(beta[1]) ** 2, (beta[0] / 100) ** 2])))
self.beta = beta
self.alphas = alphas ** 2
self.pose_num = 0
self.observation_num = 1000
self.landmark_indexes = list()
self.states_new = np.array([[]])
self.observation_new = np.array([[]])
self.graph = gtsam.NonlinearFactorGraph()
self.estimations = gtsam.Values()
self.result = gtsam.Values()
self.parameters = gtsam.ISAM2Params()
self.parameters.setRelinearizeThreshold(1e-4)
self.parameters.setRelinearizeSkip(1)
self.slam = gtsam.ISAM2(self.parameters)
self.graph.add(gtsam.PriorFactorPose2(self.pose_num, self._initial_state, self._prior_noise))
self.estimations.insert(self.pose_num, self._initial_state)
def test_jacobian(self):
"""Evaluate jacobian at optical axis"""
obj_point_on_axis = np.array([0, 0, 1])
img_point = np.array([0.0, 0.0])
pose = gtsam.Pose3()
camera = gtsam.Cal3Unified()
state = gtsam.Values()
camera_key, pose_key, landmark_key = K(0), P(0), L(0)
state.insert_cal3unified(camera_key, camera)
state.insert_point3(landmark_key, obj_point_on_axis)
state.insert_pose3(pose_key, pose)
g = gtsam.NonlinearFactorGraph()
noise_model = gtsam.noiseModel.Unit.Create(2)
factor = gtsam.GeneralSFMFactor2Cal3Unified(img_point, noise_model,
pose_key, landmark_key,
camera_key)
g.add(factor)
f = g.error(state)
gaussian_factor_graph = g.linearize(state)
H, z = gaussian_factor_graph.jacobian()
self.assertAlmostEqual(f, 0)
self.gtsamAssertEquals(z, np.zeros(2))
self.gtsamAssertEquals(H @ H.T, 4 * np.eye(2))
Dcal = np.zeros((2, 10), order='F')
Dp = np.zeros((2, 2), order='F')
camera.calibrate(img_point, Dcal, Dp)
self.gtsamAssertEquals(
Dcal,
np.array([[0., 0., 0., -1., 0., 0., 0., 0., 0., 0.],
[0., 0., 0., 0., -1., 0., 0., 0., 0., 0.]]))
self.gtsamAssertEquals(Dp, np.array([[1., -0.], [-0., 1.]]))
def step5_add_odometry_and_landmark_observations():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X2 = X(2)
X3 = X(3)
L2 = L(2)
# Update the list with the new variable keys.
pose_variables.append(X3)
landmark_variables.append(L2)
# Add the odometry measurement.
odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.1, 0.1, 0.1]))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
# Add the landmark observation.
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Set initial estimates only for the new variables.
initial_estimate = gtsam.Values()
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Update ISAM2 with the new factor graph.
isam.update(graph, initial_estimate)
def __init__(self, relinearizeThreshold=0.01, relinearizeSkip=1):
""" priorMean and priorCov should be in 1 dimensional array
"""
# init the graph
self.graph = gtsam.NonlinearFactorGraph()
# init the iSam2 solver
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(relinearizeThreshold)
parameters.setRelinearizeSkip(relinearizeSkip)
self.isam = gtsam.ISAM2(parameters)
# init container for initial values
self.initialValues = gtsam.Values()
# setting the current position
self.currentKey = 1
# current estimate
self.currentEst = False
self.currentPose = [0, 0, 0]
return
def test_factor(self):
"""Check that the InnerConstraint factor leaves the mean unchanged."""
# Make a graph with two variables, one between, and one InnerConstraint
# The optimal result should satisfy the between, while moving the other
# variable to make the mean the same as before.
# Mean of R and R' is identity. Let's make a BetweenFactor making R21 =
# R*R*R, i.e. geodesic length is 3 rather than 2.
graph = gtsam.NonlinearFactorGraph()
R12 = R.compose(R.compose(R))
graph.add(gtsam.BetweenFactorRot3(1, 2, R12, MODEL))
keys = gtsam.KeyVector()
keys.append(1)
keys.append(2)
graph.add(gtsam.KarcherMeanFactorRot3(keys))
initial = gtsam.Values()
initial.insert(1, R.inverse())
initial.insert(2, R)
expected = Rot3()
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
actual = gtsam.FindKarcherMean(
gtsam.Rot3Vector([result.atRot3(1), result.atRot3(2)]))
self.gtsamAssertEquals(expected, actual)
self.gtsamAssertEquals(
R12, result.atRot3(1).between(result.atRot3(2)))
def __init__(self,
p,
q,
r,
alphas=np.array([0.001, 0.0001]),
sensor_offset=np.zeros(2)):
# Add noise models
self.odometry_noise = gtsam.noiseModel.Diagonal.Sigmas(q)
self.measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(r)
self.alphas = alphas
self.sensor_offset = sensor_offset
# Create graph and initilize newest pose
self.graph = gtsam.NonlinearFactorGraph()
self.poses = gtsam.Values()
# To enumerate all poses and landmarks
self.kx = 1
self.kl = 1
self.landmarks = np.empty(0)
# Initilize graph with prior
prior_noise = gtsam.noiseModel.Diagonal.Sigmas(p)
self.graph.add(
gtsam.PriorFactorPose2(X(self.kx), gtsam.Pose2(0.0, 0.0, 0.0),
prior_noise))
def __init__(self, IMU_PARAMS=None, BIAS_COVARIANCE=None):
"""
Define factor graph parameters (e.g. noise, camera calibrations, etc) here
"""
self.graph = gtsam.NonlinearFactorGraph()
self.initial_estimate = gtsam.Values()
self.IMU_PARAMS = IMU_PARAMS
self.BIAS_COVARIANCE = BIAS_COVARIANCE
def add_mr_object_measurement(self,
stack,
class_realization,
new_input_object_prior=None,
new_input_object_covariance=None):
graph_add = gtsam.NonlinearFactorGraph()
initial_add = gtsam.Values()
idx = 0
for obj in stack:
cov_noise_model = stack[obj]['covariance']
exp_gtsam = gtsam.Pose3(
gtsam.Rot3.Ypr(stack[obj]['expectation'][2],
stack[obj]['expectation'][1],
stack[obj]['expectation'][0]),
gtsam.Point3(stack[obj]['expectation'][3],
stack[obj]['expectation'][4],
stack[obj]['expectation'][5]))
pose_rotation_matrix = exp_gtsam.rotation().matrix()
cov_noise_model_rotated = self.rotate_cov_6x6(
cov_noise_model, np.transpose(pose_rotation_matrix))
cov_noise_model_rotated = gtsam.noiseModel.Gaussian.Covariance(
cov_noise_model_rotated)
#updating the graph
self.graph.add(
gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam,
cov_noise_model_rotated))
graph_add.add(
gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam,
cov_noise_model_rotated))
self.prop_belief.add(
gtsam.PriorFactorPose3(self.XO(obj), exp_gtsam,
cov_noise_model_rotated))
if self.prop_initial.exists(self.XO(obj)) is False:
self.prop_initial.insert(
self.XO(obj), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
# if obj in new_input_object_prior and obj not in self.classRealization:
# self.graph.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj],
# new_input_object_covariance[obj]))
# self.prop_belief.add(gtsam.PriorFactorPose3(self.XO(obj), new_input_object_prior[obj],
# new_input_object_covariance[obj]))
self.classRealization[obj] = class_realization[obj]
if self.isam.value_exists(self.XO(obj)) is False:
initial_add.insert(self.XO(obj),
gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
idx += 1
if stack[obj]['weight'] > -322:
self.logWeight += stack[obj]['weight'] # + 150
else:
self.logWeight = -math.inf
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 __init__(self):
self.gtsam_graph = gtsam.NonlinearFactorGraph()
self.factor_edges = {} # adjacency list (dict)
# Variables
self.values = gtsam.Values()
self.vars = {}
self.n_variables = 0
def bundle_adjustment(self):
"""
Parameters:
calibration - gtsam.Cal3_S2, camera calibration
landmark_map - list, A map of landmarks and their correspondence
"""
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
initial_estimate = self.create_initial_estimate()
# Create Projection Factors
# """
# Measurement noise for bundle adjustment:
# sigma = 1.0
# measurement_noise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
# """
for landmark_idx, observation_list in enumerate(self._landmark_map):
for obersvation in observation_list:
pose_idx = obersvation[0]
key_point = obersvation[1]
# To test indeterminate system
# if(landmark_idx == 2 or landmark_idx == 480):
# continue
# if(pose_idx == 6):
# print("yes2")
# continue
graph.add(
gtsam.GenericProjectionFactorCal3_S2(
key_point, self._measurement_noise, X(pose_idx),
P(landmark_idx), self._calibration))
# Create priors for first two poses
# """
# Pose prior noise:
# rotation_sigma = np.radians(60)
# translation_sigma = 1
# pose_noise_sigmas = np.array([rotation_sigma, rotation_sigma, rotation_sigma,
# translation_sigma, translation_sigma, translation_sigma])
# """
for idx in (0, 1):
pose_i = initial_estimate.atPose3(X(idx))
graph.add(
gtsam.PriorFactorPose3(X(idx), pose_i, self._pose_prior_noise))
# Optimization
# Using QR rather than Cholesky decomposition
# params = gtsam.LevenbergMarquardtParams()
# params.setLinearSolverType("MULTIFRONTAL_QR")
# optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate, params)
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate)
sfm_result = optimizer.optimize()
# Check if factor covariances are under constrain
marginals = gtsam.Marginals( # pylint: disable=unused-variable
graph, sfm_result)
return sfm_result
def test_SFMExample(self):
options = generator.Options()
options.triangle = False
options.nrCameras = 10
[data, truth] = generator.generate_data(options)
measurementNoiseSigma = 1.0
pointNoiseSigma = 0.1
poseNoiseSigmas = np.array([0.001, 0.001, 0.001, 0.1, 0.1, 0.1])
graph = gtsam.NonlinearFactorGraph()
# Add factors for all measurements
measurementNoise = Isotropic.Sigma(2, measurementNoiseSigma)
for i in range(len(data.Z)):
for k in range(len(data.Z[i])):
j = data.J[i][k]
graph.add(gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], measurementNoise,
X(i), P(j), data.K))
posePriorNoise = Diagonal.Sigmas(poseNoiseSigmas)
graph.add(gtsam.PriorFactorPose3(X(0),
truth.cameras[0].pose(), posePriorNoise))
pointPriorNoise = Isotropic.Sigma(3, pointNoiseSigma)
graph.add(gtsam.PriorFactorPoint3(P(0),
truth.points[0], pointPriorNoise))
# Initial estimate
initialEstimate = gtsam.Values()
for i in range(len(truth.cameras)):
pose_i = truth.cameras[i].pose()
initialEstimate.insert(X(i), pose_i)
for j in range(len(truth.points)):
point_j = truth.points[j]
initialEstimate.insert(P(j), point_j)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
for i in range(5):
optimizer.iterate()
result = optimizer.values()
# Marginalization
marginals = gtsam.Marginals(graph, result)
marginals.marginalCovariance(P(0))
marginals.marginalCovariance(X(0))
# Check optimized results, should be equal to ground truth
for i in range(len(truth.cameras)):
pose_i = result.atPose3(X(i))
self.gtsamAssertEquals(pose_i, truth.cameras[i].pose(), 1e-5)
for j in range(len(truth.points)):
point_j = result.atPoint3(P(j))
self.gtsamAssertEquals(point_j, truth.points[j], 1e-5)
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 find_Karcher_mean_Rot3(rotations):
"""Find the Karcher mean of given values."""
# Cost function C(R) = \sum PriorFactor(R_i)::error(R)
# No closed form solution.
graph = gtsam.NonlinearFactorGraph()
for R in rotations:
graph.add(gtsam.PriorFactorRot3(KEY, R, MODEL))
initial = gtsam.Values()
initial.insert(KEY, gtsam.Rot3())
result = gtsam.GaussNewtonOptimizer(graph, initial).optimize()
return result.atRot3(KEY)
def __init__(self,
geo_model,
da_model,
lambda_model,
prior_mean,
prior_noise,
lambda_prior_mean=(0., 0.),
lambda_prior_noise=((0.5, 0.), (0., 0.5)),
cls_enable=True):
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
self.Lam = lambda k: L(k)
# Enable Lambda inference
self.cls_enable = cls_enable
# Camera pose prior
self.graph = gtsam.NonlinearFactorGraph()
self.graph.add(
gtsam.PriorFactorPose3(self.X(0), prior_mean, prior_noise))
# Setting initial values
self.initial = gtsam.Values()
self.initial.insert(self.X(0), prior_mean)
self.prev_step_camera_pose = prior_mean
self.daRealization = list()
# Setting up models
self.geoModel = geo_model
self.daModel = da_model
self.lambdaModel = lambda_model
# Setting up ISAM2
params2 = gtsam.ISAM2Params()
#params2.relinearize_threshold = 0.01
#params2.relinearize_skip = 1
self.isam = gtsam.ISAM2(params2)
self.isam.update(self.graph, self.initial)
# Set custom lambda
if type(lambda_prior_mean) is np.ndarray:
self.lambda_prior_mean = lambda_prior_mean
else:
self.lambda_prior_mean = np.array(lambda_prior_mean)
self.lambda_prior_cov = gtsam.noiseModel.Gaussian.Covariance(
np.matrix(lambda_prior_noise))
self.num_cls = len(self.lambda_prior_mean) + 1
# Initialize object last lambda database
self.object_lambda_dict = dict()
def __init__(self, odom_global, odom_relative, visual_global,
visual_relative):
self.odom_global = odom_global
self.odom_relative = odom_relative
self.visual_global = visual_global
self.visual_relative = visual_relative
# shorthand symbols
self.X = lambda i: int(gtsam.symbol(ord('x'), i))
# Create an empty nonlinear factor graph
self.graph = gtsam.NonlinearFactorGraph()
def setUp(cls):
test_clouds = read_ply(*scans_fnames)[:6]
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
ICP_NOISE = gtsam.noiseModel_Diagonal.Sigmas(np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
test_graph = gtsam.NonlinearFactorGraph()
test_initial_estimates = gtsam.Values()
initial_pose = gtsam.Pose3(gtsam.Rot3(0.9982740, -0.0572837, 0.0129474, 0.0575611, 0.9980955, -0.0221840, -0.0116519, 0.0228910, 0.9996701),
gtsam.Point3(-263.9464864482589, 2467.3015467381383, -19.374652610889633))
test_graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE))
test_initial_estimates.insert(0, initial_pose)
test_graph, test_initial_estimates = populate_factor_graph(test_graph, test_initial_estimates, initial_pose, test_clouds)
cls.graph = test_graph
cls.initial_estimates = test_initial_estimates
def setUp(self):
self.graph = gtsam.NonlinearFactorGraph()
odometry = gtsam.Pose2(2.0, 0.0, 0.0)
odometryNoise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.2, 0.2, 0.1]))
self.graph.add(gtsam.BetweenFactorPose2(0, 1, odometry, odometryNoise))
self.graph.add(gtsam.BetweenFactorPose2(1, 2, odometry, odometryNoise))
self.values = gtsam.Values()
self.values.insert_pose2(0, gtsam.Pose2(0., 0., 0.))
self.values.insert_pose2(1, gtsam.Pose2(2., 0., 0.))
self.values.insert_pose2(2, gtsam.Pose2(4., 0., 0.))
def trajectory_estimator(self, features, visible_map):
""" Estimate current pose with matched features through GTSAM and update the trajectory
Parameters:
features: A Feature Object. Stores all matched Superpoint features.
visible_map: A Map Object. Stores all matched Landmark points.
pose: gtsam.pose3 Object. The pose at the last state from the atrium map trajectory.
Returns:
current_pose: gtsam.pose3 Object. The current estimate pose.
Use input matched features as the projection factors of the graph.
Use input visible_map (match landmark points of the map) as the landmark priors of the graph.
Use the last time step pose from the trajectory as the initial estimate value of the current pose
"""
assert len(self.atrium_map.trajectory) != 0, "Trajectory is empty."
pose = self.atrium_map.trajectory[len(self.atrium_map.trajectory)-1]
# Initialize factor graph
graph = gtsam.NonlinearFactorGraph()
initialEstimate = gtsam.Values()
# Add projection factors with matched feature points for all measurements
measurementNoiseSigma = 1.0
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(
2, measurementNoiseSigma)
for i, feature in enumerate(features.key_point_list):
graph.add(gtsam.GenericProjectionFactorCal3_S2(
feature, measurementNoise,
X(0), P(i), self.calibration))
# Create initial estimate for the pose
# Because the robot moves slowly, we can use the previous pose as an initial estimation of the current pose.
initialEstimate.insert(X(0), pose)
# Create priors for visual
# Because the map is known, we use the landmarks from the visible map with nearly zero error as priors.
pointPriorNoise = gtsam.noiseModel_Isotropic.Sigma(3, 0.01)
for i, landmark in enumerate(visible_map.landmark_list):
graph.add(gtsam.PriorFactorPoint3(P(i),
landmark, pointPriorNoise))
initialEstimate.insert(P(i), landmark)
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initialEstimate)
result = optimizer.optimize()
# Add current pose to the trajectory
current_pose = result.atPose3(symbol(ord('x'), 0))
self.atrium_map.append_trajectory(current_pose)
return current_pose
def test_reprojectionErrors(self):
"""Test reprojectionErrors."""
pixels = np.asarray([[20, 30], [20, 30]])
I = [1, 2]
K = gtsam.Cal3_S2()
graph = gtsam.NonlinearFactorGraph()
gtsam.utilities.insertProjectionFactors(
graph, 0, I, pixels, gtsam.noiseModel.Isotropic.Sigma(2, 0.1), K)
values = gtsam.Values()
values.insert(0, gtsam.Pose3())
cam = gtsam.PinholeCameraCal3_S2(gtsam.Pose3(), K)
gtsam.utilities.insertBackprojections(values, cam, I, pixels, 10)
errors = gtsam.utilities.reprojectionErrors(graph, values)
np.testing.assert_allclose(errors, np.zeros((2, 2)))
def action_step(self, action, action_noise):
graph_add = gtsam.NonlinearFactorGraph()
initial_add = gtsam.Values()
# Setting path and initiating DA realization.
path_length = len(self.daRealization)
self.daRealization.append(0)
# Advancing the belief by camera
self.graph.add(
gtsam.BetweenFactorPose3(self.X(path_length),
self.X(path_length + 1), action,
action_noise))
graph_add.add(
gtsam.BetweenFactorPose3(self.X(path_length),
self.X(path_length + 1), action,
action_noise))
# Setting initial values, save an initial value vector for the propagated pose, need it later
if self.initial.exists(self.X(path_length + 1)) is False:
self.initial.insert(
self.X(path_length + 1),
self.initial.atPose3(self.X(path_length)).compose(action))
# self.initial.insert(self.X(path_length + 1), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
if initial_add.exists(self.X(path_length + 1)) is False:
initial_add.insert(
self.X(path_length + 1),
self.initial.atPose3(self.X(path_length)).compose(action))
# initial_add.insert(self.X(path_length + 1), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
# Setting propagated belief object for weight computation
self.prop_belief = self.graph.clone()
self.prop_initial = gtsam.Values()
for key in list(self.initial.keys()):
self.prop_initial.insert(key, self.initial.atPose3(key))
# self.optimize() #TODO: SEE IF WORKS
#print(initial_add.exists(self.X(2)))
self.isam.update(graph_add, initial_add)
self.optimize()
# non-existent object issue is resolved.
self.marginals = gtsam.Marginals(self.prop_belief, self.result)
# Setting resultsFlag to false to require optimization
self.resultsFlag = False
# Multi robot flag for twin measurements
self.sim_measurement_flag = list()
def test_initialize(self) -> None:
"""Smokescreen to ensure LAGO can be imported and run on toy data stored in a g2o file."""
g2oFile = gtsam.findExampleDataFile("noisyToyGraph.txt")
graph = gtsam.NonlinearFactorGraph()
graph, initial = gtsam.readG2o(g2oFile)
# Add prior on the pose having index (key) = 0
priorModel = gtsam.noiseModel.Diagonal.Variances(
Point3(1e-6, 1e-6, 1e-8))
graph.add(PriorFactorPose2(0, Pose2(), priorModel))
estimateLago: Values = gtsam.lago.initialize(graph)
assert isinstance(estimateLago, Values)
def __init__(self,
class_probability_prior,
geo_model,
da_model,
cls_model,
prior_mean,
prior_noise,
cls_enable=True):
# Symbol initialization
self.X = lambda i: X(i)
self.XO = lambda j: O(j)
# Camera pose prior
self.graph = gtsam.NonlinearFactorGraph()
self.graph.add(
gtsam.PriorFactorPose3(self.X(0), prior_mean, prior_noise))
# Class realization and prior probabilities. if one of the class probabilities is 0 it will
# set the logWeight to -inf
self.cls_enable = cls_enable
# Setting initial values
self.initial = gtsam.Values()
self.initial.insert(self.X(0), prior_mean)
self.prev_step_camera_pose = prior_mean
self.daRealization = list()
# self.initial.print()
# Setting up models
self.geoModel = geo_model
self.daModel = da_model
self.clsModel = cls_model
# self.classifierModel = classifierModel
# Setting up ISAM2 TODO: make ISAM2 work. As of now, it doesn't take the initial values at optimize_isam2 method
params2 = gtsam.ISAM2Params()
# params2.relinearize_threshold = 0.01
# params2.relinearize_skip = 1
self.isam = gtsam.ISAM2(params2)
self.isam.update(self.graph, self.initial)
# Setting up weight memory
self.weight_memory = list()
self.normalized_weight = 0
# Setting up class realization
self.classRealization = dict()
self.classProbabilityPrior = class_probability_prior
def test_generic_factor(self):
"""Evaluate residual using pose and point as state variables"""
graph = gtsam.NonlinearFactorGraph()
state = gtsam.Values()
measured = self.img_point
noise_model = gtsam.noiseModel.Isotropic.Sigma(2, 1)
pose_key, point_key = P(0), L(0)
k = self.stereographic
state.insert_pose3(pose_key, gtsam.Pose3())
state.insert_point3(point_key, self.obj_point)
factor = gtsam.GenericProjectionFactorCal3Unified(measured, noise_model, pose_key, point_key, k)
graph.add(factor)
score = graph.error(state)
self.assertAlmostEqual(score, 0)
