├── Homogeneous Coordinates.pdf ├── README.md ├── Robot-OpenCV - Camera Coordinates - My Code.py └── robot_position_estimation.py /Homogeneous Coordinates.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/TehseenHasan/robotic-arm-pick-and-place-OpenCv-Python/bc677667514ba0e0a1c72b586bad23090121ed64/Homogeneous Coordinates.pdf -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | These are the files/code of my pick and place robotic arm using OpenCV-Python. 2 | These code files are not so much organized as I did not find spare time to clean the code or write a good documentation/tutorial. 3 | I will try my best to wrate a complete guide and documentation about that complete system. 4 | 5 | **Steps:** 6 | 1. You have to install Python 3 7 | 2. Use this Robotic arm: https://github.com/20sffactory/community_robot_arm 8 | 3. CAD Model : https://grabcad.com/library/robot-arm-community-version-cad-3d-printed-robotic-arm-1 9 | 4. install OpenCv on your python environment 10 | 5. use a Digital camera 11 | 6. Mount the camera above the robotic arm at 90 degree position in such a way that it can capture the working area of the robotic arm clearly. 12 | 7. use a (~50mm wide, please read the code and comments in the code for details) blue rectangle cardboard as a place holder of the robotic arm and place it where you will place the robotic arm and run the robot_position_estimation.py code 13 | 8. This code will find the coordinates of the blue cardboard piece and considered it mid point as robotic arm origin. 14 | 9. place the robotic arm above that bliue cardboard precisely. 15 | 10. Now you can use the main code file. you have to calibrate the parameters in the code according to your environment and camera setup. 16 | 11. There are a lot of things to learn first before understanding these codes 17 | 12. Please watch tutorial videos from this great robotics cource: http://www.robogrok.com/index.html 18 | 13. Watach atleast these 3 videos to understand my codes: ![Contents](https://user-images.githubusercontent.com/25352528/116832263-a7fdb700-abcd-11eb-84db-5413ca39865c.jpg) 19 | -------------------------------------------------------------------------------- /Robot-OpenCV - Camera Coordinates - My Code.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import cv2 as cv 3 | import time 4 | import serial 5 | import sys 6 | import threading 7 | import yaml 8 | import warnings 9 | warnings.filterwarnings("ignore") 10 | 11 | 12 | #Constants Declaration 13 | webcam_Resolution_Width = 640.0 #Change the resolution according to your Camera's resolution 14 | webcam_Resolution_Height = 480.0 15 | home_on_boot = True # set True to Home the robotic arm on boot 16 | 17 | #Global variables 18 | Gripper_X = np.array([0.0]) #holds X value of Gripper coordinates (1D Numpy Array for float variable) 19 | Gripper_Y = np.array([0.0]) #holds Y value of Gripper coordinates 20 | Object_X = np.array([0.0]) #holds X value of Object coordinates 21 | Object_Y = np.array([0.0]) #holds Y value of Object coordinates 22 | 23 | #Load Robot position (center, rotation angle etc ) values from file generated by "robot_position_estimation.py" file 24 | with open(r'robot_position.yaml') as file: 25 | documents = yaml.full_load(file) #loading yaml file as Stream 26 | robot_position = np.array(documents['robot_position']) #extracting robot_position key and convert it into Numpy Array (2D Matrix) 27 | robot_x , robot_y , robot_angle , number_of_cm_in_Resolution_width = robot_position # Robot centroid in pixels and angle with respect to Vertical Axis of Camera frame 28 | cm_per_pixel = number_of_cm_in_Resolution_width/webcam_Resolution_Width #Convert pixels to cm - tells that how many centimeters are in 1 pixel 29 | # print ("\nRobot Position\n",robot_position) 30 | print("\Robot Position Matrix Loaded Succeccfully\n") 31 | 32 | #Homogeneous transformation matrix for Coordinates transformation (camera to robotic arm) 33 | #Rotation Vector (Visit http://www.it.hiof.no/~borres/j3d/math/homo/p-homo.html for references) 34 | #as out robot coordinate has to be only rotated along Y axis : +180 degrees 35 | R0_C = [[np.cos(np.pi),0,np.sin(np.pi)],[0,1,0],[-np.sin(np.pi),0,np.cos(np.pi)]] 36 | #Displacement Vector - Find using "robot_position_estimation.py" file 37 | robot_distance_from_X = robot_x # cm - distance from Image origin X axis to the robotic arm origin 38 | robot_distance_from_Y = -robot_y # cm - distance from Image origin Y axis to the robotic arm origin 39 | d0_C = [[robot_distance_from_X],[robot_distance_from_Y],[0]] #distance between Robot origin to the Camera coordinates orign 40 | #Homogeneous transformation matrix 41 | H0_C = np.concatenate((R0_C,d0_C),1) 42 | H0_C = np.concatenate((H0_C,[[0,0,0,1]]),0) 43 | 44 | 45 | 46 | #Connecting with Arduino - Robotic Arm 47 | try: 48 | arduino = serial.Serial('COM8', 115200, timeout=.1) 49 | time.sleep(1) #give the connection a second to settle 50 | if(home_on_boot): 51 | arduino.write(str("G28\r\n").encode()) #send Homming command to Robotic Arm 52 | time.sleep(16) #wait while Homming procedure completes 53 | arduino.write(str("M5\r\n").encode()) #Close the gripper 54 | arduino.flushInput() 55 | time.sleep(2) 56 | except: 57 | print("Arduino communication ERROR") 58 | sys.exit() 59 | 60 | 61 | 62 | def ArduinoCommands(): # Function to Send Pick and Place Commands to Arduino-Robotic Arm 63 | try: 64 | #Move the gripper to the position of the object and pick it then place it on another place 65 | #Pick 66 | command_to_Arduino = str("G0X"+str(Object_X[0])+"Y"+str(Object_Y[0])+"F80\r\n") 67 | print(command_to_Arduino) 68 | arduino.write(command_to_Arduino.encode()) #move to the object 69 | arduino.write(str("M3\r\n").encode()) #Open the gripper 70 | time.sleep(0.5) 71 | arduino.write(str("M3\r\n").encode()) 72 | time.sleep(1) 73 | arduino.write(str("G0Z-110F80\r\n").encode()) # lower the gripper to pick the object 74 | arduino.write(str("M5\r\n").encode()) #Close the gripper - Object picked 75 | arduino.write(str("M5\r\n").encode()) 76 | arduino.write(str("G0Z0F80\r\n").encode()) # higher the gripper after picking the object 77 | time.sleep(1) 78 | 79 | #Place 80 | arduino.write(str("G0 X-210 Y120 Z0 F80\r\n").encode()) #move to the place location 81 | time.sleep(2) 82 | arduino.write(str("G0Z-110F80\r\n").encode()) # lower the gripper to place the object 83 | time.sleep(2) 84 | arduino.write(str("M3\r\n").encode()) #Open the gripper 85 | time.sleep(3) 86 | arduino.write(str("M3\r\n").encode()) #Open the gripper 87 | time.sleep(2) 88 | arduino.write(str("G0Z0F80\r\n").encode()) 89 | time.sleep(2) 90 | arduino.write(str("G0 X0 Y170 Z120 F80\r\n").encode()) # go to the parking position 91 | time.sleep(2) 92 | arduino.write(str("M5\r\n").encode()) #Close the gripper 93 | time.sleep(2) 94 | arduino.write(str("M5\r\n").encode()) #Close the gripper 95 | time.sleep(3) 96 | except Exception as e: 97 | print(e) 98 | 99 | 100 | def LiveLocationOfGripper(): #Function to get Live coordinates of the Gripper of Robotic Arm 101 | cap = cv.VideoCapture(0, cv.CAP_DSHOW) 102 | while getattr(t0, "do_run", True): 103 | try: 104 | _,frame = cap.read() #reading singe frame from camera 105 | 106 | #Track the Gripper of Robotic Arm using Color Subtraction (RED colored) 107 | red = np.matrix(frame[:,:,2]) #extracting red layer (layer No 2) from RGB 108 | green = np.matrix(frame[:,:,1]) #extracting green layer (layer No 1) from RGB 109 | blue = np.matrix(frame[:,:,0]) #extracting blue layer (layer No 0) from RGB 110 | #it will display only the red colored objects bright with black background 111 | red_only = np.int16(red)-np.int16(green)-np.int16(blue) 112 | 113 | # converting Grayscale image to Black&White (applying thresholding) 114 | # _,red_only = cv.threshold(red_only,70,200,cv.THRESH_BINARY) 115 | red_only[red_only<50] =0 116 | red_only[red_only>255] =255 117 | 118 | red_only = np.uint8(red_only) 119 | 120 | # cv.namedWindow('Live Robot-Gripper', cv.WINDOW_AUTOSIZE) 121 | # cv.imshow('Live Robot-Gripper',red_only) 122 | # cv.waitKey(1) 123 | 124 | #Find Location of Robot Gripper in cm units 125 | # X location (in pixels) 126 | column_sums = np.matrix(np.sum(red_only,0)) 127 | column_numbers = np.matrix(np.arange(webcam_Resolution_Width)) 128 | column_mult = np.multiply(column_sums,column_numbers) 129 | total = np.sum(column_mult) 130 | total_total = np.sum(np.sum(red_only)) #sum of all the pixels 131 | column_location = total/total_total # the location of the column where the center of mass exists 132 | # Y location (in pixels) 133 | row_sums = np.matrix(np.sum(red_only,1)) 134 | row_sums = row_sums.transpose() 135 | row_numbers = np.matrix(np.arange(webcam_Resolution_Height)) 136 | row_mult = np.multiply(row_sums,row_numbers) 137 | total = np.sum(row_mult) 138 | total_total = np.sum(np.sum(red_only)) #sum of all the pixels 139 | row_location = total/total_total # the location of the column where the center of mass exists 140 | #Location on Gripper in Camera coordinates (pixels) 141 | #print(column_location,row_location) 142 | 143 | if np.isnan(column_location) or np.isnan(row_location): 144 | # print("Gripper NOT Found!") 145 | continue 146 | 147 | #Display coordinates, text, etc. on detected gripper location 148 | #Display Circle 149 | red_only = cv.circle(red_only, (int(column_location), int(row_location)), 40, (255,255,255), 2) #image, center coordinates (x, y), radius of the circle, stroke color in BGR, stroke thickness (in pixels) 150 | #Display center point 151 | red_only = cv.circle(red_only, (int(column_location), int(row_location)), 2, (255,255,255), 3) #image, center coordinates (x, y), radius of the circle, stroke color in BGR, stroke thickness (in pixels) 152 | 153 | 154 | #Location on Gripper in World coordinates (centimeters) 155 | x_location1 = column_location * cm_per_pixel 156 | y_location1 = row_location * cm_per_pixel 157 | # print(x_location1,y_location1) 158 | #Gripper in camera axis 159 | PC = [[x_location1],[y_location1],[0],[1]] 160 | #Gripper in robot axis (using homogeneous transformation matrix) 161 | P0 = np.dot(H0_C,PC) 162 | 163 | #These are global variables - I am using these variables in main loop below as process value in PID algorithm 164 | #Location of Gripper in Robot coordinates 165 | Gripper_X[0] = P0[0] #x coordinate of gripper in cm 166 | Gripper_Y[0] = P0[1] #y coordinate of gripper in cm 167 | 168 | #convert cm to mm and Round off to 0 decimal point 169 | Gripper_X[0] = round(Gripper_X[0]*10, 0) 170 | Gripper_Y[0] = round(Gripper_Y[0]*10, 0) 171 | # print("Live Gripper Location: ") 172 | # print(Gripper_X[0],Gripper_Y[0]) 173 | time.sleep(0.1) 174 | 175 | #Draw coordinates on image (location of object in mm in Robot coordinates) 176 | #cv2.putText(img, text, position, font, fontScale, color, thickness, lineType, bottomLeftOrigin) 177 | cv.putText(red_only, str(str(Gripper_X[0])+" , "+str(Gripper_Y[0])), (int(column_location), int(row_location)-30), cv.FONT_HERSHEY_SIMPLEX, 0.5, (255,255,255), 1, cv.LINE_AA) 178 | cv.namedWindow('Live Robot-Gripper', cv.WINDOW_AUTOSIZE) 179 | cv.imshow('Live Robot-Gripper',red_only) 180 | cv.waitKey(1) 181 | 182 | except Exception as e: 183 | print(e) 184 | cv.destroyWindow("Live Robot-Gripper") 185 | pass 186 | 187 | def calculate_XYZ(u,v): #Function to get World Coordinates from Camera Coordinates 188 | cam_mtx = camera_matrix 189 | Rt = extrinsic_matrix 190 | #Solve: From Image Pixels, find World Points 191 | 192 | scalingfactor = 40.0 #this is demo value, Calculate the Scaling Factor first 193 | tvec1 = Rt[:, 3] #Extract the 4th Column (Translation Vector) from Extrinsic Matric 194 | 195 | 196 | uv_1=np.array([[u,v,1]], dtype=np.float32) 197 | uv_1=uv_1.T 198 | suv_1=scalingfactor*uv_1 199 | inverse_cam_mtx = np.linalg.inv(cam_mtx) 200 | xyz_c=inverse_cam_mtx.dot(suv_1) 201 | xyz_c=xyz_c-tvec1 202 | R_mtx = Rt[:,[0,1,2]] #Extract first 3 columns (Rotation Matrix) from Extrinsics Matrix 203 | inverse_R_mtx = np.linalg.inv(R_mtx) 204 | XYZ=inverse_R_mtx.dot(xyz_c) 205 | return XYZ 206 | 207 | 208 | 209 | 210 | # print("\nTesting calculate_XYZ function with demo values\n") 211 | # print(calculate_XYZ(600,400)) 212 | 213 | 214 | if __name__ == "__main__": 215 | try: 216 | cap = cv.VideoCapture(0, cv.CAP_DSHOW) # reading camera feed (https://answers.opencv.org/question/227535/solvedassertion-error-in-video-capturing/)) 217 | # Capture image of background without any objects 218 | time.sleep(2) 219 | _,frame = cap.read() #reading singe frame from camera 220 | print("Background Image taken and Undistorted Successfully") 221 | #convert RGB to Grayscale 222 | gray_image1 = cv.cvtColor(frame,cv.COLOR_BGR2GRAY) 223 | cv.namedWindow('background', cv.WINDOW_AUTOSIZE) 224 | cv.imshow("background",gray_image1) 225 | cv.waitKey(1) 226 | except: 227 | print("Camera Connection ERROR") 228 | arduino.close() 229 | sys.exit() 230 | 231 | 232 | while(1): 233 | try: 234 | 235 | # Start a thread to get live location of Robotic Arm gripper 236 | try: 237 | t0 = threading.Thread(target=LiveLocationOfGripper) # https://www.geeksforgeeks.org/multithreading-python-set-1/ 238 | t0.start() # starting thread 239 | #t0.join() # wait until thread is completely executed 240 | except: 241 | print ("Error: unable to start thread") 242 | 243 | #Now Place the target object on the surface and then press Enter key to proceed further. 244 | while(1): 245 | _,temp = cap.read() 246 | cv.imshow("Live" , temp) 247 | k = cv.waitKey(5) 248 | 249 | if k == 13: #Move to next code by pressing Enter key 250 | #Capturing single frame (image) from camera 251 | _,frame = cap.read() 252 | #frame = undistortImage(frame) 253 | print("Foreground Image taken and Undistorted Successfully") 254 | break 255 | if k == 27: #exit by pressing Esc key 256 | cv.destroyAllWindows() 257 | arduino.close() 258 | t0.do_run = False # Stop Thread0 (https://stackoverflow.com/a/36499538/3661547) 259 | sys.exit() 260 | if k == 104: #Home the robotic arm by pressing h key 261 | arduino.write(str("M17\r\n").encode()) # Turn ON Stepper 262 | arduino.write(str("G28\r\n").encode()) #send Homming command to Robotic Arm 263 | time.sleep(15) 264 | if k == 120: #Turn OFF Steppers of the robotic arm by pressing x key 265 | arduino.write(str("M18\r\n").encode()) #send command to Robotic Arm 266 | time.sleep(1) 267 | if k == 117: #Update background by pressing u key 268 | _,frame = cap.read() #reading singe frame from camera 269 | #frame = undistortImage(frame) 270 | print("Background Image taken and Undistorted Successfully") 271 | #convert RGB to Grayscale 272 | gray_image1 = cv.cvtColor(frame,cv.COLOR_BGR2GRAY) 273 | cv.namedWindow('background', cv.WINDOW_AUTOSIZE) 274 | cv.imshow("background",gray_image1) 275 | cv.waitKey(1) 276 | 277 | 278 | 279 | #Now Detect the target object to be picked by Robotic Arm 280 | #convert RGB to Grayscale 281 | gray_image2 = cv.cvtColor(frame,cv.COLOR_BGR2GRAY) 282 | #cv.imshow('foreground',gray_image2) 283 | #Object detection using Difference method 284 | difference = np.absolute(np.matrix(np.int16(gray_image1))-np.matrix(np.int16(gray_image2))) 285 | difference[difference>255] = 255 286 | difference = np.uint8(difference) 287 | #cv.imshow('object',difference) 288 | 289 | # converting Grayscale image to Black&White (applying thresholding) 290 | BW = difference 291 | BW[BW<=100] = 0 292 | BW[BW>100] = 1 293 | 294 | #now we have to find the center of mass of the object (in pixel units) 295 | # X location (in pixels) 296 | column_sums = np.matrix(np.sum(BW,0)) 297 | column_numbers = np.matrix(np.arange(webcam_Resolution_Width)) 298 | column_mult = np.multiply(column_sums,column_numbers) 299 | total = np.sum(column_mult) 300 | total_total = np.sum(np.sum(BW)) #sum of all the pixels 301 | column_location = total/total_total # the location of the column where the center of mass exists 302 | #print(column_location) 303 | # Y location (in pixels) 304 | row_sums = np.matrix(np.sum(BW,1)) 305 | row_sums = row_sums.transpose() 306 | row_numbers = np.matrix(np.arange(webcam_Resolution_Height)) 307 | row_mult = np.multiply(row_sums,row_numbers) 308 | total = np.sum(row_mult) 309 | total_total = np.sum(np.sum(BW)) #sum of all the pixels 310 | row_location = total/total_total # the location of the column where the center of mass exists 311 | #Location on object in Camera coordinates (pixels) 312 | #print(column_location,row_location) 313 | 314 | if np.isnan(column_location) or np.isnan(row_location): 315 | print("No object Found!") 316 | continue 317 | 318 | #Display coordinates, text, etc. on detected object location 319 | #Display Circle 320 | frame = cv.circle(frame, (int(column_location), int(row_location)), 20, (0,0,255), 2) #image, center coordinates (x, y), radius of the circle, stroke color in BGR, stroke thickness (in pixels) 321 | #Display center point 322 | frame = cv.circle(frame, (int(column_location), int(row_location)), 2, (255,0,0), 3) #image, center coordinates (x, y), radius of the circle, stroke color in BGR, stroke thickness (in pixels) 323 | 324 | 325 | #Location on object in World coordinates (centimeters) 326 | x_location2 = column_location * cm_per_pixel 327 | y_location2 = row_location * cm_per_pixel 328 | # print(x_location2,y_location2) 329 | #Point in camera axis 330 | PC = [[x_location2],[y_location2],[0],[1]] 331 | #Point in robot axis (using homogeneous transformation matrix) 332 | P0 = np.dot(H0_C,PC) 333 | Object_X[0] = P0[0] 334 | Object_Y[0] = P0[1] 335 | # print location in cm 336 | print(Object_X[0],Object_Y[0]) 337 | 338 | #convert cm to mm then send to rootic arm and also round off the values. 339 | Object_X[0] = round(Object_X[0]*10 , 0) #Round off to 0 decimal point and convert to mm 340 | Object_Y[0] = round(Object_Y[0]*10 , 0) 341 | #Location of object in Robot coordinates in Milimeters 342 | # print(Object_X[0],Object_Y[0]) 343 | 344 | #Draw coordinates on image (location of object in mm in Robot coordinates) 345 | #cv2.putText(img, text, position, font, fontScale, color, thickness, lineType, bottomLeftOrigin) 346 | cv.putText(frame, str(str(Object_X[0])+" , "+str(Object_Y[0])), (int(column_location), int(row_location)-30), cv.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,255), 1, cv.LINE_AA) 347 | cv.namedWindow('Detected Object', cv.WINDOW_AUTOSIZE) 348 | cv.imshow('Detected Object',frame) 349 | cv.waitKey(1) 350 | 351 | 352 | # Send location of object to Robotic Arm for pick and place operation 353 | # Create a thread to run the Robot Commands 354 | try: 355 | t1 = threading.Thread(target=ArduinoCommands) # https://www.geeksforgeeks.org/multithreading-python-set-1/ 356 | t1.start() # starting thread 357 | #t1.join() # wait until thread is completely executed 358 | except: 359 | print ("Error: unable to start thread") 360 | 361 | 362 | 363 | except Exception as e: 364 | print("Error in Main Loop\n",e) 365 | cv.destroyAllWindows() 366 | arduino.close() 367 | t0.do_run = False # Stop Thread0 (https://stackoverflow.com/a/36499538/3661547) 368 | sys.exit() 369 | 370 | cv.destroyAllWindows() 371 | 372 | 373 | 374 | -------------------------------------------------------------------------------- /robot_position_estimation.py: -------------------------------------------------------------------------------- 1 | # -*- coding: utf-8 -*- 2 | """ 3 | Created on Sun Nov 8 21:58:01 2020 4 | 5 | @author: Tehseen 6 | """ 7 | 8 | # This code is used to Find the location of the Origin of the Robotic arm 9 | # with respect to the image frame. We calculate the center point (origin) of the robotic arm 10 | # as well as the rotation of the robotic arm with respect to the image frame. 11 | # These values will then be used in the Camera coordinates to the Robotic arm Coordinates Homogenius Transformation 12 | 13 | #First of all place the robotic arm base plate on the table below the camera where we will place the robotic arm afterwards 14 | # Then execute the code. The code will detect the Rectangle in the base plate tool then fild the 15 | # origin and rotation values. 16 | # At the end we will use these values in our main program. 17 | 18 | #[Resources] 19 | # https://stackoverflow.com/questions/34237253/detect-centre-and-angle-of-rectangles-in-an-image-using-opencv 20 | # https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_imgproc/py_contours/py_contours_begin/py_contours_begin.html#how-to-draw-the-contours 21 | # https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_imgproc/py_contours/py_contour_features/py_contour_features.html#b-rotated-rectangle 22 | # https://stackoverflow.com/questions/52247821/find-width-and-height-of-rotatedrect 23 | 24 | import numpy as np 25 | import cv2 26 | import sys 27 | import time 28 | import yaml 29 | import os 30 | import warnings 31 | warnings.filterwarnings("ignore") 32 | 33 | #Constants Declaration 34 | webcam_Resolution_Width = 640.0 #Change the resolution according to your Camera's resolution 35 | webcam_Resolution_Height = 480.0 36 | rectangle_width_in_mm = 49.0 #size of the calibration rectangle (longer side) along x-axis in mm. 37 | 38 | # Global Variables 39 | cx = 0.0 #object location in mm 40 | cy = 0.0 #object location in mm 41 | angle = 0.0 #robotic arm rotation angle 42 | one_pixel_length = 0.0 #length of one pixel in cm units 43 | number_of_cm_in_Resolution_width = 0.0 #total number of cm in the camera resolution width 44 | 45 | 46 | def calculate_XYZ(u,v): #Function to get World Coordinates from Camera Coordinates in mm 47 | #https://github.com/pacogarcia3/hta0-horizontal-robot-arm/blob/9121082815e3e168e35346efa9c60bd6d9fdcef1/camera_realworldxyz.py#L105 48 | cam_mtx = camera_matrix 49 | Rt = extrinsic_matrix 50 | #Solve: From Image Pixels, find World Points 51 | 52 | scalingfactor = 40.0 #this is demo value, Calculate the Scaling Factor first (depth) 53 | tvec1 = Rt[:, 3] #Extract the 4th Column (Translation Vector) from Extrinsic Matric 54 | 55 | uv_1=np.array([[u,v,1]], dtype=np.float32) 56 | uv_1=uv_1.T 57 | suv_1=scalingfactor*uv_1 58 | inverse_cam_mtx = np.linalg.inv(cam_mtx) 59 | xyz_c=inverse_cam_mtx.dot(suv_1) 60 | xyz_c=xyz_c-tvec1 61 | R_mtx = Rt[:,[0,1,2]] #Extract first 3 columns (Rotation Matrix) from Extrinsics Matrix 62 | inverse_R_mtx = np.linalg.inv(R_mtx) 63 | XYZ=inverse_R_mtx.dot(xyz_c) 64 | return XYZ 65 | 66 | 67 | 68 | if __name__ == "__main__": 69 | while(1): 70 | try: 71 | #Start reading camera feed (https://answers.opencv.org/question/227535/solvedassertion-error-in-video-capturing/)) 72 | cap = cv2.VideoCapture(0, cv2.CAP_DSHOW) 73 | 74 | #Now Place the base_plate_tool on the surface below the camera. 75 | while(1): 76 | _,frame = cap.read() 77 | # cv2.imshow("Live" , frame) 78 | k = cv2.waitKey(5) 79 | if k == 27: #exit by pressing Esc key 80 | cv2.destroyAllWindows() 81 | sys.exit() 82 | if k == 13: #Save the centroid and angle values of the rectangle in a file 83 | result_file = r'robot_position.yaml' 84 | try: 85 | os.remove(result_file) #Delete old file first 86 | except: 87 | pass 88 | print("Saving Robot Position Matrices .. in ",result_file) 89 | cx = (cx * one_pixel_length)/10.0 #pixel to cm conversion 90 | cy = (cy * one_pixel_length)/10.0 91 | data={"robot_position": [cx,cy,angle,number_of_cm_in_Resolution_width]} 92 | with open(result_file, "w") as f: 93 | yaml.dump(data, f, default_flow_style=False) 94 | 95 | 96 | red = np.matrix(frame[:,:,2]) #extracting red layer (layer No 2) from RGB 97 | green = np.matrix(frame[:,:,1]) #extracting green layer (layer No 1) from RGB 98 | blue = np.matrix(frame[:,:,0]) #extracting blue layer (layer No 0) from RGB 99 | #it will display only the Blue colored objects bright with black background 100 | blue_only = np.int16(blue)-np.int16(red)-np.int16(green) 101 | blue_only[blue_only<0] =0 102 | blue_only[blue_only>255] =255 103 | blue_only = np.uint8(blue_only) 104 | # cv2.namedWindow('blue_only', cv2.WINDOW_AUTOSIZE) 105 | # cv2.imshow("blue_only",blue_only) 106 | # cv2.waitKey(1) 107 | 108 | #https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_imgproc/py_thresholding/py_thresholding.html#otsus-binarization 109 | #Gaussian filtering 110 | blur = cv2.GaussianBlur(blue_only,(5,5),cv2.BORDER_DEFAULT) 111 | #Otsu's thresholding 112 | ret3,thresh = cv2.threshold(blur,0,255,cv2.THRESH_BINARY+cv2.THRESH_OTSU) 113 | cv2.namedWindow('Threshold', cv2.WINDOW_AUTOSIZE) 114 | cv2.imshow("Threshold",thresh) 115 | cv2.waitKey(1) 116 | contours,hierarchy = cv2.findContours(thresh,cv2.RETR_TREE,cv2.CHAIN_APPROX_NONE) 117 | 118 | 119 | for contour in contours: 120 | area = cv2.contourArea(contour) 121 | if area>100000: 122 | contours.remove(contour) 123 | 124 | cnt = contours[0] #Conture of our rectangle 125 | 126 | ############################################################## 127 | #https://stackoverflow.com/a/34285205/3661547 128 | #fit bounding rectangle around contour 129 | rotatedRect = cv2.minAreaRect(cnt) 130 | #getting centroid, width, height and angle of the rectangle conture 131 | (cx, cy), (width, height), angle = rotatedRect 132 | 133 | 134 | #centetoid of the rectangle conture 135 | cx=int(cx) 136 | cy=int(cy) 137 | # print (cx,cy) #centroid of conture of rectangle 138 | 139 | #Location of Rectangle from origin of image frame in millimeters 140 | x,y,z = calculate_XYZ(cx,cy) 141 | 142 | #but we choose the Shorter edge of the rotated rect to compute the angle between Vertical 143 | #https://stackoverflow.com/a/21427814/3661547 144 | if(width > height): 145 | angle = angle+180 146 | else: 147 | angle = angle+90 148 | # print("Angle b/w shorter side with Image Vertical: \n", angle) 149 | 150 | #cm-per-pixel calculation 151 | if(width != 0.0): 152 | one_pixel_length = rectangle_width_in_mm/width #length of one pixel in mm (rectangle_width_in_mm/rectangle_width_in_pixels) 153 | number_of_cm_in_Resolution_width = (one_pixel_length*640)/10 #in cm #Change the resolution according to your Camera's resolution 154 | print(number_of_cm_in_Resolution_width) 155 | 156 | 157 | ############################################################## 158 | 159 | 160 | #Draw rectangle around the detected object 161 | #https://opencv-python-tutroals.readthedocs.io/en/latest/py_tutorials/py_imgproc/py_contours/py_contours_begin/py_contours_begin.html#how-to-draw-the-contours 162 | im = cv2.drawContours(frame,[cnt],0,(0,0,255),2) 163 | # cv2.namedWindow('Contours', cv2.WINDOW_AUTOSIZE) 164 | # cv2.imshow("Contours",im) 165 | # cv2.waitKey(1) 166 | 167 | cv2.circle(im, (cx,cy), 2,(200, 255, 0),2) #draw center 168 | cv2.putText(im, str("Angle: "+str(int(angle))), (int(cx)-40, int(cy)+60), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,255), 1, cv2.LINE_AA) 169 | cv2.putText(im, str("Center: "+str(cx)+","+str(cy)), (int(cx)-40, int(cy)-50), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,0,255), 1, cv2.LINE_AA) 170 | cv2.namedWindow('Detected Rect', cv2.WINDOW_AUTOSIZE) 171 | cv2.imshow('Detected Rect',im) 172 | cv2.waitKey(1) 173 | 174 | 175 | 176 | except Exception as e: 177 | print("Error in Main Loop\n",e) 178 | cv2.destroyAllWindows() 179 | sys.exit() 180 | 181 | cv2.destroyAllWindows() 182 | 183 | 184 | --------------------------------------------------------------------------------