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
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 batch_factorgraph_example():
# Create an empty nonlinear factor graph.
graph = gtsam.NonlinearFactorGraph()
# Create the keys for the poses.
X1 = X(1)
X2 = X(2)
X3 = X(3)
pose_variables = [X1, X2, X3]
# Create keys for the landmarks.
L1 = L(1)
L2 = L(2)
landmark_variables = [L1, L2]
# 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))
# Add odometry factors between X1,X2 and X2,X3, respectively
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))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
odometry_noise))
# Add Range-Bearing measurements to two different landmarks L1 and L2
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))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
measurement_noise))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Create an optimizer.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
params)
# Solve the MAP problem.
result = optimizer.optimize()
# Calculate marginal covariances for all variables.
marginals = gtsam.Marginals(graph, result)
# Extract marginals
pose_marginals = []
for var in pose_variables:
pose_marginals.append(
MultivariateNormalParameters(result.atPose2(var),
marginals.marginalCovariance(var)))
landmark_marginals = []
for var in landmark_variables:
landmark_marginals.append(
MultivariateNormalParameters(result.atPoint2(var),
marginals.marginalCovariance(var)))
# You can extract the joint marginals like this.
joint_all = marginals.jointMarginalCovariance(
gtsam.KeyVector(pose_variables + landmark_variables))
print("Joint covariance over all variables:")
print(joint_all.fullMatrix())
# Plot the marginals.
plot_result(pose_marginals, landmark_marginals)
X2 = X(2)
X3 = X(3)
L1 = L(4)
L2 = L(5)
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
# Add odometry factors between X1,X2 and X2,X3, respectively
graph.add(gtsam.BetweenFactorPose2(
X1, X2, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
graph.add(gtsam.BetweenFactorPose2(
X2, X3, gtsam.Pose2(2.0, 0.0, 0.0), ODOMETRY_NOISE))
# Add Range-Bearing measurements to two different landmarks L1 and L2
graph.add(gtsam.BearingRangeFactor2D(
X1, L1, gtsam.Rot2.fromDegrees(45), np.sqrt(4.0+4.0), MEASUREMENT_NOISE))
graph.add(gtsam.BearingRangeFactor2D(
X2, L1, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
graph.add(gtsam.BearingRangeFactor2D(
X3, L2, gtsam.Rot2.fromDegrees(90), 2.0, MEASUREMENT_NOISE))
# Print graph
print("Factor Graph:\n{}".format(graph))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
def main():
"""Main runner"""
# Create an empty nonlinear factor graph
graph = gtsam.NonlinearFactorGraph()
# Create the keys corresponding to unknown variables in the factor graph
X1 = X(1)
X2 = X(2)
X3 = X(3)
L1 = L(4)
L2 = L(5)
# Add a prior on pose X1 at the origin. A prior factor consists of a mean and a noise model
graph.add(
gtsam.PriorFactorPose2(X1, gtsam.Pose2(0.0, 0.0, 0.0), PRIOR_NOISE))
# Add odometry factors between X1,X2 and X2,X3, respectively
graph.add(
gtsam.BetweenFactorPose2(X1, X2, gtsam.Pose2(2.0, 0.0, 0.0),
ODOMETRY_NOISE))
graph.add(
gtsam.BetweenFactorPose2(X2, X3, gtsam.Pose2(2.0, 0.0, 0.0),
ODOMETRY_NOISE))
# Add Range-Bearing measurements to two different landmarks L1 and L2
graph.add(
gtsam.BearingRangeFactor2D(X1, L1, gtsam.Rot2.fromDegrees(45),
np.sqrt(4.0 + 4.0), MEASUREMENT_NOISE))
graph.add(
gtsam.BearingRangeFactor2D(X2, L1, gtsam.Rot2.fromDegrees(90), 2.0,
MEASUREMENT_NOISE))
graph.add(
gtsam.BearingRangeFactor2D(X3, L2, gtsam.Rot2.fromDegrees(90), 2.0,
MEASUREMENT_NOISE))
# Print graph
print("Factor Graph:\n{}".format(graph))
# Create (deliberately inaccurate) initial estimate
initial_estimate = gtsam.Values()
initial_estimate.insert(X1, gtsam.Pose2(-0.25, 0.20, 0.15))
initial_estimate.insert(X2, gtsam.Pose2(2.30, 0.10, -0.20))
initial_estimate.insert(X3, gtsam.Pose2(4.10, 0.10, 0.10))
initial_estimate.insert(L1, gtsam.Point2(1.80, 2.10))
initial_estimate.insert(L2, gtsam.Point2(4.10, 1.80))
# Print
print("Initial Estimate:\n{}".format(initial_estimate))
# Optimize using Levenberg-Marquardt optimization. The optimizer
# accepts an optional set of configuration parameters, controlling
# things like convergence criteria, the type of linear system solver
# to use, and the amount of information displayed during optimization.
# Here we will use the default set of parameters. See the
# documentation for the full set of parameters.
params = gtsam.LevenbergMarquardtParams()
optimizer = gtsam.LevenbergMarquardtOptimizer(graph, initial_estimate,
params)
result = optimizer.optimize()
print("\nFinal Result:\n{}".format(result))
# Calculate and print marginal covariances for all variables
marginals = gtsam.Marginals(graph, result)
for (key, s) in [(X1, "X1"), (X2, "X2"), (X3, "X3"), (L1, "L1"),
(L2, "L2")]:
print("{} covariance:\n{}\n".format(s,
marginals.marginalCovariance(key)))
def update(self, motion, measurement):
if self.pose_num == 0:
self.result = self.estimations
odometry = self._get_motion_gtsam_format(motion)
noise = gtsam.noiseModel_Diagonal.Sigmas(self._get_motion_noise_covariance(motion, self.alphas))
predicted_state = self._get_motion_prediction(self.result.atPose2(self.pose_num), motion)
# adding to the graph odometry value
self.graph.add(gtsam.BetweenFactorPose2(self.pose_num, self.pose_num + 1, odometry, noise))
# adding predicted pose to the initial estimations
self.estimations.insert(self.pose_num + 1, predicted_state)
for i in range(len(measurement)):
bearing = gtsam.Rot2(measurement[i, 1])
distance = measurement[i, 0]
landmark_id = self.observation_num
# adding to the graph measurement value
self.graph.add(
gtsam.BearingRangeFactor2D(self.pose_num, landmark_id, bearing, distance, self.observation_noise))
landmark_position = self._get_landmark_position(self.result.atPose2(self.pose_num), distance,
bearing.theta())
# adding predicted landmarks position to the initial estimations
self.estimations.insert(landmark_id, landmark_position)
self.observation_num += 1
"""
for i in range(len(measurement)):
bearing = gtsam.Rot2(measurement[i, 0])
distance = measurement[i, 1]
landmark_id = 1000 + measurement[i, 2]
if landmark_id not in self.landmark_indexes:
self.landmark_indexes.append(landmark_id)
landmark_position = self._get_landmark_position(self.result.atPose2(self.pose_num), distance, bearing.theta())
self.graph.add(gtsam.BearingRangeFactor2D(self.pose_num, landmark_id, bearing, distance, self.observation_noise))
self.estimations.insert(landmark_id, landmark_position)
else:
pass
"""
# update factorization problem
#print(noise)
#print(self.observation_noise)
print(self.estimations)
#params = gtsam.LevenbergMarquardtParams()
#optimiser = gtsam.LevenbergMarquardtOptimizer(self.graph, self.estimations, params)
#optimiser.optimize()
self.slam.update(self.graph, self.estimations)
# clearing current graph and estimations
self.graph.resize(0)
self.estimations.clear()
print(self.graph)
# getting results
self.result = self.slam.calculateEstimate()
#print(self.result)
self.pose_num += 1
self._convert_to_np_format()