def test_noise3(self):
observation_noise = 0.01
odometry_noise = 0.02
sim = new_simulation(point_dim=3,
landmark_dim=1,
seed=17,
observation_noise=observation_noise,
odometry_noise=odometry_noise,
noise_matrix='diag')
fg = factorgraph.GaussianFactorGraph()
for f in sim.factors():
fg.add_factor(f)
optimizer = optim.WeightedLeastSquares(fg)
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)
mu_m_hat = np.mean(m - m_hat)
sigma_m_hat = np.sqrt(np.linalg.norm(m - m_hat)**2 / len(m))
mu_p_hat = np.mean(p - p_hat)
sigma_p_hat = np.sqrt(np.linalg.norm(p - p_hat)**2 / len(p))
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)
def test_no_measurements(self):
fg = factorgraph.GaussianFactorGraph()
optimizer = optim.WeightedLeastSquares(fg)
optimizer.optimize()
optimizer.update()
def test_no_noise2(self):
sim = new_simulation(point_dim=3, landmark_dim=3, seed=23)
fg = factorgraph.GaussianFactorGraph()
for f in sim.factors():
fg.add_factor(f)
optimizer = optim.WeightedLeastSquares(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_sequential2(self):
observation_noise = 0.01
odometry_noise = 0.02
sim = new_simulation(point_dim=3,
landmark_dim=3,
num_points=100,
seed=268,
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_odom_only(self):
import factor
sim = new_simulation(point_dim=3,
landmark_dim=1,
seed=2084,
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.WeightedLeastSquares(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))
