def loadImagePoints(imageDir, width, height, debug=False):
imageSize = (0, 0)
imagePoints = []
nimages = 0
images = cvtools.loadImages(imageDir)
for imageName in images:
# 读取图片时转换为灰度图像
im = cv2.imread(imageName, 0)
if debug:
print "正在处理图像:",imageName
imageSize = (im.shape[1], im.shape[0])
retval, corners = cv2.findChessboardCorners(im, (width, height))
if retval:
cv2.cornerSubPix(im, corners, (11, 11), (-1, -1), (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1))
nimages = nimages + 1
if debug:
im = cv2.cvtColor(im, cv2.COLOR_GRAY2BGR)
cv2.drawChessboardCorners(im, (width, height), corners, retval)
cv2.namedWindow(imageName, cv2.WINDOW_NORMAL)
cv2.imshow(imageName, im)
# print corners.reshape(-1,2)
cv2.waitKey()
imagePoints.append(corners.reshape(-1, 2))
cv2.destroyAllWindows()
print "imageSize is", imageSize;
return imagePoints, nimages, imageSize
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 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 processImage(fn):
print('processing %s... ' % fn)
img = cv.imread(fn, 0)
if img is None:
print("Failed to load", fn)
return None
assert w == img.shape[1] and h == img.shape[0], ("size: %d x %d ... " % (img.shape[1], img.shape[0]))
found, corners = cv.findChessboardCorners(img, pattern_size)
if found:
term = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_COUNT, 30, 0.1)
cv.cornerSubPix(img, corners, (5, 5), (-1, -1), term)
if debug_dir:
vis = cv.cvtColor(img, cv.COLOR_GRAY2BGR)
cv.drawChessboardCorners(vis, pattern_size, corners, found)
_path, name, _ext = splitfn(fn)
outfile = os.path.join(debug_dir, name + '_chess.png')
cv.imwrite(outfile, vis)
if not found:
print('chessboard not found')
return None
print(' %s... OK' % fn)
return (corners.reshape(-1, 2), pattern_points)
def set_lkpoint(self, point):
"""
Set a point to follow it using the L. Kallman method.
Arguments:
- self: The main object pointer.
- point: A co.cv.Point Point.
"""
cvPoint = (point.x, point.y)
self.img_lkpoints["current"] = numpy.mat((point.x,point.y),numpy.float32)
self.grey = numpy.asarray(self.grey[:,:])
if numpy.all(self.img_lkpoints["current"]):
cv2.cornerSubPix(
self.grey,
self.img_lkpoints["current"],
(20, 20), (0,0),
(cv2.TERM_CRITERIA_MAX_ITER | cv2.TERM_CRITERIA_EPS, 20, 0.03))
point.set_opencv( cvPoint )
self.img_lkpoints["points"].append(point)
setattr(point.parent, point.label, point)
if len(self.img_lkpoints["last"]) > 0:
self.img_lkpoints["last"] = numpy.append(self.img_lkpoints["last"], self.img_lkpoints["current"][0])
debug.debug( "ocvfw", "cmSetLKPoints: New LK Point Added" )
else:
self.img_lkpoints["current"] = []
def _find_chessboard(input_image):
success, corners = cv2.findChessboardCorners(
input_image, (29, 15), flags=cv2.cv.CV_CALIB_CB_ADAPTIVE_THRESH)
if not success:
raise NoChessboardError()
# Refine the corner measurements (not sure why this isn't built into
# findChessboardCorners?
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
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 _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 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 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 img_test():
"""
AZ IGY KAPOTT MATRIXSZAL A ROBOT KOORDINATA-RENDSZERENEK VEKTORAIT IRJUK FOL A MARKER KOORDINATA RENDSZEREBEN
:return:
"""
pointdict = pointdict1
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 *= 2.615
robot_coords = [pointdict[k] for k in pointdict.keys()]
imgpts = []
for k in pointdict.keys():
k = k % (outdir)
img = cv2.imread(k)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
gray = normalize(gray)
rv, corners = cv2.findChessboardCorners(gray, (9, 6))
# drawCorners(img, corners)
cv2.cornerSubPix(gray, corners, (9, 6),(-1,-1),criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1))
# drawCorners(img, corners)
imgpts_curr = corners.reshape((54,2))
imgpts.append(imgpts_curr)
rot, toc = calc_rot(imgpts, pattern_points, robot_coords)
print Utils.rpy(rot)
print rot
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 cube(img):
#img_in = cv2.imread("Picture 27.jpg")
#img = cv2.resize(img_in,None,fx=0.5, fy=0.5, interpolation = cv2.INTER_CUBIC)
#cv2.imshow('img',img)
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
#cv2.imshow('gray',gray)
ret, corners = cv2.findChessboardCorners(gray, (8,7),None)
# print ret,corners
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((7*8,3), np.float32)
objp[:,:2] = np.mgrid[0:8,0:7].T.reshape(-1,2)
#axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
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] ])
if ret == True:
cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
# Find the rotation and translation vectors.
rvecs, tvecs, inliers = cv2.solvePnPRansac(objp, corners, mtx, dist)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
#print imgpts
img = draw2(img,corners,imgpts)
return img
def detect_squares(self, images):
pattern_size = (7, 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 *= self.square_size
obj_points = []
img_points = []
h, w = 0, 0
for fn in images:
print 'processing %s...' % fn,
img = cv2.imread(os.path.join(os.path.dirname(os.path.realpath(__file__)), "cal", 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)
if self.debug_dir:
vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
cv2.drawChessboardCorners(vis, pattern_size, corners, found)
if not found:
print 'chessboard not found'
continue
img_points.append(corners.reshape(-1, 2))
obj_points.append(pattern_points)
print 'ok'
print "Calibration is a go... please wait.."
camera_matrix = np.zeros((3, 3))
dist_coefs = np.zeros(4)
img_n = len(img_points)
rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, (w, h), camera_matrix, dist_coefs)
return (obj_points, img_points, (w, h), camera_matrix, dist_coefs, rvecs, tvecs)
"""print "RMS:", rms
def calibrate_camera(img):
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((6*9, 3), np.float32)
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 point in real world space
imgpoints = [] # 2d points in image plane.
images = glob.glob('images/chess_calibration/*.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, (9,6),None)
if ret:
objpoints.append(objp)
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
imgpoints.append(corners)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
return ret, mtx, dist
def FindChessboardCorners(self, image):
"""Finds the positions of internal corners of the chessboard."""
# Processed image
gray = image.copy()
# Finds the positions of internal corners of the chessboard.
# flags = cv2.CALIB_CB_ADAPTIVE_THRESH | cv2.CALIB_CB_NORMALIZE_IMAGE | cv2.CALIB_CB_FAST_CHECK
# TODO: Change flags back ?
flags = cv2.CALIB_CB_FAST_CHECK | cv2.CALIB_CB_ADAPTIVE_THRESH
retval, corners = cv2.findChessboardCorners(gray, self.Size, flags=flags)
if retval:
# Check if the input image is a grayscale image.
if len(gray.shape) == 3:
gray = cv2.cvtColor(gray, cv2.COLOR_BGR2GRAY)
# Refines the corner locations.
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.01)
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
else:
corners = None
# Garbage Collector.
del gray
# Return the final result.
return corners
def cornerSubPix(image,
corners,
search_window=(11, 11),
zero_zone=(-1, -1),
criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
30,
0.001)):
"""The function refines locations of corners previously identified.
Iteratively the function tries to find the sub-pixel accurate location of
all provided corners.
:Parameters:
-`image` specifies the loaded image, that should be processed.
-`corners` defines the corners already identified in a previous step.
-`searchWindow` defines the size (sidelength) of the SearchWindow, in which
the sub-pixel accurate location of a corner should be investigated.
-`zeroZone` defines deaed region in the middle of the SearchWindow, in
which.
:Returns:
-`subpixelcorners`, returns the subpixel accurate locations o the found
corners.
:Notes:
"""
img = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
cv2.cornerSubPix(img, corners, search_window, zero_zone, criteria)
return corners
def get_chessboard_points(
img, pattern_rows, pattern_columns, square_size=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 square size of chessboard
:param draw: draw chessboard corners to ``img``
"""
size = (pattern_rows, pattern_columns)
found, corners = cv2.findChessboardCorners(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))
# (y, 1, 2) -> (y, 2) by reshape(-1, 2)
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 *= square_size
return img_points, obj_points
def set_camera_params(self):
print 'start calibration'
# チェスボード（X,Y,Z）座標の指定 (Z=0)
pattern_points = np.zeros((np.prod(self.pattern_size), 3), np.float32)
pattern_points[:, :2] = np.indices(self.pattern_size).T.reshape(-1, 2)
pattern_points *= self.square_size
obj_points = []
img_points = []
for fn in glob(self.img_path+"*.png"):
# 画像の取得
im = cv2.imread(fn, 0)
# チェスボードのコーナーを検出
found, corner = cv2.findChessboardCorners(im, self.pattern_size)
# コーナーがあれば
if found:
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
cv2.cornerSubPix(im, corner, (5, 5), (-1, -1), term)
# コーナーがない場合のエラー処理
if not found:
print 'chessboard not found'
continue
# appendメソッド：リストの最後に因数のオブジェクトを追加
img_points.append(corner.reshape(-1, 2))
obj_points.append(pattern_points)
# corner.reshape(-1, 2) : 検出したコーナーの画像内座標値(x, y)
# 内部パラメータを計算
self.ret, self.mtx, self.dist, self.rvecs, self.tvecs = cv2.calibrateCamera(
obj_points,
img_points,
(im.shape[1], im.shape[0]))
print 'finish calibration'
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 main():
square_size = 1.0 # 正方形のサイズ
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 = []
for fn in glob("*.jpg"):
# 画像の取得
im = cv2.imread(fn, 0)
print "loading..." + fn
# チェスボードのコーナーを検出
found, corner = cv2.findChessboardCorners(im, pattern_size)
# コーナーがあれば
if found:
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
cv2.cornerSubPix(im, corner, (5,5), (-1,-1), term)
# コーナーがない場合のエラー処理
if not found:
print 'chessboard not found'
continue
img_points.append(corner.reshape(-1, 2))
obj_points.append(pattern_points)
# 内部パラメータを計算
rms, K, d, r, t = cv2.calibrateCamera(obj_points,img_points,(im.shape[1],im.shape[0]))
# 計算結果を表示
print "RMS = ", rms
print "K = \n", K
print "d = ", d.ravel()
def render_cube(self, image, points):
# 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, image, points and axis
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
imgp = np.array(points, dtype="float32")
objp = np.array([[0., 0., 0.], [1., 0., 0.],
[1., 1., 0.], [0., 1., 0.]], dtype="float32")
axis = np.float32([[0, 0, 0], [0, 1, 0], [1, 1, 0], [1, 0, 0],
[0, 0, -1], [0, 1, -1], [1, 1, -1], [1, 0, -1]])
# project 3D points to image plane
cv2.cornerSubPix(gray, imgp, (11, 11), (-1, -1), criteria)
rvecs, tvecs, _ = cv2.solvePnPRansac(objp, imgp, mtx, dist)
imgpts, _ = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
# draw cube
self._draw_cube(image, imgpts)
return image
def getPoints(inMask, outDir, patternSize):
"""Process all images matching inMask and output debug images to outDir"""
imageNames = glob.glob(inMask)
objPoints = []
imgPoints = []
# Set the criteria for the cornerSubPix algorithm
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
passed = 0
# Process all images
for fileName in imageNames:
image = cv2.imread(fileName, 0)
found, corners, patternPoints = findCorners(image, patternSize)
# Found corners
if found:
logger.debug("Chessboard found in: %s" % fileName)
cv2.cornerSubPix(image, corners, (5, 5), (-1, -1), term)
vis = cv2.cvtColor(image, cv2.COLOR_GRAY2BGR)
# Draw the corners
cv2.drawChessboardCorners(vis, patternSize, corners, found)
path, name, ext = splitFileName(fileName)
writeFileName = "%s%s-python.bmp" % (outdir, name)
logger.debug("Writing debug image: %s" % writeFileName)
# Write off marked up images
cv2.imwrite(writeFileName, vis)
# Add image and object points to lists for calibrateCamera
imgPoints.append(corners.reshape(-1, 2))
objPoints.append(patternPoints)
passed += 1
else:
logger.warning("Chessboard not found in: %s" % fileName)
logger.info("Images passed cv2.findChessboardCorners: %d" % passed)
# We assume all images the same size, so we use last one
h, w = image.shape[:2]
return h, w, objPoints, imgPoints
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 render(self, CSH, hammer, other, image, axisList):
# load calibration data
with np.load('../params/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((6 * 7, 3), np.float32)
objp[:, :2] = np.mgrid[0:7, 0:6].T.reshape(-1, 2)
# find grid corners in image
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (7, 9), None)
if ret == True:
cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
rvecs, tvecs, _ = cv2.solvePnPRansac(objp, corners, mtx, dist)
if CSH == True:
# project 3D points to image plane
for axis in axisList:
imgpts, _ = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
self._draw_cube(image, imgpts)
return image
elif hammer == True:
imgptsOther, _ = cv2.projectPoints(
other, rvecs, tvecs, mtx, dist)
self._draw_cube(image, imgptsOther)
return image
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 calibrate(self, img):
height, width = img.shape
is_found, coordinates = cv2.findChessboardCorners(img, self._pattern_size)
if is_found & (self._n > 0):
corners_first = []
for p in coordinates:
corners_first.append(p[0])
img_points_first = np.asarray(corners_first, np.float64)
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.1)
cv2.cornerSubPix(img, coordinates, (5, 5), (-1, -1), term)
self._img_points.append(coordinates.reshape(-1, 2))
self._obj_points.append(self._pattern_points)
self._n -= 1
if self._n == 0:
rms, camera_matrix, distortion_coefficient, rotation_vectors, translation_vectors = cv2.calibrateCamera(self._obj_points, self._img_points, (width, height), self._camera_matrix, self._distortion_coefficient, flags=0)
self.is_calibrated = True
np.save(self._output_path + "/camera_matrix", camera_matrix)
np.save(self._output_path + "/distortion_coefficient", distortion_coefficient)
np.save(self._output_path + "/rotation_vectors", rotation_vectors)
np.save(self._output_path + "/translation_vectors", translation_vectors)
np.save(self._output_path + "/chessSquare_size", self._square_size)
np.save(self._output_path + "/img_points", self._img_points)
np.save(self._output_path + "/obj_points", self._obj_points)
np.save(self._output_path + "/img_points_first", img_points_first)
def refinePoints(gray, points):
if len(points) == 9:
return points
corner = pointsToCorner(points)
cv2.cornerSubPix(gray, corner, (11, 11), (-1, -1), criteria)
points = cornerToPoints(corner)
return points
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 downsample_and_detect(self, img):
"""
Downsample the input image to approximately VGA resolution and detect the
calibration target corners in the full-size image.
Combines these apparently orthogonal duties as an optimization. Checkerboard
detection is too expensive on large images, so it's better to do detection on
the smaller display image and scale the corners back up to the correct size.
Returns (scrib, corners, downsampled_corners, board, (x_scale, y_scale)).
"""
# Scale the input image down to ~VGA size
height = img.shape[0]
width = img.shape[1]
scale = math.sqrt( (width*height) / (640.*480.) )
if scale > 1.0:
scrib = cv2.resize(img, (int(width / scale), int(height / scale)))
else:
scrib = img
# Due to rounding, actual horizontal/vertical scaling may differ slightly
x_scale = float(width) / scrib.shape[1]
y_scale = float(height) / scrib.shape[0]
if self.pattern == Patterns.Chessboard:
# Detect checkerboard
(ok, downsampled_corners, board) = self.get_corners(scrib, refine = True)
# Scale corners back to full size image
corners = None
if ok:
if scale > 1.0:
# Refine up-scaled corners in the original full-res image
# TODO Does this really make a difference in practice?
corners_unrefined = downsampled_corners.copy()
corners_unrefined[:, :, 0] *= x_scale
corners_unrefined[:, :, 1] *= y_scale
radius = int(math.ceil(scale))
if len(img.shape) == 3 and img.shape[2] == 3:
mono = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
else:
mono = img
cv2.cornerSubPix(mono, corners_unrefined, (radius,radius), (-1,-1),
( cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1 ))
corners = corners_unrefined
else:
corners = downsampled_corners
else:
# Circle grid detection is fast even on large images
(ok, corners, board) = self.get_corners(img)
# Scale corners to downsampled image for display
downsampled_corners = None
if ok:
if scale > 1.0:
downsampled_corners = corners.copy()
downsampled_corners[:,:,0] /= x_scale
downsampled_corners[:,:,1] /= y_scale
else:
downsampled_corners = corners
return (scrib, corners, downsampled_corners, board, (x_scale, y_scale))
imgpoints = [] # 2d points in image plane.
images = glob.glob("*.jpg")
for fname in images:
#对每张图片，识别出角点，记录世界物体坐标和图像坐标
img = cv2.imread(fname) #source image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) #转灰度
#cv2.imshow('img',gray)
#cv2.waitKey(0)
#寻找角点，存入corners，ret是找到角点的flag
ret, corners = cv2.findChessboardCorners(gray, (11, 8), None)
#criteria:角点精准化迭代过程的终止条件
if ret == True:
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30,
0.001)
#执行亚像素级角点检测
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
objpoints.append(objp)
#print objpoints
imgpoints.append(corners2)
#在棋盘上绘制角点,只是可视化工具
img = cv2.drawChessboardCorners(gray, (cbcol, cbrow), corners2, ret)
cv2.imshow('img', img)
cv2.waitKey(10)
'''
传入所有图片各自角点的三维、二维坐标，相机标定。
每张图片都有自己的旋转和平移矩阵，但是相机内参和畸变系数只有一组。
mtx，相机内参；dist，畸变系数；revcs，旋转矩阵；tvecs，平移矩阵。
'''
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
def camera_calibration(dataDir):
# This file can be used to generate camera calibration parameters
# to improve the default values
fileNameK = "{}intrinsic.txt".format(dataDir)
fileNameD = "{}distCoeffs.txt".format(dataDir)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_100)
board = aruco.CharucoBoard_create(3,3,0.94,0.34,aruco_dict)
allCorners = []
allIds = []
decimator = 0
images = np.array([dataDir + f for f in os.listdir(dataDir) if f.endswith(".png")])
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001)
for im in images:
print("=> Processing image {0}".format(im))
frame = cv2.imread(im)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, aruco_dict)
if len(corners) > 0:
# SUB PIXEL DETECTION
for corner in corners:
cv2.cornerSubPix(gray, corner,
winSize=(3, 3),
zeroZone=(-1, -1),
criteria=criteria)
res2 = cv2.aruco.interpolateCornersCharuco(corners, ids, gray, board)
if res2[1] is not None and res2[2] is not None and len(res2[1]) > 3 and decimator % 1 == 0:
allCorners.append(res2[1])
allIds.append(res2[2])
print("Image: {}/{}".format(decimator + 1, len(images)))
print("Corners found: {}".format(len(corners)))
decimator += 1
imsize = gray.shape
print("\n")
print("Checkerboard detected in: {}/{} images".format(len(allCorners), decimator))
cameraMatrixInit = np.array([[1000., 0., imsize[0] / 2.],
[0., 1000., imsize[1] / 2.],
[0., 0., 1.]])
distCoeffsInit = np.zeros((5, 1))
flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL + cv2.CALIB_FIX_ASPECT_RATIO)
(ret, camera_matrix, distortion_coefficients0,
rotation_vectors, translation_vectors, _, _, _) = cv2.aruco.calibrateCameraCharucoExtended(
charucoCorners=allCorners,
charucoIds=allIds,
board=board,
imageSize=imsize,
cameraMatrix=cameraMatrixInit,
distCoeffs=distCoeffsInit,
flags=flags,
criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9))
np.savetxt(fileNameK, camera_matrix, delimiter=',')
np.savetxt(fileNameD, distortion_coefficients0, delimiter=',')
i = 5 # select image id
plt.figure()
frame = cv2.imread(images[i])
img_undist = cv2.undistort(frame, camera_matrix, distortion_coefficients0, None)
plt.subplot(1, 2, 1)
plt.imshow(frame)
plt.title("Raw image")
plt.axis("off")
plt.subplot(1, 2, 2)
plt.imshow(img_undist)
plt.title("Corrected image")
plt.axis("off")
plt.show()
def calibration(**kwargs):
for k, v in kwargs.items():
setattr(opt, k, v)
# 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.
objpoints_r = []
imgpoints_r = []
images = glob.glob('../left/*.jpg')
images_r = glob.glob('../right/*.jpg')
images.sort()
images_r.sort()
for fname, fname_r in zip(images, images_r):
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
img_r = cv2.imread(fname_r)
gray_r = cv2.cvtColor(img_r, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (7, 6), None)
ret_r, corners_r = cv2.findChessboardCorners(gray_r, (7, 6), None)
# If found, add object points, image points (after refining them)
if ret == True and ret_r == True:
objpoints.append(objp)
objpoints_r.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
corners2_r = cv2.cornerSubPix(gray_r, corners_r, (11, 11),
(-1, -1), criteria)
imgpoints.append(corners2)
imgpoints_r.append(corners2_r)
# Draw and display the corners
if opt.disp_calib:
cv2.imshow('img', img)
cv2.waitKey(500)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
img = cv2.imread('../left/left' + str(opt.sample) + '.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]
if opt.disp_calib:
cv2.imwrite('../calibresult/left' + str(opt.sample) + '.png', dst)
ret, mtx_r, dist_r, rvecs, tvecs = cv2.calibrateCamera(
objpoints_r, imgpoints_r, gray_r.shape[::-1], None, None)
img_r = cv2.imread('../right/right' + str(opt.sample) + '.jpg')
h, w = img_r.shape[:2]
newcameramtx_r, roi = cv2.getOptimalNewCameraMatrix(
mtx_r, dist_r, (w, h), 1, (w, h))
# undistort
dst_r = cv2.undistort(img_r, mtx_r, dist_r, None, newcameramtx_r)
# crop the image
x, y, w, h = roi
dst_r = dst_r[y:y + h, x:x + w]
if opt.disp_calib:
cv2.imwrite('../calibresult/right' + str(opt.sample) + '.png', dst)
if not opt.stereo_calib:
exit(0)
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(objpoints, imgpoints, imgpoints_r, mtx,
dist, mtx_r, dist_r, gray.shape[::-1])
if opt.matlab:
try:
R = opt.R[opt.sample]
T = opt.T[opt.sample]
except:
print('Please modify config to add R and T for ' + opt.sample)
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = cv2.stereoRectify(
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2,
gray.shape[::-1], R, T)
left_map1, left_map2 = cv2.initUndistortRectifyMap(cameraMatrix1,
distCoeffs1, R1, P1,
gray.shape[::-1],
cv2.INTER_NEAREST)
right_map1, right_map2 = cv2.initUndistortRectifyMap(
cameraMatrix2, distCoeffs2, R2, P2, gray.shape[::-1],
cv2.INTER_NEAREST)
img = cv2.imread('../left/left' + str(opt.sample) + '.jpg')
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
img = cv2.imread(('../right/right' + str(opt.sample) + '.jpg'))
gray_r = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
imgL = cv2.remap(gray, left_map1, left_map2, cv2.INTER_LINEAR)
imgR = cv2.remap(gray_r, right_map1, right_map2, cv2.INTER_LINEAR)
if opt.disp_stereo_calib:
cv2.imwrite(
'../result/stereo_calibresult/left' + str(opt.sample) + '.png',
imgL)
cv2.imwrite(
'../result/stereo_calibresult/right' + str(opt.sample) + '.png',
imgR)
plt.subplot(121)
plt.title('left')
plt.imshow(imgL, cmap='gray')
plt.axis('off')
plt.subplot(122)
plt.title('right')
plt.imshow(imgR, cmap='gray')
plt.axis('off')
plt.show()
if not opt.disparity:
exit(0)
cv2.namedWindow("depth")
cv2.namedWindow("disparity")
cv2.moveWindow("depth", 0, 0)
cv2.moveWindow("disparity", 600, 0)
def callbackFunc(e, x, y, f, p):
if e == cv2.EVENT_LBUTTONDOWN:
print(threeD[y][x])
cv2.setMouseCallback("depth", callbackFunc, None)
stereo = cv2.StereoSGBM_create(numDisparities=16 * opt.num,
blockSize=opt.blockSize)
disparity = stereo.compute(imgL, imgR)
disp = cv2.normalize(disparity,
disparity,
alpha=0,
beta=255,
norm_type=cv2.NORM_MINMAX,
dtype=cv2.CV_8U)
# 将图片扩展至3d空间中，其z方向的值则为当前的距离
threeD = cv2.reprojectImageTo3D(disparity.astype(np.float32) / 16., Q)
cv2.imshow("disparity", disp)
cv2.imshow("depth", imgL)
key = cv2.waitKey(0)
if key == ord("q"):
exit(0)
elif key == ord("s"):
cv2.imwrite("../result/disparity/disparity" + opt.sample + ".png",
disp)
import cv2
src = cv2.imread("lion_black_img.png")
dst = src.copy()
gray = cv2.cvtColor(src, cv2.COLOR_RGB2GRAY)
corners = cv2.goodFeaturesToTrack(gray,
100,
0.01,
5,
blockSize=3,
useHarrisDetector=True,
k=0.03)
for i in corners:
cv2.circle(dst, tuple(i[0]), 3, (255, 0, 0), 5)
criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 30, 0.001)
cv2.cornerSubPix(gray, corners, (5, 5), (-1, -1), criteria)
for i in corners:
cv2.circle(dst, tuple(i[0]), 3, (0, 0, 255), 5)
cv2.imshow("Dst", dst)
cv2.waitKey(0)
cv2.destroyAllWindows()
def detect_markers(gray_img,
grid_size,
min_marker_perimeter=40,
aperture=11,
visualize=False,
invert_image=False):
if invert_image:
gray_img = 255 - gray_img
edges = cv2.adaptiveThreshold(gray_img, 255, cv2.ADAPTIVE_THRESH_MEAN_C,
cv2.THRESH_BINARY, aperture, 9)
contours, hierarchy = cv2.findContours(edges,
mode=cv2.RETR_TREE,
method=cv2.CHAIN_APPROX_SIMPLE,
offset=(0, 0)) #TC89_KCOS
# remove extra encapsulation
hierarchy = hierarchy[0]
contours = np.array(contours)
# keep only contours with parents and children
contained_contours = contours[np.logical_and(hierarchy[:, 3] >= 0,
hierarchy[:, 2] >= 0)]
# turn on to debug contours
# cv2.drawContours(gray_img, contours,-1, (0,255,255))
# cv2.drawContours(gray_img, aprox_contours,-1, (255,0,0))
# contained_contours = contours #overwrite parent children check
#filter out rects
aprox_contours = [
cv2.approxPolyDP(c, epsilon=2.5, closed=True)
for c in contained_contours
]
# any rectagle will be made of 4 segemnts in its approximation
# also we dont need to find a marker so small that we cannot read it in the end...
# also we want all contours to be counter clockwise oriented, we use convex hull fot this:
rect_cand = [
cv2.convexHull(c, clockwise=True) for c in aprox_contours
if c.shape[0] == 4
and cv2.arcLength(c, closed=True) > min_marker_perimeter
]
# a non convex quadrangle is not what we are looking for.
rect_cand = [r for r in rect_cand if r.shape[0] == 4]
if visualize:
cv2.drawContours(gray_img, rect_cand, -1, (255, 100, 50))
markers = {}
size = 10 * grid_size
#top left,bottom left, bottom right, top right in image
mapped_space = np.array(((0, 0), (size, 0), (size, size), (0, size)),
dtype=np.float32).reshape(4, 1, 2)
for r in rect_cand:
if correct_gradient(gray_img, r):
r = np.float32(r)
M = cv2.getPerspectiveTransform(r, mapped_space)
flat_marker_img = cv2.warpPerspective(
gray_img, M,
(size, size)) #[, dst[, flags[, borderMode[, borderValue]]]])
# Otsu documentation here :
# https://opencv-python-tutroals.readthedocs.org/en/latest/py_tutorials/py_imgproc/py_thresholding/py_thresholding.html#thresholding
_, otsu = cv2.threshold(flat_marker_img, 0, 255,
cv2.THRESH_BINARY + cv2.THRESH_OTSU)
# getting a cleaner display of the rectangle marker
kernel = cv2.getStructuringElement(cv2.MORPH_CROSS, (3, 3))
cv2.erode(otsu, kernel, otsu, iterations=3)
# kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3,3))
# cv2.dilate(otsu,kernel,otsu, iterations=1)
marker = decode(otsu, grid_size)
if marker is not None:
angle, msg = marker
# define the criteria to stop and refine the marker verts
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
100, 0.001)
cv2.cornerSubPix(gray_img, r, (3, 3), (-1, -1), criteria)
centroid = r.sum(axis=0) / 4.
centroid.shape = (2)
# angle is number of 90deg rotations
# roll points such that the marker points correspond with oriented marker
# rolling may not make the verts appear as you expect,
# but using m_screen_to_marker() will get you the marker with proper rotation.
r = np.roll(
r, angle + 1, axis=0
) #np.roll is not the fastest when using these tiny arrays...
marker = {
'id': msg,
'verts': r,
'perimeter': cv2.arcLength(r, closed=True),
'centroid': centroid,
"frames_since_true_detection": 0
}
if visualize:
marker['img'] = np.rot90(otsu, -angle / 90)
if markers.has_key(marker['id']) and markers[
marker['id']]['perimeter'] > marker['perimeter']:
pass
else:
markers[marker['id']] = marker
return markers.values()
