From ab07a0171717f370a27b0ef179731367aab151fc Mon Sep 17 00:00:00 2001 From: Pascal Schade Date: Fri, 6 Sep 2024 15:13:44 +0200 Subject: [PATCH] fix 3d plant detection --- field_friend/automations/plant_locator.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/field_friend/automations/plant_locator.py b/field_friend/automations/plant_locator.py index 5546e0cb..96e38057 100644 --- a/field_friend/automations/plant_locator.py +++ b/field_friend/automations/plant_locator.py @@ -106,14 +106,17 @@ async def _detect_plants(self) -> None: if isinstance(camera, CalibratableUsbCamera): world_point_3d = camera.calibration.project_from_image(image_point) elif isinstance(camera, StereoCamera): - camera_point_3d: Point3d | None = camera.get_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_point_3d = camera_point_3d.in_frame(self.odometer.prediction_frame) - world_point_3d = camera_point_3d.resolve() - self.log.info(f'camera_point_3d: {camera_point_3d} -> world_point_3d: {world_point_3d.tuple}') + 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() + # rosys_1 | 2024-09-06 12:57:03.962 [INFO] field_friend/automations/plant_locator.py:130: camera_point_3d: Point3d(0.061, -0.002, 0.469) -> world_point_3d: (array([0.14884793]), array([-0.02568044]), array([0.46912057])) + # rosys_1 | 2024-09-06 12:57:04.002 [INFO] field_friend/automations/plant_locator.py:130: camera_point_3d: Point3d(0.006, -0.073, 0.525) -> world_point_3d: (array([0.09310573]), array([-0.09459265]), array([0.52506744])) + # self.log.info(f'camera_point_3d: {camera_point_3d} -> world_point_3d: {world_point_3d.tuple}') if world_point_3d is None: self.log.error('could not generate world point of detection, calibration error') continue @@ -127,6 +130,7 @@ async def _detect_plants(self) -> None: 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.log.info(f'camera_point_3d: {camera_point_3d} -> world_point_3d: {world_point_3d.tuple}') self.plant_provider.add_crop(plant) 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')