88"""
99
1010# Built-in/Generic Imports
11- from typing import Union
12- from typing import Tuple
11+ import logging
12+ from typing import Tuple , Union
1313
14- # Libs
1514import cv2
16- from cv2 import aruco
1715import matplotlib .pyplot as plt
1816import numpy as np
1917import open3d as o3d
18+ from colmap_wrapper .visualization import COLMAP
2019from PIL import Image
21- # Own modules
2220
23- import sys
24- sys .path .append ("C:/Users/meyerls/Documents/AIST/code/gaussian-splatting/colmap-wrapper" )
25- from colmap_wrapper .visualization .visualization import *
26-
27-
28- def ray_cast_aruco_corners (extrinsics : np .ndarray , intrinsics : np .ndarray , corners : tuple ) \
29- -> Tuple [np .ndarray , np .ndarray ]:
30- '''
3121
22+ def ray_cast_aruco_corners (
23+ extrinsics : np .ndarray , intrinsics : np .ndarray , corners : tuple
24+ ) -> Tuple [np .ndarray , np .ndarray ]:
25+ """
3226 n = x @ K^-1 @ R.T
3327
3428 :param extrinsics:
3529 :param intrinsics:
3630 :param corners:
3731 :return:
38- '''
32+ """
3933 R , camera_origin = extrinsics [:3 , :3 ], extrinsics [:3 , 3 ]
4034 aruco_corners = np .concatenate ((corners [0 ][0 ], np .ones ((4 , 1 ))), axis = 1 )
4135 rays = aruco_corners @ np .linalg .inv (intrinsics ).T @ R .T
4236 rays_norm = rays / np .linalg .norm (rays , ord = 2 , axis = 1 , keepdims = True )
4337
4438 return camera_origin , rays_norm
4539
46-
47- def load_image (image_path : str ) -> np .ndarray :
48- """
49- Load Image. This takes almost 50% of the time. Would be nice if it is possible to speed up this process. Any
50- ideas?
51-
52- :param image_path:
53- :return:
54- """
55- return cv2 .imread (image_path )#np.asarray(Image.open(image_path))
56-
57-
58- class ArucoDetection :
59- def __init__ (self , dict_type : int = aruco .DICT_4X4_1000 ):
60- """
61- More information on aruco parameters: https://docs.opencv.org/4.x/d1/dcd/structcv_1_1aruco_1_1DetectorParameters.html
62-
63- @param dict_type:
64- """
65- self .dict_type = dict_type
66- self .aruco_dict = aruco .Dictionary_get (dict_type )
67- self .aruco_parameters = aruco .DetectorParameters_create ()
68-
69- # aruco_parameters = aruco.DetectorParameters_create()
70- # aruco_parameters.adaptiveThreshConstant = 9.0
71- # aruco_parameters.adaptiveThreshWinSizeMax = 369
72- # aruco_parameters.adaptiveThreshWinSizeMin = 7
73- # aruco_parameters.adaptiveThreshWinSizeStep = 49
74- # aruco_parameters.cornerRefinementWinSize = 9
75- # aruco_parameters.minDistanceToBorder = 7
76- # aruco_parameters.cornerRefinementMaxIterations = 149
77- # aruco_parameters.minOtsuStdDev = 4.0
78- #
79- # aruco_parameters.minMarkerDistanceRate = 0.05
80- # aruco_parameters.minMarkerPerimeterRate = 5
81- # aruco_parameters.maxMarkerPerimeterRate = 10
82- #
83- #
84- # aruco_parameters.polygonalApproxAccuracyRate = 0.05
85- # aruco_parameters.minCornerDistanceRate = 0.05
86-
87- def detect_aruco_marker (self , image : Union [np .ndarray , str ]) -> Tuple [tuple , np .ndarray , np .ndarray ]:
88- return detect_aruco_marker (image = image , dict_type = self .aruco_dict , aruco_parameters = self .aruco_parameters )
89-
90-
91- def detect_aruco_marker (image : np .ndarray , dict_type : int = aruco .DICT_4X4_1000 ,
92- aruco_parameters : cv2 .aruco .DetectorParameters = aruco .DetectorParameters_create ()) -> Tuple [
93- tuple , np .ndarray ]:
40+ def detect_aruco_marker (
41+ image : np .ndarray ,
42+ dict_type : int = cv2 .aruco .DICT_4X4_100 ,
43+ aruco_parameters : cv2 .aruco .DetectorParameters = cv2 .aruco .DetectorParameters (),
44+ ) -> Tuple [tuple , np .ndarray ]:
9445 """
46+ Info: https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html
9547 More information on aruco parameters: https://docs.opencv.org/4.x/d1/dcd/structcv_1_1aruco_1_1DetectorParameters.html
9648
9749 @param dict_type:
9850 @param image:
9951 @param aruco_parameters:
100- """
101-
102- # Info: https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html
103- aruco_dict = aruco .Dictionary_get (dict_type )
104- aruco_parameters = aruco .DetectorParameters_create ()
10552
106- aruco_parameters .polygonalApproxAccuracyRate = 0.01
107- aruco_parameters .minMarkerPerimeterRate = 0.1
108- aruco_parameters .maxMarkerPerimeterRate = 4.0
109- """
11053 Aruco Corners
11154
11255 p1------------p2
@@ -119,64 +62,15 @@ def detect_aruco_marker(image: np.ndarray, dict_type: int = aruco.DICT_4X4_1000,
11962 :param image:
12063 :return:
12164 """
122- if isinstance (image , np .ndarray ):
123- pass
124- elif isinstance (image , str ):
125- image = load_image (image_path = image )
126- else :
127- raise NotImplementedError
12865
66+ image = cv2 .imread (image )
67+ if image is None :
68+ logging .warning (f"Failed to load image: { image } )
69+ return None , None
12970 image_size = image .shape
130- gray = cv2 .cvtColor (image , cv2 .COLOR_BGR2GRAY )
131- corners , aruco_id , rejected_img_points = aruco .detectMarkers (gray ,
132- aruco_dict ,
133- parameters = aruco_parameters )
134-
71+ aruco_dict = cv2 .aruco .getPredefinedDictionary (dict_type )
72+ detector = cv2 .aruco .ArucoDetector (aruco_dict , aruco_parameters )
73+ corners , aruco_id , _ = detector .detectMarkers (image )
13574 if aruco_id is None :
13675 return None , None , image_size
137-
138- if False :
139- if len (corners ) > 0 :
140-
141- # flatten the ArUco IDs list
142- ids = aruco_id .flatten ()
143- # loop over the detected ArUCo corners
144- for (markerCorner , markerID ) in zip (corners , ids ):
145- image = cv2 .cvtColor (image , cv2 .COLOR_BGR2RGB )
146-
147- # extract the marker corners (which are always returned in
148- # top-left, top-right, bottom-right, and bottom-left order)
149- corners_plot = markerCorner .reshape ((4 , 2 ))
150- (topLeft , topRight , bottomRight , bottomLeft ) = corners_plot
151- # convert each of the (x, y)-coordinate pairs to integers
152- topRight = (int (topRight [0 ]), int (topRight [1 ]))
153- bottomRight = (int (bottomRight [0 ]), int (bottomRight [1 ]))
154- bottomLeft = (int (bottomLeft [0 ]), int (bottomLeft [1 ]))
155- topLeft = (int (topLeft [0 ]), int (topLeft [1 ]))
156-
157- # draw the bounding box of the ArUCo detection
158- cv2 .line (image , topLeft , topRight , (0 , 255 , 0 ), 25 )
159- cv2 .line (image , topRight , bottomRight , (0 , 255 , 0 ), 25 )
160- cv2 .line (image , bottomRight , bottomLeft , (0 , 255 , 0 ), 25 )
161- cv2 .line (image , bottomLeft , topLeft , (0 , 255 , 0 ), 25 )
162- # compute and draw the center (x, y)-coordinates of the ArUco
163- # marker
164- cX = int ((topLeft [0 ] + bottomRight [0 ]) / 2.0 )
165- cY = int ((topLeft [1 ] + bottomRight [1 ]) / 2.0 )
166- cv2 .circle (image , (cX , cY ), 4 , (0 , 0 , 255 ), 5 )
167- # draw the ArUco marker ID on the image
168- cv2 .putText (image , str (markerID ),
169- (topLeft [0 ], topLeft [1 ] - 15 ), cv2 .FONT_HERSHEY_SIMPLEX ,
170- 0.5 , (0 , 255 , 0 ), 10 )
171- width = int (image .shape [1 ] * 0.3 )
172- height = int (image .shape [0 ] * 0.3 )
173- dim = (width , height )
174- image = cv2 .resize (image , dim )
175-
176- plt .imshow (image , cmap = 'gray' )
177- plt .show ()
178-
179- del gray
180- del image
181-
18276 return corners , aruco_id , image_size
0 commit comments