def get_chessboard(self, columns, rows, show=False):
"""
Take a picture with a chessboard visible in both captures.
``columns`` and ``rows`` should be the number of inside corners in the
chessboard's columns and rows. ``show`` determines whether the frames
are shown while the cameras search for a chessboard.
"""
self.total_white = 0.95
self.total_black = 0.25
found_chessboard = [False, False]
m = 0
while not all(found_chessboard):
m += 1
sys.stderr.write("Looking... %r %r\r" % (self.n, m))
frames = self.get_frames()
if show:
self.show_frames(1)
# fast check isn't used in later analysis, and if fast
# check succeeds but regular does not, that means an
# error during processing. It's still not perfect, because
# the later processing converts to gray
(found_chessboard[0], corners) = \
cv2.findChessboardCorners(frames[0], (rows, columns))
if not found_chessboard[0]:
next
(found_chessboard[1], corners) = \
cv2.findChessboardCorners(frames[1], (rows, columns))
sys.stderr.write("\nFound %r\n" % (self.n,))
self.n += 1
return frames
def detect_points_from_pattern(img_mask, is_thermal=False):
square_size = 2.15
pattern_size = (9, 6)
pattern_points = np.zeros( (np.prod(pattern_size), 3), np.float32 )
pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2)
pattern_points *= square_size
from glob import glob
import re
img_names = glob(img_mask)
img_names = sorted(img_names, key=lambda name: re.search('(left|right|top|bottom|thermal)(.*)\.(jpg|JPG)', name).groups()[1])
# debug_dir = None
obj_points = []
img_points = []
valid = []
# h, w = 0, 0
for i, fn in enumerate(img_names):
print 'processing %s...' % fn,
img_rgb = cv2.imread(fn)
# h, w = img.shape[:2]
cv2.imshow('original',img_rgb)
img = cv2.cvtColor(img_rgb, cv2.COLOR_RGB2GRAY)
if is_thermal:
cv2.imshow('original',img)
cv2.waitKey()
cv2.imshow('negative', 255-img)
cv2.waitKey()
found, corners = cv2.findChessboardCorners(255-img, pattern_size)
# flags=cv2.cv.CV_CALIB_CB_ADAPTIVE_THRESH)
# flags=cv2.cv.CV_CALIB_CB_NORMALIZE_IMAGE)
else:
found, corners = cv2.findChessboardCorners(img, pattern_size)
if found:
term = ( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1 )
cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), term)
if is_thermal:
vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
else:
vis = img_rgb
cv2.drawChessboardCorners(vis, pattern_size, corners, found)
cv2.imshow('vis', vis)
cv2.waitKey()
if is_thermal:
cv2.imwrite('/home/yuncong/Desktop/thermal_debug'+str(i)+'.png', vis)
else:
cv2.imwrite('/home/yuncong/Desktop/debug'+str(i)+'.png', vis)
else:
print 'chessboard not found'
continue
img_points.append(corners.reshape(-1, 2))
obj_points.append(pattern_points)
valid.append(i)
print 'ok'
return img_points, obj_points, valid
def homography_loop(mtx, dist, newcamermtx, x, y, w, h, file_path):
global frame_ret
global corners_ret
global frame
global corners
global count
board = cv2.imread('Data\pattern.png', cv2.CV_LOAD_IMAGE_GRAYSCALE)
im = cv2.imread(file_path)
corners_ret, corners = cv2.findChessboardCorners(board, (Height, Width), None) # points on one plane for homography
# set up video capture
cap = cv2.VideoCapture(0)
if not(cap.isOpened()):
cap.open()
while cv2.waitKey(5) != ord('q'):
frame_ret, frame = cap.read()
if frame_ret == True:
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners2_ret, corners2 = cv2.findChessboardCorners(gray, (Height, Width), None)
if corners2_ret == True:
undist = cv2.undistort(gray, mtx, dist, None, newcamermtx)
#undist= undist[y:y+h, x:x+w]
corners2_ret, corners2 = cv2.findChessboardCorners(undist, (Height, Width), None) # points on the other plane
if corners2_ret:
# compute perspective transform and apply to im
h = cv2.findHomography(corners, corners2)[0]
out = cv2.warpPerspective(im, h,(gray.shape[1], gray.shape[0]))
# create mask and mask inverse
gray_out = cv2.cvtColor(out,cv2.COLOR_BGR2GRAY)
ret, mask = cv2.threshold(gray_out, 10, 255, cv2.THRESH_BINARY)
inv_mask = cv2.bitwise_not(mask)
# create place for the warped image in the frame
frame = cv2.bitwise_and(frame, frame, mask=inv_mask)
cv2.imshow('masked frame', frame)
# grab only the ROI from the warped image
out = cv2.bitwise_and(out, out, mask = mask)
cv2.imshow('masked picture', out)
# combine the two to create AR effect
frame = cv2.add(frame, out)
cv2.imshow('warp', frame)
cv2.imshow('calib', frame)
cap.release()
def record_stereo_camera(left_video, right_video):
"""
Record stereo pairs that have known good calibration
patterns visible into our test_data directory.
"""
# Get the first frames
ret_left, frame_left = left_video.read()
ret_right, frame_right = right_video.read()
# Get the shape
shape = frame_left.shape
# While there are still images to read, and we have
# selected under a certain number images to save thus far
i = 0
while ret_left is True and ret_right is True and i < 15:
gray_left = cv2.cvtColor(frame_left, cv2.COLOR_BGR2GRAY)
gray_right = cv2.cvtColor(frame_right, cv2.COLOR_BGR2GRAY)
ret_left, corners_left = cv2.findChessboardCorners(gray_left, (6, 7), None)
ret_right, corners_right = cv2.findChessboardCorners(gray_right, (6, 7), None)
# If chessboard corners are found in both images at once
if ret_left is True and ret_right is True:
# Save the images
cv2.imwrite("test_data/videos/calibrator/left/%s.jpeg" % str(i).zfill(3), frame_left)
cv2.imwrite("test_data/videos/calibrator/right/%s.jpeg" % str(i).zfill(3), frame_right)
cv2.cornerSubPix(gray_left, corners_left, (11, 11), (-1, -1), CRITERIA)
cv2.cornerSubPix(gray_right, corners_right, (11, 11), (-1, -1), CRITERIA)
cv2.drawChessboardCorners(frame_left, (6, 7), corners_left, ret_left)
cv2.drawChessboardCorners(frame_right, (6, 7), corners_right, ret_right)
cv2.imshow('frame_left', frame_left)
cv2.imshow('frame_right', frame_right)
k = cv2.waitKey(1) & 0xFF
if k == ord('q'):
break
# Decide whether or not to keep the image
if ret_left is True and ret_right is True:
keep = raw_input("Keep this frame? (y/n) ") == "y"
if keep is True:
print i
i = i + 1
# Advance to the next pair of frames
ret_left, frame_left = left_video.read()
ret_right, frame_right = right_video.read()
cv2.destroyAllWindows()
pass
def checker_board(self, img):
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(img, self.dim, None)
ret_back, corners_back = cv2.findChessboardCorners(img, self.dim_small, None)
print "Checkerboard:",
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
if ret:
print "forward"
#cv2.drawChessboardCorners(gray, self.dim, corners, ret)
height = self.get_height(img, corners)
center = self.get_center(corners)
distance = self.get_distance(gray, height)
angle = self.get_angle(center)
print height, center, distance, angle
if ret_back:
print "back"
height = -self.get_height(img, corners_back, "small")
center = self.get_center(corners_back)
distance = self.get_distance(gray, height, "back")
angle = self.get_angle(center)
print height, center, distance, angle
if ret or ret_back:
self.motor.publish("flush")
if ret:
threshold = math.asin(.5/distance)
distance -= 10
if distance > 20:
distance = int(distance*(3.0/4))
else:
return False
if -threshold < angle < threshold:
print "Moving forward", distance, "angle was", angle
self.motor.publish("f%d" % (int(distance)))
else:
if angle < 0:
angle = 360+angle
print "Rotating", angle, "and moving forward", distance
self.motor.publish("r%df%d" % (int(angle), int(distance)))
print distance, angle, threshold
if distance < 20 and (angle < max(threshold, 8) or angle > min(360-threshold, 352)):
print "Final form entered"
self.searching = False
self.search_turn = False
self.change_state.publish("Found Base Station Final")
else:
self.searching = True
self.searh_turn = False
while rospy.wait_for_message("/motor_status", std_msgs.msg.String).data == "busy":
pass
def computeHomographyChess(src_img, undst_img, pattern_size):
ret, corners = cv2.findChessboardCorners(src_img, pattern_size, None)
srcp = np.array([corners[0, 0], corners[8, 0], corners[45, 0], corners[corners.shape[0] - 1, 0]])
ret, corners_ = cv2.findChessboardCorners(undst_img, pattern_size, None)
if ret:
dstp = np.array([corners_[0, 0], corners_[8, 0], corners_[45, 0], corners_[corners.shape[0] - 1, 0]])
global H
H, mask = cv2.findHomography(srcp, dstp)
return drawCorrespondents(undst_img, srcp, dstp)
def find_chessboard_corners_on_image(img, pattern_size, searchwin_size=5, findcbc_flags=None):
if findcbc_flags == None:
res = cv2.findChessboardCorners(img, pattern_size)
else:
res = cv2.findChessboardCorners(img, pattern_size, flags=findcbc_flags)
found, corners = res
if found:
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
cv2.cornerSubPix(img, corners, (searchwin_size, searchwin_size), (-1, -1), term)
return res
def secondPart():
firstFrame = cv2.imread(videoSequence)
pattern = cv2.imread("images/CalibrationPattern.jpg")
imgGrayPattern = cv2.cvtColor(pattern, cv2.COLOR_BGR2GRAY)
_,cornersPattern = cv2.findChessboardCorners(imgGrayPattern, (9,6))
patternP = getOuterPoints(cornersPattern)
imgGrayFrame = cv2.cvtColor(firstFrame, cv2.COLOR_BGR2GRAY)
_, cornersFrame = cv2.findChessboardCorners(imgGrayFrame, (9,6))
frame = []
frame = getOuterPoints(cornersFrame)
H = estimateHomography(patternP, frame)
np.save("numpyData/H_ff_pattern.npy", H)
def process_image(self):
print "processing image"
left_gray = cv2.cvtColor(self.left_image,cv2.COLOR_BGR2GRAY)
right_gray = cv2.cvtColor(self.right_image,cv2.COLOR_BGR2GRAY)
ret, left_corners = cv2.findChessboardCorners(left_gray, (6,5), flags=1)
ret, right_corners = cv2.findChessboardCorners(right_gray, (6,5), flags=1)
print len(left_corners), len(right_corners)
left, right, = [], []
for i in range(len(left_corners)):
left.append([left_corners[i][0][0], left_corners[i][0][1]])
right.append([right_corners[i][0][0], right_corners[i][0][1]])
f = open('calibration_data/camera_chesspts.p', 'w')
pickle.dump((left_corners, right_corners), f)
f.close()
def _detect_chessboard(self, image, flags=None):
if image is not None:
if self.config.calibration.pattern.rows > 2 and self.config.calibration.pattern.columns > 2:
gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
if flags is None:
ret, corners = cv2.findChessboardCorners(gray, (self.config.calibration.pattern.columns, self.config.calibration.pattern.rows), None)
else:
ret, corners = cv2.findChessboardCorners(gray, (self.config.calibration.pattern.columns, self.config.calibration.pattern.rows), flags=flags)
if ret:
cv2.cornerSubPix(gray, corners, (11,11), (-1, -1), self._criteria)
return corners
def process(controller):
map_initialized = False
num_images = 0
num_samples = 0
while True:
frame = controller.frame()
images = frame.images
if images[0].is_valid and images[1].is_valid:
if not map_initialized:
left_x_map, left_y_map = initDistortionMap(frame.images[0])
right_x_map, right_y_map = initDistortionMap(frame.images[1])
map_initialized = True
undistorted_left = interpolate(get_image_as_array(images[0]), left_x_map, left_y_map)
undistorted_right = interpolate(get_image_as_array(images[1]), left_x_map, left_y_map)
left_bw = cv2.adaptiveThreshold(undistorted_left, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 13, 3)
right_bw = cv2.adaptiveThreshold(undistorted_right, 255, cv2.ADAPTIVE_THRESH_MEAN_C, cv2.THRESH_BINARY, 13, 3)
corners_left = cv2.findChessboardCorners(left_bw, board_size, flags=cv2.CALIB_CB_FILTER_QUADS)
corners_right = cv2.findChessboardCorners(right_bw, board_size, flags=cv2.CALIB_CB_FILTER_QUADS)
if corners_left[0] and corners_right[0]:
cv2.cornerSubPix(left_bw, corners_left[1], (3, 3), (-1, -1), stereocalib_criteria)
cv2.cornerSubPix(right_bw, corners_right[1], (3, 3), (-1, -1), stereocalib_criteria)
cv2.drawChessboardCorners(undistorted_left, board_size, corners_left[1], corners_left[0])
cv2.drawChessboardCorners(undistorted_right, board_size, corners_right[1], corners_right[0])
image_points1.append(corners_left[1])
image_points2.append(corners_right[1])
object_points.append(obj)
num_samples += 1
print(num_samples)
if num_samples > num_boards:
break
cv2.imshow('left', undistorted_left)
cv2.imshow('right', undistorted_right)
cv2.imshow('left_bw', left_bw)
cv2.imshow('right_bw', right_bw)
if cv2.waitKey(1) & 0xFF == ord('q'):
cv2.imwrite(f'left{num_images}.jpg', left_bw)
cv2.imwrite(f'right{num_images}.jpg', right_bw)
num_images += 1
break
retval,cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
object_points, image_points1, image_points2, C1, D1, C2, D2, imageSize=(640, 240), flags=stereocalib_flags, criteria=stereocalib_criteria
)
print("params")
print(retval,cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F)
print("reprojection matrix")
a = cv2.stereoRectify(C1, D1, C2, D2, (640, 240), R, T)
print(a[-3])
def read_images(self, cal_path):
images_right = glob.glob(cal_path + 'RIGHT/*.JPG')
images_left = glob.glob(cal_path + 'LEFT/*.JPG')
images_left.sort()
images_right.sort()
for i, fname in enumerate(images_right):
img_l = cv2.imread(images_left[i])
img_r = cv2.imread(images_right[i])
gray_l = cv2.cvtColor(img_l, cv2.COLOR_BGR2GRAY)
gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret_l, corners_l = cv2.findChessboardCorners(gray_l, (9, 6), None)
ret_r, corners_r = cv2.findChessboardCorners(gray_r, (9, 6), None)
# If found, add object points, image points (after refining them)
self.objpoints.append(self.objp)
if ret_l is True:
rt = cv2.cornerSubPix(gray_l, corners_l, (11, 11),
(-1, -1), self.criteria)
self.imgpoints_l.append(corners_l)
# Draw and display the corners
ret_l = cv2.drawChessboardCorners(img_l, (9, 6),
corners_l, ret_l)
cv2.imshow(images_left[i], img_l)
cv2.waitKey(500)
if ret_r is True:
rt = cv2.cornerSubPix(gray_r, corners_r, (11, 11),
(-1, -1), self.criteria)
self.imgpoints_r.append(corners_r)
# Draw and display the corners
ret_r = cv2.drawChessboardCorners(img_r, (9, 6),
corners_r, ret_r)
cv2.imshow(images_right[i], img_r)
cv2.waitKey(500)
img_shape = gray_l.shape[::-1]
rt, self.M1, self.d1, self.r1, self.t1 = cv2.calibrateCamera(
self.objpoints, self.imgpoints_l, img_shape, None, None)
rt, self.M2, self.d2, self.r2, self.t2 = cv2.calibrateCamera(
self.objpoints, self.imgpoints_r, img_shape, None, None)
self.camera_model = self.stereo_calibrate(img_shape)
def calibrate(filenames):
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = []
for filename in filenames:
# Find the chess board corners. If found, add object points, image points (after refining them)
img = cv2.imread(filename)
if img != None:
print "Loaded " + repr(filename)
else:
print "Unable to load image " + repr(filename)
continue
images.append(img)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (7,6), None)
if ret == True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
imgpoints.append(corners2)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
print "Loaded all images and calbulated calibration"
for i, img in enumerate(images):
img = images[i]
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
x, y, w, h = roi
cv2.imwrite( 'calibrated/out_' + str(i) + '.png', dst[ y : y+h, x : x+w ])
print "Outputted calibrated image: 'calibrated/out_" + str(i) + ".png'"
def firstPart():
''' <002> Here Define the camera matrix of the first view image (01_daniel.png) recorded by the cameraCalibrate2'''
firstFrame = cv2.imread(videoSequence)
pattern_size = (9,6)
obj_points = np.zeros( (np.prod(pattern_size), 3), np.float32 )
obj_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2)
camera = np.zeros((3, 3))
h, w, _ = firstFrame.shape
found,corners = cv2.findChessboardCorners(firstFrame, pattern_size)
img_points = corners.reshape(-1, 2)
if found != 0:
_, camera, _, rotation, translation = cv2.calibrateCamera([obj_points], [img_points], (w, h) ,camera, np.zeros(4) ,flags = 0)
#constructing the projection matrix R|t
global Kt
Kt = np.zeros((3, 4))
rotMatrix = cv2.Rodrigues(rotation[0])[0]
Kt[:, :3] = rotMatrix
Kt[:, 3:] = translation[0]
''' <003> Here Load the first view image (01_daniel.png) and find the chess pattern and store the 4 corners of the pattern needed for homography estimation'''
for p in obj_points:
imgH = np.dot(camera, np.dot(Kt, [p[0], p[1], p[2], 1]))
imgP = [imgH[0] / imgH[2], imgH[1] / imgH[2]]
cv2.circle(firstFrame, (int(imgP[0]), int(imgP[1])), 3, (0, 0, 255), 4)
def calibrateSharpening():
frame = cv2.imread("failed_frame_224.png")
new_frame = sharpen(frame)
found, _ = cv2.findChessboardCorners(new_frame, (9,6))
print found
cv2.imshow("sharpened", new_frame)
cv2.waitKey(0)
def ColorImage(self, color_image):
image = self.bridge.imgmsg_to_cv2(color_image)
self.corners = cv.findChessboardCorners(image, (7,6))
if self.color_image_received == False and self.corners[0]:
print "Color", self.corners[0]
self.color_image = color_image
self.color_image_received = True
def calibrate(imagedir, cbrow, cbcol):
nimages = 0
datapoints = []
im_dims = (0,0)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = numpy.zeros((cbrow * cbcol, 3), numpy.float32)
objp[:, :2] = numpy.mgrid[0:cbcol, 0:cbrow].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
files = file_list(imagedir, ['jpg', 'jpeg', 'png'])
for f in files:
colour = cv2.imread(f)
grey = cv2.cvtColor(colour, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(grey, (cbcol, cbrow), flags=cv2.CALIB_CB_ADAPTIVE_THRESH)
if (ret):
print('using ' + f)
cv2.cornerSubPix(grey,corners,(11,11),(-1,-1),(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01))
objpoints.append(objp)
imgpoints.append(corners)
im_dims = grey.shape[:2]
if len(imgpoints) == 0:
print("Not enough good quality images. Aborting")
return
ret, K, D, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, grey.shape[::-1], None, None)
# storing results using CameraParams
C = CameraParams(lens=lens, sensorwidth=sensorwidth, xresolution=im_dims[1], yresolution=im_dims[0])
C.setParams(K, D)
C.save(os.path.join(imagedir, "paramsout.json"))
print("Saved params in " + os.path.join(imagedir, "paramsout.json"))
def detect_image_points(self, image, is_grayscale):
'''
Detect the pixels at which the checkerboard's corners 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.findChessboardCorners(image, self.calibration_object.pattern_size)
if found:
# create gray image if image is not grayscale
if not is_grayscale:
gray_image = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
else:
gray_image = image
# refine checkerboard corners to subpixel accuracy
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
cv2.cornerSubPix(gray_image, corners, (5, 5), (-1, -1), term)
else:
# could not find checkerboard
if DEBUG_OUTPUT:
print 'checkerboard not found'
return None
return corners
def getCameraMethod2(currentFrame, distortCoefs):
##object points
pattern_size=(9,6)
pattern_points = np.zeros( (np.prod(pattern_size), 3), np.float32 )
pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2)
pattern_points *= chessSquare_size
obj_points = [pattern_points]
obj_points.append(pattern_points)
obj_points = np.array(obj_points,np.float64).T
obj_points=obj_points[:,:,0].T
K,r,t = cam1.factor()
#--------
fou, imPoints = cv2.findChessboardCorners(currentFrame,(9,6))
if(not fou):
return None
try:
found, rvecs_new, tvecs_new = GetObjectPos(obj_points,imPoints,K,distortCoefs)
except ValueError:
rvecs_new, tvecs_new = GetObjectPos(obj_points,imPoints,K,distortCoefs)
found = True;
#if(not found):
# return None
rvecs_new = cv2.Rodrigues(np.array(rvecs_new))[0]
return Camera(np.dot(K, np.hstack((rvecs_new, tvecs_new))))
def getCameraCalibration(image, objp):
global criteria, mtx, dist
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
#height, width, depth = imgorg.shape
gray = cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
ret = False
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (7,7), None)
h,w =image.shape[:2]
# If found, add object points, image points (after refining them)
if ret == True:
#objpoints.append(objp)
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
#imgpoints.append(corners)
rvecs,tvecs,inliers = cv2.solvePnPRansac(objp, corners, mtx, dist)
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(image, corners, imgpts)
return img
return image
def calibrationExample():
camNum =0 # The number of the camera to calibrate
nPoints = 5 # number of images used for the calibration (space presses)
patternSize=(9,6) #size of the calibration pattern
saveImage = False
calibrated, camera_matrix,dist_coefs,rms = SIGBTools.calibrateCamera(camNum,nPoints,patternSize,saveImage)
K = camera_matrix
cam1 =Camera( np.hstack((K,np.dot(K,np.array([[0],[0],[-1]])) )) )
cam1.factor()
#Factor projection matrix into intrinsic and extrinsic parameters
print "K=", cam1.K
print "R=", cam1.R
print "t", cam1.t
if (calibrated):
capture = cv2.VideoCapture(camNum)
running = True
while running:
running, img =capture.read()
imgGray=cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ch = cv2.waitKey(1)
if(ch==27) or (ch==ord('q')): #ESC
running = False
img=cv2.undistort(img, camera_matrix, dist_coefs )
found,corners=cv2.findChessboardCorners(imgGray, patternSize )
if (found!=0):
cv2.drawChessboardCorners(img, patternSize, corners,found)
cv2.imshow("Calibrated",img)
def _find_chessboard(appsink, timeout=10):
sys.stderr.write("Searching for chessboard\n")
success = False
endtime = time.time() + timeout
while not success and time.time() < endtime:
sample = appsink.emit("pull-sample")
with _stbt.core._numpy_from_sample(sample, readonly=True) as input_image:
success, corners = cv2.findChessboardCorners(
input_image, (29, 15), flags=cv2.cv.CV_CALIB_CB_ADAPTIVE_THRESH
)
if success:
# Refine the corner measurements (not sure why this isn't built into
# findChessboardCorners?
with _stbt.core._numpy_from_sample(sample, readonly=True) as input_image:
grey_image = cv2.cvtColor(input_image, cv2.COLOR_BGR2GRAY)
cv2.cornerSubPix(grey_image, corners, (5, 5), (-1, -1), (cv2.TERM_CRITERIA_COUNT, 100, 0.1))
# Chessboard could have been recognised either way up. Match it.
if corners[0][0][0] < corners[1][0][0]:
ideal = numpy.array(
[[x * 40 - 0.5, y * 40 - 0.5] for y in range(2, 17) for x in range(2, 31)], dtype=numpy.float32
)
else:
ideal = numpy.array(
[[x * 40 - 0.5, y * 40 - 0.5] for y in range(16, 1, -1) for x in range(30, 1, -1)], dtype=numpy.float32
)
return ideal, corners
else:
raise RuntimeError("Couldn't find Chessboard")
def estimate_pose(self, cam_matrix, dist_mat):
search_size = (5, 4)
axis = np.float32(([[3, 0, 0], [0, 3, 0], [0, 0, 3]])).reshape(-1, 3)
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((search_size[0] * search_size[1], 3), np.float32)
objp[:, :2] = np.mgrid[0:search_size[0], 0:search_size[1]].T.reshape(-1, 2)
cap = cv2.VideoCapture('../videos/right_sd_test2.avi')
while(cap.isOpened()):
ret, frame = cap.read()
grey = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(grey, search_size, None)
if ret:
cv2.cornerSubPix(grey, corners, (11, 11), (-1, -1), criteria)
rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners, cam_matrix, dist_mat)
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
def findMarkers(self, gray, objpoints, imgpoints): # objp = np.zeros((self.marker_size[0]*self.marker_size[1],3), np.float32) # objp[:,:2] = np.mgrid[0:self.marker_size[0],0:self.marker_size[1]].T.reshape(-1,2) objp = np.zeros((np.prod(self.marker_size), 3), np.float32) objp[:, :2] = np.indices(self.marker_size).T.reshape(-1, 2) # make a grid of points # Find the chess board corners or circle centers if self.marker_checkerboard is True: ret, corners = cv2.findChessboardCorners(gray, self.marker_size) if ret: print '[+] chess - found corners: ', corners.size / 2 else: ret, corners = cv2.findCirclesGrid(gray, self.marker_size, flags=cv2.CALIB_CB_ASYMMETRIC_GRID) # ret, corners = cv2.findCirclesGrid(gray, self.marker_size, flags=cv2.CALIB_CB_CLUSTERING) # print '[+] circles - found corners: ', corners.size / 2, 'ret:', ret # print 'corners:', corners if ret: print '[+] circles - found corners: ', corners.size / 2 # If found, add object points, image points (after refining them) if ret is True: term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1) cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), term) imgpoints.append(corners.reshape(-1, 2)) objpoints.append(objp) else: print '[-] Couldn\'t find markers' # Draw the corners self.draw(gray, corners) return ret, objpoints, imgpoints
def get_camera_matrix():
square_size = 0.3
pattern_size = (8, 6)
pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)
pattern_points[:,:2] = np.indices(pattern_size).T.reshape(-1, 2)
pattern_points *= square_size
obj_points = []
img_points = []
h, w = 0, 0
img_names = glob.glob('*.png')
for fn in img_names:
img = cv2.imread(fn, 0)
h, w = img.shape[:2]
found, corners = cv2.findChessboardCorners(img, pattern_size)
if found:
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), term)
img_points.append(corners.reshape(-1, 2))
obj_points.append(pattern_points)
_, camera_matrix, _, _, _ = cv2.calibrateCamera(obj_points, img_points, (w, h))
return camera_matrix
def corner_det(in_img):
objpts = np.zeros((6 * 7, 3), np.float32) # making object points
objpts[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)
objectpoints, imagepoints = [], [] # containers for world points and image points
gray = cv2.cvtColor(in_img, cv2.COLOR_BGR2GRAY)
retval, corners = cv2.findChessboardCorners(gray, (7, 6), None) # finding the chess board corners
if retval == True: # storing and printing world and image points
objectpoints.append(objpts)
print objectpoints
wf = open("oworld.txt", "w")
for item in objectpoints:
wf.write("%s\n" % item)
crit = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001) # condition for stopping
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), crit)
imagepoints.append(corners2)
print imagepoints
imf = open("oimage.txt", "w")
for item in imagepoints:
imf.write("%s\n" % item)
in_img = cv2.drawChessboardCorners(in_img, (7, 6), corners2, retval) # showing the corners
cv2.imshow("BCCD v1.0", in_img)
cv2.waitKey()
cv2.destroyAllWindows()
def record_single_camera(video, name):
ret, frame = video.read()
shape = frame.shape
i = 0
while ret is True and i < 30:
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (6, 7), None)
if ret is True:
cv2.imwrite("test_data/videos/calibrator/%s/%s.jpeg" % (name, str(i).zfill(3)), frame)
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), CRITERIA)
cv2.drawChessboardCorners(frame, (6, 7), corners, ret)
cv2.imshow('frame', frame)
k = cv2.waitKey(1) & 0xFF
if k == ord('q'):
break
if ret is True:
keep = raw_input("Keep this frame? (y/n) ") == "y"
if keep is True:
print i
i = i + 1
ret, frame = video.read()
cv2.destroyAllWindows()
pass
def calibration(self):
for fname in self.img_list:
img = cv2.imread(fname)
grey = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(grey, self.size, None)
cv2.drawChessboardCorners(img, self.size, corners,ret)
# if found, show imgs
if ret:
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
cv2.cornerSubPix(grey,corners,(11,11),(-1,-1),criteria)
self.imgpoints.append(corners)
self.objpoints.append(self.objp)
self.img_list_detected.append(fname)
print fname
cv2.imshow('img',img)
cv2.waitKey(500)
cv2.destroyAllWindows()
# Step 2: Calibration
# shape[::-1]: (480,640) => (640,480)
ret, cmx, dist, rvecs, tvecs = cv2.calibrateCamera(
self.objpoints, self.imgpoints, grey.shape[::-1],None,None)
print cmx
print dist
# save calibration result
np.savez('./calibFile/calib.npz', cmx=cmx, dist=dist, rvecs=rvecs, tvecs=tvecs)
def find_board_corners(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..."
flags = cv2.CALIB_CB_ADAPTIVE_THRESH
pattern_size = (7,5)
image_coords = []
corner_images = []
for directory in board_directories:
img_name = 'cam1_frame_1.bmp'
print "Finding corners in", os.path.join(directory, img_name)
#board_image = cv2.imread(os.path.join(directory, img_name), cv2.CV_LOAD_IMAGE_COLOR)
board_image = cv2.imread(os.path.join(directory, img_name),0)
(pattern_was_found, corners) = cv2.findChessboardCorners(board_image, pattern_size, flags=flags)
if pattern_was_found:
cv2.cornerSubPix(board_image, corners, (4, 4), (-1, -1), (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 100, 1e-7))
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)
corner_image = board_image.copy()
corners = corners.squeeze()
cv2.drawChessboardCorners(corner_image, pattern_size, corners, pattern_was_found)
image_coords.append(corners)
corner_images.append(corner_image)
return np.concatenate(image_coords), corner_images
def computeCameraMatrix():
counter = int(x=1)
h, w = 0, 0
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
h, w = gray.shape[:2]
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, pattern_size, None)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
imgpoints.append(corners)
# Draw and display the corners
cv2.drawChessboardCorners(img, pattern_size, corners, ret)
cv2.imshow("img", img)
rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, (w, h))
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(camera_matrix, dist_coefs, (w, h), 1, (w, h))
dst = cv2.undistort(gray, camera_matrix, dist_coefs, None, newcameramtx)
cv2.imshow("undistort image", dst)
cv2.waitKey(100)
counter = counter + 1
else:
print ("No corners found on Picture " + str(counter))
cv2.destroyAllWindows()
def calibrate(self, image, info, depth):
model = PinholeCameraModel()
model.fromCameraInfo(info)
try:
cv_img = self.bridge.imgmsg_to_cv2(image, "bgr8")
except CvBridgeError as e:
print(e)
return False
try:
cv_depth = self.bridge.imgmsg_to_cv2(depth)
except CvBridgeError as e:
print(e)
return False
cv_depth = cv2.medianBlur(cv_depth, 5)
cv_img = cv2.cvtColor(cv_img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(
cv_img, (9, 6),
None,
flags=cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_FILTER_QUADS
| cv2.CALIB_CB_NORMALIZE_IMAGE)
if not ret:
rospy.logerr("Could not find chessboard corners")
return False
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100,
0.001)
cv2.cornerSubPix(cv_img, corners, (11, 11), (-1, -1), criteria)
corners = corners.reshape(1, -1, 2)[0]
points = []
ppoints = []
ppp = PoseArray()
ppp.header.stamp = rospy.Time.now()
ppp.header.frame_id = self.world_frame
for c in corners:
pt = list(model.projectPixelTo3dRay((c[0], c[1])))
pt[:] = [x / pt[2] for x in pt]
# depth image is noisy - let's make mean of few pixels
da = []
for x in range(int(c[0]) - 2, int(c[0]) + 3):
for y in range(int(c[1]) - 2, int(c[1]) + 3):
da.append(cv_depth[y, x] / 1000.0)
d = np.mean(da)
pt[:] = [x * d for x in pt]
ps = PointStamped()
ps.header.stamp = rospy.Time(0)
ps.header.frame_id = image.header.frame_id
ps.point.x = pt[0]
ps.point.y = pt[1]
ps.point.z = pt[2]
# transform 3D point from camera into the world coordinates
try:
ps = self.tfl.transformPoint(self.world_frame, ps)
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
rospy.logerr("Can't get transform")
return False
pp = Pose()
pp.position.x = ps.point.x
pp.position.y = ps.point.y
pp.position.z = ps.point.z
pp.orientation.x = 0
pp.orientation.y = 0
pp.orientation.z = 0
pp.orientation.w = 1.0
ppp.poses.append(pp)
# store x,y -> here we assume that points are 2D (on tabletop)
points.append([self.rpm * ps.point.x, self.rpm * ps.point.y])
self.corners_pub.publish(ppp)
dx = (self.pix_label.width() - self.pix_label.pixmap().width()) / 2.0
dy = (self.pix_label.height() - self.pix_label.pixmap().height()) / 2.0
box_size = self.pix_label.pixmap().width() / 12.0
# TODO self.scene_origin ???
# generate requested table coordinates
for y in range(0, 6):
for x in range(0, 9):
px = 2 * box_size + x * box_size + dx
py = 2 * box_size + y * box_size + dy
ppoints.append([px, py])
h, status = cv2.findHomography(np.array(points), np.array(ppoints),
cv2.LMEDS)
h_matrix = np.matrix(h)
self.emit(QtCore.SIGNAL('show_pix_label'), False) # hide chessboard
self.calibrating = False
self.calibrated = True
self.calibrated_pub.publish(self.is_calibrated())
# store homography matrix to parameter server
s = str(h_matrix.tolist())
rospy.set_param("~calibration_matrix", s)
self.init_map_from_matrix(h_matrix)
# self.h_matrix = np.matrix([[1, 0, 0], [0, 1, 0], [0, 0, 1.0]])
return True
imgpointsL = [] # 2d points in image plane.
imgpointsR = [] # 2d points in image plane.
imagesLeft = sorted(glob.glob('images/stereoLeft/*.jpg'))
imagesRight = sorted(glob.glob('images/stereoRight/*.jpg'))
for imgLeft, imgRight in zip(imagesLeft, imagesRight):
imgL = cv.imread(imgLeft)
imgR = cv.imread(imgRight)
grayL = cv.cvtColor(imgL, cv.COLOR_BGR2GRAY)
grayR = cv.cvtColor(imgR, cv.COLOR_BGR2GRAY)
# Find the chess board corners
retL, cornersL = cv.findChessboardCorners(grayL, chessboardSize, None)
retR, cornersR = cv.findChessboardCorners(grayR, chessboardSize, None)
# If found, add object points, image points (after refining them)
if retL and retR == True:
objpoints.append(objp)
cornersL = cv.cornerSubPix(grayL, cornersL, (11,11), (-1,-1), criteria)
imgpointsL.append(cornersL)
cornersR = cv.cornerSubPix(grayR, cornersR, (11,11), (-1,-1), criteria)
imgpointsR.append(cornersR)
# Draw and display the corners
cv.drawChessboardCorners(imgL, chessboardSize, cornersL, retL)
img2 = img.copy()
plt.imshow(img)
# In[3]:
# Finding the Chessboardcorners and printing image from with marked corners
# Code partly adopted from the lesson
objpoints = []
imgpoints = []
objp = np.zeros((6 * 9, 3), np.float32)
objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (9, 6), None)
if ret == True:
imgpoints.append(corners)
objpoints.append(objp)
img = cv2.drawChessboardCorners(img, (9, 6), corners, ret)
plt.imshow(img)
# In[4]:
# Using all chessboard images and their corners to calibrate the camera
# Testing the calibration on a lane picture
# I used my code, I generated in the lessons, for the the following functions
objpoints = []
imgpoints = []
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.
# Make a list of calibration images
images = glob.glob('data/*.jpg')
# Step through the list and search for chessboard corners
# print('Start finding chessboard corners...')
for idx, fname in enumerate(images):
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
plt.imshow(gray)
#Find the chessboard corners
print('find the chessboard corners of', fname)
ret, corners = cv2.findChessboardCorners(gray, (corner_x, corner_y), None)
# If found, add object points, image points
if ret == True:
objpoints.append(objp)
imgpoints.append(corners)
# Draw and display the corners
cv2.drawChessboardCorners(img, (corner_x, corner_y), corners, ret)
plt.imshow(img)
# joblib.dump((objpoints,imgpoints),'points.pkl')
# objpoints, imgpoints = joblib.load('points.pkl')
#######################################################################################################
# Homework 1 Camera Calibration #
def calibrate():
"""
Calibration routine
"""
args, img_mask = getopt.getopt(sys.argv[1:], '',
['debug=', 'square_size='])
args = dict(args)
args.setdefault('--debug', './output/')
args.setdefault('--square_size', 1.0)
if not img_mask:
img_mask = 'app/storage/calibration_inputs/*'
else:
img_mask = img_mask[0]
img_names = glob(img_mask)
debug_dir = args.get('--debug')
if not os.path.isdir(debug_dir):
os.mkdir(debug_dir)
square_size = float(args.get('--square_size'))
pattern_size = (9, 6)
pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)
pattern_points[:, :2] = np.indices(pattern_size).T.reshape(-1, 2)
pattern_points *= square_size
obj_points = []
img_points = []
height, width = 0, 0
for filename in img_names:
print('processing %s... ' % filename, end='')
img = cv2.imread(filename, 0)
if img is None:
print("Failed to load", filename)
continue
height, width = img.shape[:2]
found, corners = cv2.findChessboardCorners(img, pattern_size)
if found:
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), term)
if not found:
print('Chessboard not found.')
continue
img_points.append(corners.reshape(-1, 2))
obj_points.append(pattern_points)
print('ok')
# calculate camera distortion
rms, camera_matrix, dist_coefs, _, _ = cv2.calibrateCamera(
obj_points, img_points, (width, height), None, None)
print("\nRMS:", rms)
print("camera matrix:\n", camera_matrix)
print("distortion coefficients: ", dist_coefs.ravel())
dist_coefs = dist_coefs.ravel()
for i in range(3):
i = i + 2
dist_coefs[i] = 0
print(dist_coefs)
verify_calibration(camera_matrix, dist_coefs)
cv2.destroyAllWindows()
while True:
# Read data from the serial buffer
serial_port.write("snap")
serial_port.flush()
size = struct.unpack('<L', serial_port.read(4))[0]
buf = serial_port.read(size)
# Use numpy to construct an array from the bytes
x = np.fromstring(buf, dtype='uint8')
# Decode the array into an image
img = cv2.imdecode(x, cv2.IMREAD_UNCHANGED)
cv2.imshow("Stream:", img)
key = cv2.waitKey(20)
# save image to file, if pattern found
ret, corners = cv2.findChessboardCorners(
cv2.cvtColor(img, cv2.COLOR_BGR2GRAY), (9, 6))
if ret == True:
filename = datetime.now().strftime('%Y%m%d_%Hh%Mm%Ss%f') + '.jpg'
cv2.imwrite("./input/" + filename, img)
print("Saved {} \n").format(filename)
if key == 27:
#seial_port.close()
cv2.destroyWindow("Stream:")
break
objp[:,:2] = np.mgrid[0:9, 0:6].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.
# Make a list of calibration images
images = glob.glob('camera_cal/calibration*.jpg')
# Step through the list and search for chessboard corners
for idx, fname in enumerate(images):
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray, (9,6), None)
# If found, add object points, image points
if ret == True:
objpoints.append(objp)
imgpoints.append(corners)
# Draw and display the corners
cv2.drawChessboardCorners(img, (9,6), corners, ret)
write_name = 'corners_found'+str(idx)+'.jpg'
cv2.imwrite('camera_cal/Corners_found/'+write_name, img)
cv2.imshow('img', img)
cv2.waitKey(500)
cv2.destroyAllWindows()
(40, 1)))).astype(np.float32)
#
print(world_points)
"""
"""
#camera stereo 수행 -> 한번만 수행해도됨
#카메라의 파라미터 뽑음
_3d_points = []
_2d_points = []
_2d_points_left = []
_2d_points_right = []
img_paths_right = glob('*.jpg')
#for path in img_paths:
#im = cv2.imread(path)
ret_l, corners_l = cv2.findChessboardCorners(im_left, (8, 5))
ret_r, corners_r = cv2.findChessboardCorners(im_right, (8, 5))
if ret_l and ret_r:
_2d_points.append(corners_l)
_2d_points_left.append(corners_l)
_2d_points_right.append(corners_r)
_3d_points.append(world_points)
ret_l, mtx_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(
objectPoints=_3d_points,
imagePoints=_2d_points,
imageSize=(1536, 2048),
cameraMatrix=None,
distCoeffs=None)
ret_r, mtx_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.
# Make a list of calibration images
images = glob.glob('camera_cal/calibration*.jpg')
# Step through the list and search for chessboard corners
for idx, fname in enumerate(images):
# Read the calibration image
img = cv2.imread(fname)
# Convert the image to gray scale
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray, (chess_col_no,chess_row_no), None)
# If found, add object points, image points
if ret == True:
objpoints.append(objp)
imgpoints.append(corners)
from random import randint
import pickle
# Test undistortion on an image
# Pick a random calibration image as test image
test_idx = randint(1, 20);
test_name = 'camera_cal/calibration'+str(test_idx)+'.jpg'
img = cv2.imread(test_name)
img_size = (img.shape[1], img.shape[0])
import cv2
import numpy as np
camera_matrix = np.load(
'camera_mat.npy'
) #carga una matriz guradada en un archivo (MATRIZ DE CAMARA)
dist_coefs = np.load('dist_coefs.npy') #parametros de distorcionk k p1 p2
pattern_size = (11, 8) #numero de esquinas internas
img = cv2.imread('distortioned.JPG') #leo la iamgen distortioned
img = cv2.resize(img, (800, 600)) # re-dimenciono
img_show = img.copy() #HAGO UNA COPIA
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) #paso a escala de grises
res, corners = cv2.findChessboardCorners(
img_show, pattern_size
) #Encuentra las posiciones de las esquinas internas , paso la imagen y los número de esquinas internas por fila y columna
criteria = (
cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 30, 1e-3
) # determina la diferencia en la posición de esquina anterior y siguiente , siempre que sea menor que 1e-3 o se haya ejecutado 30 iteracciones
corners = cv2.cornerSubPix(
gray, corners, (10, 10), (-1, -1), criteria
) #refinamos nivel subpixel #imagen , coordenadas iniciales de las esquinas de entrada , la mitad de la longitud lateral de la ventana de búsqueda ,se usa una ventana de búsqueda.
#Criterios para la terminación del proceso iterativo de refinamiento de esquina
h_corners = cv2.undistortPoints(
corners, camera_matrix, dist_coefs
) # calcula ptos ideales con respeto a los meiddos (pixeles), adimencional
h_corners = np.c_[h_corners.squeeze(),
np.ones(len(h_corners))] #aca saco una dimencion
img_pts, _ = cv2.projectPoints(
# Arrays to store object points and image points from all the images.
objectPoint3dList = []
imagePoint2dList = []
imagePathList = glob.glob('calibration_images/*.png')
# Find the image points that correspond to the chessboard in all calibration images
for imagePath in imagePathList:
bgrImg = cv2.imread(imagePath)
grayImg = cv2.cvtColor(bgrImg, cv2.COLOR_BGR2GRAY)
numChessboardCornersPair = (NUM_CHESSBOARD_COL_CORNERS,
NUM_CHESSBOARD_ROW_CORNERS)
isChessboardFoundInImg, chessboardCorner2dList = \
cv2.findChessboardCorners(grayImg, numChessboardCornersPair, None)
assert isChessboardFoundInImg, 'Chessboard not found in image {}'.format(
imagePath)
objectPoint3dList.append(chessboardPoints3dList)
imagePoint2dList.append(chessboardCorner2dList)
# Calculate the calibrated intrinsic camera parameters
rmsReprojectionError, cameraMatrix, distortionCoefficients, rotationVectors, translationVectors = \
cv2.calibrateCamera(objectPoint3dList, imagePoint2dList, grayImg.shape[::-1], None, None)
# Save corrected versions of all calibration images
for imagePath in imagePathList:
bgrImg = cv2.imread(imagePath)
correctedBgrImg = cv2.undistort(bgrImg, cameraMatrix,
def main():
#cap = cv2.VideoCapture(0)
#ret, frame = cap.read()
frame = cv2.imread(
"\Users\Playtech\Pictures\Logitech Webcam\Picture 70.jpg", 1)
chess = frame.copy()
found, corners = cv2.findChessboardCorners(chess, (7, 7))
cv2.drawChessboardCorners(chess, (7, 7), corners, found)
list_corners = corners_to_list(corners)
ordered_corners = order_list(7, 7, list_corners)
# Grabbing the area in which to apply homography to
top_left = ordered_corners[0]
top_right = ordered_corners[6]
bot_left = ordered_corners[42]
bot_right = ordered_corners[48]
shifted_top_leftX = int(top_left[0] -
(ordered_corners[1][0] - top_left[0]))
shifted_top_leftY = int(top_left[1] -
(ordered_corners[7][1] - top_left[1] + 5))
shifted_top_rightX = int(top_right[0] -
(ordered_corners[5][0] - top_right[0]))
shifted_top_rightY = int(top_right[1] -
(ordered_corners[13][1] - top_right[1] + 5))
shifted_bot_leftX = int(bot_left[0] -
(ordered_corners[36][0] - bot_left[0] + 20))
shifted_bot_leftY = int(bot_left[1] -
(ordered_corners[35][1] - bot_left[1]))
shifted_bot_rightX = int(bot_right[0] -
(ordered_corners[40][0] - bot_right[0] - 20))
shifted_bot_rightY = int(bot_right[1] -
(ordered_corners[41][1] - bot_right[1]))
cv2.circle(chess, (shifted_top_leftX, shifted_top_leftY), 10, (0, 0, 255),
-1)
cv2.circle(chess, (shifted_top_rightX, shifted_top_rightY), 10,
(0, 0, 255), -1)
cv2.circle(chess, (shifted_bot_leftX, shifted_bot_leftY), 10, (0, 0, 255),
-1)
cv2.circle(chess, (shifted_bot_rightX, shifted_bot_rightY), 10,
(0, 0, 255), -1)
# Destination image
size = (1000, 1000, 3)
pts_dst = np.array([[0, 0], [500, 0], [500, 500], [0, 500]], dtype=float)
# print pts_dst
pts_src = np.array([[shifted_top_leftX, shifted_top_leftY],
[shifted_top_rightX, shifted_top_rightY],
[shifted_bot_rightX, shifted_bot_rightY],
[shifted_bot_leftX, shifted_bot_leftY]],
dtype=float)
im_dst = homography(frame, pts_dst, pts_src, size)
# reapply chess corners on new flat board
found, corners_homo = cv2.findChessboardCorners(im_dst, (7, 7))
sorted_corners = corners_to_list(corners_homo)
square_cords_sorted = order_list(7, 7, sorted_corners)
completed_board = complete_board1(square_cords_sorted)
completed_board_half = order_list(7, 9, completed_board)
complete_board_final = complete_board2(completed_board_half)
complete_board_final2 = order_list(9, 9, complete_board_final)
# creating square class
class Squares:
def __init__(self, name, topX, topY, botX, botY):
self.name = name
self.topX = topX
self.topY = topY
self.botX = botX
self.botY = botY
def displaySquare(self):
square_image = im_dst[self.topY:self.botY, self.topX:self.botX]
cv2.imshow(str(self.name), square_image)
def squareFrame(self):
square_frame = im_dst[self.topY:self.botY, self.topX:self.botX]
return square_frame
def displayWhite(self):
white_image = thresh_highlight4[self.topY:self.botY,
self.topX:self.botX]
cv2.imshow(str(self.name + "White"), white_image)
def displayBlack(self):
black_image = thresh_highlight3[self.topY:self.botY,
self.topX:self.botX]
cv2.imshow(str(self.name + "Black"), black_image)
def countWhite(self):
white_segment = gray_image4[self.topY:self.botY,
self.topX:self.botX]
nzCount_white = np.count_nonzero(white_segment)
return nzCount_white
def countBlack(self):
black_segment = gray_image3[self.topY:self.botY,
self.topX:self.botX]
nzCount_black = np.count_nonzero(black_segment)
return nzCount_black
def countWhite_to(self):
segment = difference_white_to[self.topY:self.botY,
self.topX:self.botX]
return np.count_nonzero(segment)
def countWhite_from(self):
segment = difference_white_from[self.topY:self.botY,
self.topX:self.botX]
return np.count_nonzero(segment)
def countBlack_to(self):
segment = difference_black_to[self.topY:self.botY,
self.topX:self.botX]
return np.count_nonzero(segment)
def countBlack_from(self):
segment = difference_black_from[self.topY:self.botY,
self.topX:self.botX]
return np.count_nonzero(segment)
# DICTS to populate all of the row g (g2-g7)
square_cords = dict()
row_group = dict()
diff_white_to = dict()
diff_white_from = dict()
diff_black_to = dict()
diff_black_from = dict()
for j in range(0, 8, 1):
for i in range(0, 8, 1):
square_cords['%c%dx1' % ((72 - j), (8 - i))] = int(
complete_board_final2[i + j * 9][0])
square_cords['%c%dy1' % ((72 - j), (8 - i))] = int(
complete_board_final2[i + j * 9][1])
square_cords['%c%dx2' % ((72 - j), (8 - i))] = int(
complete_board_final2[i + 10 + j * 9][0])
square_cords['%c%dy2' % ((72 - j), (8 - i))] = int(
complete_board_final2[i + 10 + j * 9][1])
row_group['%c%d' % ((72 - j), (8 - i))] = Squares(
"%c%d" % ((72 - j), (8 - i)),
square_cords['%c%dx1' % ((72 - j), (8 - i))],
square_cords['%c%dy1' % ((72 - j), (8 - i))],
square_cords['%c%dx2' % ((72 - j), (8 - i))],
square_cords['%c%dy2' % ((72 - j), (8 - i))])
diff_white_to['%c%d' % ((72 - j), (8 - i))] = 0
diff_white_from['%c%d' % ((72 - j), (8 - i))] = 0
diff_black_to['%c%d' % ((72 - j), (8 - i))] = 0
diff_black_from['%c%d' % ((72 - j), (8 - i))] = 0
# for i in range(1,9,1):
# Live homography destination @@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
size = (1000, 1000, 3)
pts_dst = np.array([[0, 0], [500, 0], [500, 500], [0, 500]], dtype=float)
pts_src = np.array([[shifted_top_leftX, shifted_top_leftY],
[shifted_top_rightX, shifted_top_rightY],
[shifted_bot_rightX, shifted_bot_rightY],
[shifted_bot_leftX, shifted_bot_leftY]],
dtype=float)
#ret, im_src = cap.read()
im_src = cv2.imread(
"\Users\Playtech\Pictures\Logitech Webcam\Picture 71.jpg", 1)
im_dst = homography(im_src, pts_dst, pts_src, size)
im_dst_copy = im_dst.copy()
im_crop = im_dst_copy[
int(complete_board_final2[0][1]):int(complete_board_final2[80][1]),
int(complete_board_final2[0][0]):int(complete_board_final2[80][0])]
hsv = cv2.cvtColor(im_crop, cv2.COLOR_BGR2HSV)
res2, center = kmeans_calc(im_crop)
#cv2.imshow('res2', res2)
cv2.waitKey()
#PROCESSING OF IMAGE <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
color_1 = BGRtoHSV(center[0])[0][0]
color_2 = BGRtoHSV(center[3])[0][0]
color_3 = BGRtoHSV(center[2])[0][0] #black
color_4 = BGRtoHSV(center[1])[0][0] #white
upper1, lower1 = color_search_range(color_1, 30, 30, 30)
upper2, lower2 = color_search_range(color_2, 30, 60, 30)
upper3, lower3 = color_search_range(color_3, 20, 20, 100)
upper4, lower4 = color_search_range(color_4, 45, 45, 30)
thresh_highlight1 = color_processing(im_crop, hsv, lower1, upper1)
thresh_highlight2 = color_processing(im_crop, hsv, lower2, upper2)
thresh_highlight3 = color_processing(im_crop, hsv, lower3, upper3)
thresh_highlight4 = color_processing(im_crop, hsv, lower4, upper4)
#cv2.imshow("image MASK 3_4", np.hstack([im_crop, thresh_highlight3, thresh_highlight4]))
gray_image3 = cv2.cvtColor(thresh_highlight3, cv2.COLOR_BGR2GRAY)
gray_image4 = cv2.cvtColor(thresh_highlight4, cv2.COLOR_BGR2GRAY)
##print "White"
# print row_group["E7"].countWhite()
#print "Black"
#print row_group["E7"].countBlack()
test_frames = [
"\Users\Playtech\Pictures\Logitech Webcam\Picture 72.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 73.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 74.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 75.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 76.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 77.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 78.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 79.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 80.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 81.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 82.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 83.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 84.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 85.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 86.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 87.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 88.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 89.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 90.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 91.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 92.jpg",
"\Users\Playtech\Pictures\Logitech Webcam\Picture 93.jpg",
]
# ############################################ KERNAL LOCATION ######################################################
count = 0
turn = "W"
pos = Position(initial, 0, (True, True), (True, True), 0, 0)
while (1):
previous_black = gray_image3
previous_white = gray_image4
# H**o
# Read in the image.
im_src = cv2.imread(
test_frames[count]
) # <<<<<<<< RE ENABLE FOR PROPER TESTING (EXTRACTING FROM THE IMAGES)
#$$$$$$$$ CHANGE FOR LIVE FEED OR IMAGE
#ret, im_src = cap.read()
#im_src = cv2.imread("\Users\Playtech\Pictures\Logitech Webcam\Picture 2.jpg", 1)
# Destination image
im_dst = homography(im_src, pts_dst, pts_src, size)
im_dst_copy = im_dst.copy()
im_crop = im_dst_copy[
int(complete_board_final2[0][1]):int(complete_board_final2[80][1]),
int(complete_board_final2[0][0]):int(complete_board_final2[80][0])]
#for j in range(0,8,1):
# for i in range (0,8,1):
# row_group['%c%d' % ((72 - j), (8 - i))].displaySquare()
hsv = cv2.cvtColor(im_crop, cv2.COLOR_BGR2HSV)
#cv2.imshow('res2', res2)
# PROCESSING OF IMAGE <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
thresh_highlight1 = color_processing(im_crop, hsv, lower1, upper1)
thresh_highlight2 = color_processing(im_crop, hsv, lower2, upper2)
thresh_highlight3 = color_processing(im_crop, hsv, lower3, upper3)
thresh_highlight4 = color_processing(im_crop, hsv, lower4, upper4)
#cv2.imshow("image MASK 3_4", np.hstack([im_crop,thresh_highlight3, thresh_highlight4]))
gray_image3 = cv2.cvtColor(thresh_highlight3, cv2.COLOR_BGR2GRAY)
gray_image4 = cv2.cvtColor(thresh_highlight4, cv2.COLOR_BGR2GRAY)
difference_black_both = cv2.absdiff(gray_image3, previous_black)
difference_black_from = cv2.medianBlur(
cv2.bitwise_and(previous_black,
previous_black,
mask=difference_black_both), 5)
difference_black_to = cv2.medianBlur(
cv2.bitwise_and(gray_image3,
gray_image3,
mask=difference_black_both), 5)
difference_white_both = cv2.absdiff(gray_image4, previous_white)
difference_white_from = cv2.medianBlur(
cv2.bitwise_and(previous_white,
previous_white,
mask=difference_white_both), 5)
difference_white_to = cv2.medianBlur(
cv2.bitwise_and(gray_image4,
gray_image4,
mask=difference_white_both), 5)
#cv2.imshow("Diff Black", np.hstack([difference_black_both,difference_black_to, difference_black_from]))
cv2.imshow(
"Diff White",
np.hstack([
difference_white_both, difference_white_to,
difference_white_from
]))
cv2.imshow("Crop", im_crop)
#cv2.imshow("Raw", im_src)
#cv2.waitKey()
for j in range(0, 8, 1):
for i in range(0, 8, 1):
diff_white_to['%c%d' % ((72 - j),
(8 - i))] = row_group['%c%d' % (
(72 - j),
(8 - i))].countWhite_to()
diff_white_from['%c%d' % ((72 - j),
(8 - i))] = row_group['%c%d' % (
(72 - j),
(8 - i))].countWhite_from()
diff_black_to['%c%d' % ((72 - j),
(8 - i))] = row_group['%c%d' % (
(72 - j),
(8 - i))].countBlack_to()
diff_black_from['%c%d' % ((72 - j),
(8 - i))] = row_group['%c%d' % (
(72 - j),
(8 - i))].countBlack_from()
my_move = (white_move(diff_white_to, diff_white_from))
if turn == "w": #will never trigger
my_move = (white_move(diff_white_to, diff_white_from))
turn = "B"
elif turn == "b":
my_move = (black_move(diff_black_to, diff_black_from))
turn = "W"
my_move = str.lower(str(my_move[0]) + str(my_move[1]))
#moving_test = move_list[count] # TEST AS ABOVE IS INVALID <<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<
moving_test = my_move
count += 2
# We add some spaces to the board before we print it.
# That makes it more readable and pleasing.
print(' '.join(pos.board))
print("Press Enter once you have made your move...")
cv2.waitKey()
print "Detected move: " + moving_test
# We query the user until she enters a legal move.
move = None
while move not in pos.genMoves():
crdn = moving_test
move = parse(crdn[0:2]), parse(crdn[2:4])
#if move not in pos.genMoves():
##print"INVALID MOVE"
pos = pos.move(move)
# After our move we rotate the board and print it again.
# This allows us to see the effect of our move.
print(' '.join(pos.rotate().board))
# Fire up the engine to look for a move.
move, score = search(pos)
if score <= -MATE_VALUE:
print("You won")
break
if score >= MATE_VALUE:
print("You lost")
break
# The black player moves from a rotated position, so we have to
# 'back rotate' the move before printing it.
print("My move:", render(119 - move[0]) + render(119 - move[1]))
pos = pos.move(move)
k = cv2.waitKey(5) & 0xFF
if k == 27:
break
elif k == 32:
square_frame = row_group["G2"].squareFrame()
cv2.imshow("G2 Snapshot", square_frame)
elif k == 122:
new_square_frame = row_group["G2"].squareFrame()
diff = cv2.subtract(new_square_frame, square_frame)
cv2.imshow("G2 diff", diff)
cv2.waitKey(0)
cv2.destroyAllWindows()
def iter_cloudify(calibration_data, folder, lasers, sequence, method=None, camera=False, interactive=False, undistort=False):
pure_images = settings.pure_mode
lm = LineMaker()
lineprocessor = getattr(lm, 'from_'+method)
lm.calibration_data = calibration_data
sliced_lines = defaultdict(lambda: [None, None])
color_slices = defaultdict(lambda: [None, None])
d_kern = np.ones((3,3),np.uint8)
RED = 2 # position of red layer
for i, n in enumerate(sequence):
yield
fullcolor = imtools.imread(folder+'/color_%03d.%s'%(n, settings.FILEFORMAT), format="rgb", calibrated=undistort and calibration_data)
if fullcolor is None:
continue
if pure_images:
ref_grey = None
else:
ref_grey = fullcolor[:,:,RED]
pictures_todisplay = []
for laser in lasers:
laser_image = imtools.imread(folder+'/laser%d_%03d.%s'%(laser, n, settings.FILEFORMAT), format="rgb", calibrated=undistort and calibration_data)
if laser_image is None:
continue
laser_grey = laser_image[:,:,RED]
gui.progress("analyse", i, len(sequence))
points, processed = lineprocessor(laser_image, laser_grey, fullcolor, ref_grey, laser_nr=laser,
mask=camera[i]['chess_contour'] if camera else None)
# validate & store
if points is not None and points[0].size:
nosave = False
if interactive:
disp = cv2.merge( np.array(( laser_grey, processed, processed)) )
txt = "Esc=SKIP, Space=OK"
gui.display(disp, txt, resize=True)
pictures_todisplay.append((processed, laser_grey))
if interactive:
# detect the chess board
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 200, 0.001)
found, corners = cv2.findChessboardCorners(ref_grey, settings.PATTERN_MATRIX_SIZE)
if found:
if not gui.ok_cancel(20):
nosave = True
else:
nosave = True
if not interactive or not nosave:
if camera:
sliced_lines[n][laser] = [ points ] + camera[i]['plane']
else:
sliced_lines[n][laser] = [ np.deg2rad(n), points, laser ]
if fullcolor is not None:
color_slices[n][laser] = np.fliplr(fullcolor[(points[1], points[0])])
# display
if i%int(settings.ui_base_i*2) == 0 and pictures_todisplay:
if DEBUG:
if len(pictures_todisplay) > 1:
pictures_todisplay = np.array(pictures_todisplay)
gref = cv2.addWeighted(pictures_todisplay[0,1], 0.5, pictures_todisplay[1,1], 0.5, 0)
nref = cv2.addWeighted(pictures_todisplay[0,0], 0.5, pictures_todisplay[1,0], 0.5, 0)
else:
gref = pictures_todisplay[0][1]
nref = pictures_todisplay[0][0]
nref = cv2.dilate(nref, d_kern).astype(np.uint8)
r = cv2.bitwise_or(gref, nref)
disp = cv2.merge( np.array(( r, gref, r)) )
gui.display(disp, "lasers" if len(lasers) > 1 else "laser %d"%lasers[0], resize=True)
else:
if len(pictures_todisplay) > 1:
gui.display(cv2.addWeighted(pictures_todisplay[1][1], 0.5, pictures_todisplay[0][1], 0.5, 0), "lasers" if len(lasers) > 1 else "laser %d"%lasers[0], resize=True)
else:
gui.display(pictures_todisplay[0][1], "lasers" if len(lasers) > 1 else "laser %d"%lasers[0], resize=True)
else:
gui.redraw()
if len(sliced_lines) == 0:
raise AnalyseError("Unable to recognize lines in picture")
if camera:
yield sliced_lines
else:
yield sliced_lines, color_slices
def __init__(self,
corners_shape=(9, 6),
img_glob="camera_cal/calibration*.jpg",
calib_fname="calib.p"):
if os.path.exists(calib_fname):
# If calibration file already exists, just get mtx, dist from it.
with open(calib_fname, 'rb') as f:
calib_data = pickle.load(f)
mtx = calib_data["cam_matrix"]
dist = calib_data["dist_coeffs"]
else:
# Prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((corners_shape[0] * corners_shape[1], 3),
np.float32)
objp[:, :2] = np.mgrid[0:corners_shape[0],
0:corners_shape[1]].T.reshape(
-1, 2) # x, y coordinates
# Arrays to store object points and image points from all the images.
obj_points = [] # 3d points in real world space
img_points = [] # 2d points in image plane.
# Make a list of calibration images
images = glob.glob(img_glob)
# step through the list and search for chessboard corners
for fname in images:
# Read image
img = cv2.imread(fname)
# Convert to grayscale
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(
gray, corners_shape, None)
# If found, add object points, image points
if ret == True:
obj_points.append(objp)
img_points.append(corners)
# Test undistortion on an image
img_size = (img.shape[1], img.shape[0])
# Calibrate camera given the object and image points
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
obj_points, img_points, img_size, None, None)
# Pickle the data and save it
calib_data = {
'cam_matrix': mtx,
'dist_coeffs': dist,
'img_size': img.shape
}
with open(calib_fname, 'wb') as f:
pickle.dump(calib_data, f)
self.mtx = mtx
self.dist = dist
x = np.linspace(640,0,9)
y = np.linspace(0,480,6)
tp = np.c_[np.asarray(list(product(x, y))),np.ones(54)]
while True:
#capture a frame
ret, img = cap.read()
## IF YOU WISH TO UNDISTORT YOUR IMAGE YOU SHOULD DO IT HERE
# Our operations on the frame come here
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (HEIGHT,WIDTH),None)
#print corners
#print corners.shape[0]
# If found, add object points, image points (after refining them)
if ret == True:
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
#cv2.drawChessboardCorners(img, (HEIGHT,WIDTH), corners,ret)
## Step 1a: Compute fp -- an Nx3 array of the 2D homogeneous coordinates of the
## detected checkerboard corners
corners = np.asmatrix(corners)
fp = np.asarray(np.c_[corners,np.ones(corners.shape[0])])
## Step 1b: Compute tp -- an Nx3 array of the 2D homogeneous coordinates of the
## samples of the image coordinates
## Note: this could be done outside of the loop either!
def calibrate(self,
calibImagePathsList,
showDebugImages=False,
nx=9,
ny=6):
"""
`calibImagePathsList` Input vector of paths to calibration files
`showDebugImages` Input to show debug images while calibration if True
`nx` Input number of corners in X direction
`ny` Input number of corners in Y direction
Calibrates the camera based on the given calibration filename list and
the number of chessboard corners in x and y direction
returns if the calibration has been successful
"""
print("Camera Calibrating...")
# start uncalibrated when running calibration routine
self.calibrated = False
if calibImagePathsList and (nx > 0) and (ny > 0):
# build vectors for imagepoints and objectpoints
imgpoints = [] # 2D points in image plane
objpoints = [] # 3D points in real world
# prepare the object points according to the parameters (z stays 0
# as the chessboards are on a flat surface)
objp = np.zeros((nx * ny, 3), np.float32)
objp[:, :2] = np.mgrid[0:nx, 0:ny].T.reshape(-1,
2) # x, y coordinates
if showDebugImages == True:
cv2.namedWindow("Calibration Debug", cv2.WINDOW_AUTOSIZE)
cv2.moveWindow("Calibration Debug", 0, 0)
for path in calibImagePathsList:
# load the image
image = mpimg.imread(path)
# convert image to grayscale
gray = cv2.cvtColor(image, cv2.COLOR_RGB2GRAY)
# find chessboard corners for further detection
ret, corners = cv2.findChessboardCorners(gray, (nx, ny), None)
if ret == True:
imgpoints.append(corners)
objpoints.append(objp)
# draw the corners on debug
if showDebugImages == True:
cv2.drawChessboardCorners(image, (nx, ny), corners,
ret)
cv2.imshow("Calibration Debug", image)
cv2.waitKey(1000)
# calibrate the camera if valid points have been found
if len(objpoints) > 0:
ret, self.mtx, self.distCoeffs, rvecs, tvecs = \
cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], \
None, None)
self.calibrated = True
if showDebugImages == True:
cv2.destroyWindow("Calibration Debug")
return self.calibrated
for i in range(100):
filename = "imgs/" + str(i) + ".png"
img = cv2.imread(filename)
if (img != None):
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
img = np.fliplr(np.flipud(img))
img = np.flipud(np.flipud(img))
#gray = cv2.cvtColor(data, cv2.COLOR_BGR2GRAY)
img = img.transpose([1, 0])
img_shape = img.shape
ret, corners = cv2.findChessboardCorners(img, (4, 4), None)
imgpoints.append(corners)
objpoints.append(objp)
print i
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
img_shape[::-1], None, None)
print 'ret', ret
print 'mtx', mtx
print 'dist', dist
print 'rvecs', rvecs
print 'tvecs', tvecs
quit()
#~
0:CHECKERBOARD[1]].T.reshape(-1, 2)
_img_shape = None
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = glob.glob('calibration/*.jpg')
for fname in images:
img = cv2.imread(fname)
if _img_shape == None:
_img_shape = img.shape[:2]
else:
assert _img_shape == img.shape[:
2], "All images must share the same size."
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(
gray, CHECKERBOARD, cv2.CALIB_CB_ADAPTIVE_THRESH +
cv2.CALIB_CB_FAST_CHECK + cv2.CALIB_CB_NORMALIZE_IMAGE)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
cv2.cornerSubPix(gray, corners, (3, 3), (-1, -1), subpix_criteria)
imgpoints.append(corners)
N_OK = len(objpoints)
K = np.zeros((3, 3))
D = np.zeros((4, 1))
rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
rms, _, _, _, _ = \
cv2.fisheye.calibrate(
objpoints,
images = glob.glob(mypath + '*.JPG')
print("images is: " + str(images))
criteria_calibrator = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30,
0.001)
criteria = criteria_calibrator
for idx, fname in enumerate(images):
print("\nImage " + fname)
if time.sleep(10):
break
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the edges
ret, corners = cv2.findChessboardCorners(gray, n_rows_and_cols, None)
# If found, add obj points and image points
if ret == True:
print(" found " + str(len(corners)) + " corners.")
objpoints.append(objp)
# cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria) didnt work,
#I couldnt make it work copying the calibrator code
imgpoints.append(corners)
# Draw and display the edges on the original photo
cv2.drawChessboardCorners(img, n_rows_and_cols, corners, ret)
cv2.imshow('img', img)
cv2.waitKey(500)
import numpy as np
import cv2 as cv
import glob
# termination criteria
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = glob.glob('*.jpg')
for fname in images:
img = cv.imread(fname)
gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv.findChessboardCorners(gray, (7,6), None)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
corners2 = cv.cornerSubPix(gray,corners, (11,11), (-1,-1), criteria)
imgpoints.append(corners)
# Draw and display the corners
cv.drawChessboardCorners(img, (7,6), corners2, ret)
cv.imshow('img', img)
cv.waitKey(500)
cv.destroyAllWindows()
def CalFromDisc():
global rows
global columns
global params
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((columns*rows,3), np.float32)
objp[:,:2] = np.mgrid[0:rows,0:columns].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = glob.glob(imgFolder + '/*.jpg')
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (rows,columns), cv2.CALIB_CB_FILTER_QUADS)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
imgpoints.append(corners2)
# Draw and display the corners
img = cv2.drawChessboardCorners(img, (rows,columns), corners2, ret)
cv2.imshow('Proyecto2: Calibracion de camara en modo almacenamiento. (Chessboard)',img)
cv2.waitKey(500)
cv2.destroyAllWindows()
camera_matrix = camera_matrix = cv2.initCameraMatrix2D(objpoints, imgpoints, gray.shape[::-1])
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], camera_matrix, None, flags = cv2.CALIB_USE_INTRINSIC_GUESS)
np.savez('camera', mtx = mtx, dist = dist, rvecs = rvecs, tvecs = tvecs)
params['camera'].append({
'K': mtx.tolist(),
'dist': dist.tolist()
})
with open('params.txt', 'w') as outfile:
json.dump(params, outfile)
img = cv2.imread(imgFolder + '/' + imgBaseNm + str(imgBaseIdx) + '.jpg')
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y : y + h, x : x + w]
cv2.imwrite('calibresult.png', dst)
# Load previously saved data
with np.load('camera.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
print("Press s in camera window for saving image.\n")
for fname in glob.glob(imgFolder + '/' + imgBaseNm + '*.jpg'):
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (rows, columns), cv2.CALIB_CB_FILTER_QUADS)
if ret == True:
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
# Find the rotation and translation vectors.
ret,rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, dist)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img, corners2, imgpts)
cv2.imshow('Proyecto2: Calibracion de camara en modo almacenamiento. (Proyeccion)', img)
k = cv2.waitKey(0) & 0xFF
if k == ord('s'):
cv2.imwrite(fname[:6]+'.png', img)
cv2.destroyAllWindows()
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# cv2.putText(img,'Number of capture: '+str(imgInd),(30,20),cv2.FONT_HERSHEY_PLAIN, 1,(0,255,0))
# cv2.putText(img,'c: Capture the image',(30,40),cv2.FONT_HERSHEY_PLAIN, 1,(0,255,0))
# cv2.putText(img,'q: Finish capturing and calcurate the camera matrix and distortion',(30,60),cv2.FONT_HERSHEY_PLAIN, 1,(0,255,0))
cv2.imshow("image" + str(cam_num), img)
key = cv2.waitKey(1) & 0xFF
# if key == ord('c'):
# 約2秒毎に撮影（ピッタリでは無い気がする）
if time_counter % (int(fps) * 2) == 0:
# Find the chess board corners
# [キャリブレーションボードからのコーナー決定]
ret, corners = cv2.findChessboardCorners(gray, (10, 7), None)
# If found, add object points, image points (after refining them)
# [コーナーが見つかったらobject pointsとimage pointsに入れておく]
if ret == True:
objPoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
imgPoints.append(corners2)
# Draw and display the corners
# [コーナーをディスプレイ上に表示]
img = cv2.drawChessboardCorners(img, (10, 7), corners2, ret)
cv2.imshow('image' + str(cam_num), img)
cv2.waitKey(500)
cv2.imwrite(
def calibrateCamera(folderName):
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6 * 8, 3), np.float32)
objp[:, :2] = np.mgrid[0:8, 0:6].T.reshape(-1, 2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = glob.glob(str(folderName+'/*'))
print(images)
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (8, 6), None)
if ret is True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
imgpoints.append(corners2)
# Draw and display the corners
img = cv2.drawChessboardCorners(img, (8, 6), corners2, ret)
cv2.imshow('img', img)
cv2.waitKey(500)
cv2.destroyAllWindows()
print("Image size:")
print(gray.shape)
image_shape = gray.shape
ret, cameraMatrix, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None, None, None, cv2.CALIB_FIX_K3)
print("Camera matrix...")
print(cameraMatrix)
mean_error = 0
for i in range(len(objpoints)):
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], cameraMatrix, dist)
error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
mean_error += error
print("Total average reprojection error: ", mean_error / len(objpoints))
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(cameraMatrix, gray.shape, 0.0, 0.0)
print("Field of view, x: "+str(fovx))
print("Field of view, y: "+str(fovy))
print("Focal Length: "+str(focalLength))
print("Principal Point: "+str(principalPoint))
print("Aspect Ratio: "+str(aspectRatio))
print("Distortion coefficients...")
print(dist[0][0])
print(dist[0][1])
print(dist[0][2])
print(dist[0][3])
f = open("camera_params.txt", 'w')
f.write(str(fovx)+"\n")
f.write(str(fovy)+"\n")
f.write(str(focalLength)+"\n")
f.write(str(cameraMatrix[0][0])+"\n")
f.write(str(cameraMatrix[1][1])+"\n")
f.write(str(cameraMatrix[0][2])+"\n")
f.write(str(cameraMatrix[1][2])+"\n")
f.write(str(dist[0][0])+"\n")
f.write(str(dist[0][1])+"\n")
f.write(str(dist[0][2])+"\n")
f.write(str(dist[0][3]))
def CalFromCam():
global rows
global columns
global params
idxCam = 0
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((columns*rows,3), np.float32)
objp[:,:2] = np.mgrid[0:rows,0:columns].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
cap = cv2.VideoCapture(0)
while(True):
# Capture frame-by-frame
ret, frame = cap.read()
img = frame
# Our operations on the frame come here
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Display the resulting frame
cv2.imshow('Proyecto2: Calibracion de camara capturando de camara. (Camara)', frame)
key = cv2.waitKey(2)
if key & 0xFF == ord('d'):
break
if key & 0xFF == ord('c'):
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (rows,columns), cv2.CALIB_CB_FILTER_QUADS)
# If found, add object points, image points (after refining them)
if ret == True:
# Save original image for future used
cv2.imwrite(imgFolder + "/chessCam" + str(idxCam) + ".jpg", frame)
#Increase camera index for files
idxCam += 1
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
imgpoints.append(corners2)
# Draw and display the corners
img = cv2.drawChessboardCorners(img, (rows,columns), corners2, ret)
#Show chessboard with found points
cv2.imshow('Proyecto2: Calibracion de camara capturando de camara. (Chessboard)', img)
cv2.waitKey(500)
else:
print("Chessboard corners not found! Please try again.\n")
cv2.destroyAllWindows()
camera_matrix = camera_matrix = cv2.initCameraMatrix2D(objpoints, imgpoints, gray.shape[::-1])
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], camera_matrix, None, flags = cv2.CALIB_USE_INTRINSIC_GUESS)
np.savez('camera', mtx = mtx, dist = dist, rvecs = rvecs, tvecs = tvecs)
params['camera'].append({
'K': mtx.tolist(),
'dist': dist.tolist()
})
with open('params.txt', 'w') as outfile:
json.dump(params, outfile)
img = cv2.imread(imgFolder + '/' + imgBaseNm + str(imgBaseIdx) + '.jpg')
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y : y + h, x : x + w]
cv2.imwrite('calibresult.png', dst)
# Load previously saved data
with np.load('camera.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
print("Press d in camera window when you are done projecting.\n")
while(True):
# Capture frame-by-frame
ret, frame = cap.read()
img = frame
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (rows, columns), cv2.CALIB_CB_FILTER_QUADS)
if ret == True:
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
# Find the rotation and translation vectors.
ret,rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, dist)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img, corners2, imgpts)
key = cv2.waitKey(20)
if key & 0xFF == ord('d'):
break
cv2.imshow('Proyecto2: Calibracion de camara en modo almacenamiento. (Proyeccion)', img)
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
objp[:, :2] = np.mgrid[0:8, 0:6].T.reshape(-1, 2)
objpoints = [] # 3d points in real world space
imgpointsR = [] # 2d points in image plane
imgpointsL = []
print('Starting calibration for the 2 cameras... ')
for i in range(
0, 101
): # Put the amount of pictures you have taken for the calibration inbetween range(0,?) wenn starting from the image number 0
t = str(i)
ChessImaR = cv2.imread('pictureR' + t + '.jpg', 0) # Right side
ChessImaL = cv2.imread('pictureL' + t + '.jpg', 0) # Left side
retR, cornersR = cv2.findChessboardCorners(
ChessImaR, (8, 6),
None) # Define the number of chees corners we are looking for
retL, cornersL = cv2.findChessboardCorners(ChessImaL, (8, 6),
None) # Left side
if (True == retR) & (True == retL):
objpoints.append(objp)
corners2R = cv2.cornerSubPix(ChessImaR, cornersR, (11, 11), (-1, -1),
criteria)
corners2L = cv2.cornerSubPix(ChessImaL, cornersL, (11, 11), (-1, -1),
criteria)
imgpointsR.append(corners2R)
imgpointsL.append(corners2L)
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(
objpoints, imgpointsR, ChessImaR.shape[::-1], None, None)
def render(self, image):
# load calibration data
#with np.load('webcam_calibration_ouput.npz') as X:
#mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
# set up criteria, object points and axis
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((7*10,3), np.float32)
objp[:,:2] = np.mgrid[0:10,0:7].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
h = 0
w =0
images = glob.glob('images3/*.jpg')
#print "hello"
for fname in images:
img = cv2.imread(fname)
#print "hello2"
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
h,w = img.shape[:2]
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (10,7),None)
#print ret
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
imgpoints.append(corners2)
# Draw and display the corners
#img = cv2.drawChessboardCorners(img, (10,7), corners2,ret)
#cv2.imshow('img',img)
#cv2.waitKey(1000)
#print objpoints
#print imgpoints
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)
axis = np.float32([[0,0,0], [0,3,0], [3,3,0], [3,0,0],
[0,0,-3],[0,3,-3],[3,3,-3],[3,0,-3] ])
# find grid corners in image
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (7,6), None)
if ret == True:
# project 3D points to image plane
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
rvecs, tvecs, _ = cv2.solvePnPRansac(objp, corners, mtx, dist)
imgpts, _ = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
# draw cube
self._draw_cube(image, imgpts)
def main():
parser = argparse.ArgumentParser()
parser.add_argument("-i",
"--input",
default=0,
help="input video file or camera device")
parser.add_argument("-grid",
"--grid",
default="20x20",
help="size of the grid (rows x cols)")
parser.add_argument("-framestep",
type=int,
default=20,
help="use every nth frame in the video")
parser.add_argument("-o",
"--output",
default="./yaml",
help="path to output yaml file")
parser.add_argument("-resolution",
"--resolution",
default="640x480",
help="resolution of the camera")
parser.add_argument("-fisheye",
"--fisheye",
action="store_true",
help="set ture if this is a fisheye camera")
args = parser.parse_args()
if not os.path.exists(args.output):
os.mkdir(args.output)
try:
source = cv2.VideoCapture(int(args.input))
except:
source = cv2.VideoCapture(args.input)
W, H = [int(x) for x in args.resolution.split("x")]
source.set(3, W)
source.set(4, H)
grid_size = tuple(int(x) for x in args.grid.split("x"))
grid_points = np.zeros((np.prod(grid_size), 3), np.float32)
grid_points[:, :2] = np.indices(grid_size).T.reshape(-1, 2)
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane
quit = False
do_calib = False
i = -1
while True:
i += 1
retcode, img = source.read()
if not retcode:
raise ValueError("cannot read frame from video")
if i % args.framestep != 0:
continue
print("searching for chessboard corners in frame " + str(i) + "...")
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
found, corners = cv2.findChessboardCorners(
gray, grid_size, cv2.CALIB_CB_ADAPTIVE_THRESH +
cv2.CALIB_CB_NORMALIZE_IMAGE + cv2.CALIB_CB_FILTER_QUADS)
if found:
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.01)
cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), term)
print("OK")
imgpoints.append(corners.reshape(1, -1, 2))
objpoints.append(grid_points.reshape(1, -1, 3))
cv2.drawChessboardCorners(img, grid_size, corners, found)
text1 = "press c to calibrate"
text2 = "press q to quit"
text3 = "device: {}".format(args.input)
fontscale = 0.6
cv2.putText(img, text1, (20, 70), cv2.FONT_HERSHEY_SIMPLEX, fontscale,
(255, 200, 0), 2)
cv2.putText(img, text2, (20, 110), cv2.FONT_HERSHEY_SIMPLEX, fontscale,
(255, 200, 0), 2)
cv2.putText(img, text3, (20, 30), cv2.FONT_HERSHEY_SIMPLEX, fontscale,
(255, 200, 0), 2)
cv2.imshow("corners", img)
key = cv2.waitKey(1) & 0xFF
if key == ord("c"):
do_calib = True
break
elif key == ord("q"):
quit = True
break
if quit:
source.release()
cv2.destroyAllWindows()
if do_calib:
print("\nPerforming calibration...\n")
N_OK = len(objpoints)
if N_OK < 12:
print("Less than 12 corners detected, calibration failed")
return
K = np.zeros((3, 3))
D = np.zeros((4, 1))
rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for _ in range(N_OK)]
tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for _ in range(N_OK)]
calibration_flags = (cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC +
cv2.fisheye.CALIB_CHECK_COND +
cv2.fisheye.CALIB_FIX_SKEW)
# 求出内参矩阵和畸变系数
if args.fisheye:
ret, mtx, dist, rvecs, tvecs = cv2.fisheye.calibrate(
objpoints, imgpoints, (W, H), K, D, rvecs, tvecs,
calibration_flags,
(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
else:
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(
objpoints, imgpoints, (W, H), None, None)
if ret:
print(ret)
data = {
"dim": np.array([W, H]).tolist(),
"K": K.tolist(),
"D": D.tolist()
}
fname = os.path.join(args.output,
"camera" + str(args.input) + ".yaml")
print(fname)
with open(fname, "w") as f:
yaml.safe_dump(data, f)
print("succesfully saved camera data")
cv2.putText(img, "Success!", (220, 240), cv2.FONT_HERSHEY_COMPLEX,
2, (0, 0, 255), 2)
else:
cv2.putText(img, "Failed!", (220, 240), cv2.FONT_HERSHEY_COMPLEX,
2, (0, 0, 255), 2)
cv2.imshow("corners", img)
cv2.waitKey(0)
def read_images(self, folder):
i = 0
images = []
images_fail = []
for file in os.listdir(folder):
if re.search(r'.*_left', file) == None:
continue
image1 = cv2.imread(folder + "/" + file)
if image1 is None:
break
file_right = re.sub(r'_left', '_right', file)
image2 = cv2.imread(folder + "/" + file_right)
if image2 is None:
break
gray_l = cv2.extractChannel(image1, 1)
gray_r = cv2.extractChannel(image2, 1)
# Find the chess board corners
ret_l, corners_l = cv2.findChessboardCorners(
gray_l, (8, 6), None, cv2.CALIB_CB_ADAPTIVE_THRESH)
if not ret_l:
print("left fail")
images_fail.append(file)
continue
ret_r, corners_r = cv2.findChessboardCorners(
gray_r, (8, 6), None, cv2.CALIB_CB_ADAPTIVE_THRESH)
if not ret_r:
print("right fail")
images_fail.append(file)
continue
images.append(file)
if ret_l and ret_r:
# If found, add object points, image points (after refining them)
self.objpoints.append(self.objp)
rt = cv2.cornerSubPix(gray_l, corners_l, (11, 11), (-1, -1),
self.criteria)
self.imgpoints_l.append(corners_l)
# Draw and display the corners
cv2.drawChessboardCorners(gray_l, (8, 6), corners_l, ret_l)
rt = cv2.cornerSubPix(gray_r, corners_r, (11, 11), (-1, -1),
self.criteria)
self.imgpoints_r.append(corners_r)
# Draw and display the corners
cv2.drawChessboardCorners(gray_r, (8, 6), corners_r, ret_r)
cv2.imshow("Image Left", gray_l)
cv2.imshow("Image Right", gray_r)
key = cv2.waitKey(1)
if key == ord('q'):
break
if key == ord('a'):
return
img_shape = gray_r.shape
self.shape = img_shape
print(f"Fails: {images_fail}", file=sys.stderr)
print("Starting camera calibration", file=sys.stderr)
flags = 0
# flags |= cv2.CALIB_FIX_INTRINSIC
# flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
# flags |= cv2.CALIB_USE_INTRINSIC_GUESS
# flags |= cv2.CALIB_FIX_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_ASPECT_RATIO
# flags |= cv2.CALIB_ZERO_TANGENT_DIST
# flags |= cv2.CALIB_RATIONAL_MODEL
# flags |= cv2.CALIB_SAME_FOCAL_LENGTH
#flags |= cv2.CALIB_FIX_K3
#flags |= cv2.CALIB_FIX_K4
#flags |= cv2.CALIB_FIX_K5
#flags |= cv2.CALIB_FIX_K6
rt, self.M1, self.d1, self.r1, self.t1, sdi, sde, pve = cv2.calibrateCameraExtended(
self.objpoints, self.imgpoints_l, img_shape, None, None)
print("Reprojection error left: " + str(rt), file=sys.stderr)
j = 0
for image in images:
print(f"{image}: {pve[j,0]}", file=sys.stderr)
j += 1
rt, self.M2, self.d2, self.r2, self.t2, sid, sde, pve = cv2.calibrateCameraExtended(
self.objpoints, self.imgpoints_r, img_shape, None, None)
print("Reprojection error right: " + str(rt), file=sys.stderr)
j = 0
for image in images:
print(f"{image}: {pve[j,0]}", file=sys.stderr)
j += 1
print("Starting stereo camrea calibration", file=sys.stderr)
self.camera_model = self.stereo_calibrate(img_shape)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d points in real world space
imgpoints = [] # 2d points in image plane.
# Make a list of calibration images
images = glob.glob('./camera_cal/*.jpg')
# Step through the list and search for chessboard corners
num_valid = 0
for idx, fname in enumerate(images):
img = imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
# Find the chessboard corners
ret, corners = cv2.findChessboardCorners(gray, (nx,ny), None)
# If found, add object points, image points
if ret == True:
num_valid += 1
objpoints.append(objp)
imgpoints.append(corners)
# Draw and display the corners
cv2.drawChessboardCorners(img, (9,6), corners, ret)
print("num valid: {}".format(num_valid))
# Do camera calibration given object points and image points
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, (720,1280), None, None)
dist_pickle = {}
def post(self, cam, h, w, square_size):
"""
Outputs and saves calibration info based on an array of corners found in get method
"""
data = request.json
obj_points = []
img_points = []
for output in data:
img_points.append(np.array(output['corners'], np.float32))
obj_points.append(np.array(output['pattern_points'], np.float32))
cam = settings.camera.get_cam(cam)
img, _ = cam.getFrame(calibrate=False)
ih, iw = img.shape[:2]
print('h={} w={}'.format(ih, iw))
rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(
obj_points, img_points, (iw, ih), None, None)
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
camera_matrix, dist_coefs, (iw, ih), 1, (iw, ih))
img, _ = cam.getFrame(calibrate=False)
cv2.imwrite('/tmp/test0.jpg', img)
img = cv2.undistort(img, camera_matrix, dist_coefs, None, newcameramtx)
cv2.imwrite('/tmp/test1.jpg', img)
x, y, iw, ih = roi
img = img[y:y + ih, x:x + iw]
cv2.imwrite('/tmp/test2.jpg', img)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
cv2.imwrite('/tmp/test3.jpg', gray)
pattern_size = (h - 1, w - 1)
print(pattern_size)
pattern_points = np.zeros((np.prod(pattern_size), 3), np.float32)
pattern_points[:, :2] = np.indices(pattern_size).T.reshape(-1, 2)
pattern_points *= square_size
found, corners = cv2.findChessboardCorners(gray, pattern_size)
rot = 0
ppmm = 0
if found:
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), term)
measurements = []
ca = corners[0]
for x in range(len(corners)):
if (x + 2) > len(corners):
break
nxt = x + 1
if rot < (h - 2):
D = dist.euclidean(
(corners[x][0][0], corners[x][0][1]),
(corners[nxt][0][0], corners[nxt][0][1]))
measurements.append(D)
rot += 1
print(D)
else:
rot = 0
ppmm = np.mean(measurements) / square_size
cam.setPixelsPerMM(ppmm)
cam.updateCalibration(square_size, rms, camera_matrix, dist_coefs)
settings.camera.save_camera_config()
return {
'ppmm': ppmm,
'rms': rms,
'camera_matrix': camera_matrix.tolist(),
'dist_coefs_shape': dist_coefs.shape,
'dist_coefs': dist_coefs.ravel().tolist()
}, 201
else:
print('not found')
cv2.imwrite('/tmp/test.jpg', gray)
return "checkerboard not found on camera {}".format(
cam.location), 404
