def test_BearingFactor2D(self):
"""
Test that `measured` works as expected for BearingFactor2D.
"""
measurement = gtsam.Rot2(.3)
factor = gtsam.BearingFactor2D(1, 2, measurement,
gtsam.noiseModel.Isotropic.Sigma(1, 1))
self.gtsamAssertEquals(measurement, factor.measured())
def test_BearingRangeFactor2D(self):
"""
Test that `measured` works as expected for BearingRangeFactor2D.
"""
range_measurement = 10.0
bearing_measurement = gtsam.Rot2(0.3)
factor = gtsam.BearingRangeFactor2D(
1, 2, bearing_measurement, range_measurement,
gtsam.noiseModel.Isotropic.Sigma(2, 1))
measurement = factor.measured()
self.assertEqual(range_measurement, measurement.range())
self.gtsamAssertEquals(bearing_measurement, measurement.bearing())
def update(self, motion, measurement):
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.estimations.atPose2(self.pose_num), motion)
self.graph.add(
gtsam.BetweenFactorPose2(self.pose_num, self.pose_num + 1,
odometry, noise))
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 = 1000 + measurement[i, 2]
self.graph.add(
gtsam.BearingRangeFactor2D(self.pose_num, landmark_id, bearing,
distance, self.observation_noise))
if landmark_id not in self.landmark_indexes:
self.landmark_indexes.append(landmark_id)
landmark_position = self._get_landmark_position(
self.estimations.atPose2(self.pose_num), distance,
bearing.theta())
self.estimations.insert(landmark_id, landmark_position)
else:
landmark_position = self._get_landmark_position(
self.estimations.atPose2(self.pose_num), distance,
bearing.theta())
self.estimations.update(landmark_id, landmark_position)
#params = gtsam.LevenbergMarquardtParams()
#params = gtsam.NonlinearOptimizerParams()
#optimiser = gtsam.LevenbergMarquardtOptimizer(self.graph, self.estimations, params)
#optimiser = gtsam.NonlinearOptimizer(self.graph, self.estimations, params)
optimiser = gtsam.GaussNewtonOptimizer(self.graph, self.estimations)
self.result = optimiser.optimize()
self.estimations = self.result
self.pose_num += 1
self._convert_to_np_format()
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 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()