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 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_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 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 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]]);
