current_image = "" while index < len(way_points): if fake_timestamp >= way_points[index].time.timestamp(): if way_points[index].latitude > 0: utm_data = utm.from_latlon(way_points[index].latitude, way_points[index].longitude) video_kalman.update_estimations(np.array(utm_data[:2]).T) print(way_points[index]) index += 1 success, image = video.read() if not success: print("Error in reading video!", file=sys.stderr) break cv.imshow("Good Weather", image) video_kalman.predict_values() zone_num, zone_letter = utm_data[2], utm_data[3] lat, long = utm.to_latlon(video_kalman.predicted_data[0], video_kalman.predicted_data[1], zone_num, zone_letter) nearest_data = geographic_db.retrieve_nearest_point(long, lat) if nearest_data is not None: # print(nearest_data["distance"]) if current_image != nearest_data["image_path"]: current_image = nearest_data["image_path"] threading.Thread(target=image_load_job, args=(current_image, )).start() if near_image is not None: cv.imshow("Bad Weather", near_image) cv.waitKey(1) clock.tick(target_fps)
def update_using_kalman(kalman: KalmanFilter, metadata): output_data = [] last_point = metadata[0] last_utm = utm.from_latlon(last_point.get("latitude", None), last_point.get("longitude", None)) start_time = last_point['time'] similar_data_count = 1 for data in metadata[1:]: if data["time"] != last_point["time"]: data_utm = utm.from_latlon(data.get("latitude", None), data.get("longitude", None)) if data["speed"] is not None: speed_vec = normalize_vec( np.array([data_utm[0], data_utm[1]]) - np.array(last_utm)) * data["speed"] new_data = np.array( [data_utm[0], data_utm[1], speed_vec[0], speed_vec[1]]).T else: new_data = np.array([data_utm[0], data_utm[1]]).T last_utm = data_utm[:2] if data["accuracy"] is not None: noise_vec = [data["accuracy"] * 0.71, data["accuracy"] * 0.71] else: noise_vec = None kalman.update_estimations(new_data=new_data, measurement_noise=noise_vec, estimate_time=data["time"]) last_point = data print(last_point['time'] - start_time, similar_data_count, data.get("latitude", None), data.get("longitude", None)) similar_data_count = 1 predicted_data = kalman.predicted_data predicted_utm = predicted_data[:2] predicted_lat_lng = utm.to_latlon(predicted_utm[0], predicted_utm[1], data_utm[2], data_utm[3]) output_data.append({ 'latitude': predicted_lat_lng[0], 'longitude': predicted_lat_lng[1], 'path': data['path'] }) else: new_time = data['time'] + 0.25 * similar_data_count data_utm = utm.from_latlon(data.get("latitude", None), data.get("longitude", None)) last_utm = data_utm[:2] kalman.predict_values(new_time) predicted_data = kalman.predicted_data predicted_utm = predicted_data[:2] predicted_lat_lng = utm.to_latlon(predicted_utm[0], predicted_utm[1], data_utm[2], data_utm[3]) output_data.append({ 'latitude': predicted_lat_lng[0], 'longitude': predicted_lat_lng[1], 'path': data['path'] }) similar_data_count += 1 return output_data