def simulate_data(num_states):
# movement = x2 - x1
state_init = fix_lag_types.State(np.array([0., 0., 1., 2.]))
states_gt = [state_init]
for i in range(num_states - 1):
states_gt.append(states_gt[-1].predict())
states_gt = [s.variables for s in states_gt]
odometry_measurements = [
fix_lag_types.OdometryMeasurement(state1_index=i, state2_index=i + 1)
for i in range(num_states - 1)
]
gps_measurements = [
fix_lag_types.GPSMeasurement(state_index=i,
gps=states_gt[i][:2] +
0.5 * np.random.rand(2))
for i in range(num_states)
]
prior_measurement = np.array([0., 0., 1., 2.])
state_init_guess = [
fix_lag_types.State(np.random.random(4)) for i in range(num_states)
]
return states_gt, state_init_guess, odometry_measurements, gps_measurements, prior_measurement
def simulate_data(num_states):
# movement = x2 - x1
movement = np.array([2., 1.])
states_gt = [t.State(i * movement) for i in range(num_states)]
distrance_measurements = [
t.DistanceBetweenStates(state1_index=i,
state2_index=i + 1,
distance=movement + 5 * np.random.rand())
for i in range(num_states - 1)
]
state_guess = [t.State(movement) for i in range(num_states)]
return states_gt, distrance_measurements, state_guess
def construct_linear_system(self, distance_measurement):
# x_i, x_i+1
size_variables = 4
# Hessian: A.T * W * A
lhs = np.zeros([size_variables, size_variables])
# A.T * W * r
rhs = np.zeros([size_variables, 1])
lhs[0:2, 0:2] += self.state_hessian
rhs[0:2] += self.state_b
assert(distance_measurement.state1_index +
1 == distance_measurement.state2_index)
dm = t.DistanceMeasurement(t.State(self.state.copy()), t.State(
self.state.copy()), distance_measurement.distance)
jacobi_wrt_s1 = dm.jacobi_wrt_state1()
jacobi_wrt_s2 = dm.jacobi_wrt_state2()
jacobi_dm = np.hstack([jacobi_wrt_s1, jacobi_wrt_s2])
lhs[0:4, 0:4] += jacobi_dm.T @ jacobi_dm
rhs[0:4] += jacobi_dm.T @ dm.residual()
return lhs, rhs
def __init__(self, prior_measurement, gps_measurement):
# using deque (linked list) for head & tail lookup
self.__state_opt_vars = deque([t.State(prior_measurement.copy())])
# mantain a lookup between state idx and state in the deque
self.__state_idx_offset = 0
# make sure the fixed-lagged graph invariant hold at the beginning
self.__odometry_measurements = deque()
self.__gps_measurements = deque([gps_measurement])
# For prior cost, Jacobian is I. Direct assign for simplicity
# Assuming Identity weight matrix.
self.__prior_hessian = np.identity(4)
self.__prior_mean = np.zeros((4, 1))
# config
self.__window_size = 5
# result for easy query
self.__result = []
self.__diag_covs = []
def extent_graph(self, odometry_measurement, gps_measurement):
# extent the graph
self.__odometry_measurements.append(odometry_measurement)
self.__gps_measurements.append(gps_measurement)
self.__state_opt_vars.append(
t.State(self.__state_opt_vars[-1].variables.copy()))