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