Skip to content

Commit cab10be

Browse files
...
1 parent 2d06883 commit cab10be

File tree

2 files changed

+43
-37
lines changed

2 files changed

+43
-37
lines changed

README.md

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,6 +27,8 @@
2727

2828
## How to Run:
2929

30+
pip install objloader
31+
3032
```bash
3133
pip install Cython==3.0.11
3234
pip install --upgrade pip setuptools wheel

scripts/arucoPoseObject.py

Lines changed: 41 additions & 37 deletions
Original file line numberDiff line numberDiff line change
@@ -7,7 +7,7 @@
77
from OpenGL.GLU import *
88
import pygame
99
from pygame.locals import *
10-
from objloader import OBJ
10+
from objloader import Obj
1111

1212
ARUCO_DICT = {
1313
"DICT_4X4_50": cv2.aruco.DICT_4X4_50,
@@ -44,38 +44,12 @@ def init_gl(width, height):
4444
gluPerspective(45.0, float(width)/float(height), 0.1, 100.0)
4545
glMatrixMode(GL_MODELVIEW)
4646

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
7953

8054
def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coefficients):
8155
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
@@ -84,11 +58,23 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
8458

8559
corners, ids, rejected_img_points = cv2.aruco.detectMarkers(gray, cv2.aruco_dict, parameters=parameters)
8660

61+
# Debug: Print number of markers detected
62+
print(f"Number of ArUco markers detected: {len(corners)}")
63+
8764
if len(corners) > 0:
8865
for i in range(0, len(ids)):
8966
rvec, tvec, markerPoints = cv2.aruco.estimatePoseSingleMarkers(corners[i], 0.02, matrix_coefficients,
9067
distortion_coefficients)
9168

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+
9278
cv2.aruco.drawDetectedMarkers(frame, corners)
9379

9480
# Convert rotation vector to rotation matrix
@@ -104,9 +90,9 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
10490
glLoadIdentity()
10591
glMultMatrixf(transformation_matrix.T)
10692

107-
# Draw the 3D cube
93+
# Draw the 3D object
10894
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
11096

11197
return frame
11298

@@ -121,12 +107,20 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
121107
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 1280)
122108
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 720)
123109

110+
# Debug: Check if camera is opened successfully
111+
if not cap.isOpened():
112+
print("Error: Could not open camera.")
113+
sys.exit()
114+
124115
# Initialize Pygame and OpenGL
125116
pygame.init()
126117
display = (1280, 720)
127118
pygame.display.set_mode(display, DOUBLEBUF | OPENGL)
128119
init_gl(1280, 720)
129120

121+
# Load the OBJ file
122+
obj = Obj("objects/cube.obj", swapyz=True)
123+
130124
while True:
131125
ret, frame = cap.read()
132126

@@ -148,6 +142,9 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
148142
# Process the frame and estimate pose
149143
output = pose_estimation(frame, ARUCO_DICT[aruco_type], intrinsic_camera, distortion)
150144

145+
# Debug: Display the frame with ArUco markers and axes
146+
cv2.imshow('ArUco Detection', output)
147+
151148
# Convert the OpenCV output to a Pygame surface
152149
output = cv2.cvtColor(output, cv2.COLOR_BGR2RGB)
153150
output = np.rot90(output)
@@ -162,7 +159,14 @@ def pose_estimation(frame, aruco_dict_type, matrix_coefficients, distortion_coef
162159
for event in pygame.event.get():
163160
if event.type == pygame.QUIT:
164161
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
166169

167170
cap.release()
168-
cv2.destroyAllWindows()
171+
cv2.destroyAllWindows()
172+
pygame.quit()

0 commit comments

Comments
 (0)