def test_sequential1(self):
observation_noise = 0.01
odometry_noise = 0.02
sim = new_simulation(point_dim=3,
landmark_dim=1,
num_points=100,
seed=266,
observation_noise=observation_noise,
odometry_noise=odometry_noise,
noise_matrix='diag')
num_partitions = 10
partition_indptr = np.sort(
np.concatenate([[0],
np.random.choice(sim.num_points,
num_partitions - 1),
[sim.num_points]]))
fpw = int(np.max(partition_indptr[1:] - partition_indptr[:-1]) + 5)
fg = factorgraph.GaussianFactorGraph(free_point_window=fpw)
optimizer = optim.WeightedLeastSquares(fg)
for i in range(num_partitions):
for f in sim.factors(
(partition_indptr[i], partition_indptr[i + 1])):
fg.add_factor(f)
optimizer.optimize()
optimizer.update()
p_hat = np.concatenate([p.position for p in fg.points])
p = np.ravel(sim.point_positions[:partition_indptr[i + 1], :])
self.assertTrue(
np.linalg.norm(optimizer.res_d)**2 < len(optimizer.res_d) * 4 *
observation_noise**2)
self.assertTrue(
np.linalg.norm(optimizer.res_t)**2 < len(optimizer.res_t) * 4 *
odometry_noise**2)
self.assertTrue(np.allclose(p, p_hat, atol=0.1))
def test_under_marginal4(self):
num_points = 2000
num_fixed_points = 111
num_free_points = 642
sim = new_simulation(point_dim=3,
landmark_dim=3,
num_points=num_points,
seed=344)
fg = factorgraph.GaussianFactorGraph(num_free_points)
for f in sim.fix_points(list(range(num_fixed_points))).factors():
fg.add_factor(f)
A, b, _ = fg.odometry_system()
x = np.ravel(sim.point_positions[-(num_points - num_fixed_points):, :])
self.assertEqual(b.size, A.shape[0])
self.assertEqual(x.size, A.shape[1])
self.assertTrue(np.allclose(A.dot(x), b))
def test_marginal_sequence(self):
num_points = 100
num_free_points = 15
sim = new_simulation(point_dim=3,
landmark_dim=1,
num_points=num_points,
seed=253)
fg = factorgraph.GaussianFactorGraph(free_point_window=num_free_points)
num_partitions = 10
partition_indptr = np.linspace(0,
100,
num_partitions + 1,
dtype=np.int)
sim_landmarks = sim.landmark_variables
for i in range(num_partitions):
if i > 0:
sim.fix_points(
list(range(partition_indptr[i - 1], partition_indptr[i])))
for f in sim.factors(
(partition_indptr[i], partition_indptr[i + 1])):
fg.add_factor(f)
Am, Ap, d, _ = fg.observation_system()
fg_landmarks = fg.landmarks
order = [sim_landmarks.index(k) for k in fg_landmarks]
m = np.ravel(sim.landmark_positions[order, :])
p = np.ravel(sim.point_positions[
max(0, partition_indptr[i + 1] -
num_free_points):partition_indptr[i + 1], :])
self.assertEqual(d.size, Am.shape[0])
self.assertEqual(d.size, Ap.shape[0])
self.assertEqual(m.size, Am.shape[1])
self.assertEqual(p.size, Ap.shape[1])
self.assertTrue(np.allclose(Am.dot(m) + Ap.dot(p), d))
def test_all_marginal2(self):
num_points = 2000
num_fixed_points = num_points
num_free_points = 0
sim = new_simulation(point_dim=3,
landmark_dim=3,
num_points=num_points,
seed=252)
fg = factorgraph.GaussianFactorGraph(num_free_points)
for f in sim.fix_points(list(range(num_fixed_points))).factors():
fg.add_factor(f)
Am, Ap, d, _ = fg.observation_system()
m = np.ravel(sim.landmark_positions)
self.assertEqual(d.size, Am.shape[0])
self.assertEqual(d.size, Ap.shape[0])
self.assertEqual(m.size, Am.shape[1])
self.assertEqual(0, Ap.shape[1])
self.assertTrue(np.allclose(Am.dot(m), d))
def test_under_marginal4(self):
num_points = 2000
num_fixed_points = 111
num_free_points = 642
sim = new_simulation(point_dim=3,
landmark_dim=3,
num_points=num_points,
seed=244)
fg = factorgraph.GaussianFactorGraph(num_free_points)
for f in sim.fix_points(list(range(num_fixed_points))).factors():
fg.add_factor(f)
Am, Ap, d, _ = fg.observation_system()
m = np.ravel(sim.landmark_positions)
p = np.ravel(sim.point_positions[-(num_points - num_fixed_points):, :])
self.assertEqual(d.size, Am.shape[0])
self.assertEqual(d.size, Ap.shape[0])
self.assertEqual(m.size + p.size, Am.shape[1] + Ap.shape[1])
self.assertTrue(np.allclose(Am.dot(m) + Ap.dot(p), d))
def test_no_noise2(self):
sim = new_simulation(point_dim=3,
landmark_dim=3,
seed=6,
observation_noise=0.0,
odometry_noise=0.0)
fg = factorgraph.GaussianFactorGraph()
for f in sim.factors():
fg.add_factor(f)
optimizer = optim.LeastSquares(fg, verbosity=True)
optimizer.optimize()
optimizer.update()
m_hat = np.concatenate([m.position for m in fg.landmarks])
p_hat = np.concatenate([p.position for p in fg.points])
m, p = np.ravel(sim.landmark_positions), np.ravel(sim.point_positions)
self.assertTrue(np.allclose(m, m_hat, atol=1e-4))
self.assertTrue(np.allclose(p, p_hat, atol=1e-3))
def test_odom_only(self):
import factor
sim = new_simulation(point_dim=3,
landmark_dim=1,
seed=1776,
observation_noise=0.0,
odometry_noise=0.0)
fg = factorgraph.GaussianFactorGraph()
for f in sim.factors():
if isinstance(f, factor.ObservationFactor):
continue
fg.add_factor(f)
optimizer = optim.Occam(fg)
optimizer.optimize()
optimizer.update()
p_hat = np.concatenate([p.position for p in fg.points])
p = np.ravel(sim.point_positions)
self.assertTrue(np.allclose(p, p_hat, atol=1e-3))
def test_merge_sequence_with_marginalization(self):
num_points = 100
num_free_points = 15
# sim = new_simulation(point_dim=3, landmark_dim=1, num_points=num_points, seed=414)
sim = new_simulation(point_dim=3,
landmark_dim=1,
num_points=num_points,
seed=66)
fg = factorgraph.GaussianFactorGraph(num_free_points)
landmarks = sim.landmark_variables
num_partitions = 10
partition_indptr = np.linspace(0,
num_points,
num_partitions + 1,
dtype=np.int)
for i in range(num_partitions):
if i > 0:
sim.fix_points(
list(range(partition_indptr[i - 1], partition_indptr[i])))
for f in sim.factors(
(partition_indptr[i], partition_indptr[i + 1])):
fg.add_factor(f)
equiv_groups, equiv_pairs = sim.equivalences(
(0, partition_indptr[i + 1]))
fg.merge_landmarks(equiv_pairs)
unique_landmarks = fg.landmarks
unique_order = [
sim.unique_index_map[landmarks.index(k)]
for k in unique_landmarks
]
self.assertEqual(len(fg.correspondence_map.set_map().keys()),
len(unique_landmarks))
fg_groups = set(
frozenset(g) for g in fg.correspondence_map.set_map().values())
diff = equiv_groups.symmetric_difference(fg_groups)
self.assertTrue(len(diff) == 0)
Am, Ap, d, _ = fg.observation_system()
Bp, t, _ = fg.odometry_system()
m = np.ravel(sim.unique_landmark_positions[unique_order, :])
p = np.ravel(sim.point_positions[
max(0, partition_indptr[i + 1] -
num_free_points):partition_indptr[i + 1], :])
self.assertEqual(d.size, Am.shape[0])
self.assertEqual(d.size, Ap.shape[0])
self.assertEqual(m.size, Am.shape[1])
self.assertEqual(p.size, Ap.shape[1])
self.assertTrue(np.allclose(Am.dot(m) + Ap.dot(p), d))
self.assertTrue(np.allclose(Bp.dot(p), t))
