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")
def validate_ekf_localization(show_plot=True):
# 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']
# Initialize EKF localization
ekf_loc = EkfLocalization(validation_run['states'][0],
NoiseParams['Sigma0'], NoiseParams['R'],
params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
# Iterate over states
ekf_states = np.zeros((T, ekf_loc.x.shape[0]))
ekf_states[0] = ekf_loc.x
scan_states = np.zeros((T_scans, ekf_loc.x.shape[0]))
scan_idx = 0
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
ekf_loc.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))
ekf_loc.measurement_update(z_raw, Q_raw)
scan_states[scan_idx] = ekf_loc.x
scan_idx += 1
# Transition update
ekf_loc.transition_update(u[i], t1 - t0)
ekf_states[i + 1] = ekf_loc.x
# Plot
plt.plot(ekf_states[:, 0],
ekf_states[:, 1],
label="EKF (known map)",
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("ekf_localization.png")
print("Plot saved to ekf_localization.png")
def validate_ekf_slam():
# 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']
# Prepare initial state
np.random.seed(1234)
x0_pose = validation_run['states'][0]
Sigma0_pose = NoiseParams['Sigma0']
N_map_lines = params.MapParams.shape[1]
x0_map = params.MapParams.T.flatten()
x0_map[4:] = x0_map[4:] + np.vstack((
NoiseParams['std_alpha'] *
np.random.randn(N_map_lines - 2), # first two lines fixed
NoiseParams['std_r'] * np.random.randn(N_map_lines - 2))).T.flatten()
Sigma0_map = np.diag(
np.concatenate(
(np.zeros(4),
np.array([
[NoiseParams['std_alpha']**2 for i in range(N_map_lines - 2)],
[NoiseParams['std_r']**2 for i in range(N_map_lines - 2)]
]).T.flatten())))
# Initialize EKF SLAM
ekf_slam = EkfSlam(np.concatenate((x0_pose, x0_map)),
scipy.linalg.block_diag(Sigma0_pose,
Sigma0_map), NoiseParams['R'],
validation_run['tf_base_to_camera'], NoiseParams['g'])
# Iterate over states
ekf_states = np.zeros((T, ekf_slam.x.shape[0]))
ekf_states[0] = ekf_slam.x
scan_states = np.zeros((T_scans, ekf_slam.x.shape[0]))
scan_idx = 0
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
ekf_slam.transition_update(u[i], t_scans[scan_idx] - t0)
t0 = t_scans[scan_idx]
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))
ekf_slam.measurement_update(z_raw, Q_raw)
scan_states[scan_idx] = ekf_slam.x
scan_idx += 1
# Transition update
ekf_slam.transition_update(u[i], t1 - t0)
ekf_states[i + 1] = ekf_slam.x
# Plot
plt.plot(ekf_states[:, 0],
ekf_states[:, 1],
label="EKF (noisy map)",
color="orange")
plt.scatter(scan_states[:, 0],
scan_states[:, 1],
marker="x",
label="measurement update",
color="blue")
plt.legend(loc=0)
plt.savefig("ekf_slam.png")
print("Plot saved to ekf_slam.png")
def validate_localization_compute_innovations():
# Test transition_model() and predicted_measurements() first
if not validate_localization_transition_model() or \
not validate_localization_compute_predicted_measurements():
print("Validation of compute_innovations cannot proceed.")
return False
# Load pickle
validation_run = load_pickle(EKF_PICKLE)
validation_input = validation_run['compute_innovations_validation_input']
validation = validation_run['compute_innovations_validation']
scans = validation_run['scans']
T_scans = validation_run['t_scans'].shape[0]
# Initialize EKF localization
ekf_loc = EkfLocalization(validation_run['states'][0],
NoiseParams['Sigma0'], NoiseParams['R'],
params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
for i in range(T_scans):
ekf_loc.x, ekf_loc.Sigma = validation_input[i]
alpha, r, Q_raw, _, _ = ExtractLines(scans[0, i, :], scans[1, i, :],
LineExtractionParams,
NoiseParams['var_theta'],
NoiseParams['var_rho'])
z_raw = np.vstack((alpha, r))
v_list, R_list, H_list = ekf_loc.compute_innovations(z_raw, Q_raw)
v_list_ref, R_list_ref, H_list_ref = validation[i]
if len(v_list) != len(v_list_ref) or len(R_list) != len(
R_list_ref) or len(H_list) != len(H_list_ref):
print(
"You may have an error in EkfLocalization.compute_innovations."
)
print("v", v_list, v_list_ref)
print("R", R_list, R_list_ref)
print("H", H_list, H_list_ref)
return False
permutation = [
np.argmin([np.linalg.norm(R - R_ref) for R_ref in R_list_ref])
for R in R_list
]
v_error = sum([
np.linalg.norm(v_list[j] - v_list_ref[k])
for j, k in enumerate(permutation)
])
R_error = sum([
np.linalg.norm(R_list[j] - R_list_ref[k])
for j, k in enumerate(permutation)
])
H_error = sum([
np.linalg.norm(H_list[j] - H_list_ref[k])
for j, k in enumerate(permutation)
])
if v_error + R_error + H_error > 1e-3:
print(
"You may have an error in EkfLocalization.compute_innovations."
)
print("v", v_list, v_list_ref)
print("R", R_list, R_list_ref)
print("H", H_list, H_list_ref)
return False
print("EkfLocalization.compute_innovations() seems to be correct")
return True
def run(self):
rate = rospy.Rate(100)
while not self.latest_pose:
rate.sleep()
x0_pose = np.array([self.latest_pose.position.x,
self.latest_pose.position.y,
get_yaw_from_quaternion(self.latest_pose.orientation)])
P0_pose = NoiseParams["Sigma0"]
self.EKF_time = self.latest_pose_time
self.EKF = EkfSlam(np.concatenate((x0_pose, self.x0_map)), scipy.linalg.block_diag(P0_pose, self.P0_map),
NoiseParams["R"], self.base_to_camera, 2*NoiseParams["g"])
self.OLC = EkfSlam(np.concatenate((x0_pose, self.x0_map)), scipy.linalg.block_diag(P0_pose, self.P0_map),
NoiseParams["R"], self.base_to_camera, 2*NoiseParams["g"])
while True:
if not self.scans:
rate.sleep()
continue
self.EKF_map_marker.points = []
for j in range((self.EKF.x.size - 3)/2):
p1, p2 = line_endpoints_from_alpha_and_r(self.EKF.x[3+2*j], self.EKF.x[3+2*j+1])
self.EKF_map_marker.points.extend([Point(p1[0], p1[1], 0), Point(p2[0], p2[1], 0)])
self.ground_truth_map_pub.publish(self.ground_truth_map_marker)
self.EKF_map_pub.publish(self.EKF_map_marker)
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
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]),
"EKF", "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)
)
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)
while len(self.scans) > 1: # keep only the last element in the queue, if we're falling behind
self.scans.popleft()