def detect(image, config): angle = None image_size = cv.GetSize(image) # create grayscale version grayscale = cv.CreateImage(image_size, 8, 1) cv.CvtColor(image, grayscale, cv.CV_BGR2GRAY) if config["EqualizeHist"]: cv.EqualizeHist(grayscale, grayscale) pattern_width = 5 pattern_height = 4 found, corners = cv.FindChessboardCorners(grayscale, (pattern_width, pattern_height)) if found: new_corners = cv.FindCornerSubPix( grayscale, corners, (11, 11), (-1, -1), (cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1)) angle = corners_to_angle(new_corners) #print "angle: ", angle def to_int(t): return (int(t[0]), int(t[1])) #nc = [to_int(corner) for corner in new_corners] #cv.Line( grayscale, nc[0], nc[4], (255,0,0), 2) #cv.Line( grayscale, nc[4], nc[19], (0,255,0),2) #cv.Line( grayscale, nc[19], nc[15], (0,0,255),2) #cv.Line( grayscale, nc[15], nc[0], (255,255,0),2) #cv.ShowImage('Processed', grayscale) return angle
def detect(self, image): corners_x = self.chess_size[0] corners_y = self.chess_size[1] #Here, we'll actually call the openCV detector found, corners = cv.FindChessboardCorners( image, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH) if found: board_corners = (corners[0], corners[corners_x - 1], corners[(corners_y - 1) * corners_x], corners[len(corners) - 1]) #find the perimeter of the checkerboard perimeter = 0.0 for i in range(len(board_corners)): next = (i + 1) % 4 xdiff = board_corners[i][0] - board_corners[next][0] ydiff = board_corners[i][1] - board_corners[next][1] perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff) #estimate the square size in pixels square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2) radius = int(square_size * 0.5 + 0.5) corners = cv.FindCornerSubPix( image, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1)) #uncomment to debug chessboard detection #cv.DrawChessboardCorners(image, (corners_x, corners_y), corners, 1) #cv.NamedWindow("image") #cv.ShowImage("image", image) #cv.WaitKey(600) #we'll also generate the object points if the user has specified spacing object_points = cv.CreateMat(3, corners_x * corners_y, cv.CV_32FC1) for y in range(corners_y): for x in range(corners_x): cv.SetReal2D(object_points, 0, y * corners_x + x, x * self.dim) cv.SetReal2D(object_points, 1, y * corners_x + x, y * self.dim) cv.SetReal2D(object_points, 2, y * corners_x + x, 0.0) #not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert corners_cv = cv.CreateMat(2, corners_x * corners_y, cv.CV_32FC1) for i in range(corners_x * corners_y): cv.SetReal2D(corners_cv, 0, i, corners[i][0]) cv.SetReal2D(corners_cv, 1, i, corners[i][1]) return (corners_cv, object_points) else: #cv.NamedWindow("image_scaled") #cv.ShowImage("image_scaled", image_scaled) #cv.WaitKey(600) rospy.logwarn("Didn't find checkerboard") return (None, None) return
def find_chessboard(img, ncol=5, nrow=4, useSubPix=True): ''' wrapper for the open cv chessboard detection img: color sensor_msgs.Image or cvmat of the image image ''' # convert image msg to cv if (type(img) == sensor_msgs.Image): img = msg2cvmat(img); # create chessboard size tuple chessboardSize = (ncol, nrow); # find corners (success, chessboard) = cv.FindChessboardCorners(img, chessboardSize, flags=cv.CV_CALIB_CB_ADAPTIVE_THRESH + \ cv.CV_CALIB_CB_NORMALIZE_IMAGE + cv.CV_CALIB_CB_FILTER_QUADS) if (success != 0): if (useSubPix): # create grayscale gray = cv.CreateImage((img.cols, img.rows), cv.IPL_DEPTH_8U, 1); cv.CvtColor(img, gray, cv.CV_RGB2GRAY); # compute window (1/2 distance betweeen two corners) c0 = np.array(chessboard[0]); c1 = np.array(chessboard[1]); w = int(np.ceil(np.sqrt(np.dot((c1 - c0), (c1 - c0)))/2.0)); # sub pixel refinement chessboard = cv.FindCornerSubPix(gray, chessboard, (w,w), (-1,-1), (cv.CV_TERMCRIT_ITER | cv.CV_TERMCRIT_EPS, 20, 0.01)) return (success, np.array(chessboard));
def detect(self, cvimage): corners_x = self.pattern['corners_x'] corners_y = self.pattern['corners_y'] found, corners = cv.FindChessboardCorners( cvimage, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH) if found: board_corners = (corners[0], corners[corners_x - 1], corners[(corners_y - 1) * corners_x], corners[len(corners) - 1]) #find the perimeter of the checkerboard perimeter = 0.0 for i in range(len(board_corners)): next = (i + 1) % 4 xdiff = board_corners[i][0] - board_corners[next][0] ydiff = board_corners[i][1] - board_corners[next][1] perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff) #estimate the square size in pixels square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2) radius = int(square_size * 0.5 + 0.5) corners = array( cv.FindCornerSubPix( cvimage, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.01)), float64) return corners else: return None
def image_filter(self, cv_image, info, copy=None): board_sz = (self.cols, self.rows) (found, corners) = cv.FindChessboardCorners( cv_image, board_sz, (cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS)) cv.DrawChessboardCorners(cv_image, (self.cols, self.rows), corners, 1) return cv_image
def detect(self, image): #resize the image base on the scaling parameters we've been configured with scaled_width = int(.5 + image.width * self.width_scaling) scaled_height = int(.5 + image.height * self.height_scaling) #in cvMat its row, col so height comes before width image_scaled = cv.CreateMat(scaled_height, scaled_width, cv.GetElemType(image)) cv.Resize(image, image_scaled, cv.CV_INTER_LINEAR) #Here, we'll actually call the openCV detector found, corners = cv.FindChessboardCorners(image_scaled, (self.corners_x, self.corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH) if found: board_corners = self.get_board_corners(corners) #find the perimeter of the checkerboard perimeter = 0.0 for i in range(len(board_corners)): next = (i + 1) % 4 xdiff = board_corners[i][0] - board_corners[next][0] ydiff = board_corners[i][1] - board_corners[next][1] perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff) #estimate the square size in pixels square_size = perimeter / ((self.corners_x - 1 + self.corners_y - 1) * 2) radius = int(square_size * 0.5 + 0.5) corners = cv.FindCornerSubPix(image_scaled, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1)) if self.display: #uncomment to debug chessboard detection cv.DrawChessboardCorners(image_scaled, (self.corners_x, self.corners_y), corners, 1) cv.NamedWindow("image_scaled") cv.ShowImage("image_scaled", image_scaled) cv.WaitKey(5) object_points = None #we'll also generate the object points if the user has specified spacing if self.spacing_x != None and self.spacing_y != None: object_points = cv.CreateMat(3, self.corners_x * self.corners_y, cv.CV_32FC1) for y in range(self.corners_y): for x in range(self.corners_x): cv.SetReal2D(object_points, 0, y*self.corners_x + x, x * self.spacing_x) cv.SetReal2D(object_points, 1, y*self.corners_x + x, y * self.spacing_y) cv.SetReal2D(object_points, 2, y*self.corners_x + x, 0.0) #not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert corners_cv = cv.CreateMat(2, self.corners_x * self.corners_y, cv.CV_32FC1) for i in range(self.corners_x * self.corners_y): cv.SetReal2D(corners_cv, 0, i, corners[i][0]) cv.SetReal2D(corners_cv, 1, i, corners[i][1]) return (corners_cv, object_points) else: rospy.logdebug("Didn't find checkerboard") return (None, None)
def process(self, gui): """Process file, find corners on chessboard and keep points """ gui.setMessage("Analyzing movie... get frame count") #open capture file capture = cv.CaptureFromFile(self.filename) frame = cv.QueryFrame(capture) #count... I know it sucks numframes=1 while frame: frame = cv.QueryFrame(capture) numframes +=1 step = int(numframes/Calibration.NUMPOINT) capture = cv.CaptureFromFile(self.filename) frame = cv.QueryFrame(capture) #grab a frame to get some information self.framesize = (frame.width, frame.height) gray = cv.CreateImage((frame.width,frame.height), 8, 1) points = [] nframe = 0 f=0 cv.NamedWindow("ChessBoard",cv.CV_WINDOW_NORMAL) gui.setMessage("Analyzing movie... find chessCorner for %d frames" % Calibration.NUMPOINT) while frame: f+=1 #find corners state,found = cv.FindChessboardCorners(frame, self.chessSize, flags=cv.CV_CALIB_CB_FILTER_QUADS) if state==1: #affine search cv.CvtColor( frame, gray, cv.CV_BGR2GRAY ) found = cv.FindCornerSubPix(gray, found, (11,11), (-1,-1), (cv.CV_TERMCRIT_ITER | cv.CV_TERMCRIT_EPS, 30, 0.1)) #draw corners on image cv.DrawChessboardCorners(frame, self.chessSize, found, state) #compute points for x,y in found: points.append((x,y)) nframe+=1 cv.ResizeWindow("ChessBoard",640,480) cv.ShowImage("ChessBoard",frame) cv.WaitKey(4) frame = cv.QueryFrame(capture) #grab a frame to get some information #skip frames to get only NUMPOINT of point to calculate while f%step != 0 and frame: f+=1 frame = cv.QueryFrame(capture) self.points = points self.nframe = nframe gui.setMessage("Analyze end, %d points found" % len(self.points))
def find_corners(im, w=8, h=6): found, corners = cv.FindChessboardCorners(im, (w, h)) corners = np.array(corners) if found: figure(1) clf() imshow(im) scatter(corners[:, 0], corners[:, 1], c='r') return found, corners
def get_corners(mono, refine=False): (ok, corners) = cv.FindChessboardCorners( mono, (num_x_ints, num_y_ints), cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_NORMALIZE_IMAGE) if refine and ok: corners = cv.FindCornerSubPix( mono, corners, (5, 5), (-1, -1), (cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1)) return (ok, corners)
def capture_pair(index, ir_img): rgb_img = freenect.sync_get_video(format=freenect.VIDEO_RGB)[0] detected, corners = cv.FindChessboardCorners(prepare_cv(rgb_img), (8, 6)) if not detected: return index imsave("ir%03d.png" % index, ir_img) imsave("rgb%03d.png" % index, rgb_img) return index + 1
def detect(self, image): #resize the image base on the scaling parameters we've been configured with scaled_width = int(.5 + image.width * self.width_scaling) scaled_height = int(.5 + image.height * self.height_scaling) #in cvMat its row, col so height comes before width image_scaled = cv.CreateMat(scaled_height, scaled_width, cv.GetElemType(image)) cv.Resize(image, image_scaled, cv.CV_INTER_LINEAR) found, corners = cv.FindChessboardCorners( image_scaled, (self.corners_x, self.corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH) if found: rospy.logdebug("Found cb") board_corners = self.get_board_corners(corners) #find the perimeter of the checkerboard perimeter = 0.0 for i in range(len(board_corners)): next = (i + 1) % 4 xdiff = board_corners[i][0] - board_corners[next][0] ydiff = board_corners[i][1] - board_corners[next][1] perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff) #estimate the square size in pixels square_size = perimeter / ( (self.corners_x - 1 + self.corners_y - 1) * 2) radius = int(square_size * 0.5 + 0.5) corners = cv.FindCornerSubPix( image_scaled, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1)) #cv.DrawChessboardCorners(image_scaled, (self.corners_x, self.corners_y), corners, 1) #cv.NamedWindow("image_scaled") #cv.ShowImage("image_scaled", image_scaled) #cv.WaitKey() object_points = None #we'll also generate the object points if they've been requested if self.spacing_x != None and self.spacing_y != None: object_points = [None] * (self.corners_x * self.corners_y) for i in range(self.corners_y): for j in range(self.corners_x): object_points[i * self.corners_x + j] = (j * self.spacing_x, i * self.spacing_y) return (corners, object_points) else: rospy.logdebug("Didn't find checkerboard") return (None, None)
def detect_chessboard(image, corners_x, corners_y, spacing_x, spacing_y): #Here, we'll actually call the openCV detector found, corners = cv.FindChessboardCorners(image, (corners_x, corners_y), cv.CV_CALIB_CB_ADAPTIVE_THRESH) if found: board_corners = get_board_corners(corners, corners_x, corners_y) #find the perimeter of the checkerboard perimeter = 0.0 for i in range(len(board_corners)): next = (i + 1) % 4 xdiff = board_corners[i][0] - board_corners[next][0] ydiff = board_corners[i][1] - board_corners[next][1] perimeter += math.sqrt(xdiff * xdiff + ydiff * ydiff) #estimate the square size in pixels square_size = perimeter / ((corners_x - 1 + corners_y - 1) * 2) radius = int(square_size * 0.5 + 0.5) corners = cv.FindCornerSubPix( image, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS | cv.CV_TERMCRIT_ITER, 30, 0.1)) #uncomment to debug chessboard detection print 'Chessboard found' #cv.DrawChessboardCorners(image_scaled, (corners_x, corners_y), corners, 1) #cv.NamedWindow("image_scaled") #cv.ShowImage("image_scaled", image_scaled) #cv.WaitKey(600) object_points = None #we'll also generate the object points if the user has specified spacing if spacing_x != None and spacing_y != None: object_points = cv.CreateMat(1, corners_x * corners_y, cv.CV_32FC3) for y in range(corners_y): for x in range(corners_x): object_points[0, y * corners_x + x] = (x * spacing_x, y * spacing_y, 0.0) #not sure why opencv functions return non opencv compatible datatypes... but they do so we'll convert corners_cv = cv.CreateMat(1, corners_x * corners_y, cv.CV_32FC2) for i in range(corners_x * corners_y): corners_cv[0, i] = (corners[i][0], corners[i][1]) return (corners_cv, object_points) else: #cv.NamedWindow("image_scaled") #cv.ShowImage("image_scaled", image_scaled) #cv.WaitKey(600) rospy.logwarn("Didn't find checkerboard") return (None, None)
def main(path, fileformat, ir_prefix, rgb_prefix, key_image, outfile, save_py): cb_points = get_chessboard_points((8, 6), 30, 30) for it, prefix in enumerate([ir_prefix, rgb_prefix]): wildcard = os.path.join(path, "%s*.%s" % (prefix, fileformat)) print "Processing images %s..." % wildcard filenames = glob.glob(wildcard) if len(filenames) == 0: raise ValueError, "no matching images in the given path" images = [imread(fname) for fname in filenames] print "Extracting corners..." corners = [ cv.FindChessboardCorners(cv.fromarray(i), (8, 6)) for i in images ] print "Calibrating..." intrinsic, extrinsic = calibrate(corners, cb_points, images[0].shape[:2]) if it == 0: K_ir = intrinsic T_ir = extrinsic else: K_rgb = intrinsic T_rgb = extrinsic # Estimate the relative transformation from IR to RGB camera. print "Estimating the relative transformation between cameras using the image %s..." % key_image T = np.dot(T_rgb[key_image], la.inv(T_ir[key_image])) print "Saving the results in %s..." % outfile if save_py: with open(outfile, "w") as f: f.write("K_rgb = %r\n" % K_rgb) f.write("K_ir = %r\n" % K_ir) f.write("T = %r\n" % T) return with open(outfile, "w") as f: f.write("# Intrinsics of the RGB camera (f_x,f_y,pp_x,pp_y)\n") f.write("%s %s %s %s\n" % (K_rgb[0, 0], K_rgb[1, 1], K_rgb[0, 2], K_rgb[1, 2])) f.write("# Intrinsics of the IR camera (f_x,f_y,pp_x,pp_y)\n") f.write("%s %s %s %s\n" % (K_ir[0, 0], K_ir[1, 1], K_ir[0, 2], K_ir[1, 2])) f.write( "# Relative orientation of camera IR from camera RGB (rotation matrix)\n" ) f.write(("%s " * 9 + "\n") % (tuple(list(T[:3, :3].ravel())))) f.write("# Relative translation of camera IR from camera RGB\n") f.write(("%s " * 3 + "\n") % (tuple(list(T[:3, 3].ravel())))) print "Success!"
def detect(image): image_size = cv.GetSize(image) # create grayscale version grayscale = cv.CreateImage(image_size, 8, 1) cv.CvtColor(image, grayscale, cv.CV_BGR2GRAY) storage = cv.CreateMemStorage(0) im = cv.CreateImage(image_size, 8, 3) status, corners = cv.FindChessboardCorners(grayscale, (dim, dim)) if status: cv.DrawChessboardCorners(image, (dim, dim), corners, status) is_x = [p[0] for p in corners] is_y = [p[1] for p in corners] return is_x, is_y return [], []
def showChessBoardCorners(imageName, size): cv.NamedWindow("Corners", 1) w1 = cv.LoadImage(imageName) c1 = cv.FindChessboardCorners(w1, pattern_size=size) foundFlag = np.array(c1[0]) print foundFlag c2 = np.array(c1[1]) cv.ShowImage("Corners", w1) cv.WaitKey(250) if (foundFlag): plt.plot(c2[:, 0, ], c2[:, 1], 'rx') cv.WaitKey(250) cv.DestroyAllWindows() return
def main(): print "This is the capture utility for the FreenetFusion calibration." print "1 - Place the calibration template in front of the Kinect sensor" print "2 - Make sure the calibration template is detected (you will see a colored grid)" print "3 - Press SPACE. DO NOT MOVE the calibration template" print "4 - Repeat from 1 with many (~10-20) different poses of the calibration pattern" print "5 - Press ESC when done\n" cv.NamedWindow('Capture') index = 0 while True: img = gray2color( freenect.sync_get_video(format=freenect.VIDEO_IR_8BIT)[0]) img_cv = prepare_cv(img) detected, corners = cv.FindChessboardCorners(img_cv, (8, 6)) if detected: cv.DrawChessboardCorners(img_cv, (8, 6), corners, 1) cv.ShowImage("Capture", img_cv) key = cv.WaitKey(1) if key == 27: break if key != 32: continue if not detected: print "Chessboard template not detected. Please, retry.\n" continue print "Capturing and detecting the chessboard template in the RGB image..." new_index = capture_pair(index, img) if new_index == index: print "The calibration template was not found in the RGB image. Please, retry.\n" continue index = new_index print "Success! Images have been saved. You have taken %d correct image pairs." % index if index < 10: print "At least 10 images are required for a good calibration." print "Please, change the position of the calibration template and repeat.\n"
def on_enter_frame(frame): global have_corners, corners, rowcols, intrinsics, distortion w, h = cv.GetSize(frame) img = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 1) cv.CvtColor(frame, img, cv.CV_RGB2GRAY) have_corners, corners = cv.FindChessboardCorners(img, rowcols) if have_corners: cv.DrawChessboardCorners(frame, rowcols, corners, True) if intrinsics and distortion: new = cv.CreateImage((w, h), cv.IPL_DEPTH_8U, 3) cv.Undistort2(frame, new, intrinsics, distortion) cv.ShowImage('Calibrated', new) cv.ShowImage('Frame', frame)
def _get_corners(img, board, refine=True): """ Get corners for a particular chessboard for an image """ w, h = cv.GetSize(img) if img.channels == 3: mono = cv.CreateMat(h, w, cv.CV_8UC1) cv.CvtColor(img, mono, cv.CV_BGR2GRAY) else: mono = img (ok, corners) = cv.FindChessboardCorners( mono, (board.n_cols, board.n_rows), cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_NORMALIZE_IMAGE | cv2.CALIB_CB_FAST_CHECK) # If any corners are within BORDER pixels of the screen edge, reject the detection by setting ok to false # NOTE: This may cause problems with very low-resolution cameras, where 8 pixels is a non-negligible fraction # of the image size. See http://answers.ros.org/question/3155/how-can-i-calibrate-low-resolution-cameras BORDER = 8 if not all([(BORDER < x < (w - BORDER)) and (BORDER < y < (h - BORDER)) for (x, y) in corners]): ok = False if refine and ok: # Use a radius of half the minimum distance between corners. This should be large enough to snap to the # correct corner, but not so large as to include a wrong corner in the search window. min_distance = float("inf") for row in range(board.n_rows): for col in range(board.n_cols - 1): index = row * board.n_rows + col min_distance = min(min_distance, _pdist(corners[index], corners[index + 1])) for row in range(board.n_rows - 1): for col in range(board.n_cols): index = row * board.n_rows + col min_distance = min( min_distance, _pdist(corners[index], corners[index + board.n_cols])) radius = int(math.ceil(min_distance * 0.5)) corners = cv.FindCornerSubPix( mono, corners, (radius, radius), (-1, -1), (cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1)) return (ok, corners)
def get_image(): im = cv.QueryFrame(camera) # cv.Flip(im, flipMode=1) # mirror effect success, corners = cv.FindChessboardCorners(im, PAT_SIZE, findFlags) if success: cv.DrawChessboardCorners(im, PAT_SIZE, corners, success) cv.PutText(im, "Found: (%.1f, %.1f)" % corners[0], infoOrigin, infoFont, (0, 255, 0)) cam_matrix = cv.CreateMat(3, 3, cv.CV_32F) dist_coeff = cv.CreateMat(1, 4, cv.CV_32F) rvecs = cv.CreateMat(1, 9, cv.CV_32F) tvecs = cv.CreateMat(1, 3, cv.CV_32F) pointArr = numpy.array([(x, y, 0) for y in xrange(PAT_SIZE[1]) for x in xrange(PAT_SIZE[0])], numpy.float32) objectPoints = cv.fromarray(pointArr) imgPointArr = numpy.array(corners, numpy.float32) imagePoints = cv.fromarray(imgPointArr) pointCounts = cv.CreateMat(1, 1, cv.CV_32S) pointCounts[0, 0] = PAT_SIZE[0] * PAT_SIZE[1] # Rodrigues version: rvecs3 = cv.CreateMat(1, 3, cv.CV_32F) cv.CalibrateCamera2(objectPoints, imagePoints, pointCounts, IMG_SIZE, cam_matrix, dist_coeff, rvecs3, tvecs) rmat3 = cv.CreateMat(3, 3, cv.CV_32F) cv.Rodrigues2(rvecs3, rmat3) # end Rodrigues version #cv.CalibrateCamera2(objectPoints, imagePoints, pointCounts, IMG_SIZE, cam_matrix, dist_coeff, rvecs, tvecs) #rmat = numpy.asarray(rvecs).reshape((3, 3), order='C') #print "RVecs:" #print rmat print "TVecs:", numpy.asarray(tvecs) # 3. column of R == rotated z versor, angle toward x; z=(2, 2), x=(0, 2) yaw = math.atan2(rmat3[0, 2], rmat3[2, 2]) * 180 / math.pi # rotated z versor, height y=(1, 2) to length==1 pitch = math.asin(rmat3[1, 2]) * 180 / math.pi # 1. column of R = rotated x versor, height y = (1, 0) to length==1 roll = math.asin(rmat3[1, 0]) * 180 / math.pi print "Yaw %5.2f, Pitch %5.2f, Roll %5.2f" % (yaw, pitch, roll) #import pdb; pdb.set_trace() print '-'*40 else: cv.PutText(im, "Not found.", infoOrigin, infoFont, (0, 255, 0)) return cv2pygame(im)
def get_corners(self, mono, refine=True): """ Get corners for a particular chessboard for an image """ (ok, corners) = cv.FindChessboardCorners(mono, (self.n_cols, self.n_rows), self.flags) if refine and ok: # Use a radius of half the minimum distance between corners. This should be large enough to snap to the # correct corner, but not so large as to include a wrong corner in the search window. min_distance = float("inf") for row in range(self.n_rows): for col in range(self.n_cols - 1): index = row*self.n_cols + col min_distance = min(min_distance, _pdist(corners[index], corners[index + 1])) for row in range(self.n_rows - 1): for col in range(self.n_cols): index = row*self.n_cols + col min_distance = min(min_distance, _pdist(corners[index], corners[index + self.n_cols])) radius = int(math.ceil(min_distance * 0.5)) corners = cv.FindCornerSubPix(mono, corners, (radius,radius), (-1,-1), ( cv.CV_TERMCRIT_EPS+cv.CV_TERMCRIT_ITER, 30, 0.1 )) return (ok, corners)
def err(w): for k in arange(len(t)): #A = [[w[0], 0., midx],[0., w[1], midy],[0., 0., 1.]] A = [[w[0], 0., w[3]], [0., w[0], w[4]], [0., 0., 1.]] Rt = [[1., 0., 0., 0.], [0., 1., 0., 0.], [0., 0., 1., t[k]]] c = dot(A, dot(Rt, dist[k])) / w[2] calc[k, 0] = c[0] / c[2] calc[k, 1] = c[1] / c[2] diff = calc - cc e = norm(diff) return e for file in files: filename = "../util/data/" + file im = cv.LoadImage(filename, cv.CV_LOAD_IMAGE_GRAYSCALE) im3 = cv.LoadImage(filename, cv.CV_LOAD_IMAGE_COLOR) status, corners = cv.FindChessboardCorners(im, (dim, dim)) cc[i, 0] = corners[i][0] cc[i, 1] = corners[i][1] i += 1 # fx fy s #v = fmin(err, [200, 200, 1000],maxiter=10000, maxfun=None) v = fmin(err, [200, 200, 1000, 320, 220], maxiter=10000, maxfun=None) print v
def calibrate(gridFiles, gridSize, gridBlockSize): cpts = [] imageSize = None for gf in gridFiles: image = cv.LoadImage(gf, False) success, corners = cv.FindChessboardCorners(image, gridSize) corners = cv.FindCornerSubPix( image, corners, (5, 5), (-1, -1), (cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1)) gridN = gridSize[0] * gridSize[1] if len(corners) != gridN: logging.debug("File failed: %s" % gf) continue # fix corners so that the first point is top left # compare corner[0] to corner[-1] if corners[0][0] > corners[1][0]: # flip left/right logging.debug("Grid is horizontally flipped") flipped = [] for x in xrange(gridSize[0]): for y in xrange(gridSize[1]): cx = gridSize[0] - x - 1 cy = y flipped.append(corners[cx + cy * gridSize[0]]) corners = flipped if corners[0][1] > corners[-1][1]: # flip top/bottom logging.debug("Grid is vertically flipped") flipped = [] for x in xrange(gridSize[0]): for y in xrange(gridSize[1]): cx = x cy = gridSize[1] - y - 1 flipped.append(corners[cx + cy * gridSize[0]]) corners = flipped cpts.append(corners) imageSize = cv.GetSize(image) nGrids = len(cpts) logging.debug("Found %i grids" % nGrids) if nGrids < 7: logging.warning("Few grids found: %i" % nGrids) if nGrids < 5: raise ValueError("Too few grids: %i" % nGrids) camMatrix = cv.CreateMat(3, 3, cv.CV_64FC1) cv.SetZero(camMatrix) camMatrix[0, 0] = 1. camMatrix[1, 1] = 1. distCoeffs = cv.CreateMat(5, 1, cv.CV_64FC1) cv.SetZero(distCoeffs) gridN = gridSize[0] * gridSize[1] imPts = cv.CreateMat(nGrids * gridN, 2, cv.CV_64FC1) objPts = cv.CreateMat(nGrids * gridN, 3, cv.CV_64FC1) ptCounts = cv.CreateMat(nGrids, 1, cv.CV_32SC1) # organize self.calibrationImgPts (to imPts) and construct objPts and ptCounts for (i, c) in enumerate(cpts): for j in xrange(gridN): imPts[j + i * gridN, 0] = c[j][0] imPts[j + i * gridN, 1] = c[j][1] # TODO should thes be actual points? how do I know what they are? objPts[j + i * gridN, 0] = j % gridSize[0] * gridBlockSize objPts[j + i * gridN, 1] = j / gridSize[0] * gridBlockSize objPts[j + i * gridN, 2] = 0. ptCounts[i, 0] = len(c) cv.CalibrateCamera2(objPts, imPts, ptCounts, imageSize, camMatrix, distCoeffs, cv.CreateMat(nGrids, 3, cv.CV_64FC1), cv.CreateMat(nGrids, 3, cv.CV_64FC1), 0) cv.Save("camMatrix.xml", camMatrix) cv.Save("distCoeffs.xml", distCoeffs)
#!/usr/bin/python import cv import sys import urllib2 if __name__ == "__main__": cv.NamedWindow("win") if len(sys.argv) > 1: filename = sys.argv[1] im = cv.LoadImage(filename, cv.CV_LOAD_IMAGE_GRAYSCALE) im3 = cv.LoadImage(filename, cv.CV_LOAD_IMAGE_COLOR) else: url = 'https://code.ros.org/svn/opencv/trunk/opencv/samples/c/left01.jpg' filedata = urllib2.urlopen(url).read() imagefiledata = cv.CreateMatHeader(1, len(filedata), cv.CV_8UC1) cv.SetData(imagefiledata, filedata, len(filedata)) im = cv.DecodeImageM(imagefiledata, cv.CV_LOAD_IMAGE_GRAYSCALE) im3 = cv.DecodeImageM(imagefiledata, cv.CV_LOAD_IMAGE_COLOR) chessboard_dim = ( 9, 6 ) found_all, corners = cv.FindChessboardCorners( im, chessboard_dim ) print found_all, len(corners) cv.DrawChessboardCorners( im3, chessboard_dim, corners, found_all ) cv.ShowImage("win", im3); cv.WaitKey()
distortion_coefficient = cv.CreateMat(5, 1, cv.CV_32FC1) # capture frames of specified properties and modification of matrix values i = 0 z = 0 # to print number of frames successes = 0 capture = cv.CaptureFromCAM(0) # capturing required number of views while (successes < n_boards): found = 0 image = cv.QueryFrame(capture) gray_image = cv.CreateImage(cv.GetSize(image), 8, 1) cv.CvtColor(image, gray_image, cv.CV_BGR2GRAY) (found, corners) = cv.FindChessboardCorners( gray_image, board_sz, cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS) corners = cv.FindCornerSubPix( gray_image, corners, (11, 11), (-1, -1), (cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1)) # if got a good image,draw chess board if found == 1: print "found frame number {0}".format(z + 1) cv.DrawChessboardCorners(image, board_sz, corners, 1) corner_count = len(corners) z = z + 1 # if got a good image, add to matrix if len(corners) == board_n: step = successes * board_n k = step
def annotate_image(cv_im, mech_info_dict, dir): #cv_im = cv.LoadImage(sys.argv[1], cv.CV_LOAD_IMAGE_GRAYSCALE) size = cv.GetSize(cv_im) print 'Image size:', size[0], size[1] # col, row wnd = 'Image Annotate' cv.NamedWindow(wnd, cv.CV_WINDOW_AUTOSIZE) disp_im = cv.CloneImage(cv_im) new_size = (size[0] / 2, size[1] / 2) scale_factor = 1 checker_origin_height = mech_info_dict['height'] # chesscoard corners mat cb_dims = (5, 8) # checkerboard dims. (x, y) sq_sz = 19 # size of checkerboard in real units. cb_offset = 500, 500 cb_coords = cv.CreateMat(2, cb_dims[0] * cb_dims[1], cv.CV_64FC1) n = 0 for r in range(cb_dims[1]): for c in range(cb_dims[0]): cb_coords[0, n] = c * sq_sz + cb_offset[0] # x coord cb_coords[1, n] = r * sq_sz + cb_offset[1] # y coord n += 1 clicked_list = [] recreate_image = False mechanism_calc_state = 0 mechanism_calc_dict = {} while True: for p in clicked_list: x, y = p[0] * scale_factor, p[1] * scale_factor cv.Circle(disp_im, (x, y), 3, (255, 0, 0)) cv.ShowImage(wnd, disp_im) k = cv.WaitKey(10) k = k % 256 if k == 27: # ESC break elif k == ord('='): # + key without the shift scale_factor = scale_factor * 1.2 recreate_image = True elif k == ord('-'): # - key scale_factor = scale_factor / 1.2 recreate_image = True elif k == ord('c'): # find chessboard corners. succ, corners = cv.FindChessboardCorners(disp_im, cb_dims) if succ == 0: print 'Chessboard detection FAILED.' else: # chessboard detection was successful cv.DrawChessboardCorners(disp_im, cb_dims, corners, succ) cb_im = cv.CreateMat(2, cb_dims[0] * cb_dims[1], cv.CV_64FC1) corners_mat = np.array(corners).T n = 0 for r in range(cb_dims[1]): for c in range(cb_dims[0]): cb_im[0, n] = corners_mat[0, n] # x coord cb_im[1, n] = corners_mat[1, n] # y coord n += 1 H = cv.CreateMat(3, 3, cv.CV_64FC1) H1 = cv.FindHomography(cb_im, cb_coords, H) Hnp = np.reshape(np.fromstring(H1.tostring()), (3, 3)) print 'Homography:' print Hnp d = cv.CloneImage(disp_im) cv.WarpPerspective(d, disp_im, H1, cv.CV_WARP_FILL_OUTLIERS) cv_im = cv.CloneImage(disp_im) elif k == ord('1'): # calculate width of the mechanism del clicked_list[:] cv.SetMouseCallback(wnd, on_mouse, (disp_im, clicked_list, scale_factor)) recreate_image = True print 'Click on two endpoints to mark the width of the mechanism' mechanism_calc_state = 1 elif k == ord('2'): # distance of handle from the hinge del clicked_list[:] cv.SetMouseCallback(wnd, on_mouse, (disp_im, clicked_list, scale_factor)) recreate_image = True print 'Click on handle and hinge to compute distance of handle from hinge.' mechanism_calc_state = 2 elif k == ord('3'): # height of the handle above the ground del clicked_list[:] cv.SetMouseCallback(wnd, on_mouse, (disp_im, clicked_list, scale_factor)) recreate_image = True print 'Click on handle top and bottom to compute height of handle above the ground.' mechanism_calc_state = 3 elif k == ord('4'): # top and bottom edge of the mechanism del clicked_list[:] cv.SetMouseCallback(wnd, on_mouse, (disp_im, clicked_list, scale_factor)) recreate_image = True print 'Click on top and bottom edges of the mechanism.' mechanism_calc_state = 4 elif k == ord('d'): # display the calculated values print 'mechanism_calc_dict:', mechanism_calc_dict print 'mech_info_dict:', mech_info_dict elif k == ord('s'): # save the pkl ut.save_pickle(mechanism_calc_dict, dir + '/mechanism_calc_dict.pkl') print 'Saved the pickle' #elif k != -1: elif k != 255: print 'k:', k if recreate_image: new_size = (int(size[0] * scale_factor), int(size[1] * scale_factor)) disp_im = cv.CreateImage(new_size, cv_im.depth, cv_im.nChannels) cv.Resize(cv_im, disp_im) cv.SetMouseCallback(wnd, on_mouse, (disp_im, clicked_list, scale_factor)) recreate_image = False if len(clicked_list) == 2: if mechanism_calc_state == 1: w = abs(clicked_list[0][0] - clicked_list[1][0]) print 'Width in mm:', w mechanism_calc_dict['mech_width'] = w / 1000. if mechanism_calc_state == 2: w = abs(clicked_list[0][0] - clicked_list[1][0]) print 'Width in mm:', w mechanism_calc_dict['handle_hinge_dist'] = w / 1000. if mechanism_calc_state == 3: p1, p2 = clicked_list[0], clicked_list[1] h1 = (cb_offset[1] - p1[1]) / 1000. + checker_origin_height h2 = (cb_offset[1] - p2[1]) / 1000. + checker_origin_height mechanism_calc_dict['handle_top'] = max(h1, h2) mechanism_calc_dict['handle_bottom'] = min(h1, h2) if mechanism_calc_state == 4: p1, p2 = clicked_list[0], clicked_list[1] h1 = (cb_offset[1] - p1[1]) / 1000. + checker_origin_height h2 = (cb_offset[1] - p2[1]) / 1000. + checker_origin_height mechanism_calc_dict['mechanism_top'] = max(h1, h2) mechanism_calc_dict['mechanism_bottom'] = min(h1, h2) mechanism_calc_state = 0
def calibrate(imagedir): nimages = 0 datapoints = [] im_dims = (0,0) for f in os.listdir(imagedir): if (f.find('pgm')<0): continue image = imagedir+'/'+f grey = cv.LoadImage(image,cv.CV_LOAD_IMAGE_GRAYSCALE) found,points=cv.FindChessboardCorners(grey,dims,cv.CV_CALIB_CB_ADAPTIVE_THRESH) points=cv.FindCornerSubPix(grey,points,(11,11),(-1,-1),(cv.CV_TERMCRIT_EPS+cv.CV_TERMCRIT_ITER,30,0.1)) if (found): print 'using ', image nimages += 1 datapoints.append(points) im_dims = (grey.width, grey.height) #Number of points in chessboard num_pts = dims[0] * dims[1] #image points ipts = cv.CreateMat(nimages * num_pts, 2, cv.CV_32FC1) #object points opts = cv.CreateMat(nimages * num_pts, 3, cv.CV_32FC1) npts = cv.CreateMat(nimages, 1, cv.CV_32SC1) for i in range(0,nimages): k=i*num_pts squareSize = 1.0 # squareSize is 1.0 (i.e. units of checkerboard) for j in range(num_pts): cv.Set2D(ipts,k,0,datapoints[i][j][0]) cv.Set2D(ipts,k,1,datapoints[i][j][1]) cv.Set2D(opts,k,0,float(j%dims[0])*squareSize) cv.Set2D(opts,k,1,float(j/dims[0])*squareSize) cv.Set2D(opts,k,2,0.0) k=k+1 cv.Set2D(npts,i,0,num_pts) K = cv.CreateMat(3, 3, cv.CV_64FC1) D = cv.CreateMat(5, 1, cv.CV_64FC1) cv.SetZero(K) cv.SetZero(D) # focal lengths have 1/1 ratio K[0,0] = im_dims[0] K[1,1] = im_dims[0] K[0,2] = im_dims[0]/2 K[1,2] = im_dims[1]/2 K[2,2] = 1.0 rcv = cv.CreateMat(nimages, 3, cv.CV_64FC1) tcv = cv.CreateMat(nimages, 3, cv.CV_64FC1) #print 'object' #print array(opts) #print 'image' #print array(ipts) #print 'npts' #print array(npts) size=cv.GetSize(grey) flags = 0 #flags |= cv.CV_CALIB_FIX_ASPECT_RATIO #flags |= cv.CV_CALIB_USE_INTRINSIC_GUESS #flags |= cv.CV_CALIB_ZERO_TANGENT_DIST #flags |= cv.CV_CALIB_FIX_PRINCIPAL_POINT cv.CalibrateCamera2(opts, ipts, npts, size, K, D, rcv, tcv, flags) # storing results using CameraParams C = CameraParams(xresolution=im_dims[0], yresolution=im_dims[1]) print array(K) print array(D) C.setParams(K, D) C.save(imagedir+"/params.json")
distortion = cv.CreateMat(4, 1, cv.CV_32FC1) corners = None corner_count = 0 successes = 0 step = 0 frame = 0 image = cv.QueryFrame(cam) gray_image = cv.CreateImage(cv.GetSize(image), 8, 1) while (successes < num_boards): frame += 1 if (frame % num_framestep == 0): corners = cv.FindChessboardCorners( image, board_size, cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS) corners = corners[1] cv.CvtColor(image, gray_image, cv.CV_BGR2GRAY) cv.FindCornerSubPix( gray_image, corners, (11, 11), (0, 0), (cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1)) if (len(corners) > 1): cv.DrawChessboardCorners(image, board_size, corners, 1) if (len(corners) == board_total): cv.ShowImage("Snapshot", image) step = successes * board_total i = step for j in range(board_total): cv.Set2D(image_points, i, 0, corners[j][0]) cv.Set2D(image_points, i, 1, corners[j][1])
def gather(imagedir, debayer, im_w, im_h): c=cv.CaptureFromCAM(0) cv.SetCaptureProperty(c,cv.CV_CAP_PROP_FRAME_WIDTH,im_w) cv.SetCaptureProperty(c,cv.CV_CAP_PROP_FRAME_HEIGHT,im_h) #cv.SetCaptureProperty(c,cv.CV_CAP_PROP_FPS,3.75) grey=cv.CreateImage((im_w,im_h),8,1) im_cnt = 0 print 'position chess board then press space to find corners' print 'when complete press q' if not os.path.exists(imagedir): os.mkdir(imagedir) if not os.path.exists(imagedir): print('failed to create image dir') sys.exit() while True: f=cv.QueryFrame(c) if (f == None): print 'failed to capture' continue #print f.width, f.height if (debayer): bgr=cv.CreateImage((im_w,im_h),8,3) cv.CvtColor(f,bgr,cv.CV_BayerGR2BGR) f = bgr if (f.channels==3): cv.CvtColor(f,grey,cv.CV_BGR2GRAY) elif (f.channels==1): # convert to 8 bit pixel depth cv.Convert(f,grey) else: print 'unsupported colourspace' break key = cv.WaitKey(100) key = key & 255 if not key in range(128): # show the image if (im_w > 640): tmp=cv.CreateImage((im_w/2,im_h/2),8,f.channels) cv.Resize(f,tmp) cv.ShowImage("calibrate", tmp) else: cv.ShowImage("calibrate", f) continue print 'key=0x%x(\'%c\')' % (key, chr(key)) key = chr(key) if (key == ' '): print 'looking for corners...' found,points=cv.FindChessboardCorners(grey,dims,cv.CV_CALIB_CB_ADAPTIVE_THRESH) if found == 0: print 'Failed to find corners. Reposition and try again' else: cv.DrawChessboardCorners(f,dims,points,found) #show the final image if (im_w > 640): tmp=cv.CreateImage((im_w/2,im_h/2),8,f.channels) cv.Resize(f,tmp) cv.ShowImage("calibrate", tmp) else: cv.ShowImage("calibrate", f) #wait indefinitely print 'Keep this image ? y/n' key = chr(cv.WaitKey(0) & 255) if (key == 'y'): print 'Keeping image ', im_cnt cv.SaveImage(imagedir+'/calib%05d.pgm' % (im_cnt), grey ) im_cnt+=1 else: print 'discarding image' print 'press any key to find next corners' elif (key == 'q'): print 'quit' break
def image_filter(self, cv_image, info, copy=None): image = cv_image #Only works on a grayscale image gray_image = cv.CreateImage(cv.GetSize(image), 8, 1) cv.CvtColor(image, gray_image, cv.CV_BGR2GRAY) print "Called with mode: %s" % self.mode if self.mode == "default" or self.mode == "save_h": print "Computing homography matrix from checkerboard" #Get the width and height of the board board_w = self.cols board_h = self.rows #Area of the board = "board_n" board_n = board_w * board_h board_sz = (board_w, board_h) #This needs to be fed with a "height", so it knows how high up the perspective transform should be. #I've found for the wide_stereo cameras, a value of -15 works well. For the prosilica, -40. Don't ask me why init_height = self.height #Uses openCV to find the checkerboard (found, corners) = cv.FindChessboardCorners( image, board_sz, (cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS)) if (not found): print "Couldn't aquire checkerboard, only found 0 of %d corners\n" % board_n gr = CloneImage(image) cv.CvtColor(gray_image, gr, cv.CV_GRAY2BGR) return gr #We need subpixel accuracy, so we tell it where the corners are and it magically does the rest. I forget what (11,11) and (-1,-1) mean. cv.FindCornerSubPix( gray_image, corners, (11, 11), (-1, -1), (cv.CV_TERMCRIT_EPS + cv.CV_TERMCRIT_ITER, 30, 0.1)) #Pull out the Image Points (3d location of the checkerboard corners, in the camera frame) #and the Object Points (2d location of the corners on the checkerboard object) objPts = point_array(4) imgPts = point_array(4) objPts[0] = (0, 0) objPts[1] = (board_w - 1, 0) objPts[2] = (0, board_h - 1) objPts[3] = (board_w - 1, board_h - 1) imgPts[0] = corners[0] imgPts[1] = corners[board_w - 1] imgPts[2] = corners[(board_h - 1) * board_w] imgPts[3] = corners[(board_h - 1) * board_w + board_w - 1] #Use GetPerspectiveTransform to populate our Homography matrix H = cv.CreateMat(3, 3, cv.CV_32FC1) cv.GetPerspectiveTransform(objPts, imgPts, H) #Since we don't get any z information from this, we populate H[2,2] with our hard-coded height H[2, 2] = init_height if self.mode == "save_h": print "Saving Homography matrix to %s" % self.matrix_location cv.Save(self.matrix_location, H) else: print "Loading Homography matrix from %s" % self.matrix_location H = cv.Load(self.matrix_location) birds_image = CloneImage(image) #birds_image = cv.CreateImage((image.width*3,image.height*3),8,3) #Uses the homography matrix to warp the perspective. cv.WarpPerspective( image, birds_image, H, cv.CV_INTER_LINEAR + cv.CV_WARP_INVERSE_MAP + cv.CV_WARP_FILL_OUTLIERS) #Note: If you need to undo the transformation, you can simply invert H and call cv.WarpPerspective again. return birds_image