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))