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 run(self):
rate = rospy.Rate(100)
particles = PointCloud()
particles.header.stamp = self.EKF_time
particles.header.frame_id = "world"
particles.points = [
Point32(0., 0., 0.) for _ in range(self.params.num_particles)
]
particle_intensities = ChannelFloat32()
particle_intensities.name = "intensity"
particle_intensities.values = [
0. for _ in range(self.params.num_particles)
]
particles.channels.append(particle_intensities)
while not self.latest_pose:
rate.sleep()
x0 = np.array([
self.latest_pose.position.x, self.latest_pose.position.y,
get_yaw_from_quaternion(self.latest_pose.orientation)
])
self.EKF_time = self.latest_pose_time
if self.params.mc:
x0s = np.tile(np.expand_dims(x0, 0),
(self.params.num_particles, 1))
from HW4.particle_filter import MonteCarloLocalization
self.EKF = MonteCarloLocalization(x0s, 10. * NoiseParams["R"],
MapParams, self.base_to_camera,
NoiseParams["g"])
self.OLC = EkfLocalization(x0, NoiseParams["Sigma0"],
NoiseParams["R"], MapParams,
self.base_to_camera, NoiseParams["g"])
else:
self.EKF = EkfLocalization(x0, NoiseParams["Sigma0"],
NoiseParams["R"], MapParams,
self.base_to_camera, NoiseParams["g"])
self.OLC = EkfLocalization(x0, NoiseParams["Sigma0"],
NoiseParams["R"], MapParams,
self.base_to_camera, NoiseParams["g"])
while True:
if not self.scans:
rate.sleep()
continue
while self.controls and self.controls[0][0] <= self.scans[0][0]:
next_timestep, next_control = self.controls.popleft()
if next_timestep < self.EKF_time: # guard against time weirdness (msgs out of order)
continue
self.EKF.transition_update(
self.current_control,
next_timestep.to_time() - self.EKF_time.to_time())
self.OLC.transition_update(
self.current_control,
next_timestep.to_time() - self.EKF_time.to_time())
self.EKF_time, self.current_control = next_timestep, next_control
label = "EKF" if not self.params.mc else "MCL"
self.tfBroadcaster.sendTransform(
create_transform_msg(
(self.EKF.x[0], self.EKF.x[1], 0),
tf.transformations.quaternion_from_euler(
0, 0,
self.EKF.x[2]), label, "world", self.EKF_time))
self.tfBroadcaster.sendTransform(
create_transform_msg(
(self.OLC.x[0], self.OLC.x[1], 0),
tf.transformations.quaternion_from_euler(
0, 0, self.OLC.x[2]), "open_loop", "world",
self.EKF_time))
if self.params.mc:
particles.header.stamp = self.EKF_time
for m in range(self.params.num_particles):
x = self.EKF.xs[m]
w = self.EKF.ws[m]
particles.points[m].x = x[0]
particles.points[m].y = x[1]
particles.channels[0].values[m] = w
self.particles_pub.publish(particles)
scan_time, theta, rho = self.scans.popleft()
if scan_time < self.EKF_time:
continue
self.EKF.transition_update(
self.current_control,
scan_time.to_time() - self.EKF_time.to_time())
self.OLC.transition_update(
self.current_control,
scan_time.to_time() - self.EKF_time.to_time())
self.EKF_time = scan_time
alpha, r, C_AR, _, _ = ExtractLines(theta, rho,
LineExtractionParams,
NoiseParams["var_theta"],
NoiseParams["var_rho"])
Z = np.vstack((alpha, r))
self.EKF.measurement_update(Z, C_AR)
if self.params.mc:
particles.header.stamp = self.EKF_time
for m in range(self.params.num_particles):
x = self.EKF.xs[m]
w = self.EKF.ws[m]
particles.points[m].x = x[0]
particles.points[m].y = x[1]
particles.channels[0].values[m] = w
self.particles_pub.publish(particles)
while len(
self.scans
) > 1: # keep only the last element in the queue, if we're falling behind
self.scans.popleft()
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")