Skip to content

Commit

Permalink
Merge remote-tracking branch 'origin/main' into tof_decoding
Browse files Browse the repository at this point in the history
# Conflicts:
#	calibrate.py
#	depthai_sdk/requirements.txt
  • Loading branch information
Erol444 committed Jun 6, 2024
2 parents 9c7f6e4 + 933d575 commit c412002
Show file tree
Hide file tree
Showing 32 changed files with 255 additions and 209 deletions.
4 changes: 2 additions & 2 deletions .github/workflows/test_install_requirements.yml
Original file line number Diff line number Diff line change
Expand Up @@ -91,9 +91,9 @@
- name: Download chocolatey
shell: pwsh
run: Set-ExecutionPolicy Bypass -Scope Process -Force; [System.Net.ServicePointManager]::SecurityProtocol = [System.Net.ServicePointManager]::SecurityProtocol -bor 3072; iex ((New-Object System.Net.WebClient).DownloadString('https://chocolatey.org/install.ps1'))
- name: Install dependencies
- name: Install pycharm-community dependency
shell: pwsh
run: choco install cmake git python --version 3.10 pycharm-community -y
run: choco install pycharm-community -y
- name: Install requrirements
run: |
python install_requirements.py
Expand Down
106 changes: 40 additions & 66 deletions calibrate.py
Original file line number Diff line number Diff line change
Expand Up @@ -164,6 +164,8 @@ def parse_args():
help="Number of pictures taken.")
parser.add_argument('-ebp', '--enablePolygonsDisplay', default=False, action="store_true",
help="Enable the display of polynoms.")
parser.add_argument('-dbg', '--debugProcessingMode', default=False, action="store_true",
help="Enable processing of images without using the camera.")
options = parser.parse_args()
# Set some extra defaults, `-brd` would override them
if options.defaultBoard is not None:
Expand All @@ -185,7 +187,10 @@ def parse_args():
raise argparse.ArgumentTypeError("-s / --squareSizeCm needs to be greater than 2.2 cm")
if options.traceLevel == 1:
print(f"Charuco board selected is: board_name = {board_name}, numX = {numX}, numY = {numY}, squareSize {options.squareSizeCm} cm, markerSize {options.markerSizeCm} cm")

if options.debugProcessingMode:
options.mode = "process"
if options.board is None:
raise argparse.ArgumentError(options.board, "Board name (-brd) of camera must be specified in case of using debug mode (-dbg).")
return options

class HostSync:
Expand Down Expand Up @@ -350,31 +355,33 @@ def __init__(self):
self.output_scale_factor = self.args.outputScaleFactor
self.aruco_dictionary = cv2.aruco.Dictionary_get(
cv2.aruco.DICT_4X4_1000)
self.device = dai.Device()
self.enablePolygonsDisplay = self.args.enablePolygonsDisplay
self.board_name = None
calibData = self.device.readCalibration()
eeprom = calibData.getEepromData()
if not self.args.debugProcessingMode:
self.device = dai.Device()
cameraProperties = self.device.getConnectedCameraFeatures()
calibData = self.device.readCalibration()
eeprom = calibData.getEepromData()
#TODO Change only in getDeviceName in next revision.
if self.args.board:
self.board_name = self.args.board
board_path = Path(self.args.board)
board_path = Path(Path(__file__).parent /self.args.board)
if not board_path.exists():
board_path = (Path(__file__).parent / 'resources/depthai_boards/boards' / self.args.board.upper()).with_suffix('.json').resolve()
if not board_path.exists():
raise ValueError(
'Board config not found: {}'.format(board_path))
'Board config not found: {}'.format(Path(Path(__file__).parent /self.args.board)))
with open(board_path) as fp:
self.board_config = json.load(fp)
self.board_config = self.board_config['board_config']
self.board_config_backup = self.board_config
else:
elif not self.args.debugProcessingMode:
try:
detection = self.device.getDeviceName()
print(f"Device name: {detection}")
detection = detection.split("-")
except:
self.cameraProperties = self.device.getConnectedCameraFeatures()
cameraProperties = self.device.getConnectedCameraFeatures()
calibData = self.device.readCalibration()
eeprom = calibData.getEepromData()
eeprom.productName = eeprom.productName.replace(" ", "-").upper()
Expand All @@ -393,7 +400,7 @@ def __init__(self):
if "9782" in detection:
detection.remove("9782")
self.board_name = '-'.join(detection)
board_path = Path(self.board_name)
board_path = Path(Path(__file__).parent /self.board_name)
if self.traceLevel == 1:
print(f"Board path specified as {board_path}")
if not board_path.exists():
Expand Down Expand Up @@ -430,23 +437,18 @@ def __init__(self):
for cam_id in self.board_config['cameras']:
name = self.board_config['cameras'][cam_id]['name']
self.coverageImages[name] = None

if (self._set_camera_features()):
print("Camera features set using board config.")
print(f"self.cameraProperties: {self.cameraProperties}")
else:
self.cameraProperties = self.device.getConnectedCameraFeatures()

for properties in self.cameraProperties:
for in_cam in self.board_config['cameras'].keys():
cam_info = self.board_config['cameras'][in_cam]
if cam_info["name"] not in self.args.disableCamera:
if properties.socket == stringToCam[in_cam]:
self.board_config['cameras'][in_cam]['sensorName'] = properties.sensorName
print('Cam: {} and focus: {}'.format(cam_info['name'], properties.hasAutofocus))
self.board_config['cameras'][in_cam]['hasAutofocus'] = properties.hasAutofocus
# self.auto_checkbox_dict[cam_info['name'] + '-Camera-connected'].check()
break
if not self.args.debugProcessingMode:
cameraProperties = self.device.getConnectedCameraFeatures()
for properties in cameraProperties:
for in_cam in self.board_config['cameras'].keys():
cam_info = self.board_config['cameras'][in_cam]
if cam_info["name"] not in self.args.disableCamera:
if properties.socket == stringToCam[in_cam]:
self.board_config['cameras'][in_cam]['sensorName'] = properties.sensorName
print('Cam: {} and focus: {}'.format(cam_info['name'], properties.hasAutofocus))
self.board_config['cameras'][in_cam]['hasAutofocus'] = properties.hasAutofocus
# self.auto_checkbox_dict[cam_info['name'] + '-Camera-connected'].check()
break

self.charuco_board = cv2.aruco.CharucoBoard_create(
self.args.squaresX, self.args.squaresY,
Expand All @@ -459,39 +461,6 @@ def mouse_event_callback(self, event, x, y, flags, param):
if event == cv2.EVENT_LBUTTONDOWN:
self.mouseTrigger = True

def _set_camera_features(self):
"""
Create mock camera properties using manually specified board config
"""

self.cameraProperties = self.device.getConnectedCameraFeatures()

for cam_id in self.board_config['cameras']:
try:
sensor_model = self.board_config['cameras'][cam_id]["model"]
config_type = self.board_config['cameras'][cam_id]["type"]
except KeyError:
print(f"Model not found for {cam_id}, skipping...")
return False

if sensor_model in camToRgbRes:
supportedTypes = [dai.CameraSensorType.COLOR]
elif sensor_model in camToMonoRes:
supportedTypes = [dai.CameraSensorType.MONO]
else:
print(f"Model {sensor_model} not supported")
return False

if supportedTypes[0].name != config_type.upper():
raise ValueError(f"Mismatch in camera type for {cam_id} with model {sensor_model} and type {config_type}, please fix the board config.")

for cam in self.cameraProperties:
if stringToCam[cam_id] == cam.socket:
cam.sensorName = sensor_model
cam.supportedTypes = supportedTypes

return True

def startPipeline(self):
pipeline = self.create_pipeline()
self.device.startPipeline(pipeline)
Expand Down Expand Up @@ -1006,7 +975,7 @@ def calibrate(self):
self.args.cameraMode,
self.args.rectifiedDisp) # Turn off enable disp rectify

if self.args.noInitCalibration:
if self.args.noInitCalibration or self.args.debugProcessingMode:
calibration_handler = dai.CalibrationHandler()
else:
calibration_handler = self.device.readCalibration()
Expand All @@ -1019,7 +988,8 @@ def calibrate(self):
calibration_handler.setBoardInfo(str(self.device.getDeviceName()), str(self.args.revision))
except Exception as e:
print('Device closed in exception..' )
self.device.close()
if not self.args.debugProcessingMode:
self.device.close()
print(e)
print(traceback.format_exc())
raise SystemExit(1)
Expand Down Expand Up @@ -1104,7 +1074,7 @@ def calibrate(self):
calibration_handler.setStereoLeft(stringToCam[cam_info['extrinsics']['to_cam']], result_config['stereo_config']['rectification_left'])
target_file.close()

if len(error_text) == 0:
if len(error_text) == 0 and not self.args.debugProcessingMode:
print('Flashing Calibration data into ')
# print(calib_dest_path)

Expand Down Expand Up @@ -1138,15 +1108,17 @@ def calibrate(self):
is_write_factory_sucessful = False

if is_write_succesful:
self.device.close()
if not self.args.debugProcessingMode:
self.device.close()
text = "EEPROM written succesfully"
resImage = create_blank(900, 512, rgb_color=green)
cv2.putText(resImage, text, (10, 250), font, 2, (0, 0, 0), 2)
cv2.imshow("Result Image", resImage)
cv2.waitKey(0)

else:
self.device.close()
if not self.args.debugProcessingMode:
self.device.close()
text = "EEPROM write Failed!!"
resImage = create_blank(900, 512, rgb_color=red)
cv2.putText(resImage, text, (10, 250), font, 2, (0, 0, 0), 2)
Expand All @@ -1155,7 +1127,8 @@ def calibrate(self):
# return (False, "EEPROM write Failed!!")

else:
self.device.close()
if not self.args.debugProcessingMode:
self.device.close()
print(error_text)
for text in error_text:
# text = error_text[0]
Expand All @@ -1164,7 +1137,8 @@ def calibrate(self):
cv2.imshow("Result Image", resImage)
cv2.waitKey(0)
except Exception as e:
self.device.close()
if not self.args.debugProcessingMode:
self.device.close()
print('Device closed in exception..' )
print(e)
print(traceback.format_exc())
Expand Down
5 changes: 3 additions & 2 deletions depthai_demo.py
Original file line number Diff line number Diff line change
Expand Up @@ -206,7 +206,8 @@ def setup(self, conf: ConfigManager):
self._nnManager.countLabel(self._conf.getCountLabel(self._nnManager))
self._pm.setNnManager(self._nnManager)

self._device = dai.Device(self._pm.pipeline.getOpenVINOVersion(), self._deviceInfo, usb2Mode=self._conf.args.usbSpeed == "usb2")
maxUsbSpeed = dai.UsbSpeed.HIGH if self._conf.args.usbSpeed == "usb2" else dai.UsbSpeed.SUPER
self._device = dai.Device(self._pm.pipeline.getOpenVINOVersion(), self._deviceInfo, maxUsbSpeed)
self._device.addLogCallback(self._logMonitorCallback)
if sentryEnabled:
try:
Expand Down Expand Up @@ -288,7 +289,7 @@ def run(self):

if self._conf.useCamera:
cameras = self._device.getConnectedCameras()
if dai.CameraBoardSocket.LEFT in cameras and dai.CameraBoardSocket.RIGHT in cameras:
if dai.CameraBoardSocket.CAM_B in cameras and dai.CameraBoardSocket.CAM_C in cameras:
self._pv.collectCalibData(self._device)

self._updateCameraConfigs({
Expand Down
4 changes: 2 additions & 2 deletions depthai_sdk/docs/source/components/stereo_component.rst
Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
StereoComponent
===============

:class:`StereoComponent <depthai_sdk.components.StereoComponent>` abstracts `StereoDepth <https://docs.luxonis.com/projects/api/en/latest/components/nodes/imu/>`__ node, its configuration,
:class:`StereoComponent <depthai_sdk.components.StereoComponent>` abstracts `StereoDepth <https://docs.luxonis.com/projects/api/en/latest/components/nodes/stereo_depth/>`__ node, its configuration,
filtering (eg. `WLS filter <https://github.com/luxonis/depthai-experiments/tree/master/gen2-wls-filter>`__), and disparity/depth viewing.

Usage
Expand Down Expand Up @@ -35,4 +35,4 @@ Reference

.. autoclass:: depthai_sdk.components.StereoComponent
:members:
:undoc-members:
:undoc-members:
Original file line number Diff line number Diff line change
Expand Up @@ -34,8 +34,8 @@ Source Code

Also `available on GitHub <https://github.com/luxonis/depthai/tree/main/depthai_sdk/examples/CameraComponent/sync_multiple_outputs.py>`__

.. literalinclude:: ../../../../examples/CameraComponent/sync_multiple_outputs.py
.. literalinclude:: ../../../../examples/mixed/sync_multiple_outputs.py
:language: python
:linenos:

.. include:: /includes/footer-short.rst
.. include:: /includes/footer-short.rst
2 changes: 1 addition & 1 deletion depthai_sdk/examples/IMUComponent/imu.py
Original file line number Diff line number Diff line change
Expand Up @@ -5,5 +5,5 @@
imu.config_imu(report_rate=400, batch_report_threshold=5)
# DepthAI viewer should open, and IMU data can be viewed on the right-side panel,
# under "Stats" tab (right of the "Device Settings" tab).
oak.visualize(imu.out.main, visualizer='viewer')
oak.visualize(imu.out.main)
oak.start(blocking=True)
5 changes: 3 additions & 2 deletions depthai_sdk/src/depthai_sdk/classes/packet_handlers.py
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
import logging

import os
from abc import abstractmethod
from queue import Queue, Empty
Expand All @@ -8,6 +8,7 @@

from depthai_sdk.classes.packets import BasePacket
from depthai_sdk.components.component import Component, ComponentOutput
from depthai_sdk.logger import LOGGER
from depthai_sdk.oak_outputs.fps import FPS
from depthai_sdk.oak_outputs.syncing import TimestampSync
from depthai_sdk.oak_outputs.xout.xout_base import XoutBase, ReplayStream
Expand Down Expand Up @@ -65,7 +66,7 @@ def configure_syncing(self,
"""
if enable_sync:
if len(self.outputs) < 2:
logging.error('Syncing requires at least 2 outputs! Skipping syncing.')
LOGGER.error('Syncing requires at least 2 outputs! Skipping syncing.')
return
self.sync = TimestampSync(len(self.outputs), threshold_ms)

Expand Down
8 changes: 4 additions & 4 deletions depthai_sdk/src/depthai_sdk/components/camera_component.py
Original file line number Diff line number Diff line change
@@ -1,11 +1,11 @@
import logging
from typing import Dict

from depthai_sdk.classes.enum import ResizeMode
from depthai_sdk.components.camera_control import CameraControl
from depthai_sdk.components.camera_helper import *
from depthai_sdk.components.component import Component, ComponentOutput
from depthai_sdk.components.parser import parse_resolution, parse_encode, encoder_profile_to_fourcc
from depthai_sdk.logger import LOGGER
from depthai_sdk.oak_outputs.xout.xout_base import XoutBase, StreamXout, ReplayStream
from depthai_sdk.oak_outputs.xout.xout_frames import XoutFrames
from depthai_sdk.replay import Replay
Expand Down Expand Up @@ -297,7 +297,7 @@ def control_with_nn(self, detection_component: 'NNComponent', auto_focus=True, a
"""

if not auto_focus and not auto_exposure:
logging.error('Attempted to control camera with NN, '
LOGGER.error('Attempted to control camera with NN, '
'but both Auto-Focus and Auto-Exposure were disabled! Attempt ignored.')
return

Expand Down Expand Up @@ -337,12 +337,12 @@ def config_color_camera(self,
chroma_denoise: Optional[int] = None,
) -> None:
if not self.is_color():
logging.info('Attempted to configure ColorCamera, '
LOGGER.info('Attempted to configure ColorCamera, '
'but this component doesn\'t have it. Config attempt ignored.')
return

if self.is_replay():
logging.info('Tried configuring ColorCamera, but replaying is enabled. Config attempt ignored.')
LOGGER.info('Tried configuring ColorCamera, but replaying is enabled. Config attempt ignored.')
return

if interleaved is not None: self.node.setInterleaved(interleaved)
Expand Down
Loading

0 comments on commit c412002

Please sign in to comment.