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 gtsamOpt2PoseStampedarray(inputfname, pose_array):
pointID = 0
graph, initial = gtsam.readG2o(inputfname, True)
# Add Prior on the first key
priorModel = gtsam.noiseModel_Diagonal.Variances(
vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys().at(0)
graphWithPrior.add(
gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity(
"Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graphWithPrior.error(initial))
print("final error = ", graphWithPrior.error(result))
resultPoses = gtsam.allPose3s(result)
keys = resultPoses.keys()
for i in range(keys.size()):
posetmp = gstamPose2Transform(resultPoses.atPose3(keys.at(i)))
posestampedtmp = PoseStamped()
posestampedtmp.header.frame_id = str(keys.at(i))
posestampedtmp.pose = posetmp
pose_array.poseArray.append(posestampedtmp)
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 gtsamOpt(inputfname, outputfname):
graph, initial = gtsam.readG2o(inputfname, True)
# Add Prior on the first key
priorModel = gtsam.noiseModel_Diagonal.Variances(
vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
graphWithPrior = graph
firstKey = initial.keys().at(0)
graphWithPrior.add(
gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity(
"Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graphWithPrior, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graphWithPrior.error(initial))
print("final error = ", graphWithPrior.error(result))
print("Writing results to file: ", outputfname)
graphNoKernel, _ = gtsam.readG2o(inputfname, True)
gtsam.writeG2o(graphNoKernel, result, outputfname)
print("Done!")
def add_gps(self, cur_pose):
print("Adding GPS factor at", self.node_idx + 1)
# self.graph.add(gtsam.GPSFactor(X(self.node_idx+1), gps_pose, self.gps_noise))
gps_pose = gen_pose(cur_pose)
self.graph.add(
gtsam.PriorFactorPose3(X(self.node_idx + 1), gps_pose,
self.gps_pose_noise))
def addPrior(self, i: int, graph: gtsam.NonlinearFactorGraph):
"""Add a prior on the navigation state at time `i`."""
state = self.scenario.navState(i)
graph.push_back(
gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
graph.push_back(
gtsam.PriorFactorVector(V(i), state.velocity(), self.velNoise))
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 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 add_prior(self, var_name, transform, noise=None):
""" Prior factor """
noise_gt = self._process_noise(noise)
transform_gt = sophus2gtsam(transform)
var = self.vars[var_name]
factor = gtsam.PriorFactorPose3(var, transform_gt, noise_gt)
self.gtsam_graph.push_back(factor)
def test_initializePoses(self):
g2oFile = gtsam.findExampleDataFile("pose3example-grid")
is3D = True
inputGraph, expectedValues = gtsam.readG2o(g2oFile, is3D)
priorModel = gtsam.noiseModel.Unit.Create(6)
inputGraph.add(gtsam.PriorFactorPose3(0, Pose3(), priorModel))
initial = gtsam.InitializePose3.initialize(inputGraph)
# TODO(frank): very loose !!
self.gtsamAssertEquals(initial, expectedValues, 0.1)
def main():
"""Main runner."""
parser = argparse.ArgumentParser(
description="A 3D Pose SLAM example that reads input from g2o, and "
"initializes Pose3")
parser.add_argument('-i', '--input', help='input file g2o format')
parser.add_argument(
'-o',
'--output',
help="the path to the output file with optimized graph")
parser.add_argument("-p",
"--plot",
action="store_true",
help="Flag to plot results")
args = parser.parse_args()
g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
else args.input
is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add Prior on the first key
priorModel = gtsam.noiseModel.Diagonal.Variances(
vector6(1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity(
"Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
graphNoKernel, _ = gtsam.readG2o(g2oFile, is3D)
gtsam.writeG2o(graphNoKernel, result, outputFile)
print("Done!")
if args.plot:
resultPoses = gtsam.utilities.allPose3s(result)
for i in range(resultPoses.size()):
plot.plot_pose3(1, resultPoses.atPose3(i))
plt.show()
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 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 __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_PriorFactor(self):
values = gtsam.Values()
key = 5
priorPose3 = gtsam.Pose3()
model = gtsam.noiseModel_Unit.Create(6)
factor = gtsam.PriorFactorPose3(key, priorPose3, model)
values.insert(key, priorPose3)
self.assertEqual(factor.error(values), 0)
key = 3
priorVector = np.array([0., 0., 0.])
model = gtsam.noiseModel_Unit.Create(3)
factor = gtsam.PriorFactorVector(key, priorVector, model)
values.insert(key, priorVector)
self.assertEqual(factor.error(values), 0)
def main():
"""Main runner."""
# Read graph from file
g2oFile = gtsam.findExampleDataFile("pose3example.txt")
is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add prior on the first key. TODO: assumes first key ios z
priorModel = gtsam.noiseModel.Diagonal.Variances(
np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(0, gtsam.Pose3(), priorModel))
# Initializing Pose3 - chordal relaxation
initialization = gtsam.InitializePose3.initialize(graph)
print(initialization)
def bundle_adjustment(self):
"""
Bundle Adjustment.
Input:
self._image_features
self._image_points
Output:
result - gtsam optimzation result
"""
# Initialize factor Graph
graph = gtsam.NonlinearFactorGraph()
pose_indices, point_indices = self.create_index_sets()
initial_estimate = self.create_initial_estimate(pose_indices)
# Create Projection Factor
sigma = 1.0
measurementNoise = gtsam.noiseModel_Isotropic.Sigma(2, sigma)
for img_idx, features in enumerate(self._image_features):
for kp_idx, keypoint in enumerate(features.keypoints):
if self.image_points[img_idx].kp_3d_exist(kp_idx):
landmark_id = self.image_points[img_idx].kp_3d(kp_idx)
if self._landmark_estimates[
landmark_id].seen >= self._min_landmark_seen:
key_point = Point2(keypoint[0], keypoint[1])
graph.add(
gtsam.GenericProjectionFactorCal3_S2(
key_point, measurementNoise, X(img_idx),
P(landmark_id), self._calibration))
# Create priors for first two poses
s = np.radians(60)
poseNoiseSigmas = np.array([s, s, s, 1, 1, 1])
posePriorNoise = gtsam.noiseModel_Diagonal.Sigmas(poseNoiseSigmas)
for idx in (0, 1):
pose_i = initial_estimate.atPose3(X(idx))
graph.add(gtsam.PriorFactorPose3(X(idx), pose_i, posePriorNoise))
# Optimization
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate)
sfm_result = optimizer.optimize()
return sfm_result, pose_indices, point_indices
def add_sfm_factors(graph, init_est, dataset_path, scale):
f = open(dataset_path + '/reconstruction.json', 'r')
data = json.load(f)
images = data[0]['shots']
for img in images.items():
#print('Adding sfm factor for ' + img[0])
img_num = int(img[0].split('.')[0])
pos = np.array(img[1]['translation']) / scale
rot_aa = np.array(img[1]['rotation']) #axis-angle
rot_gtsam = gtsam.Rot3((Rotation.from_rotvec(rot_aa)).as_dcm())
pos = (Rotation.from_rotvec(rot_aa)).inv().as_dcm() @ pos
pos[2] = -pos[2]
trans_gtsam = gtsam.Point3(pos)
pose_gtsam = gtsam.Pose3(rot_gtsam, trans_gtsam)
factor = gtsam.PriorFactorPose3(C(img_num), pose_gtsam, sfm_noise)
graph.push_back(factor)
init_est.insert(C(img_num), pose_gtsam)
def add_sfm_factors_gt(graph, init_est, dataset_path):
f = open(dataset_path + '/ep_data.pkl', 'rb')
data = pickle.load(f)
for ind in range(len(data['cam_pose'])):
pose_mat = np.eye(4)
quat = data['cam_pose'][ind][3:]
quat[0], quat[1], quat[2], quat[3] = quat[1], quat[2], quat[3], quat[0]
pose_mat[:3, :3] = (
Rotation.from_quat(quat) *
Rotation.from_euler('xyz', [0, np.pi, 0])).as_dcm()
pose_mat[:3, 3] = data['cam_pose'][ind][:3]
pos = pose_mat[:3, 3]
rot_gtsam = gtsam.Rot3(pose_mat[:3, :3])
trans_gtsam = gtsam.Point3(pos)
pose_gtsam = gtsam.Pose3(rot_gtsam, trans_gtsam)
factor = gtsam.PriorFactorPose3(C(ind), pose_gtsam, sfm_noise)
graph.push_back(factor)
init_est.insert(C(ind), pose_gtsam)
def __init__(self, config, pose_3rd):
quat = R_scipy.from_matrix(pose_3rd[:3, :3]).as_quat()
quat = np.roll(quat, 1)
t = pose_3rd[:3, -1]
init_pose = np.array([*t, *quat])
self.result = None
self.marginals = None
self.node_idx = 0
self.odom_noise = gtsam.noiseModel.Diagonal.Sigmas(
config["odom_noise"])
self.skip_noise = gtsam.noiseModel.Diagonal.Sigmas(
config["skip_noise"])
self.prior_noise = gtsam.noiseModel.Diagonal.Sigmas(
config["prior_noise"])
self.gps_pose_noise = gtsam.noiseModel.Diagonal.Sigmas(
config["gps_pose_noise"])
self.gps_noise = gtsam.noiseModel.Isotropic.Sigmas(config["gps_noise"])
self.skip_num = config["skip_num"]
# self.visualize=config["visualize"]
self.graph = gtsam.NonlinearFactorGraph()
self.initial_estimate = gtsam.Values()
self.init_pose = gen_pose(
init_pose
) #gtsam.Pose3(gtsam.Rot3.Rodrigues(*init_pose[3:]), gtsam.Point3(*init_pose[:3]))#gen_pose([0,0,0])
self.parameters = gtsam.ISAM2Params()
self.parameters.setRelinearizeThreshold(0.01)
self.parameters.setRelinearizeSkip(10)
self.isam = gtsam.ISAM2(self.parameters)
#! Add a prior on pose x0
self.graph.push_back(
gtsam.PriorFactorPose3(X(self.node_idx), self.init_pose,
self.prior_noise))
self.initial_estimate.insert(X(self.node_idx), self.init_pose)
self.current_estimate = None
self.last_transform = None
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
help="Flag to plot results")
args = parser.parse_args()
g2oFile = gtsam.findExampleDataFile("pose3example.txt") if args.input is None \
else args.input
is3D = True
graph, initial = gtsam.readG2o(g2oFile, is3D)
# Add Prior on the first key
priorModel = gtsam.noiseModel.Diagonal.Variances(vector6(1e-6, 1e-6, 1e-6,
1e-4, 1e-4, 1e-4))
print("Adding prior to g2o file ")
firstKey = initial.keys()[0]
graph.add(gtsam.PriorFactorPose3(firstKey, gtsam.Pose3(), priorModel))
params = gtsam.GaussNewtonParams()
params.setVerbosity("Termination") # this will show info about stopping conds
optimizer = gtsam.GaussNewtonOptimizer(graph, initial, params)
result = optimizer.optimize()
print("Optimization complete")
print("initial error = ", graph.error(initial))
print("final error = ", graph.error(result))
if args.output is None:
print("Final Result:\n{}".format(result))
else:
outputFile = args.output
print("Writing results to file: ", outputFile)
def add_measurements(self,
geo_measurements,
sem_measurements,
da_current_step,
class_realization_current_step,
number_of_samples=10,
weight_update_flag=True,
new_input_object_prior=None,
new_input_object_covariance=None,
ML_update=False):
graph_add = gtsam.NonlinearFactorGraph()
initial_add = gtsam.Values()
# Setting up complete class realization
running_index = 0
for obj in da_current_step:
if obj not in self.classRealization:
obj = int(obj)
# Create a class realization dict from the realization key
for obj_index in class_realization_current_step:
if obj == obj_index[0]:
self.classRealization[obj] = obj_index[1]
self.logWeight += np.log(
self.classProbabilityPrior[self.classRealization[obj] - 1])
# System is ill-defined without a prior for objects, so we define a prior.
# Default is very uninformative prior
new_object_prior = gtsam.Pose3(gtsam.Rot3.Ypr(0, 0, 0),
np.array([0, 0, 0]))
if new_input_object_prior is None:
#new_object_prior = gtsam.Pose3(gtsam.Rot3.Ypr(0, 0, 0), np.array(0, 0, 0))
pass
else:
new_object_prior = new_input_object_prior[obj]
self.graph.add(
gtsam.PriorFactorPose3(self.XO(obj), new_object_prior,
new_object_covariance))
self.prop_belief.add(
gtsam.PriorFactorPose3(self.XO(obj), new_object_prior,
new_object_covariance))
graph_add.add(
gtsam.PriorFactorPose3(self.XO(obj), new_object_prior,
new_object_covariance))
if new_input_object_covariance is None:
pass
#new_object_covariance_diag = np.array([1000, 1000, 1000, 1000, 1000, 1000])
#new_object_covariance = gtsam.noiseModel.Diagonal.Variances(new_object_covariance_diag)
else:
new_object_covariance = new_input_object_covariance[obj]
self.graph.add(
gtsam.PriorFactorPose3(self.XO(obj), new_object_prior,
new_object_covariance))
self.prop_belief.add(
gtsam.PriorFactorPose3(self.XO(obj), new_object_prior,
new_object_covariance))
graph_add.add(
gtsam.PriorFactorPose3(self.XO(obj), new_object_prior,
new_object_covariance))
# Add to total graphs
if self.initial.exists(self.XO(obj)) is False:
self.initial.insert(self.XO(obj), new_object_prior)
if self.prop_initial.exists(self.XO(obj)) is False:
self.prop_initial.insert(self.XO(obj),
new_object_prior)
if initial_add.exists(self.XO(obj)) is False:
initial_add.insert(self.XO(obj), new_object_prior)
running_index += 1
for obj in self.classRealization:
if self.initial.exists(self.XO(obj)) is False:
self.initial.insert(self.XO(obj),
gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
if initial_add.exists(self.XO(obj)) is False:
initial_add.insert(self.XO(obj),
gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)))
# Geometric measurement model
self.geoModel.add_measurement(geo_measurements, da_current_step,
self.graph, self.initial,
self.daRealization)
self.geoModel.add_measurement(geo_measurements, da_current_step,
self.prop_belief, self.initial,
self.daRealization)
self.geoModel.add_measurement(geo_measurements, da_current_step,
graph_add, self.initial,
self.daRealization)
self.isam.update(graph_add, initial_add)
#self.isam_copy.update(graph_add, initial_add)
graph_add = gtsam.NonlinearFactorGraph()
initial_add = gtsam.Values()
# Semantic measurement model
if self.cls_enable is True:
self.clsModel.add_measurement(sem_measurements, da_current_step,
self.graph, self.initial,
self.daRealization,
class_realization_current_step)
#self.clsModel.add_measurement(sem_measurements, da_current_step, graph_add,
# self.initial, self.daRealization, class_realization_current_step)
# Updating weights
if weight_update_flag is True:
if ML_update is False:
self.weight_update(da_current_step, geo_measurements,
sem_measurements, number_of_samples)
else:
self.weight_update_ML(da_current_step, geo_measurements,
sem_measurements)
# Semantic measurement model isam2
# if self.cls_enable is True:
# self.clsModel.add_measurement(sem_measurements, da_current_step, graph_add,
# self.initial, self.daRealization, class_realization_current_step)
self.isam.update(graph_add, initial_add)
self.optimize()
self.marginals_after = gtsam.Marginals(self.graph, self.result)
# Setting resultsFlag to false to require optimization
self.resultsFlag = False
params.initial_pose_covariance = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
params.initial_velocity_covariance = gtsam.noiseModel_Isotropic.Sigma(
3, 0.1)
############################### Build the factor graph ####################################
factor_graph = gtsam.NonlinearFactorGraph()
# A technical hack for defining variable names in GTSAM python bindings
def symbol(letter, index):
return int(gtsam.symbol(ord(letter), index))
# Add a prior factor on the initial position
factor_graph.push_back(
gtsam.PriorFactorPose3(symbol('x', 0), params.initial_pose,
params.initial_pose_covariance))
factor_graph.push_back(
gtsam.PriorFactorVector(symbol('v', 0), params.initial_velocity,
params.initial_velocity_covariance))
# Add IMU factors (or "motion model"/"transition" factors).
# Ideally, we would add factors between every pair (xᵢ₋₁, xᵢ). But, to save computations,
# we will add factors between pairs (x₀, xₖ), (xₖ, x₂ₖ) etc., and as an IMU "measurement"
# between e.g. x₀ and xₖ we will use combined (pre-integrated) measurements `0, 1, ..., k-1`.
# Below, `k == PREINTEGRATE_EVERY_STEPS`.
PREINTEGRATE_EVERY_STEPS = 25
# For code generalization, create pairs (0, k), (k, 2k), (2k, 3k), ..., (mk, N-1)
preintegration_steps = list(
range(0, len(imu_measurements), PREINTEGRATE_EVERY_STEPS))
if preintegration_steps[-1] != len(
def build_graph(sequence, config):
"""
Adds noise to sequence variables / measurements.
Returns graph, initial_estimate
"""
# create empty graph / estimate
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
# declare noise models
prior_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([float(config['base']['PRIOR_SIGMA'])]*6, dtype=np.float))
odometry_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([float(config['base']['ODOM_SIGMA'])]*6, dtype=np.float))
bbox_noise = gtsam.noiseModel_Diagonal.Sigmas(np.array([float(config['base']['BOX_SIGMA'])]*4, dtype=np.float))
# get noisy odometry / boxes
true_odometry = sequence.true_trajectory.as_odometry()
noisy_odometry = true_odometry.add_noise(mu=0.0, sd=float(config['base']['ODOM_NOISE']))
noisy_boxes = sequence.true_boxes.add_noise(mu=0.0, sd=float(config['base']['BOX_NOISE']))
# initialize trajectory
# TODO: ensure aligned in same reference frame
initial_trajectory = noisy_odometry.as_trajectory(sequence.true_trajectory.data()[0])
# initial_trajectory = noisy_odometry.as_trajectory()
# initialize quadrics
# NOTE: careful initializing with true quadrics and noise traj as it may not make sense
if config['base']['Initialization'] == 'SVD':
initial_quadrics = System.initialize_quadrics(initial_trajectory, noisy_boxes, sequence.calibration)
elif config['base']['Initialization'] == 'Dataset':
initial_quadrics = sequence.true_quadrics
# add prior pose
prior_factor = gtsam.PriorFactorPose3(System.X(0), initial_trajectory.at(0), prior_noise)
graph.add(prior_factor)
# add odometry measurements
for (start_key, end_key), rpose in noisy_odometry.items():
odometry_factor = gtsam.BetweenFactorPose3(System.X(start_key), System.X(end_key), rpose, odometry_noise)
graph.add(odometry_factor)
# add initial pose estimates
for pose_key, pose in initial_trajectory.items():
initial_estimate.insert(System.X(pose_key), pose)
# add valid box measurements
valid_objects = []
initialized_quadrics = initial_quadrics.keys()
for object_key in np.unique(noisy_boxes.object_keys()):
# add if quadric initialized
if object_key in initialized_quadrics:
# get all views of quadric
object_boxes = noisy_boxes.at_object(object_key)
# add if enough views
if len(object_boxes) > 3:
# add measurements
valid_objects.append(object_key)
for (pose_key, t), box in object_boxes.items():
bbf = quadricslam.BoundingBoxFactor(box, sequence.calibration, System.X(pose_key), System.Q(object_key), bbox_noise)
bbf.addToGraph(graph)
# add initial landmark estimates
for object_key, quadric in initial_quadrics.items():
# add if seen > 3 times
if (object_key in valid_objects):
quadric.addToValues(initial_estimate, System.Q(object_key))
return graph, initial_estimate
def full_bundle_adjustment_update(self, sfm_map: SfmMap):
# Variable symbols for camera poses.
X = gtsam.symbol_shorthand.X
# Variable symbols for observed 3D point "landmarks".
L = gtsam.symbol_shorthand.L
# Create a factor graph.
graph = gtsam.NonlinearFactorGraph()
# Extract the first two keyframes (which we will assume has ids 0 and 1).
kf_0 = sfm_map.get_keyframe(0)
kf_1 = sfm_map.get_keyframe(1)
# We will in this function assume that all keyframes have a common, given (constant) camera model.
common_model = kf_0.camera_model()
calibration = gtsam.Cal3_S2(common_model.fx(), common_model.fy(), 0,
common_model.cx(), common_model.cy())
# Unfortunately, the dataset does not contain any information on uncertainty in the observations,
# so we will assume a common observation uncertainty of 3 pixels in u and v directions.
obs_uncertainty = gtsam.noiseModel.Isotropic.Sigma(2, 3.0)
# Add measurements.
for keyframe in sfm_map.get_keyframes():
for keypoint_id, map_point in keyframe.get_observations().items():
obs_point = keyframe.get_keypoint(keypoint_id).point()
factor = gtsam.GenericProjectionFactorCal3_S2(
obs_point, obs_uncertainty, X(keyframe.id()),
L(map_point.id()), calibration)
graph.push_back(factor)
# Set prior on the first camera (which we will assume defines the reference frame).
no_uncertainty_in_pose = gtsam.noiseModel.Constrained.All(6)
factor = gtsam.PriorFactorPose3(X(kf_0.id()), gtsam.Pose3(),
no_uncertainty_in_pose)
graph.push_back(factor)
# Set prior on distance to next camera.
no_uncertainty_in_distance = gtsam.noiseModel.Constrained.All(1)
prior_distance = 1.0
factor = gtsam.RangeFactorPose3(X(kf_0.id()), X(kf_1.id()),
prior_distance,
no_uncertainty_in_distance)
graph.push_back(factor)
# Set initial estimates from map.
initial_estimate = gtsam.Values()
for keyframe in sfm_map.get_keyframes():
pose_w_c = gtsam.Pose3(keyframe.pose_w_c().to_matrix())
initial_estimate.insert(X(keyframe.id()), pose_w_c)
for map_point in sfm_map.get_map_points():
point_w = gtsam.Point3(map_point.point_w().squeeze())
initial_estimate.insert(L(map_point.id()), point_w)
# Optimize the graph.
params = gtsam.GaussNewtonParams()
params.setVerbosity('TERMINATION')
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimate, params)
print('Optimizing:')
result = optimizer.optimize()
print('initial error = {}'.format(graph.error(initial_estimate)))
print('final error = {}'.format(graph.error(result)))
# Update map with results.
for keyframe in sfm_map.get_keyframes():
updated_pose_w_c = result.atPose3(X(keyframe.id()))
keyframe.update_pose_w_c(SE3.from_matrix(
updated_pose_w_c.matrix()))
for map_point in sfm_map.get_map_points():
updated_point_w = result.atPoint3(L(map_point.id())).reshape(3, 1)
map_point.update_point_w(updated_point_w)
sfm_map.has_been_updated()
def addPrior(self, i, graph):
state = self.scenario.navState(i)
graph.push_back(
gtsam.PriorFactorPose3(X(i), state.pose(), self.priorNoise))
graph.push_back(
gtsam.PriorFactorVector3(V(i), state.velocity(), self.velNoise))
poses.append(
gtsam.SimpleCamera.Lookat(gtsam.Point3(10, 0, 0), gtsam.Point3(),
gtsam.Point3(0, 0, 1)).pose())
# define quadrics
quadrics = []
quadrics.append(
quadricslam.ConstrainedDualQuadric(gtsam.Pose3(),
np.array([1., 2., 3.])))
quadrics.append(
quadricslam.ConstrainedDualQuadric(
gtsam.Pose3(gtsam.Rot3(), gtsam.Point3(0.1, 0.1, 0.1)),
np.array([1., 2., 3.])))
# add prior on first pose
prior_factor = gtsam.PriorFactorPose3(X(0), poses[0], prior_noise)
graph.add(prior_factor)
# add trajectory estimate
for i, pose in enumerate(poses):
# add a perturbation to initial pose estimates to simulate noise
perturbed_pose = poses[i].compose(
gtsam.Pose3(gtsam.Rot3.Rodrigues(0.1, 0.1, 0.1),
gtsam.Point3(0.1, 0.2, 0.3)))
initial_estimate.insert(X(i), perturbed_pose)
# add quadric estimate
for i, quadric in enumerate(quadrics):
quadric.addToValues(initial_estimate, Q(i))
# load in all clouds in our dataset
clouds = read_ply(*scans_fnames)
# Setting up our factor graph
graph = gtsam.NonlinearFactorGraph()
initial_estimates = gtsam.Values()
# We get the initial pose of the car from Argo AI's dataset, and we add it to the graph as such
PRIOR_NOISE = gtsam.noiseModel_Diagonal.Sigmas(
np.array([1e-6, 1e-6, 1e-6, 1e-4, 1e-4, 1e-4]))
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))
graph.add(gtsam.PriorFactorPose3(0, initial_pose, PRIOR_NOISE))
initial_estimates.insert(0, initial_pose)
# We'll use your function to populate the factor graph
graph, initial_estimates = populate_factor_graph(graph, initial_estimates,
initial_pose, clouds)
# Now optimize for the states
parameters = gtsam.GaussNewtonParams()
parameters.setRelativeErrorTol(1e-5)
parameters.setMaxIterations(100)
optimizer = gtsam.GaussNewtonOptimizer(graph, initial_estimates, parameters)
result = optimizer.optimize()
"""Let's plot these poses to see how our vechicle moves.
Screenshot this for your reflection.
# Graph initialization
graph_1 = gtsam.NonlinearFactorGraph()
# Add prior lambda
prior_noise = gtsam.noiseModel.Diagonal.Covariance(
np.array([[3.2, 0.0, 0.0, 0.0], [0.0, 3.2, 0.0, 0.0], [0.0, 0.0, 3.2, 0.0],
[0.0, 0.0, 0.0, 3.2]]))
prior_noise_pose = gtsam.noiseModel.Diagonal.Variances(
np.array([0.003, 0.003, 0.001, 0.003, 0.003, 0.003]))
geo_noise = gtsam.noiseModel.Diagonal.Variances(
np.array([0.003, 0.003, 0.001, 0.002, 0.002, 0.003]))
graph_1.add(
lambda_prior_factor.LambdaPriorFactor(L(0), np.array([0., 0., 0., 0.]),
prior_noise))
graph_1.add(
gtsam.PriorFactorPose3(X(0), gtsam.Pose3(gtsam.Pose2(0.0, 0.0, 0.0)),
prior_noise_pose))
graph_1.add(
gtsam.BetweenFactorPose3(X(0), XO(1),
gtsam.Pose3(gtsam.Pose2(1.0, 0.0, -0 * 3.14 / 2)),
geo_noise))
graph_1.add(
joint_lambda_pose_factor_fake_2d.JLPFactor(
X(0), XO(1), Lambda(0), Lambda(1), [1.0, 0.0, 0.0, 0.0],
[1.1, 0.0, 0.0, 0.0, 1.1, 0.0, 0.0, 1.1, 0.0, 1.1]))
print(graph_1)
initial = gtsam.Values()
initial.insert(Lambda(0), [0.0, 0.0, 0.0, 0.0])
initial.insert(Lambda(1), [0.0, 0.0, 0.0, 0.0])
initial.insert(
X(0),
