def update(self, frame, events): if self.collect_new: img = frame.img status, grid_points = cv2.findCirclesGridDefault( img, (4, 11), flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if status: self.img_points.append(grid_points) self.obj_points.append(self.obj_grid) self.collect_new = False self.count -= 1 self.button.status_text = "%i to go" % (self.count) if self.count <= 0 and not self.calculated: self.calculate() self.button.status_text = '' if self.window_should_close: self.close_window() if self.show_undistortion: adjusted_k, roi = cv2.getOptimalNewCameraMatrix( cameraMatrix=self.camera_intrinsics[0], distCoeffs=self.camera_intrinsics[1], imageSize=self.camera_intrinsics[2], alpha=0.5, newImgSize=self.camera_intrinsics[2], centerPrincipalPoint=1) self.undist_img = cv2.undistort(frame.img, self.camera_intrinsics[0], self.camera_intrinsics[1], newCameraMatrix=adjusted_k)
def find_circle_centers(board_directories): """Returns an array of chessboard corners in an image from each distance of 50cm, 100cm and 450cm in that order.""" print "Finding board corners..." pattern_size = (4,11) image_coords = [] corner_images = [] for directory in board_directories: img_name = 'cam1_frame_1.bmp' print "Finding circles in", os.path.join(directory, img_name) board_image = cv2.imread(os.path.join(directory, img_name),1) # CHANGED: Loading as RGB [pattern_was_found,corners] = cv2.findCirclesGridDefault(board_image,pattern_size,flags=cv2.CALIB_CB_ASYMMETRIC_GRID) corners = corners.squeeze() # CHANGED: moved squeeze to before corner checking instead of after if not pattern_was_found and corners == None: try: corners = np.loadtxt(os.path.join(directory, 'cam1_frame_1_corners.txt'),delimiter=',') pattern_was_found = True corners = corners.astype('float32') except IndexError: print 'No corners found! Please find them yourself in Photoshop' sys.exit(-1) if not pattern_was_found and not corners==None: print "Not all corners found! Find them yourself!" corners = CornerPicker.main(board_image.copy(), corners, pattern_size) corner_image = board_image.copy() cv2.drawChessboardCorners(corner_image, pattern_size, corners, pattern_was_found) cv2.imwrite('./tmp.png',corner_image) image_coords.append(corners) corner_images.append(corner_image) return np.concatenate(image_coords), corner_images
def findMarkers(gray): # Find the chess board corners if marker_checkerboard == True: ret, corners = cv2.findChessboardCorners(gray, marker_size,None) print 'chess - found corners: ' + str(corners.size) + ' ' + str(ret) else: ret, corners = cv2.findCirclesGridDefault(gray, marker_size,None,cv2.CALIB_CB_ASYMMETRIC_GRID) print 'circles - found corners: ' + str(corners.size) + ' ' + str(ret) print corners # If found, add object points, image points (after refining them) if ret == True: objpoints.append(objp) #corners2 = corners #cv2.cornerSubPix(gray,corners2,(11,11),(-1,-1),criteria) #print (corners2-corners) imgpoints.append(corners) # Draw and display the corners cv2.drawChessboardCorners(gray, marker_size, corners,ret) cv2.imshow('camera',gray) cv2.waitKey(500) else: print 'Couldn\'t find markers' return ret
def findMarkers(gray): # Find the chess board corners if marker_checkerboard == True: ret, corners = cv2.findChessboardCorners(gray, marker_size, None) print 'chess - found corners: ' + str(corners.size) + ' ' + str(ret) else: ret, corners = cv2.findCirclesGridDefault(gray, marker_size, None, cv2.CALIB_CB_ASYMMETRIC_GRID) print 'circles - found corners: ' + str(corners.size) + ' ' + str(ret) print corners # If found, add object points, image points (after refining them) if ret == True: objpoints.append(objp) #corners2 = corners #cv2.cornerSubPix(gray,corners2,(11,11),(-1,-1),criteria) #print (corners2-corners) imgpoints.append(corners) # Draw and display the corners cv2.drawChessboardCorners(gray, marker_size, corners, ret) cv2.imshow('camera', gray) cv2.waitKey(500) else: print 'Couldn\'t find markers' return ret
def circle_grid(image, pattern_size=(4,11)): status, centers = cv2.findCirclesGridDefault(image, pattern_size, flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if status: mean = centers.sum(0)/centers.shape[0] # mean is [[x,y]] return mean[0], centers else: return None
def circle_grid(image, pattern_size=(4,11)): """Circle grid: finds an assymetric circle pattern - circle_id: sorted from bottom left to top right (column first) - If no circle_id is given, then the mean of circle positions is returned approx. center - If no pattern is detected, function returns None """ status, centers = cv2.findCirclesGridDefault(image, pattern_size, flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if status: return centers else: return None
def get_circle_points(filename,dims): ''' load in an image file and return the image pixel coordinates of circle grid points ''' # read in image img = cv2.imread(filename,0) # find center points in circle grid [found,points] = cv2.findCirclesGridDefault(img,dims,flags=(cv2.CALIB_CB_ASYMMETRIC_GRID)) return img,found,points
def get_image(): succes, im = camera.read() im = cv2.flip(im, 1) # mirror effect success, corners = cv2.findCirclesGridDefault(im, PAT_SIZE, flags=findFlags) if success: # import pdb; pdb.set_trace() cv2.drawChessboardCorners(im, PAT_SIZE, corners, success) cv2.putText(im, "Found: (%.1f, %.1f)" % tuple(corners[0, 0].tolist()), infoOrigin, cv2.FONT_HERSHEY_DUPLEX, 0.75, (0, 255, 0)) else: cv2.putText(im, "Not found.", infoOrigin, cv2.FONT_HERSHEY_DUPLEX, 0.75, (0, 255, 0)) return cv22pygame(im)
def find_circles(config, image, symmetric=False): flags = 0 #flags |= cv2.CALIB_CB_CLUSTERING if symmetric: flags |= cv2.CALIB_CB_SYMMETRIC_GRID else: flags |= cv2.CALIB_CB_ASYMMETRIC_GRID rv, points = cv2.findCirclesGridDefault(image, config.calibration.pattern_size, flags=flags) if not rv: return None return points
def update(self, frame, events): if self.collect_new: img = frame.img status, grid_points = cv2.findCirclesGridDefault(img, (4, 11), flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if status: self.img_points.append(grid_points) self.obj_points.append(self.obj_grid) self.collect_new = False self.count -= 1 self.img_shape = img.shape self.button.status_text = "%i to go" % (self.count) if self.count <= 0 and not self.calculated: self.calculate() self.button.status_text = "" if self.window_should_close: self.close_window()
def update(self, frame, events): if self.collect_new: img = frame.img status, grid_points = cv2.findCirclesGridDefault( img, (4, 11), flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if status: self.img_points.append(grid_points) self.obj_points.append(self.obj_grid) self.collect_new = False self.count -= 1 self.img_shape = img.shape self.button.status_text = "%i to go" % (self.count) if self.count <= 0 and not self.calculated: self.calculate() self.button.status_text = '' if self.window_should_close: self.close_window()
def detect_image_points(self, image, is_grayscale): ''' Detect the pixels at which the circles centers are. Returns list of (x, y) coordinates. @param image: input image with checkerboard @type image: cv2 compatible numpy image @param is_grayscale: set to true if image is grayscale @type is_grayscale: bool ''' # detect checkerboard found, corners = cv2.findCirclesGridDefault(image, self.calibration_object.pattern_size, flags=(cv2.CALIB_CB_ASYMMETRIC_GRID)) if found: return corners else: # could not find checkerboard if DEBUG_OUTPUT: print 'checkerboard not found' return None
def detect(self,img): # Statemachine if self.step > 30: self.step = 0 self.stage += 1 self.next = False # done exit now (is_done() will now return True) if self.stage > 8: return # Detection self.pos = 0,0 if self.step in range(10, 25): status, grid_points = cv2.findCirclesGridDefault(img, (4,11), flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if status: img_pos = grid_points[self.map[self.stage]][0] self.pos = normalize(img_pos, (img.shape[1],img.shape[0]),flip_y=True) # Advance if self.next or self.auto_advance: self.step += 1 self.publish()
def update(self,frame,events): if self.collect_new: img = frame.img status, grid_points = cv2.findCirclesGridDefault(img, (4,11), flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if status: self.img_points.append(grid_points) self.obj_points.append(self.obj_grid) self.collect_new = False self.count -=1 self.button.status_text = "%i to go"%(self.count) if self.count<=0 and not self.calculated: self.calculate() self.button.status_text = '' if self.window_should_close: self.close_window() if self.show_undistortion: self.undist_img = cv2.undistort(frame.img, self.camera_intrinsics[0], self.camera_intrinsics[1])
def update(self,frame,recent_pupil_positions,events): if self.collect_new: img = frame.img status, grid_points = cv2.findCirclesGridDefault(img, (4,11), flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if status: self.img_points.append(grid_points) self.obj_points.append(self.obj_grid) self.collect_new = False self.count -=1 if self.count in range(1,10): audio.say("%i" %(self.count)) self.img_shape = img.shape if not self.count and not self.calculated: self.calculate() if self.window_should_close: self.close_window() if self.window_should_open: self.open_window()
def update(self,frame,events): if self.collect_new: img = frame.img status, grid_points = cv2.findCirclesGridDefault(img, (4,11), flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if status: self.img_points.append(grid_points) self.obj_points.append(self.obj_grid) self.collect_new = False self.count -=1 self.button.status_text = "%i to go"%(self.count) if self.count<=0 and not self.calculated: self.calculate() self.button.status_text = '' if self.window_should_close: self.close_window() if self.show_undistortion: adjusted_k,roi = cv2.getOptimalNewCameraMatrix(cameraMatrix= self.camera_intrinsics[0], distCoeffs=self.camera_intrinsics[1], imageSize=self.camera_intrinsics[2], alpha=0.5,newImgSize=self.camera_intrinsics[2],centerPrincipalPoint=1) self.undist_img = cv2.undistort(frame.img, self.camera_intrinsics[0], self.camera_intrinsics[1],newCameraMatrix=adjusted_k)
def get_circles_grid_points( img, pattern_rows, pattern_columns, distance=1, draw=False): """ :param img: input image :param pattern_rows: the number of rows in chessboard :param pattern_columns: the number of columns in chessboard :param square_size: each distance between neighber circles :param draw: draw chessboard corners to ``img`` """ size = (pattern_rows, pattern_columns) found, corners = cv2.findCirclesGridDefault(img, size) if not found: return None if draw: cv2.drawChessboardCorners(img, size, corners, found) cv2.cornerSubPix( cv2.cvtColor(img, cv2.COLOR_BGR2GRAY), corners, (3, 3), (-1, -1), (cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 20, 0.03)) img_points = corners.reshape(-1, 2) obj_points = np.zeros((np.prod(size), 3), np.float32) obj_points[:,:2] = np.indices(size).T.reshape(-1, 2) obj_points *= distance return img_points, obj_points
def _findSymmetricCircles(self, flags=cv2.CALIB_CB_SYMMETRIC_GRID): (didFindCorners, corners) = cv2.findCirclesGridDefault( self.img, self.opts['size'], flags=flags | cv2.CALIB_CB_CLUSTERING) return didFindCorners, corners
def stereo_calibration(check_img_folder, nimages, display=False, dims=(4, 11), size=(640, 480)): """reads files from a directory of stereo images of opencv circle grid. calibrates intrinsics of each camera, then extrinsics of stereo rig """ # grab calbration frames directory for dir in os.listdir(check_img_folder): if fnmatch.fnmatch(dir, "calibration_frames*"): check_img_folder = check_img_folder + dir + "/" break # Number of points in circle grid num_pts = dims[0] * dims[1] if not os.path.exists(check_img_folder + "images_used/"): os.mkdir(check_img_folder + "images_used/") # evaluate image points nimg = 0 # number of images with found corners iptsF1 = [] # image point arrays to fill up iptsF2 = [] random_images = random.sample(range(500), nimages) # for n in range(0,nimages,2): for n in random_images: filename1 = check_img_folder + "cam1_frame_" + str(n + 1) + ".bmp" filename2 = check_img_folder + "cam2_frame_" + str(n + 1) + ".bmp" if os.path.exists(filename1) and os.path.exists(filename2): img1 = cv2.imread(filename1, 0) img2 = cv2.imread(filename2, 0) # find center points in circle grid [found1, points1] = cv2.findCirclesGridDefault(img1, dims, flags=(cv2.CALIB_CB_ASYMMETRIC_GRID)) [found2, points2] = cv2.findCirclesGridDefault(img2, dims, flags=(cv2.CALIB_CB_ASYMMETRIC_GRID)) # copy the found points into the ipts matrices temp1 = np.zeros((num_pts, 2)) temp2 = np.zeros((num_pts, 2)) if found1 and found2: for i in range(num_pts): temp1[i, 0] = points1[i, 0, 0] temp1[i, 1] = points1[i, 0, 1] temp2[i, 0] = points2[i, 0, 0] temp2[i, 1] = points2[i, 0, 1] iptsF1.append(temp1) iptsF2.append(temp2) nimg = nimg + 1 # increment image counter # save images with points identified drawn_boards_1 = img1.copy() drawn_boards_2 = img2.copy() cv2.drawChessboardCorners(drawn_boards_1, dims, points1, found1) cv2.drawChessboardCorners(drawn_boards_2, dims, points2, found2) cv2.imwrite(check_img_folder + "images_used/" + "cam1_frame_" + str(n + 1) + ".bmp", drawn_boards_1) cv2.imwrite(check_img_folder + "images_used/" + "cam2_frame_" + str(n + 1) + ".bmp", drawn_boards_2) print "\n Usable stereo pairs: " + str(nimg) # convert image points to numpy iptsF1 = np.array(iptsF1, dtype=np.float32) iptsF2 = np.array(iptsF2, dtype=np.float32) # evaluate object points opts = object_points(dims, nimg, 4.35) # initialize camera parameters intrinsics1 = np.zeros((3, 3)) intrinsics2 = np.zeros((3, 3)) distortion1 = np.zeros((8, 1)) distortion2 = np.zeros((8, 1)) # Set initial guess for intrinsic camera parameters (focal length = 0.35cm) intrinsics1[0, 0] = 583.3 intrinsics1[1, 1] = 583.3 intrinsics1[0, 2] = 320 intrinsics1[1, 2] = 240 intrinsics1[2, 2] = 1.0 intrinsics2[0, 0] = 583.3 intrinsics2[1, 1] = 583.3 intrinsics2[0, 2] = 320 intrinsics2[1, 2] = 240 intrinsics2[2, 2] = 1.0 # calibrate cameras print "Calibrating camera 1..." (cam1rms, intrinsics1, distortion1, rotv1, trav1) = cv2.calibrateCamera( opts, iptsF1, size, intrinsics1, distortion1, flags=int(cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_RATIONAL_MODEL), ) print "\nEstimated intrinsic parameters for camera 1:" for i in range(3): print [intrinsics1[i, j] for j in range(3)] print "\nEstimated distortion parameters for camera 1:" print distortion1 print "Calibrating camera 2..." (cam2rms, intrinsics2, distortion2, rotv2, trav2) = cv2.calibrateCamera( opts, iptsF2, size, intrinsics2, distortion2, flags=int(cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_RATIONAL_MODEL), ) print "\nEstimated intrinsic parameters for camera 2:" for i in range(3): print [intrinsics2[i, j] for j in range(3)] print "\nEstimated distortion parameters for camera 2:" print distortion2 print "\n rms pixel error:" print "cam1 orig: " + str(cam1rms) print "cam2 orig: " + str(cam2rms) # Estimate extrinsic parameters from stereo point correspondences print "\n Stereo estimating..." # (stereorms, intrinsics1, distortion1, intrinsics2, distortion2, R, T, E, F) = cv2.stereoCalibrate(opts, iptsF1, iptsF2, intrinsics1, distortion1, intrinsics2, distortion2, size,criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 300, 1e-7), flags=(cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_RATIONAL_MODEL)) (stereorms, intrinsics1, distortion1, intrinsics2, distortion2, R, T, E, F) = cv2.stereoCalibrate( opts, iptsF1, iptsF2, size, intrinsics1, distortion1, intrinsics2, distortion2, criteria=(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 300, 1e-7), flags=(cv2.CALIB_USE_INTRINSIC_GUESS | cv2.CALIB_RATIONAL_MODEL), ) print "\nEstimated extrinsic parameters between cameras 1 and 2:\nRotation:" for i in range(3): print [R[i, j] for j in range(3)] print "\nTranslation:" print [T[i, 0] for i in range(3)]
def getPattern(self, single_board): # find patterns in range boardsize = (self.rows, self.cols) objectPoints = dict() imagePoints = dict() if self.display: cv2.namedWindow('Grids', cv2.cv.CV_WINDOW_NORMAL) print "Beginning frame by frame pattern search" sys.stdout.flush() n = 0 skipping = True for frame in range(self.stop): retval, raw = self.movie.read() # read the frame if retval and (frame >= self.start): print 'Reading frame ' + str(frame - self.start) + ' of ' + str( int(self.stop - self.start)) sys.stdout.flush() draw = raw gray = cv2.cvtColor(raw, cv2.COLOR_RGB2GRAY) # convert to gray if self.dots: # detect dot pattern try: retval, corners = cv2.findCirclesGrid(gray, boardsize) except: retval, corners = cv2.findCirclesGridDefault( gray, boardsize) else: # detect chessboard retval, corners = cv2.findChessboardCorners( gray, boardsize) if retval: cv2.cornerSubPix(gray, corners, (3, 3), (-1, -1), (cv2.cv.CV_TERMCRIT_ITER | cv2.cv.CV_TERMCRIT_EPS, 30, 0.1)) # show results cv2.drawChessboardCorners(draw, boardsize, corners, retval) # add quality control check here for duplicate points # or very tiny boards if retval: check = np.array(corners) check = check.reshape(-1, 2) dists = scipy.spatial.distance.pdist(check) if (dists < 1).any(): retval = False print "Duplicate point detected" sys.stdout.flush() if np.max(dists) < 100: retval = False print "Pattern too small" sys.stdout.flsuh() # add to output if retval: objectPoints[frame] = single_board imagePoints[frame] = corners n += 1 # display result if ordered to if self.display: newSize = (self.imageSize[0] / 2, self.imageSize[1] / 2) cv2.imshow('Grids', cv2.resize(draw, newSize)) cv2.waitKey(1) else: if skipping: print 'Skipping to start frame...' sys.stdout.flush() skipping = False # save results print "Saving results to {0}".format(self.ofile) sys.stdout.flush() ofile = open(self.ofile, "wb") pickle.dump(objectPoints, ofile) pickle.dump(imagePoints, ofile) pickle.dump(self.imageSize, ofile) ofile.close() print "Found " + str( n ) + " patterns in total\nIf this is too few, modify settings and try again."
def estimate_cam_trafo(self, n, tto): """Estimate the transformation between Baxter's base frame and the camera frame using singular value decomposition. :param n: The number of absolute end effector poses to use. :param tto: The homogeneous transform between TCP and the origin of the calibration pattern. :return: The homogeneous transform between Baxter's base frame and the camera frame. """ bto = list() cto = list() self.logger.info("Record %d absolute point cloud pairs." % n) pattern = self._pattern.T[:, np.newaxis, :] hom_pattern = np.concatenate( [self._pattern, np.ones((1, self._pattern.shape[1]))], axis=0) while len(bto) < n and not rospy.is_shutdown(): if self._move(): rospy.sleep(1.0) bttn = self._robot.hom_gripper_to_robot(arm=self._arm) color, _, _ = self._kinect.collect_data(color=True) self._pub_vis.publish(img_to_imgmsg(img=color)) patternfound, centers = cv2.findCirclesGridDefault( image=color, patternSize=self._patternsize, flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if patternfound == 0: self.logger.debug("No pattern found!") continue rot, trans, _ = cv2.solvePnPRansac( objectPoints=pattern, imagePoints=centers, cameraMatrix=self._kinect.color.camera_matrix, distCoeffs=self._kinect.color.distortion_coeff, flags=cv2.CV_ITERATIVE) rot, _ = cv2.Rodrigues(rot) cton = np.eye(4) cton[:-1, :-1] = rot cton[:-1, -1] = np.squeeze(trans) fname = os.path.join(self._sink, "2_tto_%d" % (len(bto) + 1)) cv2.imwrite(fname + ".jpg", color) cv2.drawChessboardCorners(image=color, patternSize=self._patternsize, corners=centers, patternWasFound=patternfound) self._pub_vis.publish(img_to_imgmsg(img=color)) cv2.imwrite(fname + "_det.jpg", color) bto.append(np.dot(np.dot(bttn, tto), hom_pattern)) fname = os.path.join(self._sink, "2_bto_%d.npz" % len(bto)) np.savez(fname, bttn=bto[-1]) cto.append(np.dot(cton, hom_pattern)) fname = os.path.join(self._sink, "2_cto_%d.npz" % len(cto)) np.savez(fname, cton=cto[-1]) self.logger.info("Compute affine transformation from 3D point " "correspondences.") # see http://stackoverflow.com/questions/15963960/opencv-2-4-estimateaffine3d-in-python # data of shape (Nx3) cto = np.array(cto).reshape((4, -1)).T[:, :-1] bto = np.array(bto).reshape((4, -1)).T[:, :-1] cto_mean = cto.mean(axis=0) bto_mean = bto.mean(axis=0) # compute covariance cov = np.dot((cto - cto_mean).T, bto - bto_mean) u, s, v = np.linalg.svd(cov) rot = np.dot(u, v.T) if np.linalg.det(rot) < 0: v[:, 2] = -v[:, 2] rot = np.dot(u, v.T) trans = bto_mean - np.dot(rot, cto_mean) trafo = np.eye(4) trafo[:-1, :-1] = rot trafo[:-1, -1] = trans fname = os.path.join(self._sink, "2_btc.npz") np.savez(fname, btc=trafo) return trafo
def __init__( self, verticalFormat, imagePath, imageFilename, numberOfPointsPerRow, numberOfPointsPerColumn, drawCentres, pathToImageswithCentres, imageWithCentresFilename): #so the filename and path can be used with other functions self.imagePath = imagePath self.imageFilename = imageFilename #load the image #colour self.unorientedColourCalibrationImage = cv2.imread(imagePath + imageFilename, 1) #greyscale self.unorientedGreyscaleCalibrationImage = cv2.imread(imagePath + imageFilename, 0) #orient the images if verticalFormat: self.colourCalibrationImage = cv2.transpose(self.unorientedColourCalibrationImage) self.greyscaleCalibrationImage = cv2.transpose(self.unorientedGreyscaleCalibrationImage) self.colourCalibrationImage = cv2.flip(self.colourCalibrationImage, 0) self.greyscaleCalibrationImage = cv2.flip(self.greyscaleCalibrationImage, 0) #end if else: self.colourCalibrationImage = numpy.copy(self.unorientedColourCalibrationImage) self.greyscaleCalibrationImage = numpy.copy(self.unorientedGreyscaleCalibrationImage) #find the centres of the circles in the circles grid self.centreLocations = cv2.findCirclesGridDefault(self.colourCalibrationImage, (numberOfPointsPerRow, numberOfPointsPerColumn), flags = cv2.CALIB_CB_ASYMMETRIC_GRID) #set success flag if (type(self.centreLocations) != None): self.success = True #end if else: self.success = False #end else #if centres are found, draw corners if required if self.success: if drawCentres: colourCalibrationImageCentres = numpy.copy(self.colourCalibrationImage) cv2.drawChessboardCorners( colourCalibrationImageCentres, (numberOfPointsPerRow, numberOfPointsPerColumn), self.centreLocations, self.success) #save image cv2.imwrite(pathToImageswithCentres + imageWithCentresFilename, colourCalibrationImageCentres) #end if #end if #end __init__ #end chessboardImage
def _findSymmetricCircles(self, flags=cv2.CALIB_CB_SYMMETRIC_GRID): (didFindCorners, corners) = cv2.findCirclesGridDefault( self.img, self.opts['size'], flags=flags|cv2.CALIB_CB_CLUSTERING) return didFindCorners, corners
def visual_test(self, tto, btc): """Visualize the result of the calibration. Compute the robot coordinates of the calibration pattern. Then project them to camera space and compute corresponding pixel coordinates. Draw them onto the image to see if the markings are reasonable close to the marks on the calibration pattern. :param tto: The homogeneous transform between TCP and the origin of the calibration pattern. :param btc: The homogeneous transform between Baxter's base frame and the camera frame. :return: """ hom_pattern = np.concatenate( [self._pattern, np.ones((1, self._pattern.shape[1]))], axis=0) tcp_pattern = np.dot(tto, hom_pattern) btc_inv = inv_trafo_matrix(trafo=btc) self.logger.info("Detect pattern and compare it with estimate.") patternfound = 0 while patternfound == 0 and not rospy.is_shutdown(): if self._move(): rospy.sleep(1.0) btt = self._robot.hom_gripper_to_robot(arm=self._arm) color, _, _ = self._kinect.collect_data(color=True) self._pub_vis.publish(img_to_imgmsg(img=color)) patternfound, centers = cv2.findCirclesGridDefault( image=color, patternSize=self._patternsize, flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if patternfound == 0: self.logger.debug("No pattern found!") continue fname = os.path.join(self._sink, "3_vis") cv2.imwrite(fname + ".jpg", color) cv2.drawChessboardCorners(image=color, patternSize=self._patternsize, corners=centers, patternWasFound=patternfound) self._pub_vis.publish(img_to_imgmsg(img=color)) cv2.imwrite(fname + "_det.jpg", color) centers = centers[:, 0, :] rob_pattern = np.dot(btt, tcp_pattern) cam_pattern = np.dot(btc_inv, rob_pattern) pixels = np.zeros_like(centers) for i in xrange(cam_pattern.shape[1]): coord = list(cam_pattern[:-1, i]) print i, coord, pixels[i] = self._kinect.color.projection_camera_to_pixel( position=coord) # flip in y direction pixels[i, 0] = color.shape[1] - pixels[i, 0] print centers[i], pixels[i] cv2.circle(color, tuple(int(x) for x in pixels[i]), 3, [255, 0, 0] if i == 0 else [0, 255, 0], 2) self._pub_vis.publish(img_to_imgmsg(img=color)) cv2.imwrite(fname + "_est.jpg", color) delta = centers - pixels self.logger.info("Offset in detected and estimated pixel " "coordinates:") self.logger.info("Mean: {}".format(delta.mean(axis=0))) self.logger.info("Std: {}".format(delta.std(axis=0))) self.logger.info("Median: {}".format(np.median(delta, axis=0)))
def estimate_hand_trafo(self, n): """Estimate the transformation between Baxter's end effector and the origin of the calibration pattern mounted on it using the algorithm by Tsai and Lenz. :param n: The number of absolute end effector poses to use. :return: The homogeneous transform between TCP and the origin of the calibration pattern. """ if n < 3: raise ValueError("At least 3 poses are needed to compute the " "transformation!") btt = list() cto = list() pattern = self._pattern.T[:, np.newaxis, :] self.logger.info("Record %d absolute pose and pattern pairs." % n) while len(btt) < n and not rospy.is_shutdown(): if self._move(): rospy.sleep(1.0) bttn = self._robot.hom_gripper_to_robot(arm=self._arm) color, _, _ = self._kinect.collect_data(color=True) self._pub_vis.publish(img_to_imgmsg(img=color)) patternfound, centers = cv2.findCirclesGridDefault( image=color, patternSize=self._patternsize, flags=cv2.CALIB_CB_ASYMMETRIC_GRID) if patternfound == 0: self.logger.debug("No pattern found!") continue rot, trans, _ = cv2.solvePnPRansac( objectPoints=pattern, imagePoints=centers, cameraMatrix=self._kinect.color.camera_matrix, distCoeffs=self._kinect.color.distortion_coeff, flags=cv2.CV_ITERATIVE) rot, _ = cv2.Rodrigues(rot) cton = np.eye(4) cton[:-1, :-1] = rot cton[:-1, -1] = np.squeeze(trans) fname = os.path.join(self._sink, "1_tto_%d" % (len(btt) + 1)) cv2.imwrite(fname + ".jpg", color) cv2.drawChessboardCorners(image=color, patternSize=self._patternsize, corners=centers, patternWasFound=patternfound) self._pub_vis.publish(img_to_imgmsg(img=color)) cv2.imwrite(fname + "_det.jpg", color) btt.append(bttn) fname = os.path.join(self._sink, "1_btt_%d.npz" % len(btt)) np.savez(fname, bttn=btt[-1]) cto.append(cton) fname = os.path.join(self._sink, "1_cto_%d.npz" % len(cto)) np.savez(fname, cton=cto[-1]) self.logger.info("Compute %d relative transforms." % ((n**2 - n) / 2)) bttij = list() ctoij = list() for i in range(n): for j in range(i + 1, n): bttij.append(np.dot(inv_trafo_matrix(btt[j]), btt[i])) ctoij.append(np.dot(cto[j], inv_trafo_matrix(cto[i]))) self.logger.info('Apply algorithm by Tsai and Lenz (1989).') tto = tsai_lenz_89(a=bttij, b=ctoij) fname = os.path.join(self._sink, "1_tto.npz") np.savez(fname, tto=tto) return tto
# print a message to indicate calibration is about to start if i == 10: print "please focus on the red cross in the image." print "now turn your head until you are dizzy"; ####################################################### # CALIBRATION ####################################################### # assume by i == 20 the user is looking at the gray square if i > 20 and not done: # Search for target targetFound, worldcenters = \ cv2.findCirclesGridDefault(frames['worldBW'], (4,11), \ flags=cv2.CALIB_CB_ASYMMETRIC_GRID); # Calibration target found if targetFound: worldpt = worldcenters.sum(0)/worldcenters.shape[0]; cv2.circle(frames['worldBW'], tuple(worldpt[0]), \ 3, (255, 100, 255)); # if the pupil center was calculated, try to store a point if edgePoints.shape[0] > 6: # find the calibration image if eyepts_initialized == True: print str(len(eyepts)); cv2.circle(frames['worldColor'], tuple([worldpt[0][0],worldpt[0][1]]),5, (0, 0, 255)); worldpts.append([worldpt[0][0], \ worldpt[0][1]]);