7
7
from OpenGL .GLU import *
8
8
import pygame
9
9
from pygame .locals import *
10
- from objloader import OBJ
10
+ from objloader import Obj
11
11
12
12
ARUCO_DICT = {
13
13
"DICT_4X4_50" : cv2 .aruco .DICT_4X4_50 ,
@@ -44,38 +44,12 @@ def init_gl(width, height):
44
44
gluPerspective (45.0 , float (width )/ float (height ), 0.1 , 100.0 )
45
45
glMatrixMode (GL_MODELVIEW )
46
46
47
- def draw_cube (size ):
48
- glBegin (GL_QUADS )
49
- glVertex3f ( size , size , - size )
50
- glVertex3f (- size , size , - size )
51
- glVertex3f (- size , size , size )
52
- glVertex3f ( size , size , size )
53
-
54
- glVertex3f ( size , - size , size )
55
- glVertex3f (- size , - size , size )
56
- glVertex3f (- size , - size , - size )
57
- glVertex3f ( size , - size , - size )
58
-
59
- glVertex3f ( size , size , size )
60
- glVertex3f (- size , size , size )
61
- glVertex3f (- size , - size , size )
62
- glVertex3f ( size , - size , size )
63
-
64
- glVertex3f ( size , - size , - size )
65
- glVertex3f (- size , - size , - size )
66
- glVertex3f (- size , size , - size )
67
- glVertex3f ( size , size , - size )
68
-
69
- glVertex3f (- size , size , size )
70
- glVertex3f (- size , size , - size )
71
- glVertex3f (- size , - size , - size )
72
- glVertex3f (- size , - size , size )
73
-
74
- glVertex3f ( size , size , - size )
75
- glVertex3f ( size , size , size )
76
- glVertex3f ( size , - size , size )
77
- glVertex3f ( size , - size , - size )
78
- glEnd ()
47
+ def draw_axis (img , imgpts , corner , length = 0.01 ):
48
+ origin = tuple (corner .ravel ().astype (int ))
49
+ img = cv2 .line (img , origin , tuple (imgpts [0 ].ravel ().astype (int )), (255 ,0 ,0 ), 5 )
50
+ img = cv2 .line (img , origin , tuple (imgpts [1 ].ravel ().astype (int )), (0 ,255 ,0 ), 5 )
51
+ img = cv2 .line (img , origin , tuple (imgpts [2 ].ravel ().astype (int )), (0 ,0 ,255 ), 5 )
52
+ return img
79
53
80
54
def pose_estimation (frame , aruco_dict_type , matrix_coefficients , distortion_coefficients ):
81
55
gray = cv2 .cvtColor (frame , cv2 .COLOR_BGR2GRAY )
@@ -84,11 +58,23 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
84
58
85
59
corners , ids , rejected_img_points = cv2 .aruco .detectMarkers (gray , cv2 .aruco_dict , parameters = parameters )
86
60
61
+ # Debug: Print number of markers detected
62
+ print (f"Number of ArUco markers detected: { len (corners )} " )
63
+
87
64
if len (corners ) > 0 :
88
65
for i in range (0 , len (ids )):
89
66
rvec , tvec , markerPoints = cv2 .aruco .estimatePoseSingleMarkers (corners [i ], 0.02 , matrix_coefficients ,
90
67
distortion_coefficients )
91
68
69
+ # Debug: Print pose information
70
+ print (f"Marker { ids [i ][0 ]} - Rotation: { rvec } , Translation: { tvec } " )
71
+
72
+ # Draw the axes of the marker
73
+ axis_length = 0.01
74
+ axis = np .float32 ([[axis_length ,0 ,0 ], [0 ,axis_length ,0 ], [0 ,0 ,- axis_length ]]).reshape (- 1 ,3 )
75
+ imgpts , jac = cv2 .projectPoints (axis , rvec , tvec , matrix_coefficients , distortion_coefficients )
76
+ frame = draw_axis (frame , imgpts , corners [i ][0 ][0 ])
77
+
92
78
cv2 .aruco .drawDetectedMarkers (frame , corners )
93
79
94
80
# Convert rotation vector to rotation matrix
@@ -104,9 +90,9 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
104
90
glLoadIdentity ()
105
91
glMultMatrixf (transformation_matrix .T )
106
92
107
- # Draw the 3D cube
93
+ # Draw the 3D object
108
94
glColor3f (0.0 , 1.0 , 0.0 ) # Set color to green
109
- draw_cube ( 0.02 ) # Draw a cube with side length 0.02 (same as marker size)
95
+ obj . render ( ) # Render the loaded OBJ file
110
96
111
97
return frame
112
98
@@ -121,12 +107,20 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
121
107
cap .set (cv2 .CAP_PROP_FRAME_WIDTH , 1280 )
122
108
cap .set (cv2 .CAP_PROP_FRAME_HEIGHT , 720 )
123
109
110
+ # Debug: Check if camera is opened successfully
111
+ if not cap .isOpened ():
112
+ print ("Error: Could not open camera." )
113
+ sys .exit ()
114
+
124
115
# Initialize Pygame and OpenGL
125
116
pygame .init ()
126
117
display = (1280 , 720 )
127
118
pygame .display .set_mode (display , DOUBLEBUF | OPENGL )
128
119
init_gl (1280 , 720 )
129
120
121
+ # Load the OBJ file
122
+ obj = Obj ("objects/cube.obj" , swapyz = True )
123
+
130
124
while True :
131
125
ret , frame = cap .read ()
132
126
@@ -148,6 +142,9 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
148
142
# Process the frame and estimate pose
149
143
output = pose_estimation (frame , ARUCO_DICT [aruco_type ], intrinsic_camera , distortion )
150
144
145
+ # Debug: Display the frame with ArUco markers and axes
146
+ cv2 .imshow ('ArUco Detection' , output )
147
+
151
148
# Convert the OpenCV output to a Pygame surface
152
149
output = cv2 .cvtColor (output , cv2 .COLOR_BGR2RGB )
153
150
output = np .rot90 (output )
@@ -162,7 +159,14 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
162
159
for event in pygame .event .get ():
163
160
if event .type == pygame .QUIT :
164
161
pygame .quit ()
165
- quit ()
162
+ cap .release ()
163
+ cv2 .destroyAllWindows ()
164
+ sys .exit ()
165
+
166
+ # Break the loop if 'q' is pressed
167
+ if cv2 .waitKey (1 ) & 0xFF == ord ('q' ):
168
+ break
166
169
167
170
cap .release ()
168
- cv2 .destroyAllWindows ()
171
+ cv2 .destroyAllWindows ()
172
+ pygame .quit ()
0 commit comments