diff --git a/config.yaml b/config.yaml index b0dd9811..44bdd6df 100644 --- a/config.yaml +++ b/config.yaml @@ -34,3 +34,8 @@ geolocation: camera_orientation_yaw: 0.0 camera_orientation_pitch: -1.57079632679 camera_orientation_roll: 0.0 + +cluster_estimation: + min_activation_threshold: 25 + min_new_points_to_run: 5 + random_state: 0 diff --git a/main_2024.py b/main_2024.py index f6297226..1af95726 100644 --- a/main_2024.py +++ b/main_2024.py @@ -19,9 +19,10 @@ from modules.data_merge import data_merge_worker from modules.geolocation import geolocation_worker from modules.geolocation import camera_properties -from modules.common.modules.logger import logger -from modules.common.modules.logger import logger_main_setup -from modules.common.modules.read_yaml import read_yaml +from modules.cluster_estimation import cluster_estimation_worker +from modules.common.logger.modules import logger +from modules.common.logger.modules import logger_setup_main +from modules.common.logger.read_yaml.modules import read_yaml from utilities.workers import queue_proxy_wrapper from utilities.workers import worker_controller from utilities.workers import worker_manager @@ -85,8 +86,10 @@ def main() -> int: VIDEO_INPUT_SAVE_PREFIX = str(pathlib.Path(logging_path, VIDEO_INPUT_SAVE_NAME_PREFIX)) DETECT_TARGET_WORKER_COUNT = config["detect_target"]["worker_count"] - detect_target_option_int = config["detect_target"]["option"] - DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(detect_target_option_int) + DETECT_TARGET_OPTION_INT = config["detect_target"]["option"] + DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(DETECT_TARGET_OPTION_INT) + DETECT_TARGET_OPTION_INT = config["detect_target"]["option"] + DETECT_TARGET_OPTION = detect_target_factory.DetectTargetOption(DETECT_TARGET_OPTION_INT) DETECT_TARGET_DEVICE = "cpu" if args.cpu else config["detect_target"]["device"] DETECT_TARGET_MODEL_PATH = config["detect_target"]["model_path"] DETECT_TARGET_OVERRIDE_FULL_PRECISION = args.full @@ -111,6 +114,14 @@ def main() -> int: GEOLOCATION_CAMERA_ORIENTATION_YAW = config["geolocation"]["camera_orientation_yaw"] GEOLOCATION_CAMERA_ORIENTATION_PITCH = config["geolocation"]["camera_orientation_pitch"] GEOLOCATION_CAMERA_ORIENTATION_ROLL = config["geolocation"]["camera_orientation_roll"] + + MIN_ACTIVATION_THRESHOLD = config["cluster_estimation"]["min_activation_threshold"] + MIN_NEW_POINTS_TO_RUN = config["cluster_estimation"]["min_new_points_to_run"] + RANDOM_STATE = config["cluster_estimation"]["random_state"] + + MIN_ACTIVATION_THRESHOLD = config["cluster_estimation"]["min_activation_threshold"] + MIN_NEW_POINTS_TO_RUN = config["cluster_estimation"]["min_new_points_to_run"] + RANDOM_STATE = config["cluster_estimation"]["random_state"] # pylint: enable=invalid-name except KeyError as exception: main_logger.error(f"Config key(s) not found: {exception}", True) @@ -139,7 +150,11 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) - geolocation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + flight_interface_decision_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) + geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper( mp_manager, QUEUE_MAX_SIZE, ) @@ -147,6 +162,14 @@ def main() -> int: mp_manager, QUEUE_MAX_SIZE, ) + geolocation_to_cluster_estimation_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) + cluster_estimation_to_main_queue = queue_proxy_wrapper.QueueProxyWrapper( + mp_manager, + QUEUE_MAX_SIZE, + ) result, camera_intrinsics = camera_properties.CameraIntrinsics.create( GEOLOCATION_RESOLUTION_X, @@ -228,6 +251,7 @@ def main() -> int: FLIGHT_INTERFACE_WORKER_PERIOD, ), input_queues=[flight_interface_decision_queue], + input_queues=[flight_interface_decision_queue], output_queues=[flight_interface_to_data_merge_queue], controller=controller, local_logger=main_logger, @@ -266,7 +290,7 @@ def main() -> int: camera_extrinsics, ), input_queues=[data_merge_to_geolocation_queue], - output_queues=[geolocation_to_main_queue], + output_queues=[geolocation_to_cluster_estimation_queue], controller=controller, local_logger=main_logger, ) @@ -277,6 +301,22 @@ def main() -> int: # Get Pylance to stop complaining assert geolocation_worker_properties is not None + result, cluster_estimation_worker_properties = worker_manager.WorkerProperties.create( + count=1, + target=cluster_estimation_worker.cluster_estimation_worker, + work_arguments=(MIN_ACTIVATION_THRESHOLD, MIN_NEW_POINTS_TO_RUN, RANDOM_STATE), + input_queues=[geolocation_to_cluster_estimation_queue], + output_queues=[cluster_estimation_to_main_queue], + controller=controller, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create arguments for Video Input", True) + return -1 + + # Get Pylance to stop complaining + assert cluster_estimation_worker_properties is not None + # Create managers worker_managers = [] @@ -345,6 +385,32 @@ def main() -> int: worker_managers.append(geolocation_manager) + result, cluster_estimation_manager = worker_manager.WorkerManager.create( + worker_properties=cluster_estimation_worker_properties, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create manager for Flight Interface", True) + return -1 + + # Get Pylance to stop complaining + assert cluster_estimation_manager is not None + + worker_managers.append(cluster_estimation_manager) + + result, cluster_estimation_manager = worker_manager.WorkerManager.create( + worker_properties=cluster_estimation_worker_properties, + local_logger=main_logger, + ) + if not result: + main_logger.error("Failed to create manager for Flight Interface", True) + return -1 + + # Get Pylance to stop complaining + assert cluster_estimation_manager is not None + + worker_managers.append(cluster_estimation_manager) + # Run for manager in worker_managers: manager.start_workers() @@ -357,9 +423,11 @@ def main() -> int: return -1 try: - geolocation_data = geolocation_to_main_queue.queue.get_nowait() + geolocation_data = geolocation_to_cluster_estimation_queue.queue.get_nowait() + geolocation_data = geolocation_to_cluster_estimation_queue.queue.get_nowait() except queue.Empty: - geolocation_data = None + cluster_estimation_data = None + cluster_estimation_data = None if geolocation_data is not None: for detection_world in geolocation_data: @@ -375,6 +443,16 @@ def main() -> int: "geolocation confidence: " + str(detection_world.confidence), True ) + try: + cluster_estimations = cluster_estimation_to_main_queue.queue.get_nowait() + except queue.Empty: + cluster_estimations = None + if cluster_estimations is not None: + for cluster in cluster_estimations: + main_logger.debug("Cluser in world: True") + main_logger.debug("Cluster location x: " + str(cluster.location_x)) + main_logger.debug("Cluster location y: " + str(cluster.location_y)) + main_logger.debug("Cluster spherical variance: " + str(cluster.spherical_variance)) if cv2.waitKey(1) == ord("q"): # type: ignore main_logger.info("Exiting main loop", True) break @@ -386,8 +464,11 @@ def main() -> int: detect_target_to_data_merge_queue.fill_and_drain_queue() flight_interface_to_data_merge_queue.fill_and_drain_queue() data_merge_to_geolocation_queue.fill_and_drain_queue() - geolocation_to_main_queue.fill_and_drain_queue() + geolocation_to_cluster_estimation_queue.fill_and_drain_queue() + flight_interface_decision_queue.fill_and_drain_queue() + geolocation_to_cluster_estimation_queue.fill_and_drain_queue() flight_interface_decision_queue.fill_and_drain_queue() + cluster_estimation_to_main_queue.fill_and_drain_queue() for manager in worker_managers: manager.join_workers() diff --git a/modules/cluster_estimation/cluster_estimation.py b/modules/cluster_estimation/cluster_estimation.py index 31675f47..2bc5d58f 100644 --- a/modules/cluster_estimation/cluster_estimation.py +++ b/modules/cluster_estimation/cluster_estimation.py @@ -1,6 +1,6 @@ """ Take in bounding box coordinates from Geolocation and use to estimate landing pad locations. -Returns an array of classes, each containing the x coordinate, y coordinate, and spherical +Returns an array of classes, each containing the x coordinate, y coordinate, and spherical covariance of each landing pad estimation. """ @@ -124,7 +124,7 @@ def __init__( def run( self, detections: "list[detection_in_world.DetectionInWorld]", run_override: bool - ) -> "tuple[bool, list[object_in_world.ObjectInWorld] | None]": + ) -> "tuple[bool, object_in_world.ObjectInWorld | None]": """ Take in list of landing pad detections and return list of estimated landing pad locations if number of detections is sufficient, or if manually forced to run. @@ -193,14 +193,16 @@ def run( # Create output list of remaining valid clusters detections_in_world = [] for cluster in model_output: - result, landing_pad = object_in_world.ObjectInWorld.create( + + result, temp_object = object_in_world.ObjectInWorld.create( cluster[0][0], cluster[0][1], cluster[2], + 0, ) if result: - detections_in_world.append(landing_pad) + detections_in_world.append(temp_object) return True, detections_in_world diff --git a/modules/object_in_world.py b/modules/object_in_world.py index 1ce9f65e..0dc413c6 100644 --- a/modules/object_in_world.py +++ b/modules/object_in_world.py @@ -12,16 +12,19 @@ class ObjectInWorld: @classmethod def create( - cls, location_x: float, location_y: float, spherical_variance: float + cls, location_x: float, location_y: float, spherical_variance: float, label: int ) -> "tuple[bool, ObjectInWorld | None]": """ location_x, location_y: Location of the object. spherical_variance: Uncertainty of the location. + label: type of object in real world. """ if spherical_variance < 0.0: return False, None - return True, ObjectInWorld(cls.__create_key, location_x, location_y, spherical_variance) + return True, ObjectInWorld( + cls.__create_key, location_x, location_y, spherical_variance, label + ) def __init__( self, @@ -29,6 +32,7 @@ def __init__( location_x: float, location_y: float, spherical_variance: float, + label: int, ) -> None: """ Private constructor, use create() method. @@ -38,3 +42,4 @@ def __init__( self.location_x = location_x self.location_y = location_y self.spherical_variance = spherical_variance + self.label = label diff --git a/tests/unit/test_decision.py b/tests/unit/test_decision.py index 3b3cf68f..ce9192ea 100644 --- a/tests/unit/test_decision.py +++ b/tests/unit/test_decision.py @@ -43,7 +43,9 @@ def best_pad_within_tolerance() -> object_in_world.ObjectInWorld: # type: ignor location_x = BEST_PAD_LOCATION_X location_y = BEST_PAD_LOCATION_Y spherical_variance = 1.0 - result, pad = object_in_world.ObjectInWorld.create(location_x, location_y, spherical_variance) + result, pad = object_in_world.ObjectInWorld.create( + location_x, location_y, spherical_variance, 0 + ) assert result assert pad is not None @@ -58,7 +60,9 @@ def best_pad_outside_tolerance() -> object_in_world.ObjectInWorld: # type: igno location_x = 100.0 location_y = 200.0 spherical_variance = 5.0 # variance outside tolerance - result, pad = object_in_world.ObjectInWorld.create(location_x, location_y, spherical_variance) + result, pad = object_in_world.ObjectInWorld.create( + location_x, location_y, spherical_variance, 0 + ) assert result assert pad is not None @@ -70,15 +74,15 @@ def pads() -> "list[object_in_world.ObjectInWorld]": # type: ignore """ Create a list of ObjectInWorld instances for the landing pads. """ - result, pad_1 = object_in_world.ObjectInWorld.create(30.0, 40.0, 2.0) + result, pad_1 = object_in_world.ObjectInWorld.create(30.0, 40.0, 2.0, 0) assert result assert pad_1 is not None - result, pad_2 = object_in_world.ObjectInWorld.create(50.0, 60.0, 3.0) + result, pad_2 = object_in_world.ObjectInWorld.create(50.0, 60.0, 3.0, 0) assert result assert pad_2 is not None - result, pad_3 = object_in_world.ObjectInWorld.create(70.0, 80.0, 4.0) + result, pad_3 = object_in_world.ObjectInWorld.create(70.0, 80.0, 4.0, 0) assert result assert pad_3 is not None diff --git a/tests/unit/test_landing_pad_tracking.py b/tests/unit/test_landing_pad_tracking.py index fd6fce60..57793bf3 100644 --- a/tests/unit/test_landing_pad_tracking.py +++ b/tests/unit/test_landing_pad_tracking.py @@ -30,23 +30,23 @@ def detections_1() -> "list[object_in_world.ObjectInWorld]": # type: ignore """ Sample instances of ObjectInWorld for testing. """ - result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 8) + result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 8, 0) assert result assert obj_1 is not None - result, obj_2 = object_in_world.ObjectInWorld.create(2, 2, 4) + result, obj_2 = object_in_world.ObjectInWorld.create(2, 2, 4, 0) assert result assert obj_2 is not None - result, obj_3 = object_in_world.ObjectInWorld.create(-2, -2, 2) + result, obj_3 = object_in_world.ObjectInWorld.create(-2, -2, 2, 0) assert result assert obj_3 is not None - result, obj_4 = object_in_world.ObjectInWorld.create(3, 3, 10) + result, obj_4 = object_in_world.ObjectInWorld.create(3, 3, 10, 0) assert result assert obj_4 is not None - result, obj_5 = object_in_world.ObjectInWorld.create(-3, -3, 6) + result, obj_5 = object_in_world.ObjectInWorld.create(-3, -3, 6, 0) assert result assert obj_5 is not None @@ -59,23 +59,23 @@ def detections_2() -> "list[object_in_world.ObjectInWorld]": # type: ignore """ Sample instances of ObjectInWorld for testing. """ - result, obj_1 = object_in_world.ObjectInWorld.create(0.5, 0.5, 1) + result, obj_1 = object_in_world.ObjectInWorld.create(0.5, 0.5, 1, 0) assert result assert obj_1 is not None - result, obj_2 = object_in_world.ObjectInWorld.create(1.5, 1.5, 3) + result, obj_2 = object_in_world.ObjectInWorld.create(1.5, 1.5, 3, 0) assert result assert obj_2 is not None - result, obj_3 = object_in_world.ObjectInWorld.create(4, 4, 7) + result, obj_3 = object_in_world.ObjectInWorld.create(4, 4, 7, 0) assert result assert obj_3 is not None - result, obj_4 = object_in_world.ObjectInWorld.create(-4, -4, 5) + result, obj_4 = object_in_world.ObjectInWorld.create(-4, -4, 5, 0) assert result assert obj_4 is not None - result, obj_5 = object_in_world.ObjectInWorld.create(5, 5, 9) + result, obj_5 = object_in_world.ObjectInWorld.create(5, 5, 9, 0) assert result assert obj_5 is not None @@ -88,23 +88,23 @@ def detections_3() -> "list[object_in_world.ObjectInWorld]": # type: ignore """ Sample instances of ObjectInWorld for testing. """ - result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 8) + result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 8, 0) assert result assert obj_1 is not None - result, obj_2 = object_in_world.ObjectInWorld.create(0.5, 0.5, 4) + result, obj_2 = object_in_world.ObjectInWorld.create(0.5, 0.5, 4, 0) assert result assert obj_2 is not None - result, obj_3 = object_in_world.ObjectInWorld.create(-2, -2, 2) + result, obj_3 = object_in_world.ObjectInWorld.create(-2, -2, 2, 0) assert result assert obj_3 is not None - result, obj_4 = object_in_world.ObjectInWorld.create(3, 3, 10) + result, obj_4 = object_in_world.ObjectInWorld.create(3, 3, 10, 0) assert result assert obj_4 is not None - result, obj_5 = object_in_world.ObjectInWorld.create(-3, -3, 6) + result, obj_5 = object_in_world.ObjectInWorld.create(-3, -3, 6, 0) assert result assert obj_5 is not None @@ -123,11 +123,11 @@ def test_is_similar_positive_equal_to_threshold(self) -> None: Test case where the second landing pad has positive coordinates and the distance between them is equal to the distance threshold. """ - result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0) + result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0, 0) assert result assert obj_1 is not None - result, obj_2 = object_in_world.ObjectInWorld.create(1, 1, 0) + result, obj_2 = object_in_world.ObjectInWorld.create(1, 1, 0, 0) assert result assert obj_2 is not None @@ -145,11 +145,11 @@ def test_is_similar_negative_equal_to_threshold(self) -> None: Test case where the second landing pad has negative coordinates and the distance between them is equal to the distance threshold. """ - result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0) + result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0, 0) assert result assert obj_1 is not None - result, obj_2 = object_in_world.ObjectInWorld.create(-1, -1, 0) + result, obj_2 = object_in_world.ObjectInWorld.create(-1, -1, 0, 0) assert result assert obj_2 is not None @@ -168,11 +168,11 @@ def test_is_similar_positive_less_than_threshold(self) -> None: Test case where the second landing pad has positive coordinates and the distance between them is less than the distance threshold. """ - result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0) + result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0, 0) assert result assert obj_1 is not None - result, obj_2 = object_in_world.ObjectInWorld.create(0.5, 0.5, 0) + result, obj_2 = object_in_world.ObjectInWorld.create(0.5, 0.5, 0, 0) assert result assert obj_2 is not None @@ -191,11 +191,11 @@ def test_is_similar_negative_less_than_threshold(self) -> None: Test case where the second landing pad has negative coordinates and the distance between them is less than the distance threshold. """ - result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0) + result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0, 0) assert result assert obj_1 is not None - result, obj_2 = object_in_world.ObjectInWorld.create(-0.5, -0.5, 0) + result, obj_2 = object_in_world.ObjectInWorld.create(-0.5, -0.5, 0, 0) assert result assert obj_2 is not None @@ -214,11 +214,11 @@ def test_is_similar_positive_more_than_threshold(self) -> None: Test case where the second landing pad has positive coordinates and the distance between them is more than the distance threshold. """ - result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0) + result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0, 0) assert result assert obj_1 is not None - result, obj_2 = object_in_world.ObjectInWorld.create(2, 2, 0) + result, obj_2 = object_in_world.ObjectInWorld.create(2, 2, 0, 0) assert result assert obj_2 is not None @@ -237,11 +237,11 @@ def test_is_similar_negative_more_than_threshold(self) -> None: Test case where the second landing pad has negative coordinates and the distance between them is more than the distance threshold. """ - result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0) + result, obj_1 = object_in_world.ObjectInWorld.create(0, 0, 0, 0) assert result assert obj_1 is not None - result, obj_2 = object_in_world.ObjectInWorld.create(-2, -2, 0) + result, obj_2 = object_in_world.ObjectInWorld.create(-2, -2, 0, 0) assert result assert obj_2 is not None @@ -269,7 +269,7 @@ def test_mark_false_positive_no_similar( """ Test if marking false positive adds detection to list of false positives. """ - _, false_positive = object_in_world.ObjectInWorld.create(20, 20, 20) + _, false_positive = object_in_world.ObjectInWorld.create(20, 20, 20, 0) assert false_positive is not None tracker._LandingPadTracking__unconfirmed_positives = detections_1 # type: ignore @@ -296,7 +296,7 @@ def test_mark_false_positive_with_similar( Test if marking false positive adds detection to list of false positives and removes. similar landing pads """ - _, false_positive = object_in_world.ObjectInWorld.create(1, 1, 1) + _, false_positive = object_in_world.ObjectInWorld.create(1, 1, 1, 0) assert false_positive is not None tracker._LandingPadTracking__unconfirmed_positives = detections_2 # type: ignore @@ -316,10 +316,10 @@ def test_mark_multiple_false_positive( """ Test if marking false positive adds detection to list of false positives. """ - _, false_positive_1 = object_in_world.ObjectInWorld.create(0, 0, 1) + _, false_positive_1 = object_in_world.ObjectInWorld.create(0, 0, 1, 0) assert false_positive_1 is not None - _, false_positive_2 = object_in_world.ObjectInWorld.create(2, 2, 1) + _, false_positive_2 = object_in_world.ObjectInWorld.create(2, 2, 1, 0) assert false_positive_2 is not None tracker._LandingPadTracking__unconfirmed_positives = detections_1 # type: ignore @@ -344,7 +344,7 @@ def test_mark_confirmed_positive( """ Test if marking confirmed positive adds detection to list of confirmed positives. """ - _, confirmed_positive = object_in_world.ObjectInWorld.create(1, 1, 1) + _, confirmed_positive = object_in_world.ObjectInWorld.create(1, 1, 1, 0) assert confirmed_positive is not None expected = [confirmed_positive] @@ -359,10 +359,10 @@ def test_mark_multiple_confirmed_positives( """ Test if marking confirmed positive adds detection to list of confirmed positives. """ - _, confirmed_positive_1 = object_in_world.ObjectInWorld.create(1, 1, 1) + _, confirmed_positive_1 = object_in_world.ObjectInWorld.create(1, 1, 1, 0) assert confirmed_positive_1 is not None - _, confirmed_positive_2 = object_in_world.ObjectInWorld.create(2, 2, 1) + _, confirmed_positive_2 = object_in_world.ObjectInWorld.create(2, 2, 1, 0) assert confirmed_positive_2 is not None expected = [confirmed_positive_1, confirmed_positive_2]