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 __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 __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,
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 configure(self):
assert (self.nssm_params.cov_samples == 0
or self.nssm_params.cov_samples <
self.nssm_params.initialization_params[0] *
self.nssm_params.initialization_params[1])
assert (self.ssm_params.cov_samples == 0 or self.ssm_params.cov_samples
< self.ssm_params.initialization_params[0] *
self.ssm_params.initialization_params[1])
assert self.nssm_params.source_frames < self.nssm_params.min_st_sep
self.prior_model = self.create_noise_model(self.prior_sigmas)
self.odom_model = self.create_noise_model(self.odom_sigmas)
self.icp_odom_model = self.create_noise_model(self.icp_odom_sigmas)
self.isam = gtsam.ISAM2(self.isam_params)
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 __init__(self,
pose0=np.eye(4),
pose0_to_pose1_range=1.0,
K=np.eye(3),
relinearizeThreshold=0.1,
relinearizeSkip=10,
proj_noise_val=1.0):
self.graph = NonlinearFactorGraph()
# Add a prior on pose x0. 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]))
x0factor = PriorFactorPose3(iSAM2Wrapper.get_key('x', 0),
gtsam.gtsam.Pose3(pose0), pose_noise)
self.graph.push_back(x0factor)
# Set scale between pose 0 and pose 1 to Unity
x0x1_noise = gtsam.noiseModel_Isotropic.Sigma(1, 0.1)
x1factor = RangeFactorPose3(iSAM2Wrapper.get_key('x', 0),
iSAM2Wrapper.get_key('x', 1),
pose0_to_pose1_range, x0x1_noise)
self.graph.push_back(x1factor)
iS2params = gtsam.ISAM2Params()
iS2params.setRelinearizeThreshold(relinearizeThreshold)
iS2params.setRelinearizeSkip(relinearizeSkip)
self.isam2 = gtsam.ISAM2(iS2params)
self.projection_noise = gtsam.noiseModel_Isotropic.Sigma(
2, proj_noise_val)
self.K = gtsam.Cal3_S2(iSAM2Wrapper.CameraMatrix_to_Cal3_S2(K))
#self.opt_params = gtsam.DoglegParams()
#self.opt_params.setVerbosity('Error')
#self.opt_params.setErrorTol(0.1)
self.initial_estimate = gtsam.Values()
self.initial_estimate.insert(iSAM2Wrapper.get_key('x', 0),
gtsam.gtsam.Pose3(pose0))
self.lm_factor_ids = set()
def clone(self):
"""
:type new_object: GaussianBelief
"""
new_object = copy.copy(self)
new_object.graph = self.graph.clone()
new_object.daRealization = self.daRealization.copy()
new_object.classRealization = self.classRealization.copy()
params3 = gtsam.ISAM2Params()
#params3.relinearize_threshold = 0.01
#params3.relinearize_skip = 1
new_object.isam = gtsam.ISAM2(params3)
new_object.isam.update(self.graph, self.initial)
new_object.initial = gtsam.Values()
for key in self.initial.keys():
try:
new_object.initial.insert(key, self.initial.atPose3(key))
except:
new_object.initial.insert(key, self.initial.atVector(key))
return new_object
def visual_ISAM2_example():
plt.ion()
# Define the camera calibration parameters
K = gtsam.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 an iSAM2 object. Unlike iSAM1, which performs periodic batch steps
# to maintain proper linearization and efficient variable ordering, iSAM2
# performs partial relinearization/reordering at each step. A parameter
# structure is available that allows the user to set various properties, such
# as the relinearization threshold and type of linear solver. For this
# example, we we set the relinearization threshold small so the iSAM2 result
# will approach the batch result.
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(0.01)
parameters.setRelinearizeSkip(1)
isam = gtsam.ISAM2(parameters)
# Create a Factor Graph and Values to hold the new data
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
# Loop over the different poses, adding the observations to iSAM incrementally
for i, pose in enumerate(poses):
# Add factors for each landmark observation
for j, point in enumerate(points):
camera = gtsam.PinholeCameraCal3_S2(pose, K)
measurement = camera.project(point)
graph.push_back(
gtsam.GenericProjectionFactorCal3_S2(measurement,
measurement_noise, X(i),
L(j), K))
# Add an initial guess for the current pose
# Intentionally initialize the variables off from the ground truth
initial_estimate.insert(
X(i),
pose.compose(
gtsam.Pose3(gtsam.Rot3.Rodrigues(-0.1, 0.2, 0.25),
gtsam.Point3(0.05, -0.10, 0.20))))
# If this is the first iteration, add a prior on the first pose to set the
# coordinate frame and a prior on the first landmark to set the scale.
# Also, as iSAM solves incrementally, we must wait until each is observed
# at least twice before adding it to iSAM.
if i == 0:
# Add a prior on pose x0
pose_noise = gtsam.noiseModel_Diagonal.Sigmas(
np.array([0.3, 0.3, 0.3, 0.1, 0.1,
0.1])) # 30cm std on x,y,z 0.1 rad on roll,pitch,yaw
graph.push_back(gtsam.PriorFactorPose3(X(0), poses[0], pose_noise))
# Add a prior on landmark l0
point_noise = gtsam.noiseModel_Isotropic.Sigma(3, 0.1)
graph.push_back(
gtsam.PriorFactorPoint3(L(0), points[0],
point_noise)) # add directly to graph
# Add initial guesses to all observed landmarks
# Intentionally initialize the variables off from the ground truth
for j, point in enumerate(points):
initial_estimate.insert(
L(j),
gtsam.Point3(point.x() - 0.25,
point.y() + 0.20,
point.z() + 0.15))
else:
# Update iSAM with the new factors
isam.update(graph, initial_estimate)
# Each call to iSAM2 update(*) performs one iteration of the iterative nonlinear solver.
# If accuracy is desired at the expense of time, update(*) can be called additional
# times to perform multiple optimizer iterations every step.
isam.update()
current_estimate = isam.calculateEstimate()
print("****************************************************")
print("Frame", i, ":")
for j in range(i + 1):
print(X(j), ":", current_estimate.atPose3(X(j)))
for j in range(len(points)):
print(L(j), ":", current_estimate.atPoint3(L(j)))
visual_ISAM2_plot(current_estimate)
# Clear the factor graph and values for the next iteration
graph.resize(0)
initial_estimate.clear()
plt.ioff()
plt.show()
def mhjcbb(sim,
num_tracks=10,
prob=0.95,
posterior_pose_md_threshold=1.5,
prune2_skip=10,
max_observed_diff=3):
slams = [[gtsam.ISAM2(), set()]]
prune2_count = 1
observed = set()
for x, (odom, obs) in enumerate(sim.step()):
for isam2, observed in slams:
graph = gtsam.NonlinearFactorGraph()
values = gtsam.Values()
if x == 0:
prior_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta]))
prior_factor = gtsam.PriorFactorPose2(X(0), odom, prior_model)
graph.add(prior_factor)
values.insert(X(0), odom)
else:
odom_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta]))
odom_factor = gtsam.BetweenFactorPose2(X(x - 1), X(x), odom,
odom_model)
graph.add(odom_factor)
pose0 = isam2.calculateEstimatePose2(X(x - 1))
values.insert(X(x), pose0.compose(odom))
isam2.update(graph, values)
################################################################################
mhjcbb = gtsam.da_MHJCBB2(num_tracks, prob,
posterior_pose_md_threshold)
for isam2, observed, in slams:
mhjcbb.initialize(isam2)
for l, br in obs.items():
br_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_bearing, sim.sigma_range]))
mhjcbb.add(gtsam.Rot2(br[0]), br[1], br_model)
mhjcbb.match()
################################################################################
new_slams = []
for i in range(mhjcbb.size()):
track, keys = mhjcbb.get(i)
keys = [gtsam.symbolIndex(keys.at(i)) for i in range(keys.size())]
isam2 = gtsam.ISAM2()
isam2.update(slams[track][0].getFactorsUnsafe(),
slams[track][0].calculateEstimate())
graph = gtsam.NonlinearFactorGraph()
values = gtsam.Values()
observed = set(slams[track][1])
for (l_true, br), l in zip(obs.items(), keys):
br_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_bearing, sim.sigma_range]))
br_factor = gtsam.BearingRangeFactor2D(X(x), L(l),
gtsam.Rot2(br[0]),
br[1], br_model)
graph.add(br_factor)
if l not in observed:
pose1 = isam2.calculateEstimatePose2(X(x))
point = gtsam.Point2(br[1] * np.cos(br[0]),
br[1] * np.sin(br[0]))
values.insert(L(l), pose1.transform_from(point))
observed.add(l)
isam2.update(graph, values)
new_slams.append([isam2, observed])
slams = new_slams
slams = prune1(slams, x, posterior_pose_md_threshold)
if len(slams[0][1]) > prune2_count * prune2_skip:
slams = prune2(slams, max_observed_diff)
prune2_count += 1
result = []
for isam2, observed in slams:
traj_est = [
isam2.calculateEstimatePose2(X(x)) for x in range(len(sim.traj))
]
traj_est = np.array([(p.x(), p.y(), p.theta()) for p in traj_est])
landmark_est = [isam2.calculateEstimatePoint2(L(l)) for l in observed]
landmark_est = np.array([(p.x(), p.y()) for p in landmark_est])
result.append((traj_est, landmark_est))
return result
x[val[0]] = val[1]
elif data[i][0:2] == 'l0':
i+=1
lString = data[i].split('[')[1].split(']')[0]
lVals = lString[1:-1].split('), (')
for elem in lVals:
val = elem.split(',')
l[val[0]] = val[1]
parameters = gtsam.ISAM2Params()
parameters.relinearize_threshold = 0.01
parameters.relinearize_skip = 1
isam = gtsam.ISAM2(parameters)
graph = gtsam.NonlinearFactorGraph()
initialEstimate = gtsam.Values()
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.000, 0.0, 0.0]))
graph.add(gtsam.PriorFactorPose2(1, gtsam.Pose2(0.000, 0.0, 0.0), priorNoise))
initialEstimate.insert(1, gtsam.Pose2(0.0, 0.0, 0.0))
priorNoise = gtsam.noiseModel.Diagonal.Sigmas(np.array([0.000, 0.0, 0.0]))
graph.add(gtsam.PriorFactorPose2(5, gtsam.Pose2(2.034, 0.0, 0.0), priorNoise))
initialEstimate.insert(5, gtsam.Pose2(2.034, 0.0, 0.0))
isam.update(graph,initialEstimate)
initialEstimate.clear()
def incremental_factorgraph_example():
# Create an ISAM2 object.
# You can balance computation time vs accuracy by changing the parameters to ISAM2 in the ISAM2Params struct
# (see https://github.com/borglab/gtsam/blob/develop/gtsam/nonlinear/ISAM2Params.h).
# The parameters are here set to default.
isam_params = gtsam.ISAM2Params()
isam = gtsam.ISAM2(isam_params)
# We will keep the variables in lists for simplicity.
pose_variables = []
landmark_variables = []
# Step 1: Initialize with a prior on the first pose X1.
def step1_initialize():
# Create an empty nonlinear factor graph.
# We will need to do this for every update.
graph = gtsam.NonlinearFactorGraph()
# Create a key for the first pose.
X1 = X(1)
# Update the list with the new pose variable key.
pose_variables.append(X1)
# 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))
# Set an initial estimate for the first pose.
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
# Update ISAM2 with the initial factor graph.
isam.update(graph, initial_estimate)
step1_initialize()
# Compute the current MAP estimate for all variables.
result = isam.calculateEstimate()
# Define a function that extracts the marginals (which we will reuse below).
# Notice that we compute the marginal covariance directly from the isam object.
def extract_marginals():
# Extract the marginal distributions for each variable
X_marginals = []
for var in pose_variables:
X_marginals.append(
MultivariateNormalParameters(result.atPose2(var),
isam.marginalCovariance(var)))
L_marginals = []
for var in landmark_variables:
L_marginals.append(
MultivariateNormalParameters(result.atPoint2(var),
isam.marginalCovariance(var)))
return X_marginals, L_marginals
pose_marginals, landmark_marginals = extract_marginals()
# Show current results.
plot_result(pose_marginals, landmark_marginals)
# Step 2: Add a landmark observation from the pose X1
def step2_add_landmark_observation():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X1 = X(1)
L1 = L(1)
# Update the list with the new landmark variable key.
landmark_variables.append(L1)
# Add the landmark observation.
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))
# Set initial estimates only for the new variables.
initial_estimate = gtsam.Values()
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
# Update ISAM2 with the new factor graph.
isam.update(graph, initial_estimate)
step2_add_landmark_observation()
# ISAM2 performs only one iteration of the iterative nonlinear solver per update() call.
# We can use "free time" until the next update from the sensors to perform additional iterations.
for _ in range(5):
isam.update()
# Compute the updated MAP estimate.
result = isam.calculateEstimate()
pose_marginals, landmark_marginals = extract_marginals()
# Show updated results.
plot_result(pose_marginals, landmark_marginals)
# Step 3: Add a new pose with a relative odometry constraint.
def step3_add_pose_from_odometry():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X1 = X(1)
X2 = X(2)
# Update the list with the new pose variable key.
pose_variables.append(X2)
# Add an odometry measurement.
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))
# Set initial estimates only for the new variables.
initial_estimate = gtsam.Values()
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
# Update ISAM2 with the new factor graph.
isam.update(graph, initial_estimate)
step3_add_pose_from_odometry()
# Run a few additional update iterations.
for _ in range(5):
isam.update()
# Compute the updated MAP estimate.
result = isam.calculateEstimate()
pose_marginals, landmark_marginals = extract_marginals()
# Show updated results.
plot_result(pose_marginals, landmark_marginals)
# Step 4: Add new observation of the landmark L1 from X2.
def step4_add_new_landmark_observation():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create keys for the involved variables.
X2 = X(2)
L1 = L(1)
# Add the landmark observation.
measurement_noise = gtsam.noiseModel.Diagonal.Sigmas(
np.array([0.05, 0.1]))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Update ISAM2 with the new factor graph.
# There are no new variables this update, so no new initial values.
isam.update(graph, gtsam.Values())
step4_add_new_landmark_observation()
# Run a few additional update iterations.
for _ in range(5):
isam.update()
# Compute the updated MAP estimate.
result = isam.calculateEstimate()
pose_marginals, landmark_marginals = extract_marginals()
# Show updated results.
plot_result(pose_marginals, landmark_marginals)
# Step 5: Add both new odometry and landmark observations for X3.
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)
step5_add_odometry_and_landmark_observations()
# Run a few additional update iterations.
for _ in range(5):
isam.update()
# Compute the updated MAP estimate.
result = isam.calculateEstimate()
pose_marginals, landmark_marginals = extract_marginals()
# Show updated results.
plot_result(pose_marginals, landmark_marginals)
def slam(sim, da='jcbb', prob=0.95):
isam2 = gtsam.ISAM2()
graph = gtsam.NonlinearFactorGraph()
values = gtsam.Values()
observed = set()
for x, (odom, obs) in enumerate(sim.step()):
if x == 0:
prior_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta]))
prior_factor = gtsam.PriorFactorPose2(X(0), odom, prior_model)
graph.add(prior_factor)
values.insert(X(0), odom)
else:
odom_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_x, sim.sigma_y, sim.sigma_theta]))
odom_factor = gtsam.BetweenFactorPose2(X(x - 1), X(x), odom,
odom_model)
graph.add(odom_factor)
pose0 = isam2.calculateEstimatePose2(X(x - 1))
values.insert(X(x), pose0.compose(odom))
isam2.update(graph, values)
graph.resize(0)
values.clear()
estimate = isam2.calculateEstimate()
if da == 'dr':
for l_true, br in obs.items():
l = len(observed)
br_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_bearing, sim.sigma_range]))
br_factor = gtsam.BearingRangeFactor2D(X(x), L(l),
gtsam.Rot2(br[0]),
br[1], br_model)
graph.add(br_factor)
if l not in observed:
pose1 = isam2.calculateEstimatePose2(X(x))
point = gtsam.Point2(br[1] * np.cos(br[0]),
br[1] * np.sin(br[0]))
values.insert(L(l), pose1.transform_from(point))
observed.add(l)
elif da == 'perfect':
for l_true, br in obs.items():
br_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_bearing, sim.sigma_range]))
br_factor = gtsam.BearingRangeFactor2D(X(x), L(l_true),
gtsam.Rot2(br[0]),
br[1], br_model)
graph.add(br_factor)
if l_true not in observed:
pose1 = isam2.calculateEstimatePose2(X(x))
point = gtsam.Point2(br[1] * np.cos(br[0]),
br[1] * np.sin(br[0]))
values.insert(L(l_true), pose1.transform_from(point))
observed.add(l_true)
elif da == 'jcbb':
################################################################################
jcbb = gtsam.da_JCBB2(isam2, prob)
for l, br in obs.items():
br_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_bearing, sim.sigma_range]))
jcbb.add(gtsam.Rot2(br[0]), br[1], br_model)
keys = jcbb.match()
################################################################################
keys = [gtsam.symbolIndex(keys.at(i)) for i in range(keys.size())]
for (l_true, br), l in zip(obs.items(), keys):
br_model = gtsam.noiseModel_Diagonal.Sigmas(
np.array([sim.sigma_bearing, sim.sigma_range]))
br_factor = gtsam.BearingRangeFactor2D(X(x), L(l),
gtsam.Rot2(br[0]),
br[1], br_model)
graph.add(br_factor)
if l not in observed:
pose1 = isam2.calculateEstimatePose2(X(x))
point = gtsam.Point2(br[1] * np.cos(br[0]),
br[1] * np.sin(br[0]))
values.insert(L(l), pose1.transform_from(point))
observed.add(l)
isam2.update(graph, values)
graph.resize(0)
values.clear()
traj_est = [
isam2.calculateEstimatePose2(X(x)) for x in range(len(sim.traj))
]
traj_est = np.array([(p.x(), p.y(), p.theta()) for p in traj_est])
landmark_est = [isam2.calculateEstimatePoint2(L(l)) for l in observed]
landmark_est = np.array([(p.x(), p.y()) for p in landmark_est])
return [[traj_est, landmark_est]]
def optimize(gps_measurements: List[GpsMeasurement],
imu_measurements: List[ImuMeasurement],
sigma_init_x: gtsam.noiseModel.Diagonal,
sigma_init_v: gtsam.noiseModel.Diagonal,
sigma_init_b: gtsam.noiseModel.Diagonal,
noise_model_gps: gtsam.noiseModel.Diagonal,
kitti_calibration: KittiCalibration, first_gps_pose: int,
gps_skip: int) -> gtsam.ISAM2:
"""Run ISAM2 optimization on the measurements."""
# Set initial conditions for the estimated trajectory
# initial pose is the reference frame (navigation frame)
current_pose_global = Pose3(gtsam.Rot3(),
gps_measurements[first_gps_pose].position)
# the vehicle is stationary at the beginning at position 0,0,0
current_velocity_global = np.zeros(3)
current_bias = gtsam.imuBias.ConstantBias() # init with zero bias
imu_params = getImuParams(kitti_calibration)
# Set ISAM2 parameters and create ISAM2 solver object
isam_params = gtsam.ISAM2Params()
isam_params.setFactorization("CHOLESKY")
isam_params.relinearizeSkip = 10
isam = gtsam.ISAM2(isam_params)
# Create the factor graph and values object that will store new factors and
# values to add to the incremental graph
new_factors = gtsam.NonlinearFactorGraph()
# values storing the initial estimates of new nodes in the factor graph
new_values = gtsam.Values()
# Main loop:
# (1) we read the measurements
# (2) we create the corresponding factors in the graph
# (3) we solve the graph to obtain and optimal estimate of robot trajectory
print("-- Starting main loop: inference is performed at each time step, "
"but we plot trajectory every 10 steps")
j = 0
included_imu_measurement_count = 0
for i in range(first_gps_pose, len(gps_measurements)):
# At each non=IMU measurement we initialize a new node in the graph
current_pose_key = X(i)
current_vel_key = V(i)
current_bias_key = B(i)
t = gps_measurements[i].time
if i == first_gps_pose:
# Create initial estimate and prior on initial pose, velocity, and biases
new_values.insert(current_pose_key, current_pose_global)
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
new_factors.addPriorPose3(current_pose_key, current_pose_global,
sigma_init_x)
new_factors.addPriorVector(current_vel_key,
current_velocity_global, sigma_init_v)
new_factors.addPriorConstantBias(current_bias_key, current_bias,
sigma_init_b)
else:
t_previous = gps_measurements[i - 1].time
# Summarize IMU data between the previous GPS measurement and now
current_summarized_measurement = gtsam.PreintegratedImuMeasurements(
imu_params, current_bias)
while (j < len(imu_measurements)
and imu_measurements[j].time <= t):
if imu_measurements[j].time >= t_previous:
current_summarized_measurement.integrateMeasurement(
imu_measurements[j].accelerometer,
imu_measurements[j].gyroscope, imu_measurements[j].dt)
included_imu_measurement_count += 1
j += 1
# Create IMU factor
previous_pose_key = X(i - 1)
previous_vel_key = V(i - 1)
previous_bias_key = B(i - 1)
new_factors.push_back(
gtsam.ImuFactor(previous_pose_key, previous_vel_key,
current_pose_key, current_vel_key,
previous_bias_key,
current_summarized_measurement))
# Bias evolution as given in the IMU metadata
sigma_between_b = gtsam.noiseModel.Diagonal.Sigmas(
np.asarray([
np.sqrt(included_imu_measurement_count) *
kitti_calibration.accelerometer_bias_sigma
] * 3 + [
np.sqrt(included_imu_measurement_count) *
kitti_calibration.gyroscope_bias_sigma
] * 3))
new_factors.push_back(
gtsam.BetweenFactorConstantBias(previous_bias_key,
current_bias_key,
gtsam.imuBias.ConstantBias(),
sigma_between_b))
# Create GPS factor
gps_pose = Pose3(current_pose_global.rotation(),
gps_measurements[i].position)
if (i % gps_skip) == 0:
new_factors.addPriorPose3(current_pose_key, gps_pose,
noise_model_gps)
new_values.insert(current_pose_key, gps_pose)
print(f"############ POSE INCLUDED AT TIME {t} ############")
print(gps_pose.translation(), "\n")
else:
new_values.insert(current_pose_key, current_pose_global)
# Add initial values for velocity and bias based on the previous
# estimates
new_values.insert(current_vel_key, current_velocity_global)
new_values.insert(current_bias_key, current_bias)
# Update solver
# =======================================================================
# We accumulate 2*GPSskip GPS measurements before updating the solver at
# first so that the heading becomes observable.
if i > (first_gps_pose + 2 * gps_skip):
print(f"############ NEW FACTORS AT TIME {t:.6f} ############")
new_factors.print()
isam.update(new_factors, new_values)
# Reset the newFactors and newValues list
new_factors.resize(0)
new_values.clear()
# Extract the result/current estimates
result = isam.calculateEstimate()
current_pose_global = result.atPose3(current_pose_key)
current_velocity_global = result.atVector(current_vel_key)
current_bias = result.atConstantBias(current_bias_key)
print(f"############ POSE AT TIME {t} ############")
current_pose_global.print()
print("\n")
return isam
def Pose2SLAM_ISAM2_example():
"""Perform 2D SLAM given the ground truth changes in pose as well as
simple loop closure detection."""
plt.ion()
# Declare the 2D translational standard deviations of the prior factor's Gaussian model, in meters.
prior_xy_sigma = 0.3
# Declare the 2D rotational standard deviation of the prior factor's Gaussian model, in degrees.
prior_theta_sigma = 5
# Declare the 2D translational standard deviations of the odometry factor's Gaussian model, in meters.
odometry_xy_sigma = 0.2
# Declare the 2D rotational standard deviation of the odometry factor's Gaussian model, in degrees.
odometry_theta_sigma = 5
# Although this example only uses linear measurements and Gaussian noise models, it is important
# to note that iSAM2 can be utilized to its full potential during nonlinear optimization. This example
# simply showcases how iSAM2 may be applied to a Pose2 SLAM problem.
PRIOR_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([prior_xy_sigma,
prior_xy_sigma,
prior_theta_sigma*np.pi/180]))
ODOMETRY_NOISE = gtsam.noiseModel.Diagonal.Sigmas(np.array([odometry_xy_sigma,
odometry_xy_sigma,
odometry_theta_sigma*np.pi/180]))
# Create a Nonlinear factor graph as well as the data structure to hold state estimates.
graph = gtsam.NonlinearFactorGraph()
initial_estimate = gtsam.Values()
# Create iSAM2 parameters which can adjust the threshold necessary to force relinearization and how many
# update calls are required to perform the relinearization.
parameters = gtsam.ISAM2Params()
parameters.setRelinearizeThreshold(0.1)
parameters.relinearizeSkip = 1
isam = gtsam.ISAM2(parameters)
# Create the ground truth odometry measurements of the robot during the trajectory.
true_odometry = [(2, 0, 0),
(2, 0, math.pi/2),
(2, 0, math.pi/2),
(2, 0, math.pi/2),
(2, 0, math.pi/2)]
# Corrupt the odometry measurements with gaussian noise to create noisy odometry measurements.
odometry_measurements = [np.random.multivariate_normal(true_odom, ODOMETRY_NOISE.covariance())
for true_odom in true_odometry]
# Add the prior factor to the factor graph, and poorly initialize the prior pose to demonstrate
# iSAM2 incremental optimization.
graph.push_back(gtsam.PriorFactorPose2(1, gtsam.Pose2(0, 0, 0), PRIOR_NOISE))
initial_estimate.insert(1, gtsam.Pose2(0.5, 0.0, 0.2))
# Initialize the current estimate which is used during the incremental inference loop.
current_estimate = initial_estimate
for i in range(len(true_odometry)):
# Obtain the noisy odometry that is received by the robot and corrupted by gaussian noise.
noisy_odom_x, noisy_odom_y, noisy_odom_theta = odometry_measurements[i]
# Determine if there is loop closure based on the odometry measurement and the previous estimate of the state.
loop = determine_loop_closure(odometry_measurements[i], current_estimate, i, xy_tol=0.8, theta_tol=25)
# Add a binary factor in between two existing states if loop closure is detected.
# Otherwise, add a binary factor between a newly observed state and the previous state.
if loop:
graph.push_back(gtsam.BetweenFactorPose2(i + 1, loop,
gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE))
else:
graph.push_back(gtsam.BetweenFactorPose2(i + 1, i + 2,
gtsam.Pose2(noisy_odom_x, noisy_odom_y, noisy_odom_theta), ODOMETRY_NOISE))
# Compute and insert the initialization estimate for the current pose using the noisy odometry measurement.
computed_estimate = current_estimate.atPose2(i + 1).compose(gtsam.Pose2(noisy_odom_x,
noisy_odom_y,
noisy_odom_theta))
initial_estimate.insert(i + 2, computed_estimate)
# Perform incremental update to iSAM2's internal Bayes tree, optimizing only the affected variables.
isam.update(graph, initial_estimate)
current_estimate = isam.calculateEstimate()
# Report all current state estimates from the iSAM2 optimzation.
report_on_progress(graph, current_estimate, i)
initial_estimate.clear()
# Print the final covariance matrix for each pose after completing inference on the trajectory.
marginals = gtsam.Marginals(graph, current_estimate)
i = 1
for i in range(1, len(true_odometry)+1):
print(f"X{i} covariance:\n{marginals.marginalCovariance(i)}\n")
plt.ioff()
plt.show()
def IMU_example():
"""Run iSAM 2 example with IMU factor."""
# Start with a camera on x-axis looking at origin
radius = 30
up = gtsam.Point3(0, 0, 1)
target = gtsam.Point3(0, 0, 0)
position = gtsam.Point3(radius, 0, 0)
camera = gtsam.SimpleCamera.Lookat(position, target, up, gtsam.Cal3_S2())
pose_0 = camera.pose()
# Create the set of ground-truth landmarks and poses
angular_velocity = math.radians(180) # rad/sec
delta_t = 1.0/18 # makes for 10 degrees per step
angular_velocity_vector = vector3(0, -angular_velocity, 0)
linear_velocity_vector = vector3(radius * angular_velocity, 0, 0)
scenario = gtsam.ConstantTwistScenario(
angular_velocity_vector, linear_velocity_vector, pose_0)
# Create a factor graph
newgraph = gtsam.NonlinearFactorGraph()
# Create (incremental) ISAM2 solver
isam = gtsam.ISAM2()
# Create the initial estimate to the solution
# Intentionally initialize the variables off from the ground truth
initialEstimate = gtsam.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.3, 0.3, 0.3, 0.1, 0.1, 0.1]))
newgraph.push_back(gtsam.PriorFactorPose3(X(0), pose_0, noise))
# Add imu priors
biasKey = gtsam.symbol(ord('b'), 0)
biasnoise = gtsam.noiseModel_Isotropic.Sigma(6, 0.1)
biasprior = gtsam.PriorFactorConstantBias(biasKey, gtsam.imuBias_ConstantBias(),
biasnoise)
newgraph.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 = gtsam.PriorFactorVector(V(0), n_velocity, velnoise)
newgraph.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 = gtsam.BetweenFactorConstantBias(
biasKey - 1, biasKey, gtsam.imuBias_ConstantBias(), BIAS_COVARIANCE)
newgraph.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 = gtsam.ImuFactor(
X(i - 1), V(i - 1), X(i), V(i), biasKey, accum)
newgraph.add(imufac)
# insert new velocity, which is wrong
initialEstimate.insert(V(i), n_velocity)
accum.resetIntegration()
# Incremental solution
isam.update(newgraph, initialEstimate)
result = isam.calculateEstimate()
ISAM2_plot(result)
# reset
newgraph = gtsam.NonlinearFactorGraph()
initialEstimate.clear()
def __init__(self, params):
""" Initialize variables """
self.imu_meas_predict = [] # IMU measurements for pose prediction
self.imu_opt_meas = [] # IMU measurements for pose prediction between measurement updates
self.imu_samples = [] # imu samples
self.opt_meas = [] # optimizes measurements
self.__opt_meas_buffer_time = params['opt_meas_buffer_time']
""" IMU Preintegration """
# IMU preintegration parameters
self.imu_preint_params = gtsam.PreintegrationParams(np.asarray(params['g']))
self.imu_preint_params.setAccelerometerCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2))
self.imu_preint_params.setGyroscopeCovariance(np.eye(3) * np.power(params['acc_nd_sigma'], 2))
self.imu_preint_params.setIntegrationCovariance(np.eye(3) * params['int_cov_sigma']**2)
self.imu_preint_params.setUse2ndOrderCoriolis(params['setUse2ndOrderCoriolis'])
self.imu_preint_params.setOmegaCoriolis(np.array(params['omega_coriolis']))
""" Initialize Parameters """
# ISAM2 initialization
isam2_params = gtsam.ISAM2Params()
isam2_params.setRelinearizeThreshold(params['relinearize_th'])
isam2_params.setRelinearizeSkip(params['relinearize_skip'])
isam2_params.setFactorization(params['factorization'])
# self.isam2 = gtsam.ISAM2(isam2_params)
self.isam2 = gtsam.ISAM2()
self.new_factors = gtsam.NonlinearFactorGraph()
self.new_initial_ests = gtsam.Values()
self.min_imu_sample = 2 # minimum imu sample count needed for integration
# ISAM2 keys
self.poseKey = gtsam.symbol(ord('x'), 0)
self.velKey = gtsam.symbol(ord('v'), 0)
self.biasKey = gtsam.symbol(ord('b'), 0)
""" Set initial variables """
# Initial state
self.last_opt_time = self.current_time = 0
self.current_global_pose = numpy_pose_to_gtsam(
params['init_pos'],
params['init_ori'])
self.current_global_vel = np.asarray(
params['init_vel'])
self.current_bias = gtsam.imuBias_ConstantBias(
np.asarray(params['init_acc_bias']),
np.asarray(params['init_gyr_bias']))
self.imu_accum = gtsam.PreintegratedImuMeasurements(self.imu_preint_params)
# uncertainty of the initial state
self.init_pos_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([
params['init_ori_cov'], params['init_ori_cov'], params['init_ori_cov'],
params['init_pos_cov'], params['init_pos_cov'], params['init_pos_cov']]))
self.init_vel_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([
params['init_vel_cov'], params['init_vel_cov'], params['init_vel_cov']]))
self.init_bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.array([
params['init_acc_bias_cov'], params['init_acc_bias_cov'], params['init_acc_bias_cov'],
params['init_gyr_bias_cov'], params['init_gyr_bias_cov'], params['init_gyr_bias_cov']]))
# measurement noise
self.dvl_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray(
params['dvl_cov']))
self.bar_cov = gtsam.noiseModel_Isotropic.Sigmas(np.asarray(
params['bar_cov']))
self.bias_cov = gtsam.noiseModel_Isotropic.Sigmas(np.concatenate((
params['sigma_acc_bias_evol'],
params['sigma_gyr_bias_evol'])))
""" Set initial state """
# Initial factors
prior_pose_factor = gtsam.PriorFactorPose3(
self.poseKey,
self.current_global_pose,
self.init_pos_cov)
self.new_factors.add(prior_pose_factor)
prior_vel_factor = gtsam.PriorFactorVector(
self.velKey,
self.current_global_vel,
self.init_vel_cov)
self.new_factors.add(prior_vel_factor)
prior_bias_factor = gtsam.PriorFactorConstantBias(
self.biasKey,
self.current_bias,
self.init_bias_cov)
self.new_factors.add(prior_bias_factor)
# Initial estimates
self.new_initial_ests.insert(self.poseKey, self.current_global_pose)
self.new_initial_ests.insert(self.velKey, self.current_global_vel)
self.new_initial_ests.insert(self.biasKey, self.current_bias)
def initialize(data, truth, options):
# Initialize iSAM
params = gtsam.ISAM2Params()
if options.alwaysRelinearize:
params.relinearizeSkip = 1
isam = gtsam.ISAM2(params=params)
# Add constraints/priors
# TODO: should not be from ground truth!
newFactors = gtsam.NonlinearFactorGraph()
initialEstimates = gtsam.Values()
for i in range(2):
ii = symbol('x', i)
if i == 0:
if options.hardConstraint: # add hard constraint
newFactors.add(
gtsam.NonlinearEqualityPose3(ii, truth.cameras[0].pose()))
else:
newFactors.add(
gtsam.PriorFactorPose3(ii, truth.cameras[i].pose(),
data.noiseModels.posePrior))
initialEstimates.insert(ii, truth.cameras[i].pose())
nextPoseIndex = 2
# Add visual measurement factors from two first poses and initialize
# observed landmarks
for i in range(2):
ii = symbol('x', i)
for k in range(len(data.Z[i])):
j = data.J[i][k]
jj = symbol('l', j)
newFactors.add(
gtsam.GenericProjectionFactorCal3_S2(
data.Z[i][k], data.noiseModels.measurement, ii, jj,
data.K))
# TODO: initial estimates should not be from ground truth!
if not initialEstimates.exists(jj):
initialEstimates.insert(jj, truth.points[j])
if options.pointPriors: # add point priors
newFactors.add(
gtsam.PriorFactorPoint3(jj, truth.points[j],
data.noiseModels.pointPrior))
# Add odometry between frames 0 and 1
newFactors.add(
gtsam.BetweenFactorPose3(symbol('x', 0), symbol('x', 1),
data.odometry[1], data.noiseModels.odometry))
# Update ISAM
if options.batchInitialization: # Do a full optimize for first two poses
batchOptimizer = gtsam.LevenbergMarquardtOptimizer(
newFactors, initialEstimates)
fullyOptimized = batchOptimizer.optimize()
isam.update(newFactors, fullyOptimized)
else:
isam.update(newFactors, initialEstimates)
# figure(1)tic
# t=toc plot(frame_i,t,'r.') tic
result = isam.calculateEstimate()
# t=toc plot(frame_i,t,'g.')
return isam, result, nextPoseIndex
