From 4257f477e62b1b22db2c454d3a38097e07fda9c1 Mon Sep 17 00:00:00 2001 From: Marc Pulte Date: Fri, 19 Jan 2024 19:36:38 -0500 Subject: [PATCH 1/4] Removed unused packages from requirements --- requirements.txt | 38 -------------------------------------- 1 file changed, 38 deletions(-) diff --git a/requirements.txt b/requirements.txt index c270e07..9be4604 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,46 +1,8 @@ -appnope==0.1.3 -asttokens==2.4.1 -black==23.12.1 -click==8.1.7 -comm==0.2.1 -contourpy==1.2.0 -cycler==0.12.1 -debugpy==1.8.0 -decorator==5.1.1 -executing==2.0.1 -fonttools==4.47.0 -ipykernel==6.28.0 -ipython==8.19.0 -jedi==0.19.1 -jupyter_client==8.6.0 -jupyter_core==5.7.0 -kiwisolver==1.4.5 -matplotlib-inline==0.1.6 -mypy-extensions==1.0.0 -nest-asyncio==1.5.8 numpy==1.26.3 opencv-contrib-python==4.7.0.68 opencv-python==4.7.0.68 -packaging==23.2 -parso==0.8.3 -pathspec==0.12.1 -pexpect==4.9.0 pillow==10.2.0 -platformdirs==4.1.0 -prompt-toolkit==3.0.43 -psutil==5.9.7 -ptyprocess==0.7.0 -pure-eval==0.2.2 -Pygments==2.17.2 pyntcore==2024.1.1.1 -pyparsing==3.1.1 -python-dateutil==2.8.2 -pyzmq==25.1.2 robotpy-wpimath==2024.1.1.1 robotpy-wpinet==2024.1.1.1 robotpy-wpiutil==2024.1.1.1 -six==1.16.0 -stack-data==0.6.3 -tornado==6.4 -traitlets==5.14.1 -wcwidth==0.2.12 From ff387a67787a29c5d2058892423290d04c681240 Mon Sep 17 00:00:00 2001 From: Marc Pulte Date: Sun, 21 Jan 2024 10:19:55 -0500 Subject: [PATCH 2/4] Added nt output of packet with targeting info --- config/ConfigSource.py | 5 +- main.py | 10 +-- output/OutputPublisher.py | 76 ++++++++++++++++++- output/Packet.py | 45 +++++++++++ output/StreamServer.py | 6 +- pipeline/CameraPoseEstimator.py | 129 +++++++++++++++++--------------- pipeline/FiducialDetector.py | 5 +- requirements.txt | 3 +- vision_types.py | 5 +- 9 files changed, 203 insertions(+), 81 deletions(-) create mode 100644 output/Packet.py diff --git a/config/ConfigSource.py b/config/ConfigSource.py index feabc8c..0a3b692 100644 --- a/config/ConfigSource.py +++ b/config/ConfigSource.py @@ -67,8 +67,11 @@ def update(self, config_store: ConfigStore) -> None: "camera_gain").subscribe(RemoteConfig.camera_gain) self._fiducial_size_m_sub = nt_table.getDoubleTopic( "fiducial_size_m").subscribe(RemoteConfig.fiducial_size_m) - self._tag_layout_sub = nt_table.getStringTopic( + + global_config_table = ntcore.NetworkTableInstance.getDefault().getTable("CubStar/config") + self._tag_layout_sub = global_config_table.getStringTopic( "tag_layout").subscribe("") + self._init_complete = True # Read config data diff --git a/main.py b/main.py index 7da50b3..8105764 100644 --- a/main.py +++ b/main.py @@ -10,7 +10,7 @@ from calibration.CalibrationSession import CalibrationSession from config.config import ConfigStore, LocalConfig, RemoteConfig from config.ConfigSource import ConfigSource, FileConfigSource, NTConfigSource -from output.OutputPublisher import NTOutputPublisher, OutputPublisher +from output.OutputPublisher import NTPacketPublisher, OutputPublisher from output.overlay_util import * from output.StreamServer import MjpegServer from pipeline.CameraPoseEstimator import MultiTargetCameraPoseEstimator @@ -30,7 +30,7 @@ fiducial_detector = ArucoFiducialDetector(cv2.aruco.DICT_APRILTAG_36h11) camera_pose_estimator = MultiTargetCameraPoseEstimator() tag_pose_estimator = SquareTargetPoseEstimator() - output_publisher: OutputPublisher = NTOutputPublisher() + output_publisher: OutputPublisher = NTPacketPublisher() stream_server = MjpegServer() calibration_session = CalibrationSession() @@ -77,15 +77,15 @@ # Normal mode image_observations = fiducial_detector.detect_fiducials(image, config) [overlay_image_observation(image, x) for x in image_observations] - camera_pose_observation = camera_pose_estimator.solve_camera_pose( + fiducial_pose_observations, camera_pose_observation = camera_pose_estimator.solve_camera_pose( [x for x in image_observations if x.tag_id != DEMO_ID], config) demo_image_observations = [x for x in image_observations if x.tag_id == DEMO_ID] demo_pose_observation: Union[FiducialPoseObservation, None] = None if len(demo_image_observations) > 0: demo_pose_observation = tag_pose_estimator.solve_fiducial_pose(demo_image_observations[0], config) - latency = int((time.time() - frame_capture_time) * 1000) + latency = (time.time() - frame_capture_time) * 1000 latency_sum += latency - output_publisher.send(config, timestamp, camera_pose_observation, demo_pose_observation, fps, latency) + output_publisher.send(config, timestamp, fiducial_pose_observations, camera_pose_observation, demo_pose_observation, fps, latency) else: # No calibration diff --git a/output/OutputPublisher.py b/output/OutputPublisher.py index 249371b..2d94028 100644 --- a/output/OutputPublisher.py +++ b/output/OutputPublisher.py @@ -1,23 +1,93 @@ import math +import time from typing import List, Union import ntcore from config.config import ConfigStore +from output.Packet import Packet from vision_types import CameraPoseObservation, FiducialPoseObservation class OutputPublisher: - def send(self, config_store: ConfigStore, timestamp: float, observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None]) -> None: + def send(self, config_store: ConfigStore, timestamp: float, fiducial_pose_observations: List[FiducialPoseObservation], observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None], latency: float) -> None: raise NotImplementedError +class NTPacketPublisher(): + _init_complete: bool = False + _observations_pub: ntcore.RawPublisher + _demo_observations_pub: ntcore.RawPublisher + _fps_pub: ntcore.IntegerPublisher + _heartbeat_pub: ntcore.IntegerPublisher + + def send(self, config_store: ConfigStore, timestamp: float, fiducial_pose_observations: List[FiducialPoseObservation], observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None], latency: float) -> None: + # Initialize publishers on first call + if not self._init_complete: + nt_table = ntcore.NetworkTableInstance.getDefault().getTable( + "CubStar/" + config_store.local_config.device_id + "/output") + nt_table = ntcore.NetworkTableInstance.getDefault().getTable( + "CubStar/" + config_store.local_config.device_id + "/output") + self._observations_pub = nt_table.getRawTopic("observations").publish("ObservationsPacket", + ntcore.PubSubOptions(periodic=0, sendAll=True, keepDuplicates=True)) + self._demo_observations_pub = nt_table.getRawTopic("demo_observations").publish("FiducialPoseObservation", + ntcore.PubSubOptions(periodic=0, sendAll=True, keepDuplicates=True)) + self._fps_pub = nt_table.getIntegerTopic("fps").publish() + self._heartbeat_pub = nt_table.getIntegerTopic("heartbeat").publish() + + # Build the observation + observation_packet = Packet() + + observation_packet.encodeDouble(latency) + + observation_packet.encode8(len(fiducial_pose_observations)) + for fiducial_observation in fiducial_pose_observations: + observation_packet.encode8(fiducial_observation.tag_id) + if fiducial_observation.error_0 < fiducial_observation.error_1: + observation_packet.encodeDouble(fiducial_observation.error_0) + observation_packet.encodeTransform(fiducial_observation.pose_0) + observation_packet.encodeDouble(fiducial_observation.error_1) + observation_packet.encodeTransform(fiducial_observation.pose_1) + else: + observation_packet.encodeDouble(fiducial_observation.error_1) + observation_packet.encodeTransform(fiducial_observation.pose_1) + observation_packet.encodeDouble(fiducial_observation.error_0) + observation_packet.encodeTransform(fiducial_observation.pose_0) + + if observation == None: + observation_packet.encode8(0) + else: + observation_packet.encode8(len(observation.tag_ids)) + for tag_id in observation.tag_ids: + observation_packet.encode8(tag_id) + observation_packet.encodeDouble(observation.error_0) + observation_packet.encodeTransform(observation.pose_0) + if observation.error_1 != None and observation.pose_1 != None: + observation_packet.encodeDouble(observation.error_1) + observation_packet.encodeTransform(observation.pose_1) + + # Build demo observation + demo_observation_packet = Packet() + if demo_observation != None: + demo_observation_packet.encode8(demo_observation.tag_id) + demo_observation_packet.encodeDouble(demo_observation.error_0) + demo_observation_packet.encodeTransform(demo_observation.pose_0) + demo_observation_packet.encodeDouble(demo_observation.error_1) + demo_observation_packet.encodeTransform(demo_observation.pose_1) + + # Send data + self._observations_pub.set(bytes(observation_packet.getData())) + self._demo_observations_pub.set(bytes(demo_observation_packet.getData()), math.floor(timestamp * 1000000)) + if fps != None: + self._fps_pub.set(fps) + + class NTOutputPublisher(OutputPublisher): _init_complete: bool = False _observations_pub: ntcore.DoubleArrayPublisher - _observations_pub: ntcore.DoubleArrayPublisher + _demo_observations_pub: ntcore.DoubleArrayPublisher _fps_pub: ntcore.IntegerPublisher - def send(self, config_store: ConfigStore, timestamp: float, observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None], latency: int) -> None: + def send(self, config_store: ConfigStore, timestamp: float, fiducial_pose_observations: Union[FiducialPoseObservation, None], observation: Union[CameraPoseObservation, None], demo_observation: Union[FiducialPoseObservation, None], fps: Union[int, None], latency: float) -> None: # Initialize publishers on first call if not self._init_complete: nt_table = ntcore.NetworkTableInstance.getDefault().getTable( diff --git a/output/Packet.py b/output/Packet.py new file mode 100644 index 0000000..5467561 --- /dev/null +++ b/output/Packet.py @@ -0,0 +1,45 @@ +from typing import Any +from wpimath.geometry import Transform3d +import struct + +class Packet: + def __init__(self): + self._data: list[int] = [] + + def getData(self) -> list[int]: + return self._data + + def getSize(self) -> int: + return len(self._data) + + def _encodeGeneric(self, packFormat: str, *value: Any) -> None: + self._data.extend(struct.pack(packFormat, *value)) + + def encode8(self, value: int) -> None: + self._encodeGeneric('>b', value) + return self + + def encode32(self, value: int) -> None: + self._encodeGeneric('>l', value) + return self + + def encodeDouble(self, value: float) -> None: + self._encodeGeneric('>d', value) + return self + + def encodeBoolean(self, value: bool) -> None: + self.encode8(1 if value else 0) + return self + + def encodeTransform(self, value: Transform3d) -> None: + self.encodeDouble(value.X()) + self.encodeDouble(value.Y()) + self.encodeDouble(value.Z()) + + quaternion = value.rotation().getQuaternion() + self.encodeDouble(quaternion.W()) + self.encodeDouble(quaternion.X()) + self.encodeDouble(quaternion.Y()) + self.encodeDouble(quaternion.Z()) + + return self diff --git a/output/StreamServer.py b/output/StreamServer.py index 6bf587c..7503f63 100644 --- a/output/StreamServer.py +++ b/output/StreamServer.py @@ -27,7 +27,7 @@ class MjpegServer(StreamServer): _frame: cv2.Mat _has_frame: bool = False _fps: float = 0.0 - _latency: int = 0 + _latency: float = 0.0 _last_served_timestamp: float = 0.0 def _make_handler(self_mjpeg): # type: ignore @@ -80,7 +80,7 @@ def do_GET(self): timestamp = time.time() frame = self_mjpeg._frame.copy() cv2.putText(frame, f"FPS: {self_mjpeg._fps:.2f}", (10, 30), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) - cv2.putText(frame, f"Latency: {self_mjpeg._latency}ms", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) + cv2.putText(frame, f"Latency: {self_mjpeg._latency:.0f}ms", (10, 60), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) cv2.putText(frame, f"Stream FPS: {(1 / (timestamp - self_mjpeg._last_served_timestamp)):.2f}", (10, 90), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2) self_mjpeg._last_served_timestamp = timestamp @@ -115,7 +115,7 @@ def _run(self, port: int) -> None: def start(self, config_store: ConfigStore) -> None: threading.Thread(target=self._run, daemon=True, args=(config_store.local_config.stream_port,)).start() - def set_frame(self, frame: cv2.Mat, fps: Union[float, None], latency: Union[int, None]) -> None: + def set_frame(self, frame: cv2.Mat, fps: Union[float, None], latency: Union[float, None]) -> None: if fps != None: self._fps = fps if latency != None: diff --git a/pipeline/CameraPoseEstimator.py b/pipeline/CameraPoseEstimator.py index 0b63b35..288e2f4 100644 --- a/pipeline/CameraPoseEstimator.py +++ b/pipeline/CameraPoseEstimator.py @@ -1,9 +1,9 @@ -from typing import List, Union +from typing import List, Tuple, Union import cv2 import numpy from config.config import ConfigStore -from vision_types import CameraPoseObservation, FiducialImageObservation +from vision_types import CameraPoseObservation, FiducialImageObservation, FiducialPoseObservation from wpimath.geometry import * from pipeline.coordinate_systems import (openCvPoseToWpilib, @@ -14,7 +14,7 @@ class CameraPoseEstimator: def __init__(self) -> None: raise NotImplementedError - def solve_camera_pose(self, image_observations: List[FiducialImageObservation], config_store: ConfigStore) -> Union[CameraPoseObservation, None]: + def solve_camera_pose(self, image_observations: List[FiducialImageObservation], config_store: ConfigStore) -> Tuple[List[FiducialPoseObservation], Union[CameraPoseObservation, None]]: raise NotImplementedError @@ -22,21 +22,23 @@ class MultiTargetCameraPoseEstimator(CameraPoseEstimator): def __init__(self) -> None: pass - def solve_camera_pose(self, image_observations: List[FiducialImageObservation], config_store: ConfigStore) -> Union[CameraPoseObservation, None]: + def solve_camera_pose(self, image_observations: List[FiducialImageObservation], config_store: ConfigStore) -> Tuple[List[FiducialPoseObservation], Union[CameraPoseObservation, None]]: # Exit if no tag layout available if config_store.remote_config.tag_layout == None: - return None + return [], None # Exit if no observations available if len(image_observations) == 0: - return None + return [], None # Create set of object and image points fid_size = config_store.remote_config.fiducial_size_m - object_points = [] - image_points = [] + all_object_points = [] + all_image_points = [] tag_ids = [] - tag_poses = [] + tag_poses: List[Pose3d] = [] + fiducial_observations: List[FiducialPoseObservation] = [] + for observation in image_observations: tag_pose = None for tag_data in config_store.remote_config.tag_layout["tags"]: @@ -53,33 +55,34 @@ def solve_camera_pose(self, image_observations: List[FiducialImageObservation], tag_data["pose"]["rotation"]["quaternion"]["Y"], tag_data["pose"]["rotation"]["quaternion"]["Z"] ))) - if tag_pose != None: - # Add object points by transforming from the tag center - corner_0 = tag_pose + Transform3d(Translation3d(0, fid_size / 2.0, -fid_size / 2.0), Rotation3d()) - corner_1 = tag_pose + Transform3d(Translation3d(0, -fid_size / 2.0, -fid_size / 2.0), Rotation3d()) - corner_2 = tag_pose + Transform3d(Translation3d(0, -fid_size / 2.0, fid_size / 2.0), Rotation3d()) - corner_3 = tag_pose + Transform3d(Translation3d(0, fid_size / 2.0, fid_size / 2.0), Rotation3d()) - object_points += [ - wpilibTranslationToOpenCv(corner_0.translation()), - wpilibTranslationToOpenCv(corner_1.translation()), - wpilibTranslationToOpenCv(corner_2.translation()), - wpilibTranslationToOpenCv(corner_3.translation()) - ] - - # Add image points from observation - image_points += [ - [observation.corners[0][0][0], observation.corners[0][0][1]], - [observation.corners[0][1][0], observation.corners[0][1][1]], - [observation.corners[0][2][0], observation.corners[0][2][1]], - [observation.corners[0][3][0], observation.corners[0][3][1]] - ] - - # Add tag ID and pose - tag_ids.append(observation.tag_id) - tag_poses.append(tag_pose) - - # Single tag, return two poses - if len(tag_ids) == 1: + break + if tag_pose == None: + continue + + # Add object points by transforming from the tag center + corner_0 = tag_pose + Transform3d(Translation3d(0, fid_size / 2.0, -fid_size / 2.0), Rotation3d()) + corner_1 = tag_pose + Transform3d(Translation3d(0, -fid_size / 2.0, -fid_size / 2.0), Rotation3d()) + corner_2 = tag_pose + Transform3d(Translation3d(0, -fid_size / 2.0, fid_size / 2.0), Rotation3d()) + corner_3 = tag_pose + Transform3d(Translation3d(0, fid_size / 2.0, fid_size / 2.0), Rotation3d()) + all_object_points += [ + wpilibTranslationToOpenCv(corner_0.translation()), + wpilibTranslationToOpenCv(corner_1.translation()), + wpilibTranslationToOpenCv(corner_2.translation()), + wpilibTranslationToOpenCv(corner_3.translation()) + ] + + # Add image points from observation + image_points = [ + [observation.corners[0][0][0], observation.corners[0][0][1]], + [observation.corners[0][1][0], observation.corners[0][1][1]], + [observation.corners[0][2][0], observation.corners[0][2][1]], + [observation.corners[0][3][0], observation.corners[0][3][1]] + ] + all_image_points += image_points + + tag_ids.append(observation.tag_id) + tag_poses.append(tag_pose) + object_points = numpy.array([[-fid_size / 2.0, fid_size / 2.0, 0.0], [fid_size / 2.0, fid_size / 2.0, 0.0], [fid_size / 2.0, -fid_size / 2.0, 0.0], @@ -88,36 +91,40 @@ def solve_camera_pose(self, image_observations: List[FiducialImageObservation], _, rvecs, tvecs, errors = cv2.solvePnPGeneric(object_points, numpy.array(image_points), config_store.local_config.camera_matrix, config_store.local_config.distortion_coefficients, flags=cv2.SOLVEPNP_IPPE_SQUARE) except: - return None + continue # Calculate WPILib camera poses - field_to_tag_pose = tag_poses[0] camera_to_tag_pose_0 = openCvPoseToWpilib(tvecs[0], rvecs[0]) camera_to_tag_pose_1 = openCvPoseToWpilib(tvecs[1], rvecs[1]) camera_to_tag_0 = Transform3d(camera_to_tag_pose_0.translation(), camera_to_tag_pose_0.rotation()) camera_to_tag_1 = Transform3d(camera_to_tag_pose_1.translation(), camera_to_tag_pose_1.rotation()) - field_to_camera_0 = field_to_tag_pose.transformBy(camera_to_tag_0.inverse()) - field_to_camera_1 = field_to_tag_pose.transformBy(camera_to_tag_1.inverse()) - field_to_camera_pose_0 = Pose3d(field_to_camera_0.translation(), field_to_camera_0.rotation()) - field_to_camera_pose_1 = Pose3d(field_to_camera_1.translation(), field_to_camera_1.rotation()) - - # Return result - return CameraPoseObservation(tag_ids, field_to_camera_pose_0, errors[0][0], field_to_camera_pose_1, errors[1][0]) - # Multi-tag, return one pose - else: - # Run SolvePNP with all tags - try: - _, rvecs, tvecs, errors = cv2.solvePnPGeneric(numpy.array(object_points), numpy.array(image_points), - config_store.local_config.camera_matrix, config_store.local_config.distortion_coefficients, flags=cv2.SOLVEPNP_SQPNP) - except: - return None + fiducial_observations.append(FiducialPoseObservation(observation.tag_id, camera_to_tag_0, errors[0][0], camera_to_tag_1, errors[1][0])) - # Calculate WPILib camera pose - camera_to_field_pose = openCvPoseToWpilib(tvecs[0], rvecs[0]) - camera_to_field = Transform3d(camera_to_field_pose.translation(), camera_to_field_pose.rotation()) - field_to_camera = camera_to_field.inverse() - field_to_camera_pose = Pose3d(field_to_camera.translation(), field_to_camera.rotation()) - - # Return result - return CameraPoseObservation(tag_ids, field_to_camera_pose, errors[0][0], None, None) + # Don't include a camera pose if there are no tags or we failed to resolve a tag pose + if len(fiducial_observations) == 0: + return fiducial_observations, None + + # Only one target, use result of IPPE Square + if len(tag_ids) == 1: + tag_to_camera_pose_0 = fiducial_observations[0].pose_0.inverse() + camera_pose_0 = tag_poses[0].transformBy(tag_to_camera_pose_0) + tag_to_camera_pose_1 = fiducial_observations[0].pose_1.inverse() + camera_pose_1 = tag_poses[0].transformBy(tag_to_camera_pose_1) + return fiducial_observations, CameraPoseObservation(tag_ids, camera_pose_0, fiducial_observations[0].error_0, camera_pose_1, fiducial_observations[0].error_1) + + # Run SolvePNP with all tags + try: + _, rvecs, tvecs, errors = cv2.solvePnPGeneric(numpy.array(all_object_points), numpy.array(all_image_points), + config_store.local_config.camera_matrix, config_store.local_config.distortion_coefficients, flags=cv2.SOLVEPNP_SQPNP) + except: + return fiducial_observations, None + + # Calculate WPILib camera pose + camera_to_field_pose = openCvPoseToWpilib(tvecs[0], rvecs[0]) + camera_to_field = Transform3d(camera_to_field_pose.translation(), camera_to_field_pose.rotation()) + field_to_camera = camera_to_field.inverse() + field_to_camera_pose = Pose3d(field_to_camera.translation(), field_to_camera.rotation()) + + # Return result + return fiducial_observations, CameraPoseObservation(tag_ids, field_to_camera_pose, errors[0][0], None, None) diff --git a/pipeline/FiducialDetector.py b/pipeline/FiducialDetector.py index 30a0836..6c8403f 100644 --- a/pipeline/FiducialDetector.py +++ b/pipeline/FiducialDetector.py @@ -15,11 +15,10 @@ def detect_fiducials(self, image: cv2.Mat, config_store: ConfigStore) -> List[Fi class ArucoFiducialDetector(FiducialDetector): def __init__(self, dictionary_id) -> None: - self._aruco_dict = cv2.aruco.getPredefinedDictionary(dictionary_id) - self._aruco_params = cv2.aruco.DetectorParameters() + self._detector = cv2.aruco.ArucoDetector(cv2.aruco.getPredefinedDictionary(dictionary_id), cv2.aruco.DetectorParameters()) def detect_fiducials(self, image: cv2.Mat, config_store: ConfigStore) -> List[FiducialImageObservation]: - corners, ids, _ = cv2.aruco.detectMarkers(image, self._aruco_dict, parameters=self._aruco_params) + corners, ids, _ = self._detector.detectMarkers(image) if len(corners) == 0: return [] return [FiducialImageObservation(id[0], corner) for id, corner in zip(ids, corners)] diff --git a/requirements.txt b/requirements.txt index 9be4604..feb7c3f 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,6 +1,5 @@ numpy==1.26.3 -opencv-contrib-python==4.7.0.68 -opencv-python==4.7.0.68 +opencv-python==4.9.0.80 pillow==10.2.0 pyntcore==2024.1.1.1 robotpy-wpimath==2024.1.1.1 diff --git a/vision_types.py b/vision_types.py index cb0e499..ce282c3 100644 --- a/vision_types.py +++ b/vision_types.py @@ -15,12 +15,11 @@ class FiducialImageObservation: @dataclass(frozen=True) class FiducialPoseObservation: tag_id: int - pose_0: Pose3d + pose_0: Transform3d error_0: float - pose_1: Pose3d + pose_1: Transform3d error_1: float - @dataclass(frozen=True) class CameraPoseObservation: tag_ids: List[int] From 4d1eb06cba0a2b043d404b6bc38c9a56d4b9e43a Mon Sep 17 00:00:00 2001 From: tjkr0wn Date: Mon, 29 Jan 2024 23:27:32 -0500 Subject: [PATCH 3/4] Updated .gitignore, added Dockerfile and build script for building OpenCV --- .gitignore | 4 ++++ docker/Dockerfile | 40 ++++++++++++++++++++++++++++++++++++++++ docker/build_opencv.sh | 23 +++++++++++++++++++++++ 3 files changed, 67 insertions(+) create mode 100644 docker/Dockerfile create mode 100644 docker/build_opencv.sh diff --git a/.gitignore b/.gitignore index 68bc17f..2d481c1 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,7 @@ +#OpenCV build artifacts +docker/cv2/ +docker/lib/ + # Byte-compiled / optimized / DLL files __pycache__/ *.py[cod] diff --git a/docker/Dockerfile b/docker/Dockerfile new file mode 100644 index 0000000..8645041 --- /dev/null +++ b/docker/Dockerfile @@ -0,0 +1,40 @@ +FROM ubuntu:22.04 + +RUN mkdir -p /usr/src/app +WORKDIR /usr/src/app + +# No interaction when installing packages +ARG DEBIAN_FRONTEND=noninteractive + +# Note: looks like the OPi's are running 3.10.6, but the subversion for default python in ubuntu 22.04 is 3.10.12 +# python3.11 isn't needed here, but I'm scared to change stuff +# Install minimal prerequisites +RUN apt-get update && apt-get install -y cmake g++ wget unzip build-essential apt-utils git python3.11 python3-dev python3-numpy openssh-server +RUN apt-get install -y libavcodec-dev libavformat-dev libswscale-dev libgstreamer-plugins-base1.0-dev libgstreamer1.0-dev libgtk-3-dev + +# Kick off SSH server if stuff breaks +RUN sed -i 's/PermitRootLogin prohibit-password/PermitRootLogin yes/' /etc/ssh/sshd_config +RUN useradd -m -s /bin/bash user +RUN echo "user:1701robocubs" | chpasswd +EXPOSE 22 +ENTRYPOINT service ssh start && bash + +# Download and unpack sources +RUN wget -O opencv.zip https://github.com/opencv/opencv/archive/4.x.zip +RUN wget -O opencv_contrib.zip https://github.com/opencv/opencv_contrib/archive/4.x.zip +RUN unzip opencv.zip +RUN unzip opencv_contrib.zip + +# Make CMAKE build dir +RUN mkdir -p build +WORKDIR /usr/src/app/build + +# Configure +RUN cmake -DBUILD_EXAMPLES=OFF -DBUILD_TESTS=OFF -DBUILD_PERF_TESTS=OFF -DWITH_GSTREAMER=ON -DOPENCV_EXTRA_MODULES_PATH=../opencv_contrib-4.x/modules ../opencv-4.x + +# Build +RUN cmake --build . + +# Do standard build and install so it's easier to grab the necessary python module +RUN make +RUN make install \ No newline at end of file diff --git a/docker/build_opencv.sh b/docker/build_opencv.sh new file mode 100644 index 0000000..214a370 --- /dev/null +++ b/docker/build_opencv.sh @@ -0,0 +1,23 @@ +# Start by building the docker image +docker build --tag 'opencv_build' . + +# Run the container and grab the id +CONTAINER_ID=$(docker run --detach 'opencv_build') + +# Clean any previous dirs +rm -r ./cv2 +rm -r ./lib + +# Copy the python package directory +docker cp "$CONTAINER_ID":/usr/local/lib/python3.10/dist-packages/cv2/ . + +# Copy the built shared objects +""" +WARNING: This copies EVERYTHING, including the shared objects. Since we're in a docker container, +there really shouldn't be anything else installed here. +""" +docker cp "$CONTAINER_ID":/usr/local/lib/ . + +# TODO: Get these automatically implemented, do not copy dirs in lib though +#scp -r ./lib/ root@10.0.100.139:/usr/local/lib +#scp -r ./cv2 root@10.0.100.139:/usr/local/lib/python3.10/dist-packages/ \ No newline at end of file From 0333e4237c56971c6a1eed1b5116382ba687bc2d Mon Sep 17 00:00:00 2001 From: tjkr0wn Date: Tue, 30 Jan 2024 09:49:37 -0500 Subject: [PATCH 4/4] Reject captured calibration frame if not enough ID's are detected --- calibration/CalibrationSession.py | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/calibration/CalibrationSession.py b/calibration/CalibrationSession.py index 026acc1..1583d39 100644 --- a/calibration/CalibrationSession.py +++ b/calibration/CalibrationSession.py @@ -37,9 +37,12 @@ def process_frame(self, image: cv2.Mat, save: bool) -> None: # Save corners if save: - self._all_charuco_corners.append(charuco_corners) - self._all_charuco_ids.append(charuco_ids) - print("Saved calibration frame") + if charuco_ids.size > 4: + self._all_charuco_corners.append(charuco_corners) + self._all_charuco_ids.append(charuco_ids) + print("Saved calibration frame") + else: + print("Bad frame: not enough ID's captured") def finish(self) -> None: if len(self._all_charuco_corners) == 0: