def validate_localization_compute_predicted_measurements():
# Load pickle
validation_run = load_pickle(EKF_PICKLE)
validation = validation_run['compute_predicted_measurements_validation']
# Initialize EKF localization
ekf_loc = EkfLocalization(validation_run['states'][0],
NoiseParams['Sigma0'], NoiseParams['R'],
params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
# Compare measurements
hs, Hs = ekf_loc.compute_predicted_measurements()
for j in range(ekf_loc.map_lines.shape[1]):
h, Hx = hs[:, j], Hs[j]
h_ref, Hx_ref = validation[j]
if np.linalg.norm(h - h_ref) + np.linalg.norm(Hx - Hx_ref) > 1e-3:
print(
"At state x = {0} with m = {1} got EkfLocalization.compute_predicted_measurements:\n"
.format(ekf_loc.x, ekf_loc.map_lines[:, j]))
print(h)
print(Hx)
print("\nvs. the expected values\n")
print(h_ref)
print(Hx_ref)
return False
print(
"EkfLocalization.compute_predicted_measurements() seems to be correct")
return True
def validate_localization_transition_model():
# Load pickle
validation_run = load_pickle(EKF_PICKLE)
u = validation_run['controls']
T = validation_run['t'].shape[0]
validation = validation_run['transition_model_validation']
# Initialize EKF localization
ekf_loc = EkfLocalization(validation_run['states'][0],
NoiseParams['Sigma0'], NoiseParams['R'],
params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
# Simulate controls
for i in range(T):
g, Gx, Gu = ekf_loc.transition_model(u[i], 0.1)
g_ref, Gx_ref, Gu_ref = validation[i]
if np.linalg.norm(g - g_ref) + np.linalg.norm(
Gx - Gx_ref) + np.linalg.norm(Gu - Gu_ref) > 1e-2:
print(
"At state x = {0} with u = {1} and dt = {2} got EkfLocalization.transition_model output:\n"
.format(ekf_loc.x, u, 0.1))
print(g)
print(Gx)
print(Gu)
print("\nvs. the expected values\n")
print(g_ref)
print(Gx_ref)
print(Gu_ref)
return False
print("EkfLocalization.transition_model() seems to be correct")
return True
def validate_ekf_transition_update(show_plot=True):
# Load pickle
validation_run = load_pickle(EKF_PICKLE)
ground_truth_states = validation_run['states']
u = validation_run['controls']
dt = validation_run['t'][1:] - validation_run['t'][:-1]
T = validation_run['t'].shape[0]
# Initialize EKF localization
ekf_loc = EkfLocalization(ground_truth_states[0], NoiseParams['Sigma0'],
NoiseParams['R'], params.MapParams,
validation_run['tf_base_to_camera'],
NoiseParams['g'])
# Simulate controls
open_loop_states = np.zeros((T, ekf_loc.x.shape[0]))
open_loop_states[0] = ekf_loc.x
for i in range(T - 1):
ekf_loc.transition_update(u[i], dt[i])
open_loop_states[i + 1] = ekf_loc.x
# Plot
plt.clf()
plt.plot(ground_truth_states[:, 0],
ground_truth_states[:, 1],
label="ground truth",
color="black")
plt.plot(open_loop_states[:, 0],
open_loop_states[:, 1],
label="open loop",
color="green")
plt.axis("equal")
if show_plot:
plt.legend(loc=0)
plt.savefig("ekf_open_loop.png")
print("Plot saved to ekf_open_loop.png")
class LocalizationVisualizer:
def __init__(self):
rospy.init_node('turtlebot_localization')
self.params = LocalizationParams(verbose=True)
## Use simulation time (i.e. get time from rostopic /clock)
rospy.set_param('use_sim_time', 'true')
rate = rospy.Rate(10)
while rospy.Time.now() == rospy.Time(0):
rate.sleep()
## Get transformation of camera frame with respect to the base frame
self.tfBuffer = tf2_ros.Buffer()
self.tfListener = tf2_ros.TransformListener(self.tfBuffer)
while True:
try:
# notably camera_link and not camera_depth_frame below, not sure why
self.raw_base_to_camera = self.tfBuffer.lookup_transform(
"base_footprint", "base_scan", rospy.Time()).transform
break
except tf2_ros.LookupException:
rate.sleep()
rotation = self.raw_base_to_camera.rotation
translation = self.raw_base_to_camera.translation
tf_theta = get_yaw_from_quaternion(rotation)
self.base_to_camera = [translation.x, translation.y, tf_theta]
## Colocate the `ground_truth` and `base_footprint` frames for visualization purposes
tf2_ros.StaticTransformBroadcaster().sendTransform(
create_transform_msg((0, 0, 0), (0, 0, 0, 1), "ground_truth",
"base_footprint"))
## Initial state for EKF
self.EKF = None
self.EKF_time = None
self.current_control = np.zeros(2)
self.latest_pose = None
self.latest_pose_time = None
self.controls = deque()
self.scans = deque()
## Set up publishers and subscribers
self.tfBroadcaster = tf2_ros.TransformBroadcaster()
rospy.Subscriber('/scan', LaserScan, self.scan_callback)
rospy.Subscriber('/cmd_vel', Twist, self.control_callback)
rospy.Subscriber('/gazebo/model_states', ModelStates,
self.state_callback)
self.ground_truth_ct = 0
if self.params.mc:
self.particles_pub = rospy.Publisher('particle_filter',
PointCloud,
queue_size=10)
def scan_callback(self, msg):
if self.EKF:
self.scans.append((msg.header.stamp,
np.array([
i * msg.angle_increment + msg.angle_min
for i in range(len(msg.ranges))
]), np.array(msg.ranges)))
def control_callback(self, msg):
if self.EKF:
self.controls.append(
(rospy.Time.now(), np.array([msg.linear.x, msg.angular.z])))
def state_callback(self, msg):
self.ground_truth_ct = self.ground_truth_ct + 1
# `rostopic hz /gazebo/model_states` = 1000; let's broadcast the transform at 20Hz to reduce lag
if self.ground_truth_ct % 50 == 0:
self.latest_pose_time = rospy.Time.now()
self.latest_pose = msg.pose[msg.name.index("turtlebot3_burger")]
self.tfBroadcaster.sendTransform(
create_transform_msg(
(self.latest_pose.position.x, self.latest_pose.position.y,
0), (self.latest_pose.orientation.x,
self.latest_pose.orientation.y,
self.latest_pose.orientation.z,
self.latest_pose.orientation.w), "base_footprint",
"world", self.latest_pose_time))
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 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_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_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