def validate_predicted_measurements():
# Load pickle
validation_run = load_pickle(EKF_PICKLE)
validation_pf = load_pickle(PF_PICKLE)
x0 = validation_pf['x0']
xs_input = validation_pf['x_input']
dt = validation_run['t'][1] - validation_run['t'][0]
ground_truth_hs = validation_pf['predicted_measurements_validation']
# Initialize MC localization
np.random.seed(1234)
mcl = MonteCarloLocalization(xs_input,
10. * NoiseParams['R'],
params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
hs = mcl.compute_predicted_measurements()
if np.linalg.norm(hs - ground_truth_hs, axis=1).sum() >= 1e-2:
print("Got MonteCarloLocalization.compute_predicted_measurements() output:\n")
print(hs)
print("\nvs. the expected values\n")
print(ground_truth_hs)
return False
print("MonteCarloLocalization.compute_predicted_measurements() seems to be correct")
return True
def validate_transition_model():
# Load pickle
validation_run = load_pickle(EKF_PICKLE)
validation_pf = load_pickle(PF_PICKLE)
x0 = validation_pf['x0']
xs_validation = validation_pf['transition_model_validation']
dt = validation_run['t'][1] - validation_run['t'][0]
# Initialize MC localization
np.random.seed(1234)
mcl = MonteCarloLocalization(validation_pf['x0'],
10. * NoiseParams['R'],
params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
xs = mcl.transition_model(np.random.multivariate_normal(validation_run['controls'][0],
10. * dt * NoiseParams['R'],
(x0.shape[0],)), dt)
if np.linalg.norm(xs_validation - xs, axis=1).sum() >= 1e-6:
print("Got MonteCarloLocalization.transition_model() particles:\n")
print(xs)
print("\nvs. the expected particles\n")
print(xs_validation)
return False
print("MonteCarloLocalization.transition_model() seems to be correct")
return True
def validate_resample():
# Load pickle
validation_run = load_pickle(EKF_PICKLE)
validation_pf = load_pickle(PF_PICKLE)
x0 = validation_pf['x0']
xs_input = validation_pf['x_input']
ws_input = validation_pf['w_input']
xs_validation = validation_pf['resample_validation']['xs']
ws_validation = validation_pf['resample_validation']['ws']
# Initialize MC localization
np.random.seed(1234)
mcl = MonteCarloLocalization(x0,
10. * NoiseParams['R'],
params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
mcl.resample(xs_input, ws_input)
if np.linalg.norm(xs_validation - mcl.xs, axis=1).sum() >= 1e-6:
print("Got MonteCarloLocalization.resample() particles:\n")
print(mcl.xs)
print("\nvs. the expected particles\n")
print(xs_validation)
return False
if np.linalg.norm(ws_validation - mcl.ws) >= 1e-6:
print("Got MonteCarloLocalization.resample() weights:\n")
print(mcl.ws)
print("\nvs. the expected weights\n")
print(ws_validation)
return False
print("MonteCarloLocalization.resample() seems to be correct")
return True
def validate_compute_innovations():
# Load pickle
validation_run = load_pickle(EKF_PICKLE)
validation_pf = load_pickle(PF_PICKLE)
x0 = validation_pf['x0']
xs_input = validation_pf['x_input']
dt = validation_run['t'][1] - validation_run['t'][0]
scans = validation_run['scans']
ground_truth_vs = validation_pf['predicted_compute_innovations']
# Initialize MC localization
np.random.seed(1234)
mcl = MonteCarloLocalization(xs_input,
10. * NoiseParams['R'],
params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
alpha, r, Q_raw, _, _ = ExtractLines(scans[0,-1,:],
scans[1,-1,:],
LineExtractionParams,
NoiseParams['var_theta'],
NoiseParams['var_rho'])
z_raw = np.vstack((alpha, r))
vs = mcl.compute_innovations(z_raw, np.array(Q_raw))
if np.linalg.norm(vs - ground_truth_vs, axis=1).sum() >= 1e-3:
print("Got MonteCarloLocalization.compute_innovations() output:\n")
print(vs)
print("\nvs. the expected values\n")
print(ground_truth_vs)
return False
print("MonteCarloLocalization.compute_innovations() seems to be correct")
return True
def validate_mc_localization(show_plot=True):
NUM_PARTICLES=100
# Plot open loop
validate_ekf_transition_update(False)
# Load pickle
validation_run = load_pickle(EKF_PICKLE)
u = validation_run['controls']
t = validation_run['t']
T = t.shape[0]
t_scans = validation_run['t_scans']
T_scans = t_scans.shape[0]
scans = validation_run['scans']
x0 = np.tile(validation_run['states'][0], (NUM_PARTICLES, 1))
# Initialize MC localization
np.random.seed(1234)
mcl = MonteCarloLocalization(x0,
10. * NoiseParams['R'],
params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
X = np.zeros((T, NUM_PARTICLES, 3))
W = np.zeros((T, NUM_PARTICLES))
# Iterate over states
mcl_states = np.zeros((T, mcl.x.shape[0]))
mcl_states[0] = mcl.x
scan_states = np.zeros((T_scans, mcl.x.shape[0]))
scan_idx = 0
X[0] = mcl.xs
W[0] = mcl.ws
for i in range(T - 1):
t1 = t[i+1]
t0 = t[i]
# Iterate over scans
while scan_idx < T_scans and t_scans[scan_idx] < t1:
# Transition update
mcl.transition_update(u[i], t_scans[scan_idx] - t0)
t0 = t_scans[scan_idx]
# Measurement update
alpha, r, Q_raw, _, _ = ExtractLines(scans[0,scan_idx,:],
scans[1,scan_idx,:],
LineExtractionParams,
NoiseParams['var_theta'],
NoiseParams['var_rho'])
z_raw = np.vstack((alpha, r))
mcl.measurement_update(z_raw, Q_raw)
scan_states[scan_idx] = mcl.x
scan_idx += 1
# Transition update
mcl.transition_update(u[i], t1 - t0)
mcl_states[i+1] = mcl.x
X[i+1] = mcl.xs
W[i+1] = mcl.ws
# Plot
plt.plot(mcl_states[:,0], mcl_states[:,1], label="MCL", color="red")
plt.scatter(scan_states[:,0], scan_states[:,1], marker = "x", label="measurement update", color="blue")
if show_plot:
plt.legend(loc=0)
plt.savefig("mc_localization.png")
print("Plot saved to mc_localization.png")