diff options
Diffstat (limited to 'src/argaze/ArUcoMarkers/ArUcoCamera.py')
| -rw-r--r-- | src/argaze/ArUcoMarkers/ArUcoCamera.py | 83 |
1 files changed, 42 insertions, 41 deletions
diff --git a/src/argaze/ArUcoMarkers/ArUcoCamera.py b/src/argaze/ArUcoMarkers/ArUcoCamera.py index 95df135..5b535b5 100644 --- a/src/argaze/ArUcoMarkers/ArUcoCamera.py +++ b/src/argaze/ArUcoMarkers/ArUcoCamera.py @@ -79,12 +79,7 @@ class ArUcoCamera(ArFeatures.ArCamera): # Create default optic parameters adapted to frame size # Note: The choice of 1000 for default focal length should be discussed... - self.__aruco_detector.optic_parameters = ArUcoOpticCalibrator.OpticParameters(rms=-1, dimensions=self.size, - K=ArUcoOpticCalibrator.K0( - focal_length=( - 1000., 1000.), - width=self.size[0], - height=self.size[1])) + self.__aruco_detector.optic_parameters = ArUcoOpticCalibrator.OpticParameters(rms=-1, dimensions=self.size, K=ArUcoOpticCalibrator.K0(focal_length=(1000., 1000.), width=self.size[0], height=self.size[1])) # Edit parent if self.__aruco_detector is not None: @@ -134,64 +129,70 @@ class ArUcoCamera(ArFeatures.ArCamera): cv2.rectangle(image, (0, 0), (self.__sides_mask, height), (0, 0, 0), -1) cv2.rectangle(image, (width - self.__sides_mask, 0), (width, height), (0, 0, 0), -1) - # Detect aruco markers - logging.debug('\t> detect markers') - - self.__aruco_detector.detect_markers(image) - # Fill camera frame background with timestamped image self.background = image - # Clear former layers projection into camera frame - for layer_name, layer in self.layers.items(): + # Read projection from the cache if required + if not self._read_projection_cache(image.timestamp): + + # Detect aruco markers + logging.debug('\t> detect markers') + + self.__aruco_detector.detect_markers(image) - # Initialize layer if needed - if layer.aoi_scene is None: + # Clear former layers projection into camera frame + self._clear_projection() - layer.aoi_scene = AOI2DScene.AOI2DScene() + # Project each aoi 3d scene into camera frame + for scene_name, scene in self.scenes.items(): - else: + ''' TODO: Enable aruco_aoi processing + if scene.aruco_aoi: - layer.aoi_scene.clear() + try: - # Project each aoi 3d scene into camera frame - for scene_name, scene in self.scenes.items(): + # Build AOI scene directly from detected ArUco marker corners + self.layers[??].aoi_2d_scene |= scene.build_aruco_aoi_scene(self.__aruco_detector.detected_markers()) - ''' TODO: Enable aruco_aoi processing - if scene.aruco_aoi: + except ArFeatures.PoseEstimationFailed: + + pass + ''' + + # Estimate scene pose from detected scene markers + logging.debug('\t> estimate %s scene pose', scene_name) try: - # Build AOI scene directly from detected ArUco marker corners - self.layers[??].aoi_2d_scene |= scene.build_aruco_aoi_scene(self.__aruco_detector.detected_markers()) + tvec, rmat, _ = scene.estimate_pose(self.__aruco_detector.detected_markers(), timestamp=image.timestamp) - except ArFeatures.PoseEstimationFailed: + # Project scene into camera frame according estimated pose + for layer_name, layer_projection in scene.project(tvec, rmat, self.visual_hfov, self.visual_vfov, timestamp=image.timestamp): - pass - ''' + logging.debug('\t> project %s scene %s layer', scene_name, layer_name) - # Estimate scene pose from detected scene markers - logging.debug('\t> estimate %s scene pose', scene_name) + try: - tvec, rmat, _ = scene.estimate_pose(self.__aruco_detector.detected_markers(), timestamp=self.timestamp) + # Update camera layer aoi + self.layers[layer_name].aoi_scene |= layer_projection - # Project scene into camera frame according estimated pose - for layer_name, layer_projection in scene.project(tvec, rmat, self.visual_hfov, self.visual_vfov, - timestamp=self.timestamp): + # Timestamp camera layer + self.layers[layer_name].timestamp = image.timestamp - logging.debug('\t> project %s scene %s layer', scene_name, layer_name) + except KeyError: - try: + pass - # Update camera layer aoi - self.layers[layer_name].aoi_scene |= layer_projection + # Write projection into the cache if required + self._write_projection_cache(image.timestamp) - # Timestamp camera layer - self.layers[layer_name].timestamp = self.timestamp + except DataFeatures.TimestampedException as e: - except KeyError: + # Write exception into the cache if required + self._write_projection_cache(image.timestamp, e) - pass + # Raise exception + raise e @DataFeatures.PipelineStepImage def image(self, draw_detected_markers: dict = None, draw_scenes: dict = None, |