From 5f78ea82ea808ae4d7a423003af95ce085b23c85 Mon Sep 17 00:00:00 2001
From: Pascal Schade <pascal@zauberzeug.com>
Date: Tue, 24 Sep 2024 00:51:24 +0200
Subject: [PATCH] plant locator cleanup

---
 field_friend/automations/plant_locator.py | 28 +++++++++--------------
 1 file changed, 11 insertions(+), 17 deletions(-)

diff --git a/field_friend/automations/plant_locator.py b/field_friend/automations/plant_locator.py
index d82c407d..b8614402 100644
--- a/field_friend/automations/plant_locator.py
+++ b/field_friend/automations/plant_locator.py
@@ -103,23 +103,21 @@ async def _detect_plants(self) -> None:
                     continue
             image_point = rosys.geometry.Point(x=d.cx, y=d.cy)
             world_point_3d: rosys.geometry.Point3d | None = None
-            if isinstance(camera, CalibratableUsbCamera) or isinstance(camera, StereoCamera):
+            if isinstance(camera, StereoCamera):
+                world_point_3d = camera.calibration.project_from_image(image_point)
+                # camera_point_3d: Point3d | None = await camera.get_point(
+                #     int(d.cx), int(d.cy))
+                # if camera_point_3d is None:
+                #     self.log.error('could not get a depth value for detection')
+                #     continue
+                # camera.calibration.extrinsics = camera.calibration.extrinsics.as_frame(
+                #     'zedxmini').in_frame(self.odometer.prediction_frame)
+                # world_point_3d = camera_point_3d.in_frame(camera.calibration.extrinsics).resolve()
+            else:
                 world_point_3d = camera.calibration.project_from_image(image_point)
-            # elif isinstance(camera, StereoCamera):
-            #     camera_point_3d: Point3d | None = await camera.get_point(
-            #         int(d.cx), int(d.cy))
-            #     if camera_point_3d is None:
-            #         self.log.error('could not get a depth value for detection')
-            #         continue
-            #     camera.calibration.extrinsics = camera.calibration.extrinsics.as_frame(
-            #         'zedxmini').in_frame(self.odometer.prediction_frame)
-            #     world_point_3d = camera_point_3d.in_frame(camera.calibration.extrinsics).resolve()
             if world_point_3d is None:
                 self.log.error('could not generate world point of detection, calibration error')
                 continue
-            if abs(world_point_3d.y) >= 0.11:
-                continue
-            world_point_3d.z = 0.0
             plant = Plant(type=d.category_name,
                           detection_time=rosys.time(),
                           detection_image=new_image)
@@ -127,14 +125,10 @@ async def _detect_plants(self) -> None:
             plant.confidences.append(d.confidence)
             if d.category_name in self.weed_category_names and d.confidence >= self.minimum_weed_confidence:
                 # self.log.info('weed found')
-                # self.log.info(f'image_point: {image_point} -> world_point_3d: {world_point_3d}')
                 await self.plant_provider.add_weed(plant)
             elif d.category_name in self.crop_category_names and d.confidence >= self.minimum_crop_confidence:
                 # self.log.info(f'{d.category_name} found')
                 self.plant_provider.add_crop(plant)
-                # self.log.info(f'image_point: {image_point} -> world_point_3d: {world_point_3d}')
-                # test_point = camera.calibration.project_to_image(Point3d(x=0.0, y=0.0, z=0.0))
-                # self.log.info(f'test_point: {test_point}')
             elif d.category_name not in self.crop_category_names and d.category_name not in self.weed_category_names:
                 self.log.info(f'{d.category_name} not in categories')
             # else: