├── .gitignore ├── GUI.py ├── PBVS.py ├── README.md ├── __pycache__ ├── GUI.cpython-310.pyc ├── GUI.cpython-38.pyc ├── GUI2.cpython-38.pyc ├── PBVS.cpython-38.pyc ├── aruco_axis.cpython-310.pyc ├── axis.cpython-310.pyc ├── config.cpython-38.pyc ├── parameters.cpython-38.pyc ├── rotm2euler.cpython-310.pyc └── rotm2euler.cpython-38.pyc ├── aruco_axis.py ├── bin ├── camera_matrix.npz ├── desired_euler_angles.npy ├── desired_pose.npy └── desired_realworld_tvec.npy └── rotm2euler.py /.gitignore: -------------------------------------------------------------------------------- 1 | /GIFs 2 | *.pdf -------------------------------------------------------------------------------- /GUI.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import numpy as np 3 | import matplotlib.pyplot as plt 4 | 5 | # Colors 6 | RED = (0, 0, 255) 7 | GREEN = (100, 255, 0) 8 | BLUE = (255, 100, 0) 9 | WHITE = (255, 255, 255) 10 | 11 | # Text parameters 12 | LINE_TYPE = cv2.LINE_AA 13 | FONT_TYPE = cv2.FONT_HERSHEY_PLAIN 14 | 15 | # Tolerance 16 | MILIMETERS_TOLERANCE = 10 # milimeters 17 | DEGREES_TOLERANCE = 10 # degrees 18 | 19 | def display_background(img): 20 | """Outputs the background lines for the GUI. 21 | 22 | Parameters 23 | ---------- 24 | img : array-like 25 | Image to be displayed. 26 | 27 | Returns 28 | ------- 29 | None 30 | """ 31 | # Flip by default shape to discharge in the correct variables 32 | x, y = img.shape[:2][::-1] 33 | 34 | cv2.line(img, (0, y//4), (x, y//4), WHITE, 3) 35 | cv2.line(img, (0, y//2+20), (x, y//2+20), WHITE, 3) 36 | cv2.line(img, (x//3, 0), (x//3, y//2+20), WHITE, 3) 37 | cv2.line(img, ((x//3)*2, 0), ((x//3)*2, y//2+20), WHITE, 3) 38 | 39 | def display_translation_info(img, tvec, tvec_d): 40 | """Outputs the translation info of the ArUco marker. 41 | 42 | Parameters 43 | ---------- 44 | img : array-like 45 | Image to be written on the information. 46 | tvec : array-like 47 | Array with the x, y, and z positions. 48 | tvec_d : array-like 49 | Array with the desired x, y, and z positions. 50 | 51 | Returns 52 | ------- 53 | None 54 | """ 55 | x = tvec[0] 56 | y = tvec[1] 57 | z = tvec[2] 58 | 59 | x_d = tvec_d[0] 60 | y_d = tvec_d[1] 61 | z_d = tvec_d[2] 62 | 63 | error_x = x - x_d 64 | error_y = y - y_d 65 | error_z = z - z_d 66 | 67 | current_x_str = f'x: {x:.0f} mm' 68 | current_y_str = f'y: {y:.0f} mm' 69 | current_z_str = f'z: {z:.0f} mm' 70 | 71 | desired_x_str = f'xd: {x_d:.0f} mm' 72 | desired_y_str = f'yd: {y_d:.0f} mm' 73 | desired_z_str = f'zd: {z_d:.0f} mm' 74 | 75 | error_x_str = f'xe: {error_x:.0f} mm' 76 | error_y_str = f'ye: {error_y:.0f} mm' 77 | error_z_str = f'ze: {error_z:.0f} mm' 78 | 79 | cv2.putText(img, current_x_str, (10, 20), FONT_TYPE, 2, GREEN, 1, LINE_TYPE) 80 | cv2.putText(img, current_y_str, (10, 70), FONT_TYPE, 2, RED, 1, LINE_TYPE) 81 | cv2.putText(img, current_z_str, (10, 120), FONT_TYPE, 2, BLUE, 1, LINE_TYPE) 82 | 83 | cv2.putText(img, desired_x_str, (250, 20), FONT_TYPE, 2, GREEN, 1, LINE_TYPE) 84 | cv2.putText(img, desired_y_str, (250, 75), FONT_TYPE, 2, RED, 1, LINE_TYPE) 85 | cv2.putText(img, desired_z_str, (250, 120), FONT_TYPE, 2, BLUE, 1, LINE_TYPE) 86 | 87 | cv2.putText(img, error_x_str, (490, 20), FONT_TYPE, 2, GREEN, 1, LINE_TYPE) 88 | cv2.putText(img, error_y_str, (490, 75), FONT_TYPE, 2, RED, 1, LINE_TYPE) 89 | cv2.putText(img, error_z_str, (490, 120), FONT_TYPE, 2, BLUE, 1, LINE_TYPE) 90 | 91 | def display_rotation_info(img, euler, euler_d): 92 | """Outputs the translation info of the ArUco marker. 93 | 94 | Parameters 95 | ---------- 96 | img : array-like 97 | Image to be written on the information. 98 | euler : array-like: 99 | Array with the roll, pitch, and yaw orientations. 100 | euler_d : array-like 101 | Array with the desired roll, pitch, and yaw orientations. 102 | 103 | Returns 104 | ------- 105 | None 106 | """ 107 | roll = euler[0] 108 | pitch = euler[1] 109 | yaw = euler[2] 110 | 111 | roll_d = euler_d[0] 112 | pitch_d = euler_d[1] 113 | yaw_d = euler_d[2] 114 | 115 | error_roll = roll - roll_d 116 | error_pitch = pitch - pitch_d 117 | error_yaw = yaw - yaw_d 118 | 119 | current_roll_str = f'R: {roll:.0f} deg' 120 | current_pitch_str = f'P: {pitch:.0f} deg' 121 | current_yaw_str = f'Y: {yaw:.0f} deg' 122 | 123 | desired_roll_str = f'Rd: {roll_d:.0f} deg' 124 | desired_pitch_str = f'Pd: {pitch_d:.0f} deg' 125 | desired_yaw_str = f'Yd: {yaw_d:.0f} deg' 126 | 127 | error_roll_str = f'Re: {error_roll:.0f} deg' 128 | error_pitch_str = f'Pe: {error_pitch:.0f} deg' 129 | error_yaw_str = f'Ye: {error_yaw:.0f} deg' 130 | 131 | cv2.putText(img, current_roll_str, (10, 200), FONT_TYPE, 2, GREEN, 1, LINE_TYPE) 132 | cv2.putText(img, current_pitch_str, (10, 250), FONT_TYPE, 2, RED, 1, LINE_TYPE) 133 | cv2.putText(img, current_yaw_str, (10, 300), FONT_TYPE, 2, BLUE, 1, LINE_TYPE) 134 | 135 | cv2.putText(img, desired_roll_str, (250, 200), FONT_TYPE, 2, GREEN, 1, LINE_TYPE) 136 | cv2.putText(img, desired_pitch_str, (250, 250), FONT_TYPE, 2, RED, 1, LINE_TYPE) 137 | cv2.putText(img, desired_yaw_str, (250, 300), FONT_TYPE, 2, BLUE, 1, LINE_TYPE) 138 | 139 | cv2.putText(img, error_roll_str, (490, 200), FONT_TYPE, 2, GREEN, 1, LINE_TYPE) 140 | cv2.putText(img, error_pitch_str, (490, 250), FONT_TYPE, 2, RED, 1, LINE_TYPE) 141 | cv2.putText(img, error_yaw_str, (490, 300), FONT_TYPE, 2, BLUE, 1, LINE_TYPE) 142 | 143 | def display_interpretation(img, tvec, euler, tvec_d, euler_d): 144 | """Outputs a message to the user/robot stating how to move. 145 | 146 | Parameters 147 | ---------- 148 | img : array-like 149 | Image to be written on the information. 150 | tvec : array-like 151 | Array with the x, y, and z positions. 152 | euler : array-like 153 | Array with the roll, pitch, and yaw orientations. 154 | tvec_d : array-like 155 | Array with the desired x, y, and z positions. 156 | euler_d : array-like 157 | Array with the desired roll, pitch, and yaw orientations. 158 | 159 | Returns 160 | ------- 161 | None 162 | """ 163 | x = tvec[0] 164 | y = tvec[1] 165 | z = tvec[2] 166 | 167 | roll = euler[0] 168 | pitch = euler[1] 169 | yaw = euler[2] 170 | 171 | x_d = tvec_d[0] 172 | y_d = tvec_d[1] 173 | z_d = tvec_d[2] 174 | 175 | roll_d = euler_d[0] 176 | pitch_d = euler_d[1] 177 | yaw_d = euler_d[2] 178 | 179 | error_x = x - x_d 180 | error_y = y - y_d 181 | error_z = z - z_d 182 | 183 | error_roll = roll - roll_d 184 | error_pitch = pitch - pitch_d 185 | error_yaw = yaw - yaw_d 186 | 187 | if is_success_roll(img, error_roll): 188 | if is_success_pitch(img, error_pitch): 189 | if is_success_yaw(img, error_yaw): 190 | if is_success_x(img, error_x): 191 | if is_success_y(img, error_y): 192 | is_success_z(img, error_z) 193 | 194 | def is_success_x(img, error): 195 | """Displays the success message for the x-axis.""" 196 | message_right = f'Translate {abs(error):.0f}mm to the right' 197 | message_left = f'Translate {abs(error):.0f}mm to the left' 198 | message_x_success = 'You\'ve reached the desired position in x!' 199 | 200 | if error >= -MILIMETERS_TOLERANCE and error <= MILIMETERS_TOLERANCE: 201 | cv2.putText(img, message_x_success, (10, 430), FONT_TYPE, 1, GREEN, 1, LINE_TYPE) 202 | return True 203 | elif error > MILIMETERS_TOLERANCE: 204 | cv2.putText(img, message_left, (10, 430), FONT_TYPE, 1, WHITE, 1, LINE_TYPE) 205 | elif error < MILIMETERS_TOLERANCE: 206 | cv2.putText(img, message_right, (10, 430), FONT_TYPE, 1, WHITE, 1, LINE_TYPE) 207 | 208 | def is_success_y(img, error): 209 | """Displays the success message for the y-axis.""" 210 | message_up = f'Translate {abs(error):.0f}mm up' 211 | message_down = f'Translate {abs(error):.0f}mm down' 212 | message_y_success = 'You\'ve reached the desired position in y!' 213 | 214 | if error >= -MILIMETERS_TOLERANCE and error <= MILIMETERS_TOLERANCE: 215 | cv2.putText(img, message_y_success, (10, 450), FONT_TYPE, 1, GREEN, 1, LINE_TYPE) 216 | return True 217 | elif error > MILIMETERS_TOLERANCE: 218 | cv2.putText(img, message_down, (10, 450), FONT_TYPE, 1, WHITE, 1, LINE_TYPE) 219 | elif error < MILIMETERS_TOLERANCE: 220 | cv2.putText(img, message_up, (10, 450), FONT_TYPE, 1, WHITE, 1, LINE_TYPE) 221 | 222 | def is_success_z(img, error): 223 | """Displays the success message for the z-axis.""" 224 | message_frontwards = f'Translate {abs(error):.0f}mm frontwards' 225 | message_backwards = f'Translate {abs(error):.0f}mm backwards' 226 | message_z_success = 'You\'ve reached the desired position in z!' 227 | 228 | if error >= -MILIMETERS_TOLERANCE and error <= MILIMETERS_TOLERANCE: 229 | cv2.putText(img, message_z_success, (10, 470), FONT_TYPE, 1, GREEN, 1, LINE_TYPE) 230 | return True 231 | elif error > MILIMETERS_TOLERANCE: 232 | cv2.putText(img, message_frontwards, (10, 470), FONT_TYPE, 1, WHITE, 1, LINE_TYPE) 233 | elif error < MILIMETERS_TOLERANCE: 234 | cv2.putText(img, message_backwards, (10, 470), FONT_TYPE, 1, WHITE, 1, LINE_TYPE) 235 | 236 | def is_success_roll(img, error): 237 | """Displays the success message for the roll angle.""" 238 | message_roll = f'Rotate {error:.0f} deg around y' 239 | message_roll_success = 'You\'ve reached the desired position in roll!' 240 | 241 | if error >= -DEGREES_TOLERANCE and error <= DEGREES_TOLERANCE: 242 | cv2.putText(img, message_roll_success, (10, 350), FONT_TYPE, 1, GREEN, 1, LINE_TYPE) 243 | return True 244 | elif error > DEGREES_TOLERANCE or error < DEGREES_TOLERANCE: 245 | cv2.putText(img, message_roll, (10, 350), FONT_TYPE, 1, WHITE, 1, LINE_TYPE) 246 | 247 | def is_success_pitch(img, error): 248 | """Displays the success message for the pitch angle.""" 249 | message_pitch = f'Rotate {error:.0f} deg around x' 250 | message_pitch_success = 'You\'ve reached the desired position in pitch!' 251 | 252 | if error >= -DEGREES_TOLERANCE and error <= DEGREES_TOLERANCE: 253 | cv2.putText(img, message_pitch_success, (10, 370), FONT_TYPE, 1, GREEN, 1, LINE_TYPE) 254 | return True 255 | elif error > DEGREES_TOLERANCE or error < DEGREES_TOLERANCE: 256 | cv2.putText(img, message_pitch, (10, 370), FONT_TYPE, 1, WHITE, 1, LINE_TYPE) 257 | 258 | def is_success_yaw(img, error): 259 | """Displays the success message for the yaw angle.""" 260 | message_yaw = f'Rotate {error:.0f} deg around z' 261 | message_yaw_success = 'You\'ve reached the desired position in yaw!' 262 | 263 | if error >= -DEGREES_TOLERANCE and error <= DEGREES_TOLERANCE: 264 | cv2.putText(img, message_yaw_success, (10, 390), FONT_TYPE, 1, GREEN, 1, LINE_TYPE) 265 | return True 266 | elif error > DEGREES_TOLERANCE or error < DEGREES_TOLERANCE: 267 | cv2.putText(img, message_yaw, (10, 390), FONT_TYPE, 1, WHITE, 1, LINE_TYPE) 268 | 269 | def display_info_on_screen(img, tvec, euler, tvec_d, euler_d): 270 | """Outputs the pose and desired pose of the ArUco marker on screen. 271 | 272 | Parameters 273 | ---------- 274 | img : array-like 275 | Image to be written on the information. 276 | tvec : array-like 277 | Array with the x, y, and z positions. 278 | euler : array-like 279 | Array with the roll, pitch, and yaw orientations. 280 | tvec_d : array-like 281 | Array with the desired x, y, and z positions. 282 | euler_d : array-like 283 | Array with the desired roll, pitch, and yaw orientations. 284 | 285 | Returns 286 | ------- 287 | None 288 | """ 289 | x = tvec[0] 290 | y = tvec[1] 291 | z = tvec[2] 292 | 293 | roll = euler[0] 294 | pitch = euler[1] 295 | yaw = euler[2] 296 | 297 | x_d = tvec_d[0] 298 | y_d = tvec_d[1] 299 | z_d = tvec_d[2] 300 | 301 | roll_d = euler_d[0] 302 | pitch_d = euler_d[1] 303 | yaw_d = euler_d[2] 304 | 305 | desired_realworld_tvec_x_str = f'Desired x: {x_d:.0f} mm' 306 | desired_realworld_tvec_y_str = f'Desired y: {y_d:.0f} mm' 307 | desired_realworld_tvec_z_str = f'Desired z: {z_d:.0f} mm' 308 | 309 | error_x_str = f'Error x: {x - x_d:.0f} mm' 310 | error_y_str = f'Error y: {y - y_d:.0f} mm' 311 | error_z_str = f'Error z: {z - z_d:.0f} mm' 312 | 313 | desired_euler_angles_roll_str = f'Desired roll: {roll_d:.0f} deg' 314 | desired_euler_angles_pitchstr = f'Desired pitch: {pitch_d:.0f} deg' 315 | desired_euler_angles_yaw_str = f'Desired yaw: {yaw_d:.0f} deg' 316 | 317 | error_roll_str = f'Error roll: {roll - roll_d:.0f} deg' 318 | error_pitch_str = f'Error pitch: {pitch - pitch_d:.0f} deg' 319 | error_yaw_str = f'Error yaw: {yaw - yaw_d:.0f} deg' 320 | 321 | current_x_str = f'x = {x:.0f} mm' 322 | current_y_str = f'y = {y:.0f} mm' 323 | current_z_str = f'z = {z:.0f} mm' 324 | 325 | current_pitch_str = f'pitch = {pitch:.0f} deg' 326 | current_roll_str = f'roll = {roll:.0f} deg' 327 | current_yaw_str = f'yaw = {yaw:.0f} deg' 328 | 329 | cv2.putText(img, desired_realworld_tvec_x_str, (450, 10), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 330 | cv2.putText(img, desired_realworld_tvec_y_str, (450, 20), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 331 | cv2.putText(img, desired_realworld_tvec_z_str, (450, 30), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 332 | 333 | cv2.putText(img, error_x_str, (450, 50), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 334 | cv2.putText(img, error_y_str, (450, 60), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 335 | cv2.putText(img, error_z_str, (450, 70), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 336 | 337 | cv2.putText(img, desired_euler_angles_roll_str, (450, 90), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 338 | cv2.putText(img, desired_euler_angles_pitchstr, (450, 100), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 339 | cv2.putText(img, desired_euler_angles_yaw_str, (450, 110), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 340 | 341 | cv2.putText(img, error_roll_str, (450, 130), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 342 | cv2.putText(img, error_pitch_str, (450, 140), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 343 | cv2.putText(img, error_yaw_str, (450, 150), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 344 | 345 | cv2.putText(img, current_x_str, (5, 10), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 346 | cv2.putText(img, current_y_str, (5, 20), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 347 | cv2.putText(img, current_z_str, (5, 30), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 348 | 349 | cv2.putText(img, current_roll_str, (5, 50), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 350 | cv2.putText(img, current_pitch_str, (5, 60), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 351 | cv2.putText(img, current_yaw_str, (5, 70), FONT_TYPE, 0.8, RED, 1, LINE_TYPE) 352 | 353 | def display_pose_graphs(time, current_time, x, y, z, R, P, Y, axis): 354 | """Draws the pose graphs of the ArUco marker. 355 | 356 | Parameters 357 | ---------- 358 | time : list 359 | List containing the time elapsed. 360 | x : list 361 | List containing the x positions stored of the ArUco marker. 362 | y : list 363 | List containing the y positions stored of the ArUco marker. 364 | z : list 365 | List containing the z positions stored of the ArUco marker. 366 | R : list 367 | List containing the roll orientation stored of the ArUco marker. 368 | P : list 369 | List containing the pitch orientation stored of the ArUco marker. 370 | Y : list 371 | List containing the yaw orientation stored of the ArUco marker. 372 | axis : array-like 373 | Axis to be displayed on. 374 | 375 | Returns 376 | ------- 377 | None 378 | """ 379 | axis[0].plot(time, x, color='b', label='x') 380 | axis[0].plot(time, y, color='g', label='y') 381 | axis[0].plot(time, z, color='r', label='z') 382 | 383 | axis[1].plot(time, R, color='g', label='R') 384 | axis[1].plot(time, P, color='r', label='P') 385 | axis[1].plot(time, Y, color='b', label='Y') 386 | 387 | axis[0].set_xlim(left=max(0, current_time-10), right=current_time+10) 388 | axis[1].set_xlim(left=max(0, current_time-10), right=current_time+10) 389 | 390 | if len(x) == 1: 391 | axis[0].legend(loc='upper right') 392 | axis[0].set_ylabel('Camera \nposition (mm)') 393 | 394 | axis[1].legend(loc='upper right') 395 | axis[1].set_xlabel('Time (s)') 396 | axis[1].set_ylabel('Camera \norientation (deg)') 397 | 398 | def display_error_graphs(time, current_time, x_e, y_e, z_e, R_e, P_e, Y_e, axis): 399 | """Draws the pose graphs of the ArUco marker. 400 | 401 | Parameters 402 | ---------- 403 | time : list 404 | List containing the time elapsed. 405 | x_e : list 406 | List containing the x position error recorded of the ArUco marker. 407 | y_e : list 408 | List containing the y position error recorded of the ArUco marker. 409 | z_e : list 410 | List containing the z position error recorded of the ArUco marker. 411 | R_e : list 412 | List containing the roll angle recorded of the ArUco marker. 413 | P_e : list 414 | List containing the pitch angle recorded of the ArUco marker. 415 | Y_e : list 416 | List containing the yaw angle recorded of the ArUco marker. 417 | axis : array-like 418 | Axis to be displayed on. 419 | 420 | Returns 421 | ------- 422 | None 423 | """ 424 | axis[0].plot(time, x_e, color='g', label='Error x') 425 | axis[0].plot(time, y_e, color='r', label='Error y') 426 | axis[0].plot(time, z_e, color='b', label='Error z') 427 | 428 | axis[1].plot(time, R_e, color='g', label='Error R') 429 | axis[1].plot(time, P_e, color='r', label='Error P') 430 | axis[1].plot(time, Y_e, color='b', label='Error Y') 431 | 432 | axis[0].set_xlim(left=max(0, current_time-10), right=current_time+10) 433 | axis[1].set_xlim(left=max(0, current_time-10), right=current_time+10) 434 | 435 | if len(x_e) == 1: 436 | axis[0].legend(loc='upper right') 437 | axis[0].set_ylabel('Camera \nposition error (mm)') 438 | 439 | axis[1].legend(loc='upper right') 440 | axis[1].set_xlabel('Time (s)') 441 | axis[1].set_ylabel('Camera \norientation error (deg)') -------------------------------------------------------------------------------- /PBVS.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | import cv2.aruco as aruco 3 | import numpy as np 4 | import rotm2euler 5 | import matplotlib.pyplot as plt 6 | import math 7 | import os 8 | import time 9 | import GUI 10 | import aruco_axis 11 | import argparse 12 | 13 | # Command line arguments 14 | parser = argparse.ArgumentParser(description='Implements the Position-Based Visual Servo.') 15 | parser.add_argument('-sc', '--show_charts', type=bool, action=argparse.BooleanOptionalAction, 16 | metavar='', required=False, help='Shows the charts of position and orientationin real time') 17 | parser.add_argument('-sg', '--show_gui', default=True, type=bool, 18 | action=argparse.BooleanOptionalAction, metavar='', required=False, 19 | help='Shows the gui to reach the desired pose of the ArUco marker') 20 | args = parser.parse_args() 21 | 22 | MARKER_SIZE = 95 # milimeters 23 | RED = (0, 0, 255) 24 | GREEN = (0, 255, 0) 25 | BLUE = (255, 0, 0) 26 | 27 | def main(): 28 | # Read intrinsic parameters of the camera 29 | with np.load('bin/camera_matrix.npz') as X: 30 | K, dist, rvecs, tvecs = [X[i] for i in ('camera_matrix', 'dist', 'rvecs', 'tvecs')] 31 | 32 | # Define the 4X4 bit ArUco tag 33 | ARUCO_DICT = aruco.getPredefinedDictionary(aruco.DICT_4X4_250) 34 | 35 | # Define camera to use and set resolution and frame rate 36 | cap = cv2.VideoCapture(1) 37 | cap.set(cv2.CAP_PROP_FOURCC,cv2.VideoWriter_fourcc('M','J','P','G')) 38 | 39 | # Define coordinates in object coordinate space (3D space) 40 | obj_points = np.zeros((5, 3), np.float32) 41 | obj_points[1, 0], obj_points[1, 1], obj_points[2, 0] = -MARKER_SIZE/2, -MARKER_SIZE/2, MARKER_SIZE/2 42 | obj_points[2, 1], obj_points[3, 0], obj_points[3, 1] = -MARKER_SIZE/2, MARKER_SIZE/2, MARKER_SIZE/2 43 | obj_points[4, 0], obj_points[4, 1] = -MARKER_SIZE/2, MARKER_SIZE/2 44 | 45 | # 3D axis coordinates to be drawn on the ArUco marker 46 | axis = np.float32([[45, 0, 0], [0, -45, 0], [0, 0, -45]]).reshape(-1, 3) 47 | 48 | estimated_pose = np.identity(n=4, dtype=np.float64) # Estimated pose matrix 49 | desired_pose = np.identity(n=4, dtype=np.float64) # Desired pose matrix 50 | 51 | # Lists that will store pose info to plot it afterwards 52 | roll_list, pitch_list, yaw_list = [], [], [] 53 | x_list, y_list, z_list = [], [], [] 54 | x_e_list, y_e_list, z_e_list = [], [], [] 55 | roll_e_list, pitch_e_list, yaw_e_list = [], [], [] 56 | time_list = [] 57 | 58 | if args.show_charts: 59 | figure1, ax1 = plt.subplots(nrows=2, ncols=1, figsize=(7, 5)) 60 | figure2, ax2 = plt.subplots(nrows=2, ncols=1, figsize=(7, 5)) 61 | start_time = time.time() 62 | 63 | if args.show_gui: 64 | # Set up the GUI window to display the info 65 | root = 'PBVS - info' 66 | cv2.namedWindow(root) 67 | img_info = np.ones((600, 700, 3), np.uint8) 68 | 69 | while True: 70 | # Read frames of the camera 71 | ret, frame = cap.read() 72 | 73 | if args.show_gui: 74 | cv2.namedWindow(root) 75 | img_info = np.ones((600, 700, 3), np.uint8) 76 | 77 | # Convert image to gray scale 78 | frame_gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) 79 | 80 | corners, ids, rejected = aruco.detectMarkers(frame_gray, ARUCO_DICT, K, dist) 81 | 82 | # Verify at least one ArUco marker was detected 83 | if len(corners) > 0 or ids is not None: 84 | try: 85 | desired_pose = np.load('bin/desired_pose.npy') 86 | desired_realworld_tvec = np.load('bin/desired_realworld_tvec.npy') 87 | desired_euler_angles = np.load('bin/desired_euler_angles.npy') 88 | except FileNotFoundError: 89 | print('[INFO]: FileNotFoundError handled, check if all .npy files were loaded') 90 | pass 91 | 92 | aruco.drawDetectedMarkers(frame, corners) 93 | 94 | # Center point between the 4 corners 95 | aruco_center = np.asarray((abs(corners[0][0][2][0] + corners[0][0][0][0])//2, 96 | abs(corners[0][0][2][1] + corners[0][0][0][1])//2)).astype(int) 97 | 98 | # Array with the center of the ArUco marker 99 | new_corners = np.array([np.vstack((aruco_center, corners[0][0]))]) 100 | 101 | # Draw axis and corners of the markers 102 | for marker in range(len(ids)): 103 | for corner in range(4): 104 | try: 105 | # Find the rotation and translation vectors 106 | _, rvec, tvec = cv2.solvePnP(obj_points, new_corners[marker], K, dist) 107 | 108 | corner_x, corner_y = corners[marker][0, corner] 109 | center_coordinates = tuple((int(corner_x), int(corner_y))) 110 | 111 | cv2.circle(frame, center_coordinates, 3, BLUE, -1) 112 | cv2.circle(frame, aruco_center, 3, BLUE, -1) 113 | 114 | corner_xy_str = '({0}, {1})'.format(corner_x, corner_y) 115 | cv2.putText(frame, corner_xy_str, center_coordinates, cv2.FONT_HERSHEY_PLAIN, 116 | 0.8, RED, 1, cv2.LINE_AA) 117 | 118 | img_pts, jac = cv2.projectPoints(axis, rvec, tvec, K, dist) 119 | aruco_axis.draw_axis(frame, new_corners[marker], img_pts) 120 | 121 | rvec_flipped = -rvec.ravel() 122 | tvec_flipped = -tvec.ravel() 123 | 124 | # Convert rvec to a rotation matrix, and then to a Euler angles 125 | R, jacobian = cv2.Rodrigues(rvec_flipped) 126 | 127 | # From image plane to world coordinates 128 | realworld_tvec = np.dot(R, tvec_flipped) 129 | realworld_tvec[1], realworld_tvec[2] = -realworld_tvec[1], -realworld_tvec[2] 130 | 131 | # Conversion euler angles in radians, and then to degrees 132 | pitch, roll, yaw = rotm2euler.rotation_matrix_to_euler_angles(R) 133 | pitch, roll, yaw = math.degrees(pitch), math.degrees(roll), math.degrees(yaw) 134 | estimated_euler_angles = np.array([roll, pitch, yaw]) 135 | 136 | # Construct homogeneous transformation matrix 137 | estimated_pose[:3, :3] = R 138 | estimated_pose[:3, 3] = realworld_tvec 139 | except IndexError: 140 | print('[INFO]: IndexError handled') 141 | continue 142 | 143 | if args.show_gui: 144 | GUI.display_info_on_screen(img=frame, tvec=realworld_tvec, euler=estimated_euler_angles, 145 | tvec_d=desired_realworld_tvec, euler_d=desired_euler_angles) 146 | 147 | # Store pose, and pose error values to plot them 148 | x_list.append(realworld_tvec[0]) 149 | y_list.append(realworld_tvec[1]) 150 | z_list.append(realworld_tvec[2]) 151 | 152 | roll_list.append(roll) 153 | pitch_list.append(pitch) 154 | yaw_list.append(yaw) 155 | 156 | x_e_list.append(realworld_tvec[0] - desired_realworld_tvec[0]) 157 | y_e_list.append(realworld_tvec[1] - desired_realworld_tvec[1]) 158 | z_e_list.append(realworld_tvec[2] - desired_realworld_tvec[2]) 159 | 160 | roll_e_list.append(roll - desired_euler_angles[0]) 161 | pitch_e_list.append(pitch - desired_euler_angles[1]) 162 | yaw_e_list.append(yaw - desired_euler_angles[2]) 163 | 164 | 165 | if args.show_charts: 166 | current_time = time.time() - start_time 167 | time_list.append(current_time) 168 | GUI.display_pose_graphs(time=time_list, current_time=current_time, x=x_list, y=y_list, 169 | z=z_list, R= roll_list, P=pitch_list, Y=yaw_list, axis=ax1) 170 | 171 | GUI.display_error_graphs(time=time_list, current_time=current_time, x_e=x_e_list, 172 | y_e=y_e_list, z_e=z_e_list, R_e= roll_e_list, P_e=pitch_e_list, Y_e=yaw_e_list, 173 | axis=ax2) 174 | 175 | plt.pause(0.001) # To constantly refresh the graph 176 | 177 | if args.show_gui: 178 | GUI.display_translation_info(img=img_info, tvec=realworld_tvec, 179 | tvec_d=desired_realworld_tvec) 180 | GUI.display_rotation_info(img=img_info, euler=estimated_euler_angles, 181 | euler_d=desired_euler_angles) 182 | GUI.display_interpretation(img=img_info, tvec=realworld_tvec, euler=estimated_euler_angles, 183 | tvec_d=desired_realworld_tvec, euler_d=desired_euler_angles) 184 | 185 | if args.show_gui: 186 | GUI.display_background(img_info) 187 | cv2.imshow(root, img_info) 188 | 189 | cv2.imshow('PVBS - RGB', frame) 190 | 191 | # If 'q' pressed, save the current pose of the ArUco marker 192 | if cv2.waitKey(1) & 0xFF == ord('q'): 193 | desired_pose = estimated_pose 194 | desired_euler_angles = estimated_euler_angles 195 | desired_realworld_tvec = realworld_tvec 196 | 197 | np.save('bin/desired_pose.npy', desired_pose) 198 | np.save('bin/desired_euler_angles.npy', desired_euler_angles) 199 | np.save('bin/desired_realworld_tvec.npy', desired_realworld_tvec) 200 | 201 | print('[INFO]: ArUco marker pose saved') 202 | 203 | # If ESC pressed exit 204 | if cv2.waitKey(1) & 0xFF == 27: 205 | break 206 | 207 | # Close all 208 | cap.release() 209 | cv2.destroyAllWindows() 210 | 211 | if __name__ == '__main__': 212 | main() -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Position-Based Visual Servo 2 | 3 |

4 | 5 | 6 |

7 | 8 | ## Description 9 | An implementation of the Position-Based Visual Servo (PBVS) in Python 3.10 with OpenCV 4.6.0. The main reference paper is [Visual servo control, Part I: Basic approaches](https://hal.inria.fr/inria-00350283/file/2006_ieee_ram_chaumette.pdf) 10 | by F. Chaumette and S. Hutchinson. 11 | 12 | The purpose of visual servoing is to have control over the end-effector relative to the goal. For this, image features of the target are extracted, and preprocessed to 13 | eventually localize the target by its position. 14 | 15 |

16 | 17 |

18 | 19 |

20 | Fig.1. Position-based visual servo. 21 |

22 | 23 | ### ArUco Marker 24 | The target used as goal is a [fiducial marker](https://en.wikipedia.org/wiki/Fiducial_marker) known as ArUco marker, more concise information can be found in [Automatic generation and detection of highly reliable fiducial markers 25 | under occlusion](https://www.google.com/url?sa=t&rct=j&q=&esrc=s&source=web&cd=&cad=rja&uact=8&ved=2ahUKEwiXuceWuLP5AhUcL0QIHXQ4BosQgAMoAHoECAEQAg&url=https%3A%2F%2Fscholar.google.com.mx%2Fscholar_url%3Furl%3Dhttps%3A%2F%2Fcode.ihub.org.cn%2Fprojects%2F641%2Frepository%2Frevisions%2Fmaster%2Fentry%2Freaded%2FAutomatic%252520generation%252520and%252520detection%252520of%252520highly%252520reliable%252520fiducial%252520markersnunder%252520occlusion.pdf%26hl%3Des%26sa%3DX%26ei%3DsQLvYqiwC8KjywSbu76AAg%26scisig%3DAAGBfm201CMNLnD07tNRdlkyyUG_rd1aJg%26oi%3Dscholarr&usg=AOvVaw0gk6onTq5VDi690-8xUhiS) 26 | by S. Garrido-Jurado, R. Muñoz-Salinas, F. J. Madrid-Cuevas, and M. J. Marín-Jiménez. 27 | 28 | The main reason for using this fiducial marker is because of the ease with which the pose of the camera can be estimated. Also, OpenCV offers a complete submodule dedicated exclusively for [detecting ArUco markers](https://docs.opencv.org/4.x/d5/dae/tutorial_aruco_detection.html). 29 | 30 | There exists a convenient platform to easily generate [online ArUco markers](https://chev.me/arucogen/). The **configuration that this project uses is:** 31 | 32 | - _Dictionary:_ **4x4 (50, 100, 250, 1000)**. 33 | - _Marker ID:_ **0 and 1**. 34 | - _Marker size, mm:_ **100**. 35 | 36 | However, I don't really use the _Marker ID_ parameter so that it can be any of the allowed ID's (0, 1, 2, 3...). Another recommendation is to crop around the ArUco in such a way 37 | that some white space is left, this will help for the better detection of the ArUco. Additionally, I recommend sticking it on a hard surface which will prevent 38 | that the ArUco bends, and it also helps for a better dectection. A piece of cardboard will be enough. Leaving a kind of cardboard piece at the end in order to 39 | have a better grip of the ArUco helped a lot. Otherwise, it's difficult not to occlude the ArUco. 40 | 41 | ### Graphical User Interface 42 | Essentially, the Graphical User Interface (GUI) works as a guide for whoever is running the program and wants to reach the final position with the help of a minimal but sufficient text indicator. 43 | This text let the user know what are the steps to follow to reach the goal position, and they are displayed sequentially. 44 | 45 |

46 | 47 | 48 |

49 | 50 | At the moment of showing the information, different windows are displayed. The top-left window is the current position in $x, y$ and $z$ the bottom-left window is the current 51 | orientation in the Euler angles $\phi, \theta$ and $\psi$. The top-middle window is the desired position $x_d, y_d$ and $z_d$ and the bottom-middle window is the desired 52 | orientation $\phi_d, \theta_d$ and $\psi_d$. Finally, the top-right window is the error in the position $x_e, y_e$ and $z_e$ and the bottom-right window is the error in the 53 | orientation $\phi_e, \theta_e$ and $\psi_e$. 54 | 55 | ### Charts 56 | I have decided to add some charts as a reference using the well-known ```matplotlib.pyplot``` library, this to have a tracking of the position and orientation as well 57 | as their position and orientation error of the camera with respect to the ArUco marker. 58 | 59 |

60 | 61 | 62 |

63 | 64 |

65 | Fig.2. Left: Position and orientation. Right: Position and orientation error. 66 |

67 | 68 | The charts are displayed in real-time which is something to consider before using it since it may consume a lot of the processor to draw the charts and therefore make 69 | the program to run slow or even the computer. The recommendation is to run the charts only when it is really necessary. 70 | 71 | 72 | ## Usage 73 | ``` 74 | usage: PBVS.py [-h] [-sc | --show_charts | --no-show_charts] [-sg | --show_gui | --no-show_gui] 75 | 76 | Implements the Position-Based Visual Servo. 77 | 78 | options: 79 | -h, --help show this help message and exit 80 | -sc, --show_charts, --no-show_charts 81 | Shows the charts of position and orientationin real time 82 | -sg, --show_gui, --no-show_gui 83 | Shows the gui to reach the desired pose of the ArUco marker (default: True) 84 | ``` 85 | 86 | To save the current pose of the ArUco marker relative to the camera the key ```q``` can be pressed. This will save the pose into a ```.npy``` file in the ```/bin``` directory. 87 | 88 | ## Examples 89 | To show the charts in real-time 90 | 91 | ```python3 PBVS.py --show_charts``` 92 | 93 | To not show the GUI 94 | 95 | ```python3 PBVS.py --no-show_gui``` 96 | 97 | ## License 98 | MIT License 99 | 100 | Copyright (c) [2022] [Angelo Espinoza] 101 | 102 | Permission is hereby granted, free of charge, to any person obtaining a copy 103 | of this software and associated documentation files (the "Software"), to deal 104 | in the Software without restriction, including without limitation the rights 105 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 106 | copies of the Software, and to permit persons to whom the Software is 107 | furnished to do so, subject to the following conditions: 108 | 109 | The above copyright notice and this permission notice shall be included in all 110 | copies or substantial portions of the Software. 111 | 112 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 113 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 114 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 115 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 116 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 117 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 118 | SOFTWARE. 119 | -------------------------------------------------------------------------------- /__pycache__/GUI.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/GUI.cpython-310.pyc -------------------------------------------------------------------------------- /__pycache__/GUI.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/GUI.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/GUI2.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/GUI2.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/PBVS.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/PBVS.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/aruco_axis.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/aruco_axis.cpython-310.pyc -------------------------------------------------------------------------------- /__pycache__/axis.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/axis.cpython-310.pyc -------------------------------------------------------------------------------- /__pycache__/config.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/config.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/parameters.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/parameters.cpython-38.pyc -------------------------------------------------------------------------------- /__pycache__/rotm2euler.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/rotm2euler.cpython-310.pyc -------------------------------------------------------------------------------- /__pycache__/rotm2euler.cpython-38.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/__pycache__/rotm2euler.cpython-38.pyc -------------------------------------------------------------------------------- /aruco_axis.py: -------------------------------------------------------------------------------- 1 | import cv2 2 | 3 | # Colors 4 | RED = (0, 0, 255) 5 | GREEN = (0, 255, 0) 6 | BLUE = (255, 0, 0) 7 | 8 | def draw_axis(img, start_pts, img_pts): 9 | start_point = list(start_pts[0].ravel().astype(int)) 10 | end_point_x = list(img_pts[0].ravel().astype(int)) 11 | end_point_y = list(img_pts[1].ravel().astype(int)) 12 | end_point_z = list(img_pts[2].ravel().astype(int)) 13 | 14 | cv2.arrowedLine(img, start_point, end_point_x, RED, 2) 15 | cv2.arrowedLine(img, start_point, end_point_y, GREEN, 2) 16 | cv2.arrowedLine(img, start_point, end_point_z, BLUE, 2) 17 | 18 | end_point_x[0] = end_point_x[0] + 10 19 | end_point_y[1] = end_point_y[1] - 10 20 | end_point_z[1] = end_point_z[1] + 10 21 | end_point_z[0] = end_point_z[0] - 10 22 | 23 | cv2.putText(img, 'x', end_point_x, cv2.FONT_HERSHEY_PLAIN, 0.8, RED, 1, cv2.LINE_AA) 24 | cv2.putText(img, 'y', end_point_y, cv2.FONT_HERSHEY_PLAIN, 0.8, GREEN, 1, cv2.LINE_AA) 25 | cv2.putText(img, 'z', end_point_z, cv2.FONT_HERSHEY_PLAIN, 0.8, BLUE, 1, cv2.LINE_AA) -------------------------------------------------------------------------------- /bin/camera_matrix.npz: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/bin/camera_matrix.npz -------------------------------------------------------------------------------- /bin/desired_euler_angles.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/bin/desired_euler_angles.npy -------------------------------------------------------------------------------- /bin/desired_pose.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/bin/desired_pose.npy -------------------------------------------------------------------------------- /bin/desired_realworld_tvec.npy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/AngeloEspinoza/position-based-visual-servo/995baf7c0be29dbfd5ff4bc557336230ae291a70/bin/desired_realworld_tvec.npy -------------------------------------------------------------------------------- /rotm2euler.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import math 3 | 4 | def is_rotation_matrix(R): 5 | """Checks if a matrix is a valid rotation matrix. 6 | 7 | Parameters 8 | ---------- 9 | R : array-like 10 | Rotation matrix. 11 | 12 | Returns 13 | ------- 14 | float 15 | Norm close to 0 (with precision of < 1e-6). 16 | """ 17 | Rt = np.transpose(R) 18 | should_be_identity = np.dot(Rt, R) 19 | I = np.identity(n=3, dtype=R.dtype) 20 | n = np.linalg.norm(I - should_be_identity) 21 | 22 | return n < 1e-6 23 | 24 | def rotation_matrix_to_euler_angles(R): 25 | """Calculates rotation matrix to euler angles 26 | The result is the same as MATLAB except the order 27 | the euler angles (x and z are swapped). 28 | 29 | Parameters 30 | ---------- 31 | R : array-like 32 | Rotation matrix. 33 | 34 | Returns 35 | ------- 36 | array-like 37 | Euler angles, a.k.a yaw, pitch, roll. 38 | """ 39 | 40 | assert(is_rotation_matrix(R)) 41 | 42 | sy = math.sqrt(R[0,0] * R[0, 0] + R[1, 0] * R[1, 0]) 43 | 44 | singular = sy < 1e-6 45 | 46 | if not singular: 47 | x = math.atan2(R[2, 1] , R[2, 2]) 48 | y = math.atan2(-R[2, 0], sy) 49 | z = math.atan2(R[1, 0], R[0, 0]) 50 | else: 51 | x = math.atan2(-R[1, 2], R[1, 1]) 52 | y = math.atan2(-R[2, 0], sy) 53 | z = 0 54 | 55 | return np.array([x, y, z]) --------------------------------------------------------------------------------