def coeffs(self):
if not self._coeffs:
if not self.findCount:
raise NothingFound(
'can create camera calibration because no corners have been found'
)
#http://en.wikipedia.org/wiki/Reprojection_error
try:
(reprojectionError, cameraMatrix, distortionCoeffs,
rotationVecs, translationVecs) = cv2.calibrateCamera(
self.objpoints, self.opts['imgPoints'],
self.img.shape[::-1], None, None)
print('reprojectionError=%s' % reprojectionError)
except Exception, err:
raise NothingFound(err)
self._coeffs = OrderedDict([
('reprojectionError', reprojectionError),
('apertureSize', self.apertureSize),
('cameraMatrix', cameraMatrix),
('distortionCoeffs', distortionCoeffs),
('shape', self.img.shape),
#('rotationVecs',rotationVecs),
#('translationVecs',translationVecs),
])
if self.apertureSize is not None:
(fovx, fovy, focalLength, principalPoint,
aspectRatio) = cv2.calibrationMatrixValues(
cameraMatrix, self.img.shape, *self.apertureSize)
self._coeffs.update(
OrderedDict([('fovx', fovx), ('fovy', fovy),
('focalLength', focalLength),
('principalPoint', principalPoint),
('aspectRatio', aspectRatio)]))
def describe(self):
"""Describe calibration info as a string. Use human-understandable
metrics."""
image_size = (self.image_size or self._DEFAULT_IMAGE_SIZE)
aperture = self._DEFAULT_SENSOR_SIZE
assert self.camera_matrix is not None, 'Calibration not loaded'
# Center coordinates -- in percent, with (0, 0) being image center
c_x = (self.camera_matrix[0, 2] *1.0 / image_size[0] - 0.5) * 100.0
c_y = (self.camera_matrix[1, 2] *1.0 / image_size[1] - 0.5) * 100.0
# f_x/f_y - if object size is same a distance to object, how much of a
# frame will it take? in percent
f_x = self.camera_matrix[0, 0] * 100.0 / image_size[0]
f_y = self.camera_matrix[1, 1] * 100.0 / image_size[1]
fov_x, fov_y, focal_len, principal, aspect = \
cv2.calibrationMatrixValues(self.camera_matrix, image_size,
aperture[0], aperture[0])
fixups = self.fixups
return ("FOV(deg)=({fov_x:.1f}/{fov_y:.1f}) "
"principal=({principal[0]:.1f}/{principal[1]:.1f}) "
"center=({c_x:.1f},{c_y:.1f})% "
"focal_len=({f_x:.1f},{f_y:.1f})% "
"focal_len_mm={focal_len:.2f} aspect={aspect:.3f} "
"fixups=({fixups[0]:.2f},{fixups[1]:.2f})"
).format(**locals())
def getCameraViewAngle(self): fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues( self.camera['mtx'], (self.w, self.h), 5.6, 5.6 ) return fovy
def CalculateFOVfromCalibration(self, calibFile):
cv_file = cv2.FileStorage(calibFile, cv2.FILE_STORAGE_READ)
k = cv_file.getNode("k").mat()
D = cv_file.getNode("D").mat()
size = cv_file.getNode("size").mat()
h = size[0][0]
w = size[1][0]
cv_file.release()
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
k, (w, h), 1, 1)
print("{} : fovx {} fovy {}".format(calibFile, fovx, fovy))
return fovx, fovy, h, w
def main(argv): camera = np.load(argv[0]) fname = argv[1] img = cv2.imread(fname) fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues( camera['mtx'], img.shape[:2], 5.6, 5.6 ) print fovx, fovy, focalLength, principalPoint, aspectRatio
def main(fqfn):
camp = cameratransform.getCameraParametersFromExif(fqfn, verbose=True)
xform = np.zeros((3, 3))
xform[2, 2] = 1
xform[0, 0] = xform[1, 1] = camp[0]
xform[0, 2] = camp[1][0]/2
xform[1, 2] = camp[1][1]/2
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(xform, (camp[2][0],camp[2][1]),camp[1][0],camp[1][1])
print((fovx, fovy))
fovxx = 2 * np.arctan(camp[1][0] / (2*camp[0]))
fovxy = 2 * np.arctan(camp[1][1] / (2*camp[0]))
print((fovxx, fovxy))
return camp
def calibrate_3d(frame, features):
ret, corners = cv2.findChessboardCorners(frame, (7,6))
corners_3d = gen_3d_coords()
if ret:
features['chessboard'] = {
'dimensions': (7,6),
'corners': corners
}
corners = corners.reshape(7*6, 2).astype('float32')
frame_h, frame_w = frame.shape
cam_matrix, dist_coefs = calibrate_camera(corners, corners_3d, (frame_w, frame_h))
print(cv2.calibrationMatrixValues(cam_matrix, (frame_w, frame_h), 10.0, 10.0))
else:
print(ret)
def zad_1(data):
image_points = []
object_points_all = []
flags = []
for image in data:
# image = cv2.imread('calib_basler21130751/img_21130751_0000.bmp')
flag_found, corners = cv2.findChessboardCorners(image, (8, 5))
flags.append(flag_found)
# print(corners)
if flag_found:
image_points.append(corners)
print('Corners found')
corners = cv2.cornerSubPix(
cv2.cvtColor(image, cv2.COLOR_BGR2GRAY),
corners, (11, 11), (-1, -1),
(cv2.TERM_CRITERIA_EPS + cv2.TermCriteria_MAX_ITER, 30, 0.1)
)
# print(corners[0])
object_points = np.zeros((8 * 5, 3), np.float32)
object_points[:, :2] = np.mgrid[0:8, 0:5].T.reshape(-1, 2)
# print(object_points[0:3])
object_points_all.append(object_points)
else:
print("Corners not found")
print("Processing...")
retval, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(object_points_all, image_points, data[0].shape[:2], None, None)
fovx, fovy, focallLength, principalPoints, aspectRatio = cv2.calibrationMatrixValues(
camera_matrix, data[0].shape[0:2], 7.2, 5.4
)
print("Fovx: ",fovx)
print("Fovy: ",fovy)
print("Focall Length: ",focallLength)
image_with_corners = cv2.drawChessboardCorners(data[0], (8, 5), image_points[0], flags[0])
img_undistored = cv2.undistort(data[0], camera_matrix, dist_coeffs)
while (1):
if cv2.waitKey(20) & 0xFF == 27:
break
cv2.imshow('image_with_conrners', image_with_corners)
cv2.imshow('image_undistored', img_undistored)
cv2.destroyAllWindows()
def calibration(j,i): #repeat 5 times, 93mm. 160
global pic, ret, video
num_img = 0
key = 0
distance = 0
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# do not forget to change chess dimensions
objp, objpoints,imgpoints = obj_parameters()
video = cv2.VideoCapture(0)
cv2.namedWindow('Raw')
cv2.setMouseCallback('Raw',coordinates)
while(num_img < 10):
(ret, img) = video.read()
pic = cv2.flip(img,1)
gray = cv2.cvtColor(pic, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (8,6), None)#(8,6) Obj square points
distance = px_distance(distance,pic)
if ret == True:
objpoints.append(objp) # Square coordinates, 0 to 48 in (6,8) matrix
imgpoints.append(corners) # Pixel values according to objpoints
num_img += 1
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
cv2.drawChessboardCorners(pic, (8,6), corners2, ret)
cv2.imshow('Raw', pic)
key = cv2.waitKey(20)
cv2.waitKey(500)
video.release()
cv2.destroyAllWindows()
if(num_img > 0):
print "Wait a second..."
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None, flags=cv2.CALIB_USE_INTRINSIC_GUESS)
# rvecs = 3X1, tvecs = 3X1
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(cameraMatrix, gray.shape[::-1], 1.0, 1.0)
# mtx = K matrix (Intrinsic Parameters), rvecs = R and tvecs = t (External Parameters)
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))
## Python dictionary data structure:
Camera = {"Ret": ret,"Original Matrix" : mtx, "Distortion" : dist, "Rotational Distortion":rvecs, "Translation Distortion": tvecs, "Improved Matrix": newcameramtx, "Region of Interest": roi}
tvecs,rvecs = Intrinsics(Camera,objp,corners2,Camera['Original Matrix'],Camera['Distortion'])
save_xml(ret,mtx,dist,rvecs,tvecs,newcameramtx,roi,j,i)
#errors(objpoints,imgpoints,Camera)
return img, Camera
def print_calibration_matrix(camera, apertureWidth, apertureHeight):
fovx, \
fovy, \
focalLength, \
principalPoint, \
aspectRatio = cv2.calibrationMatrixValues(camera.calibrationParams['mtx'],
camera.calibrationParams['imageSize'],
apertureWidth,
apertureHeight)
print('FOVx:\t\t\t\t', fovx)
print('FOVy:\t\t\t\t', fovy)
print('Focal Length:\t\t', focalLength)
print('Principal Point:\t', principalPoint)
print('Aspect Ratio:\t\t', aspectRatio)
print('Camera Matrix:')
for c1, c2, c3 in camera.calibrationParams['mtx']:
print("\t\t\t\t\t%04.2f \t|\t %04.2f \t|\t %04.2f" % (c1, c2, c3))
print('')
def get_camera_info(self,
size=(640, 480), aperture_height=0, aperture_width=0):
"""Return fovy and aspectRatio from camera matrix.
These parameters can be used in ``gluPerspective()``.
:param size: tuple of input image width and height
:type size: (int, int)
:param aperture_height: physical height of the camera
:param aperture_width: physical width of the camera
.. caution::
The accuracy in using this method and gluPerspective() is not good.
You had better use :func:`Reconstructor.get_gl_frustum_parameters`.
.. note::
I don't understand the reason why ``aperture_height`` and
``aperture_width`` are needed, so I set 0 to them as default.
"""
fovx, fovy, focal_length, principal_point, aspect_ratio = \
cv2.calibrationMatrixValues(self.calibrator.camera_matrix, size,
aperture_height, aperture_width)
return fovx, fovy, focal_length, principal_point, aspect_ratio
def coeffs(self):
if not self._coeffs:
if not self.findCount:
raise NothingFound(
'can create camera calibration because no corners have been found')
# http://en.wikipedia.org/wiki/Reprojection_error
try:
(reprojectionError, cameraMatrix, distortionCoeffs,
rotationVecs, translationVecs) = cv2.calibrateCamera(
self.objpoints,
self.opts['imgPoints'],
self.img.shape[::-1], None, None)
print('reprojectionError=%s' % reprojectionError)
except Exception as err:
raise NothingFound(err)
self._coeffs = OrderedDict([
('reprojectionError', reprojectionError),
('apertureSize', self.apertureSize),
('cameraMatrix', cameraMatrix),
('distortionCoeffs', distortionCoeffs),
('shape', self.img.shape),
#('rotationVecs',rotationVecs),
#('translationVecs',translationVecs),
])
if self.apertureSize is not None:
(fovx, fovy, focalLength, principalPoint,
aspectRatio) = cv2.calibrationMatrixValues(
cameraMatrix, self.img.shape, *self.apertureSize)
self._coeffs.update(OrderedDict([
('fovx', fovx),
('fovy', fovy),
('focalLength', focalLength),
('principalPoint', principalPoint),
('aspectRatio', aspectRatio)])
)
return self._coeffs
def calibrate(self):
rvecs = ()
tvecs = ()
cameraMatrix = ()
distCoeffs = ()
self.findCorners()
rms, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(
self.objpoints, self.imgpoints, self.imageSize, cameraMatrix,
distCoeffs, rvecs, tvecs, 0)
apertureWidth = 4.8
apertureHeight = 3.6
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
cameraMatrix, self.imageSize, apertureWidth, apertureHeight)
# Cuentas a mano para el fov
fx = cameraMatrix[0, 0]
fy = cameraMatrix[1, 1]
print 'fx:' + str(fx) + ' fy:' + str(fy) + ' fovx:' + str(
fovx) + ' fovy:' + str(fovy) + ' focalLength:' + str(focalLength)
def calibrate(self):
rvecs = ()
tvecs = ()
cameraMatrix = ()
distCoeffs = ()
self.findCorners()
rms, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(
self.objpoints, self.imgpoints, self.imageSize, cameraMatrix, distCoeffs, rvecs, tvecs, 0)
apertureWidth = 4.8
apertureHeight = 3.6
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
cameraMatrix, self.imageSize, apertureWidth, apertureHeight)
# Cuentas a mano para el fov
fx = cameraMatrix[0,0]
fy = cameraMatrix[1,1]
print 'fx:' + str(fx) + ' fy:' + str(fy) + ' fovx:' + str(fovx) + ' fovy:' + str(fovy) + ' focalLength:' + str(focalLength)
def cameraCalibration(detectedCorners, sensorWidth, sensorHeight,
patternColumns, patternRows, imageSize):
"""
Calculates necessary camera calibration metrics from an image set of a calibration chessboard.
Args:
imgs: List of images containing the image set.
sensorWidth: Width of the Image Sensor in mm.
sensorHeight: Height of the Image Sensor in mm.
patternColums: Number of interior columns on calibration pattern.
patternRows: Number of interior rows on calibration pattern.
Returns:
matrix: Camera Matrix
distortion: Distortion Coefficents
fov: (horizontal, vertical) field of view in degrees
"""
# Create Board Template
board = makeChessboard(patternColumns, patternRows)
# Confirm Sensor Size
sensor = sensorOptimizer(sensorWidth, sensorHeight, imageSize)
# Prepare Arrays for openCV Calculations
imagePoints, objectPoints = createCalibrationArrays(board, detectedCorners)
# Camera Matrix Calculations
initialMatrix = cv2.initCameraMatrix2D(objectPoints, imagePoints,
imageSize)
error, matrix, distortion, rot, trans = cv2.calibrateCamera(
objectPoints, imagePoints, imageSize, initialMatrix, None)
# FOV Calculation
fovH, fovV, focal, principal, ratio = cv2.calibrationMatrixValues(
matrix, imageSize, sensor[0], sensor[1])
return matrix, distortion, (fovH, fovV)
def main():
parser = argparse.ArgumentParser(
description=
'Camera Calibrator - OpenCV and Emanuele Ruffaldi SSSA 2014-2015')
parser.add_argument('path',
help='path where images can be found (png or jpg)',
nargs="+")
parser.add_argument(
'--save',
help='name of output calibration in YAML otherwise prints on console')
parser.add_argument('--verbose', action="store_true")
parser.add_argument('--ir', action='store_true')
parser.add_argument('--threshold', type=int, default=0)
parser.add_argument('--calib',
type=str,
help="default calib for guess or nocalibrate mode")
parser.add_argument('--flipy', action='store_true')
parser.add_argument('--flipx', action='store_true')
parser.add_argument('--side', help="side: all,left,right", default="all")
#parser.add_argument('--load',help="read intrinsics from file")
#parser.add_argument('--nocalibrate',action="store_true",help="performs only reprojection")
#parser.add_argument('--noextract',action="store_true",help="assumes features already computed (using yaml files and not the images)")
parser.add_argument('--debug',
help="debug dir for chessboard markers",
default="")
parser.add_argument('--pattern_size',
default=(6, 9),
help="pattern as (w,h)",
type=lambda s: coords(s, 'Pattern', int))
parser.add_argument('--square_size2',
default=(0, 0),
help="alt square size",
type=lambda s: coords(s, 'Square Size', float))
parser.add_argument('--grid_offset',
default=(0, 0),
help="grid offset",
type=lambda s: coords(s, 'Square Size', float))
parser.add_argument('--target_size',
default=None,
help="target image as (w,h) pixels",
type=lambda s: coords(s, 'Target Image', int),
nargs=2)
parser.add_argument('--aperture',
default=None,
help="sensor size in m as (w,h)",
type=lambda s: coords(s, 'Aperture', float),
nargs=2)
parser.add_argument('--square_size',
help='square size in m',
type=float,
default=0.025)
parser.add_argument('--nodistortion', action="store_true")
parser.add_argument('--outputpath', help="path for output yaml files")
parser.add_argument('--nocalibrate', action="store_true")
args = parser.parse_args()
# From documentation http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
# C default of cvFindChessboardCorners is ADAPT+NORM
# Python default of cvFindChessboardCorners is ADAPT
if args.ir:
eflags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
else:
eflags = cv2.CALIB_CB_ADAPTIVE_THRESH
#eflags += cv2.CALIB_CB_FAST_CHECK #+ cv2.CV_CALIB_CB_FILTER_QUADS
#CV_CALIB_CB_FILTER_QUADS
if False:
if args.intrinsics != None:
# load yaml
pass
if args.nocalibrate:
pass
if args.noextract:
pass
if args.calib:
calib = loadcalib(args.calib)
else:
calib = dict(camera_matrix=None, dist=None)
img_names = []
for p in args.path:
img_names.extend(glob(p))
debug_dir = args.debug
square_size = args.square_size
print("square_size is", square_size)
pattern_size_cols_rows = (args.pattern_size[0], args.pattern_size[1])
pattern_points = np.zeros((np.prod(pattern_size_cols_rows), 3), np.float32)
pattern_points[:, :2] = np.indices(pattern_size_cols_rows).T.reshape(-1, 2)
if args.flipy:
for i in range(0, pattern_points.shape[0]):
pattern_points[i,
1] = args.pattern_size[1] - pattern_points[i, 1] - 1
if args.flipx:
for i in range(0, pattern_points.shape[0]):
pattern_points[i,
0] = args.pattern_size[0] - pattern_points[i, 0] - 1
# Non square patterns, broadcast product making a non-square grid
if args.square_size2[0] != 0:
pattern_points *= np.array(
[args.square_size2[0], args.square_size2[1], 0.0])
else:
pattern_points *= args.square_size
if args.grid_offset[0] != 0 or args.grid_offset[1] != 0:
pattern_points[:, 0] += args.grid_offset[0]
pattern_points[:, 1] += args.grid_offset[1]
obj_points = []
img_points = []
yaml_done = []
target = args.target_size
h, w = 0, 0
lastsize = None
criteriasub = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30,
0.001)
criteriacal = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 120,
0.001)
#giacomo
#for both sub and cal cv::TermCriteria term_criteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON);
print("images", img_names)
j = 0
img_namesr = []
for fn in (sorted(img_names)):
if os.path.isdir(fn):
for y in os.listdir(fn):
if y.endswith(".jpg") or y.endswith(".png"):
img_namesr.append(os.path.join(fn, y))
else:
img_namesr.append(fn)
img_names = img_namesr
for fn in (sorted(img_names)):
if fn.endswith(".yaml"):
continue
j = j + 1
print(fn, 'processing', j)
img = cv2.imread(fn, -1)
if img is None:
print(fn, "failed to load")
continue
h, w = img.shape[:2]
if args.side == "left":
if len(img.shape) == 3:
img = img[0:h, 0:w / 2, :]
else:
img = img[0:h, 0:w / 2]
w = w / 2
elif args.side == "right":
if len(img.shape) == 3:
img = img[0:h, w / 2:, :]
else:
img = img[0:h, w / 2:]
w = w / 2
if target is not None and (h, w) != target:
print(fn, (h, w), "->", target)
img = cv2.resize(img, target)
h, w = target
else:
if lastsize is None:
lastsize = (h, w)
print("using", (h, w))
else:
if lastsize != (h, w):
print(fn, "all images should be the same size, enforcing")
target = lastsize
img = cv2.resize(img, target)
h, w = target
if len(img.shape) == 3 and img.shape[2] == 3:
img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
else:
pattern_size_cols_rows
if args.threshold > 0:
retval, img = cv2.threshold(img, args.threshold, 255,
cv2.THRESH_BINARY)
print("thresholded ", img.shape, gray.dtype)
cv2.imshow("ciao", img)
cv2.waitKey(0)
#255-gray if we flipped it
found, corners = cv2.findChessboardCorners(img,
pattern_size_cols_rows,
flags=eflags)
if found:
# Giacomo (11,11)
cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), criteriasub)
if not found:
print(fn, 'chessboard not found')
continue
if args.outputpath:
yamlfile = os.path.join(
args.outputpath,
os.path.splitext(os.path.split(fn)[1])[0] + ".yaml")
else:
yamlfile = os.path.splitext(fn)[0] + ".yaml"
info = dict(width=w,
height=h,
image_points=corners.reshape(-1, 2).tolist(),
world_points=pattern_points.tolist())
yaml.dump(info, open(yamlfile, "wb"))
print("\tgenerated yaml")
img_points.append(corners.reshape(-1, 2))
obj_points.append(pattern_points)
yaml_done.append(yamlfile)
if debug_dir is not None:
vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
cv2.drawChessboardCorners(vis, pattern_size_cols_rows, corners,
found)
path, name = os.path.split(fn)
name, ext = os.path.splitext(name)
dd = '%s/%s_chess.png' % (debug_dir, name)
cv2.imwrite(dd, vis)
print("\twriting debug", dd)
if not args.nocalibrate:
#CV_CALIB_USE_INTRINSIC_GUESS
if len(obj_points) == 0:
print("cannot find corners")
return
flags = 0
if args.nodistortion:
flags = cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5 | cv2.CALIB_FIX_K6 | cv2.CALIB_ZERO_TANGENT_DIST
if args.calib:
flags = flags | cv2.CV_CALIB_USE_INTRINSIC_GUESS
print("calibrating...", len(img_points), "images")
rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(
obj_points,
img_points, (w, h),
calib["camera_matrix"],
calib["dist"],
criteria=criteriacal,
flags=flags)
print("error:", rms)
print("camera matrix:\n", camera_matrix)
print("distortion coefficients:", dist_coefs.transpose())
#cv2.destroyAllWindows()
#apertureWidth
#apertureHeight
if args.aperture:
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
camera_matrix, (w, h), args.aperture[0], args.aperture[1])
outname = args.save
if outname is not None:
ci = dict(image_width=w,
image_height=h,
pattern_size=list(pattern_size_cols_rows),
rms=rms,
camera_matrix=camera_matrix.tolist(),
dist=dist_coefs.ravel().tolist(),
square_size=square_size)
print(ci)
yaml.dump(ci, open(outname, "wb"))
for i, y in enumerate(yaml_done):
o = yaml.load(open(y, "rb"))
o["rms"] = float(
recaberror(img_points[i], obj_points[i], rvecs[i], tvecs[i],
camera_matrix, dist_coefs))
o["rvec"] = rvecs[i].tolist()
o["tvec"] = tvecs[i].tolist()
yaml.dump(o, open(y, "wb"))
elif calib["camera_matrix"] is not None:
for i, y in enumerate(yaml_done):
retval, rvec, tvec = cv2.solvePnP(obj_points[i], img_points[i],
calib["camera_matrix"],
calib["dist"])
o = yaml.load(open(y, "rb"))
o["rms"] = float(
recaberror(img_points[i], obj_points[i], rvec, tvec,
calib["camera_matrix"], calib["dist"]))
o["rvec"] = rvec.tolist()
o["tvec"] = tvec.tolist()
yaml.dump(o, open(y, "wb"))
def calculate_camera_fov(mtx, image_size):
fov_x, fov_y, _, _, _ = cv2.calibrationMatrixValues(
mtx, image_size, 1, 1)
return fov_x, fov_y
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]))
(chessboard_shape[0] - 1)),
corners2, ret)
cv2.imshow('calib', img)
cv2.waitKey(500)
# out.write(img)
cv2.destroyAllWindows()
#out.release()
# Calibration
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
mtx, gray.shape[::-1], 1.0, 1.0)
# Reprojection error
tot_error = 0
for i, obj_pts in enumerate(objpoints):
imgpoints2, _ = cv2.projectPoints(obj_pts, rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
tot_error += error
print(error)
print('total error: ' + str(tot_error / len(objpoints)))
#####
#https://gist.github.com/aarmea/629e59ac7b640a60340145809b1c9013
# Stereo calibration
import src_stereo_calib.stereo_calib
# fx and fy are the focal lengths expressed in pixel units.
fx = fy = 1.0
# (cx,cy) is a principal point that is usually at the image center.
cx = cy = 0.0
cameraMatrix = np.array([[fx, 0, cx], [0, fy, cy], [0, 0, 1]])
ret, frame = cap.read()
[rows, cols, channels] = frame.shape
aperture = _DEFAULT_SENSOR_SIZE
# print('aperture_width:',aperture[0])
# print('aperture_height:',aperture[1])
fov_x, fov_y, focal_len, principal, aspect = \
cv2.calibrationMatrixValues(cameraMatrix, (cols,rows),
aperture[0], aperture[1])
print('fov_x:',fov_x)
print('fov_y:',fov_y)
print('focal length',focal_len)
print('principal:',principal)
print('aspect:',aspect)
# while(True):
# # Capture frame-by-frame
# ret, frame = cap.read()
#
# # Our operations on the frame come here
# # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
#
def cook(scriptOp):
scriptOp.clear()
objp = scriptOp.inputs[0]
imgp = scriptOp.inputs[1]
if objp.numRows > 5 and imgp.numRows > 5:
geoPoints = [(float(objp[x + 1, 'tx'].val), float(objp[x + 1,
'ty'].val),
float(objp[x + 1, 'tz'].val))
for x in range(objp.numRows - 1)]
imgPoints = [(float(imgp[x + 1, 'x'].val), float(imgp[x + 1, 'y'].val))
for x in range(imgp.numRows - 1)]
# get image size
pWidth = parent().par.w
pHeight = parent().par.h
# get fov
fov = parent.camSchnappr.par.Fov.eval()
# near and far
near = parent.camSchnappr.par.Near.eval()
far = parent.camSchnappr.par.Far.eval()
# get flags
intrinsic = parent.camSchnappr.par.Intrinsic.eval()
fixaspect = parent.camSchnappr.par.Aspect.eval()
zeroTangent = parent.camSchnappr.par.Tangent.eval()
fixPrincipal = parent.camSchnappr.par.Principal.eval()
fixK1 = parent.camSchnappr.par.Fixk1.eval()
fixK2 = parent.camSchnappr.par.Fixk2.eval()
fixK3 = parent.camSchnappr.par.Fixk3.eval()
# get criteria params
maxIter = parent.camSchnappr.par.Maxiterations.eval()
precision = parent.camSchnappr.par.Precision.eval()
#############################################
# do with cv2.calibrateCamera
#############################################
# build input for calibrateCamera
size = (pWidth, pHeight)
cvGeoPoints = np.array([geoPoints], np.float32)
cvImgPoints = np.array([imgPoints], np.float32)
# set camera matrix
f = pWidth * (fov / 180 * 3.14)
camMtx = [[f, 0, pWidth / 2], [0, f, pHeight / 2], [0, 0, 1]]
camMtx = np.matrix(camMtx, np.float32)
# set empty distortion vector
dist = np.zeros((5, ), np.float32)
# set flags
flags = 0
if intrinsic:
flags += cv2.CALIB_USE_INTRINSIC_GUESS
if fixaspect:
flags += cv2.CALIB_FIX_ASPECT_RATIO
if zeroTangent:
flags += cv2.CALIB_ZERO_TANGENT_DIST
if fixPrincipal:
flags += cv2.CALIB_FIX_PRINCIPAL_POINT
if fixK1:
flags += cv2.CALIB_FIX_K1
if fixK2:
flags += cv2.CALIB_FIX_K2
if fixK3:
flags += cv2.CALIB_FIX_K3
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
maxIter, precision)
# run calibration
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(cvGeoPoints,
cvImgPoints,
size,
camMtx,
dist,
None,
None,
flags=flags,
criteria=criteria)
# get rotation matrix
rot, jacob = cv2.Rodrigues(rvecs[0], None)
# fill rotation table
cvRotate = op('cvRotate')
for i, v in enumerate(rot):
for j, rV in enumerate(v):
cvRotate[i, j] = rV
# fill translate vector
cvTranslate = op('cvTranslate')
for i, v in enumerate(tvecs[0]):
cvTranslate[0, i] = v[0]
# break appart camera matrix
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
mtx, size, pWidth, pHeight)
# fill values table
cvValues = op('cvValues')
cvValues['focalX', 1] = focalLength
cvValues['focalY', 1] = focalLength
cvValues['principalX', 1] = principalPoint[0]
cvValues['principalY', 1] = principalPoint[1]
cvValues['width', 1] = pWidth
cvValues['height', 1] = pHeight
l = near * (-principalPoint[0]) / focalLength
r = near * (pWidth - principalPoint[0]) / focalLength
b = near * (principalPoint[1] - pHeight) / focalLength
t = near * (principalPoint[1]) / focalLength
A = (r + l) / (r - l)
B = (t + b) / (t - b)
C = (far + near) / (near - far)
D = (2 * far * near) / (near - far)
nrl = (2 * near) / (r - l)
ntb = (2 * near) / (t - b)
projMat = tdu.Matrix([nrl, 0, 0, 0], [0, ntb, 0, 0], [A, B, C, -1],
[0, 0, D, 0])
scriptOp.appendRow(projMat)
# transform Matrix
transMat = tdu.Matrix(
[rot[0][0], rot[0][1], rot[0][2], tvecs[0][0]],
[-rot[1][0], -rot[1][1], -rot[1][2], -tvecs[0][1]],
[-rot[2][0], -rot[2][1], -rot[2][2], -tvecs[0][2]], [0, 0, 0, 1])
scriptOp.appendRow(transMat)
return
# calibrationMatrixValues
# Parameters:
# cameraMatrix – Input camera matrix that can be estimated by calibrateCamera() or stereoCalibrate() .
# imageSize – Input image size in pixels.
# apertureWidth – Physical width of the sensor.
# apertureHeight – Physical height of the sensor.
# fovx – Output field of view in degrees along the horizontal sensor axis.
# fovy – Output field of view in degrees along the vertical sensor axis.
# focalLength – Focal length of the lens in mm.
# principalPoint – Principal point in pixels.
# aspectRatio – fy/fx
# <codecell>
# useful camera characteristics from the camera matrix
cv2.calibrationMatrixValues(cameraMatrix = cameramatrix, imageSize = (image_width, image_height),
apertureWidth = , apertureHeight = )
# <codecell>
# initUndistortRectifyMap(cameraMatrix, distCoeffs, R, newCameraMatrix, size, m1type[, map1[, map2]]) -> map1, map2
a_deg = 0 # angle in degrees
a = a_deg * np.pi / 180.0
R = np.float64([[np.cos(a), np.sin(a), 0],
[-np.sin(a), np.cos(a), 0],
[0, 0, 1]])
map1, map2 = cv2.initUndistortRectifyMap(cameraMatrix = cameramatrix, distCoeffs = dist_coefs,
R = R, newCameraMatrix = cameramatrix,
size = (1 * image_width, 1 * image_height), m1type = cv2.CV_32FC1)
def __init__(self, markerImage='marker.jpg', calib_file='test.npz'):
ShowBase.__init__(self)
base.disableMouse()
self.marker = cv2.imread(markerImage)
self.marker = cv2.flip(self.marker, 0)
self.kp_marker, self.des_marker = getDes(self.marker)
if useCamera:
self.cap = cv2.VideoCapture(0)
ret, frame = self.cap.read()
else:
ret, frame = True, cv2.imread("sample_0.jpg")
if ret:
self.frame = frame
self.criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER,
30, 0.001)
with np.load(calib_file) as calib_File:
self.K = calib_File['mtx']
self.D = calib_File['coef']
(h, w) = frame.shape[0:2]
print w, h
far = 100
near = 0.1
fovx, fovy, f, (cx, cy), a = cv2.calibrationMatrixValues(
self.K, (w, h), w, h)
print fovx, fovy, f, cx, cy
base.camLens.setFilmSize(w, h)
base.camLens.setFilmOffset(w * 0.5 - cx, h * 0.5 - cy)
base.camLens.setFocalLength(f)
base.camLens.setFov(fovx, fovy)
base.camLens.setNearFar(near, far)
#base.camLens.setCoordinateSystem(4)
base.camLens.setCoordinateSystem(4)
#base.camLens.setViewVector(Vec3(0,0,1), Vec3(0,1,0))
#self.render.setAttrib(CullFaceAttrib.make(CullFaceAttrib.MCullCounterClockwise))
self.tex = Texture("detect") #self.buff.getTexture()
self.tex.setCompression(Texture.CMOff)
self.tex.setup2dTexture(w, h, Texture.TUnsignedByte, Texture.FRgb)
self.b = OnscreenImage(parent=render2d, image=self.tex)
base.cam.node().getDisplayRegion(0).setSort(20)
self.taskMgr.add(self.updateFrameTask, "UpdateCameraFrameTask")
self.modelroot = NodePath('ARRootNode')
self.modelroot.reparentTo(self.render)
'''
self.x = self.loader.loadModel("models/box")
self.x.reparentTo(self.modelroot)
self.x.setScale(3, 0.1, 0.1)
self.x.setPos(0, -0.05, -0.05)
self.x.setColor(1,0,0,1,1)
self.y = self.loader.loadModel("models/box")
self.y.reparentTo(self.modelroot)
self.y.setScale(0.1, 3, 0.1)
self.y.setPos(-0.05, 0, -0.05)
self.y.setColor(0,1,0,1,1)
self.z = self.loader.loadModel("models/box")
self.z.reparentTo(self.modelroot)
self.z.setScale(0.1, 0.1, 3)
self.z.setPos(-0.05, -0.05, 0)
self.z.setColor(0,0,1,1,1)
'''
self.panda = NodePath('PandaRoot')
self.panda.reparentTo(self.modelroot)
# Load and transform the panda actor.
self.pandaActor = Actor("models/panda-model",
{"walk": "models/panda-walk4"})
self.pandaActor.setScale(0.003, 0.003, 0.003)
self.pandaActor.reparentTo(self.panda)
self.pandaActor.loop("walk")
self.pandaActor.setH(180)
#self.pandaMotion = MopathInterval("Panda Path", self.panda, "Interval Name")
self.pathCurve = createNurbsCurve()
for i in range(0, 30):
self.pathCurve.addPoint(
(random.uniform(1, 7), random.uniform(1, 7), 0))
'''
self.pathCurve.addPoint((1, 5, 0))
self.pathCurve.addPoint((5, 5, 0))
self.pathCurve.addPoint((5, 1, 0))
self.pathCurve.addPoint((1, 1, 0))
'''
curveNode = self.pathCurve.getNodepath()
self.myMopath = Mopath()
self.myMopath.loadNodePath(curveNode)
self.myMopath.fFaceForward = True
myInterval = MopathInterval(self.myMopath,
self.panda,
duration=100,
name="Name")
myInterval.loop()
cv2.TERM_CRITERIA_MAX_ITER, # termination criteria type
100, # max number of iterations
0.001) # min accuracy
rms, _, _, _, _ = fe.calibrate(objpoints, imgpoints, img.shape[::-1], K, D,
rvecs, tvecs, calibrationFlags,
calibrationCriteria)
# Estos son los valores de la PTZ - Averiguar fe
apertureWidth = 4.8 #[mm]
apertureHeight = 3.6
# fovx: Output field of view in degrees along the horizontal sensor axis
# fovy: Output field of view in degrees along the vertical sensor axis
# focalLength: Focal length of the lens in mm
# principalPoint: Principal point in mm
# aspectRatio: fy/fx
fovx,fovy,focalLength,principalPoint,aspectRatio = \
cv2.calibrationMatrixValues(K,
img.shape[::-1], # imageSize in pixels
apertureWidth, # Physical width in mm of the sensor
apertureHeight # Physical height in mm of the sensor
)
print fovx
print fovy
print focalLength
print principalPoint
print aspectRatio
# Compute matrices
_found_all, _corners = cv2.findChessboardCorners(
im3,
chessboard_dim,
flags=cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS)
cv2.drawChessboardCorners(im3, chessboard_dim, _corners, _found_all)
#~ _found_all, _corners = cv2.findChessboardCorners(im3, chessboard_dim, flags=cv.CV_CALIB_CB_ADAPTIVE_THRESH | cv.CV_CALIB_CB_FILTER_QUADS)
#~ #cv2.drawChessboardCorners(im3, chessboard_dim, _corners, _found_all)
#~ retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera([objectPoints.astype('float32')], [_corners.astype('float32')], im3.shape, np.eye(3), np.zeros((5, 1)))
#~ fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(cameraMatrix, im3.shape, 1.0, 1.0)
imageSize = (im3.shape[0], im3.shape[1])
retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(
[objectPoints.astype('float32')], [_corners.astype('float32')], imageSize,
np.eye(3), np.zeros((5, 1)))
#~ _corners.astype('float32'),
#~ imageSize=im3.shape)
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
cameraMatrix, imageSize, 1.0, 1.0)
print fovx, fovy, focalLength, principalPoint, aspectRatio
#~ print cameraMatrix
cv2.imshow('orig', im3)
cv2.waitKey(0)
cv2.destroyAllWindows()
if args.temp:
new_path = os.path.join(args.temp, os.path.basename(path))
cv2.imwrite(new_path, image)
print '---'
rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, image.shape[:2], None, None)
print "RMS:", rms
print "camera matrix:\n", camera_matrix
print "distortion coefficients: ", dist_coefs.ravel()
print "rvecs: ", rvecs
print "tvecs: ", tvecs
print '---'
fovx, fovy, focal_length, principal_point, aspect_ratio = cv2.calibrationMatrixValues(camera_matrix, image.shape[:2], 23.9, 35.8)
print 'fovx:', fovx
print 'fovy:', fovy
print 'focal_length:', focal_length
print 'principal_point:', principal_point
print 'aspect_ratio:', aspect_ratio
if args.temp:
for image_i, path in enumerate(args.image):
image = cv2.imread(path)
w, h = image.shape[:2]
image = cv2.resize(image, (1024, int(1024 * w / h)))
image = cv2.undistort(image, camera_matrix, dist_coefs)
new_path = os.path.join(args.temp, os.path.basename(path) + '-square.jpg')
# -- Undistort an image -- #
img_g = cv.imread(img_list[0], cv.IMREAD_GRAYSCALE)
img_ud = cv.undistort(img_g, camera_matrix, dist_coefs, None, camera_matrix)
os.chdir(root)
save_params(f"{img_dir}.npy", rms, camera_matrix, dist_coefs, rvecs, tvecs)
# with open('calib_L.npy', 'wb') as f:
# np.savez(f, rms=rms, camera_matrix=camera_matrix, dist_coefs=dist_coefs, rvecs=rvecs, tvecs=tvecs)
my_cam = load_params(f"{img_dir}.npy")
print(my_cam.camera_matrix)
# print('\n\n')
# with open('calib_L.npy', 'rb') as f:
# myfile = np.load(f)
# a,b,c,d,e = myfile['rms'],myfile['camera_matrix'],myfile['dist_coefs'],myfile['rvecs'],myfile['tvecs']
fovx, fovy, focal_length, principal_point, aspect_ratio = cv.calibrationMatrixValues(camera_matrix, (w,h), c.sensor_size[0], c.
c.sensor_size[1])
# print(f"fovx: {fovx}\nfovy: {fovy}\nfocal length: {focal_length}\nprincipal point: {principal_point}\naspect ratio: {aspect_ratio}")
plt.subplot(121)
plt.title('Original')
plt.imshow(img_g, cmap='gray')
plt.axis('off')
plt.subplot(122)
plt.title('Undistorted')
plt.imshow(img_ud, cmap='gray')
plt.axis('off')
plt.show()
def CalibrateProjector(self):
"""
this function should calibrate the projector given the known points based on the cameras calibration. Then it does a stereo calibration between the camera and the projector together.
"""
print('projector calibration')
projRez = (1920, 1080)
objectPointsAccum = parent().fetch('3dCirclesFound')
circleDat = op('null_circle_centers')
projCirclePoints = np.zeros((circleDat.numRows, 2), np.float32)
for rr in range(0, circleDat.numRows):
projCirclePoints[rr] = (float(circleDat[rr,
0]), float(circleDat[rr,
1]))
projCirclePoints = projCirclePoints.astype('float32')
# objectPointsAccum = objectPointsAccum.astype('float32')
objectPointsAccum = np.asarray(objectPointsAccum, dtype=np.float32)
# print(objectPointsAccum)
print(len(objectPointsAccum))
# print(projCirclePoints)
projCirlcleList = []
for ix in range(0, len(objectPointsAccum)):
projCirlcleList.append(projCirclePoints)
# This can be omitted and can be substituted with None below.
K_proj = cv2.initCameraMatrix2D(objectPointsAccum, projCirlcleList,
projRez)
#K_proj = None
dist_coef_proj = None
flags = 0
#flags |= cv2.CALIB_FIX_INTRINSIC
flags |= cv2.CALIB_USE_INTRINSIC_GUESS
#flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
#flags |= cv2.CALIB_FIX_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_ASPECT_RATIO
# flags |= cv2.CALIB_ZERO_TANGENT_DIST
# flags |= cv2.CALIB_SAME_FOCAL_LENGTH
# flags |= cv2.CALIB_RATIONAL_MODEL
# flags |= cv2.CALIB_FIX_K3
# flags |= cv2.CALIB_FIX_K4
# flags |= cv2.CALIB_FIX_K5
# the actual function that figures out the projectors projection matrix
ret, K_proj, dist_coef_proj, rvecs, tvecs = cv2.calibrateCamera(
objectPointsAccum,
projCirlcleList,
projRez,
K_proj,
dist_coef_proj,
flags=flags)
print("proj calib mat after\n%s" % K_proj)
print("proj dist_coef %s" % dist_coef_proj.T)
print("calibration reproj err %s" % ret)
cameraCirclePoints = parent().fetch('2dCirclesFound')
camera_intrinsics = parent().fetch('camera_intrinsics')
K = camera_intrinsics['K']
dist_coef = camera_intrinsics['dist_coeff']
# rvecs=camera_intrinsics['rvecs']
# tvecs=camera_intrinsics['tvecs']
print("stereo calibration")
ret, K, dist_coef, K_proj, dist_coef_proj, proj_R, proj_T, _, _ = cv2.stereoCalibrate(
objectPointsAccum,
cameraCirclePoints,
projCirlcleList,
K,
dist_coef,
K_proj,
dist_coef_proj,
projRez,
flags=cv2.CALIB_USE_INTRINSIC_GUESS)
proj_rvec, _ = cv2.Rodrigues(proj_R)
print("R \n%s" % proj_R)
print("T %s" % proj_T.T)
print("proj calib mat after\n%s" % K_proj)
print("proj dist_coef %s" % dist_coef_proj.T)
print("cam calib mat after\n%s" % K)
print("cam dist_coef %s" % dist_coef.T)
print("reproj err %f" % ret)
parent().store(
'proj_intrinsics', {
'ret': ret,
'K': K_proj,
'dist_coeff': dist_coef_proj,
'rvecs': rvecs,
'tvecs': tvecs
})
camera_intrinsics['K'] = K
camera_intrinsics['dist_coeff'] = dist_coef
#### below are two tests to try and get pose information back into touchdesigner camera components
matrix_comp = op('base_camera_matrix')
# Fill rotation table
cv_rotate_table = matrix_comp.op('cv_rotate')
for i, v in enumerate(proj_R):
for j, rv in enumerate(v):
cv_rotate_table[i, j] = rv
# Fill translate vector
cv_translate_table = matrix_comp.op('cv_translate')
for i, v in enumerate(proj_T.T):
cv_translate_table[0, i] = v[0]
# Break appart camera matrix
size = projRez
# Computes useful camera characteristics from the camera matrix.
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
K_proj, size, 1920, 1080)
near = .1
far = 2000
####
INTRINSIC = op('INTRINSIC')
INTRINSIC.clear()
INTRINSIC.appendRow([
'K', 'dist', 'fovx', 'fovy', 'focal', 'image_width', 'image_height'
])
INTRINSIC.appendRow([
K_proj.tolist(),
dist_coef_proj.tolist(),
fovx,
fovy,
focalLength,
1920,
1080,
])
# Fill values table
cv_values = matrix_comp.op('cv_values')
cv_values['focalX', 1] = focalLength
cv_values['focalY', 1] = focalLength
cv_values['principalX', 1] = principalPoint[0]
cv_values['principalY', 1] = principalPoint[1]
cv_values['width', 1] = 1920
cv_values['height', 1] = 1080
l = near * (-principalPoint[0]) / focalLength
r = near * (1920 - principalPoint[0]) / focalLength
b = near * (principalPoint[1] - 1080) / focalLength
t = near * (principalPoint[1]) / focalLength
A = (r + l) / (r - l)
B = (t + b) / (t - b)
C = (far + near) / (near - far)
D = (2 * far * near) / (near - far)
nrl = (2 * near) / (r - l)
ntb = (2 * near) / (t - b)
table = matrix_comp.op('table_camera_matrices')
proj_mat = tdu.Matrix([nrl, 0, 0, 0], [0, ntb, 0, 0], [A, B, C, -1],
[0, 0, D, 0])
# Transformation matrix
tran_mat = tdu.Matrix(
[proj_R[0][0], proj_R[0][1], proj_R[0][2], proj_T[0]],
[-proj_R[1][0], -proj_R[1][1], -proj_R[1][2], -proj_T[1]],
[-proj_R[2][0], -proj_R[2][1], -proj_R[2][2], -proj_T[2]],
[0, 0, 0, 1])
matrix_comp.op('table_camera_matrices').clear()
matrix_comp.op('table_camera_matrices').appendRow(proj_mat)
matrix_comp.op('table_camera_matrices').appendRow(tran_mat)
new_path = os.path.join(args.temp, os.path.basename(path))
cv2.imwrite(new_path, image)
print '---'
rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(
obj_points, img_points, image.shape[:2], None, None)
print "RMS:", rms
print "camera matrix:\n", camera_matrix
print "distortion coefficients: ", dist_coefs.ravel()
print "rvecs: ", rvecs
print "tvecs: ", tvecs
print '---'
fovx, fovy, focal_length, principal_point, aspect_ratio = cv2.calibrationMatrixValues(
camera_matrix, image.shape[:2], 23.9, 35.8)
print 'fovx:', fovx
print 'fovy:', fovy
print 'focal_length:', focal_length
print 'principal_point:', principal_point
print 'aspect_ratio:', aspect_ratio
if args.temp:
for image_i, path in enumerate(args.image):
image = cv2.imread(path)
w, h = image.shape[:2]
image = cv2.resize(image, (1024, int(1024 * w / h)))
image = cv2.undistort(image, camera_matrix, dist_coefs)
new_path = os.path.join(args.temp,
os.path.basename(path) + '-square.jpg')
def cal_with_chessboard(self):
# 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((9 * 6, 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.
self.objpoints = [] # 3d point in real world space
self.imgpoints = [] # 2d points in image plane.
images = glob.glob('*.png')
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 found, add object points, image points (after refining them)
if ret == True:
self.objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1),
criteria)
self.imgpoints.append(corners2)
# Draw and display the corners
cv2.drawChessboardCorners(img, (9, 6), corners2, ret)
cv2.imshow('img', img)
cv2.waitKey(500)
cv2.destroyAllWindows()
##calibration
self.ret, self.mtx, self.dist, self.rvecs, self.tvecs = cv2.calibrateCamera(
self.objpoints, self.imgpoints, gray.shape[::-1], None, None)
# print("camera matrix: ", self.mtx)
# print("k1, k2, p1, p2, k3 = ", self.dist) # dist = distortion coefficients (k1, k2, p1, p2, k3)
# Computes useful camera characteristics from the camera matrix
self.apertureWidth = 4.2
self.apertureHeight = 2.4 # Sensor Size: 4.2 mm x 2.4 mm (for daA1920-30uc)
self.fovx, self.fovy, self.focalLength, self.principalPoint, self.aspectRatio = cv2.calibrationMatrixValues(
self.mtx, gray.shape[::-1], self.apertureWidth,
self.apertureHeight)
# print ("fovx(degree): ", self.fovx)
# print ("fovy(degree): ", self.fovy)
# print ("focal length(mm): ", self.focalLength)
# print ("principal point(mm): ", self.principalPoint)
# print ("aspect ratio(fy/fx): ", self.aspectRatio)
return self.mtx, self.dist
def calibrate_extrinsic(intr_cam_calib_path, extr_img_path, extr_pts_path):
img = cv2.imread(extr_img_path, cv2.IMREAD_UNCHANGED)
camera_matrix, dist_coeffs, w, h = smocap.camera.load_intrinsics(
intr_cam_calib_path)
new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(
camera_matrix, dist_coeffs, (w, h), 1, (w, h))
img_undistorted = cv2.undistort(img, camera_matrix, dist_coeffs, None,
new_camera_matrix)
cv2.imwrite('/tmp/foo.png', img_undistorted)
pts_name, pts_img, pts_world = trrvu.read_point(extr_pts_path)
#pdb.set_trace()
pts_img_undistorted = cv2.undistortPoints(pts_img.reshape(
(-1, 1, 2)), camera_matrix, dist_coeffs, None,
new_camera_matrix).squeeze()
(success, rotation_vector,
translation_vector) = cv2.solvePnP(pts_world,
pts_img.reshape(-1, 1, 2),
camera_matrix,
dist_coeffs,
flags=cv2.SOLVEPNP_ITERATIVE)
LOG.info("PnP {} {} {}".format(success, rotation_vector.squeeze(),
translation_vector.squeeze()))
rep_pts_img = cv2.projectPoints(pts_world, rotation_vector,
translation_vector, camera_matrix,
dist_coeffs)[0].squeeze()
rep_err = np.mean(np.linalg.norm(pts_img - rep_pts_img, axis=1))
LOG.info('reprojection error {} px'.format(rep_err))
world_to_camo_T = smocap.utils.T_of_t_r(translation_vector.squeeze(),
rotation_vector)
world_to_camo_t, world_to_camo_q = smocap.utils.tq_of_T(world_to_camo_T)
print(' world_to_camo_t {} world_to_camo_q {}'.format(
world_to_camo_t, world_to_camo_q))
camo_to_world_T = np.linalg.inv(world_to_camo_T)
camo_to_world_t, camo_to_world_q = smocap.utils.tq_of_T(camo_to_world_T)
print(' cam_to_world_t {} cam_to_world_q {}'.format(
camo_to_world_t, camo_to_world_q))
T_c2co = np.array([[0., -1., 0., 0], [0., 0., -1., 0], [1., 0., 0., 0],
[0., 0., 0., 1.]])
T_co2c = np.linalg.inv(T_c2co)
caml_to_ref_T = np.dot(camo_to_world_T, T_c2co)
caml_to_ref_t, caml_to_ref_q = smocap.utils.tq_of_T(caml_to_ref_T)
print(' caml_to_ref_t {} caml_to_ref_q {}'.format(caml_to_ref_t,
caml_to_ref_q))
caml_to_ref_rpy = np.asarray(
tf.transformations.euler_from_matrix(caml_to_ref_T, 'sxyz'))
print(' caml_to_ref_rpy {}'.format(caml_to_ref_rpy))
apertureWidth, apertureHeight = 6.784, 5.427
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
camera_matrix, (h, w), apertureWidth, apertureHeight)
print(fovx, fovy, focalLength, principalPoint, aspectRatio)
def draw(cam_img, pts_id, keypoints_img, rep_keypoints_img, pts_world):
for i, p in enumerate(keypoints_img.astype(int)):
cv2.circle(cam_img, tuple(p), 1, (0, 255, 0), -1)
cv2.putText(cam_img, '{}'.format(pts_id[i][:1]), tuple(p),
cv2.FONT_HERSHEY_SIMPLEX, 1.5, (0, 255, 0), 2)
cv2.putText(cam_img, '{}'.format(pts_world[i][:2]),
tuple(p + [0, 25]), cv2.FONT_HERSHEY_SIMPLEX, 0.7,
(0, 255, 0), 1)
for i, p in enumerate(rep_keypoints_img.astype(int)):
cv2.circle(cam_img, tuple(p), 1, (0, 0, 255), -1)
draw(img, pts_name, pts_img, rep_pts_img, pts_world)
cv2.imshow('original image', img)
for i, p in enumerate(pts_img_undistorted.astype(int)):
cv2.circle(img_undistorted, tuple(p), 1, (0, 255, 0), -1)
cv2.imshow('undistorted image', img_undistorted)
cv2.waitKey(0)
cv2.destroyAllWindows()
RDK.ShowMessage("Failed to process calibration file")
quit()
CAMERA_MTX = cv_file.getNode("camera_matrix").mat()
DIST_COEFFS = cv_file.getNode("dist_coeff").mat()
CALIB_SIZE = cv_file.getNode("camera_size").mat().astype(int)
cv_file.release()
# If the calibrated camera resolution and the current resolution differs, approximate the camera matrix
c_width, c_height = CALIB_SIZE
if (width, height) != (c_width, c_height):
RDK.ShowMessage("The calibrated resolution and the current resolution differs. Approximated calibration matrix will be used.")
CAMERA_MTX[0][0] = (width / c_width) * CAMERA_MTX[0][0] # fx' = (dimx' / dimx) * fx
CAMERA_MTX[1][1] = (height / c_height) * CAMERA_MTX[1][1] # fy' = (dimy' / dimy) * fy
# Assuming an aperture of 2 mm, update if needed
fovx, fovy, focal_length, p_point, ratio = cv.calibrationMatrixValues(CAMERA_MTX, (width, height), CAMERA_APERTURE, CAMERA_APERTURE)
#----------------------------------------------
# Close camera windows, if any
RDK.Cam2D_Close(0)
# Create the charuco frame for the pose estimation
board_frame_name = 'ChArUco Frame'
board_ref = RDK.Item(board_frame_name, ITEM_TYPE_FRAME)
if not board_ref.Valid():
board_ref = RDK.AddFrame(board_frame_name)
# Create a frame on which to attach the camera
cam_frame_name = 'Camera Frame'
cam_ref = RDK.Item(cam_frame_name, ITEM_TYPE_FRAME)
if not cam_ref.Valid():
def main():
# ファイル名宣言
jsonName = "calibrateParameter.json"
# パターン宣言
square_size = 23.0 # パターン1マスの1辺サイズ[mm]
pattern_size = (10, 7) # パターンの行列数
# prepare object points
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
object_points = [] # 3D point in real world spece
image_points = [] # 2D point in image plane
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
apertureWidth = 0 # 物理センサの幅[mm]
apertureHeight = 0 # 物理センサの高さ[mm]
for filename in glob("*.jpg"):
# 画像の取得
image = cv2.imread(filename, 0)
print "loading : " + filename
# チェスボードのコーナーを検出
found, corners = cv2.findChessboardCorners(image, pattern_size)
# コーナーを検出した場合
if found:
print "Success : chessboard found"
cv2.cornerSubPix(image, corners, (11, 11), (-1, -1), criteria)
# コーナを検出できない場合
if not found:
print "Fail : chessboard not found"
continue
image_points.append(corners.reshape(-1, 2))
object_points.append(pattern_points)
# 内部パラメータ計算
retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(object_points, image_points, image.shape[::-1], None, None)
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(cameraMatrix, image.shape[::-1], apertureWidth, apertureHeight)
# 計算結果を表示
print "RMS = ", retval
print "cameraMatrix = \n", cameraMatrix
print "distCoeffs = ", distCoeffs
print "fovx = ", fovx
print "fovy = ", fovy
print "focalLength = ", focalLength
print "principalPoint = ", principalPoint
print "aspectRatio = ", aspectRatio
# JSONの作成
jsonObject = {}
dic_retval = {"retval": retval}
dic_cameraMatrix = {"CameraMatrix": cameraMatrix.tolist()}
dic_distCoeffs = {"DistortCoeffs": distCoeffs.tolist()}
dic_fovx = {"fov_x": fovx}
dic_fovy = {"fov_y": fovy}
dic_focalLength = {"focal_Length": focalLength}
dic_principalPoint = {"principal_Point": principalPoint}
dic_aspectRatio = {"aspect_Ratio": aspectRatio}
jsonObject.update(dic_retval)
jsonObject.update(dic_cameraMatrix)
jsonObject.update(dic_distCoeffs)
jsonObject.update(dic_fovx)
jsonObject.update(dic_fovy)
jsonObject.update(dic_focalLength)
jsonObject.update(dic_principalPoint)
jsonObject.update(dic_aspectRatio)
outfile = open(jsonName, "w")
json.dump(jsonObject, outfile)
outfile.close()
print len(objpoints), len(imgpoints), gray.shape
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
print 'ret', ret
print 'Camera Matrix', mtx
print 'Distortion Array', dist
print 'Rotation Matrix', rvecs
print 'Translation Matrix', tvecs
apertureSize = 1
apertureHeight = 1
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
mtx, gray.shape, apertureSize, apertureHeight)
print 'Fovx', fovx
print 'Fovy', fovy
print 'Focal Length', focalLength
print 'Principal point', principalPoint
print 'aspect ratio', aspectRatio
img = cv2.imread('Camera Calibration/Left12.bmp')
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
def task_1():
image = cv2.imread('./../_data/sw_s01e09/img_21130751_0005.bmp')
flag_found, corners = cv2.findChessboardCorners(image, (8, 5))
print(f'len(corners): {len(corners)}')
image_with_corners_raw = cv2.drawChessboardCorners(image, (8, 5), corners,
flag_found)
cv2.imshow('image_with_corners_raw', image_with_corners_raw)
cv2.waitKey(0)
if flag_found:
print(corners[0])
print('Corners found, refining their positions')
corners = cv2.cornerSubPix(
cv2.cvtColor(image,
cv2.COLOR_BGR2GRAY), corners, (11, 11), (-1, -1),
(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.1))
print(corners[0])
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
object_points = np.zeros((8 * 5, 3), np.float32)
print(object_points[1])
print(object_points.shape)
object_points[:, :2] = np.mgrid[0:8, 0:5].T.reshape(-1, 2)
print(object_points[1])
print(object_points.shape)
object_points_for = [] # np.zeros_like(object_points)
for i in range(0, 5):
for j in range(0, 8):
# print(object_points_for[i, j])
# object_points_for[i, j] = [i, j, 0]
object_points_for.append([j, i, 0])
object_points_for = np.array(object_points_for, dtype=np.float32)
print(f'(object_points[0:3]: {object_points[0:3]}')
print(object_points.shape)
print(f'object_points_for[0:3]: {object_points_for[0:3]}')
print(object_points_for.shape)
image_points = [corners] # [corners1, corners2, corners3]
object_points = [object_points
] # [object_points1, object_points2, object_points3]
retval, camera_matrix, dist_coeffs, rvecs, tvecs = cv2.calibrateCamera(
object_points, image_points, image.shape[:2], None, None)
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(
camera_matrix, image.shape[:2], 7.2, 5.4)
print(fovx)
print(fovy)
print(focalLength)
img_undistorted = cv2.undistort(image, camera_matrix, dist_coeffs)
cv2.imshow('img_undistorted', img_undistorted)
cv2.waitKey(0)
else:
print('Corners not found')
image_with_corners = cv2.drawChessboardCorners(image, (8, 5), corners,
flag_found)
cv2.imshow('image_with_corners', image_with_corners)
cv2.waitKey(0)
def main(argv):
print(argv[1])
folder_path = "calibration/"
if ((int(argv[1]) == 0) or (int(argv[1]) == 2)):
pipe_name = "image_pipe"
if not os.path.exists(pipe_name):
os.mkfifo(pipe_name)
pipein = os.open(pipe_name, os.O_RDONLY)
frame_id = 11
while (1):
data = read_from_pipe(pipein)
if data is not None:
cv_image = np.reshape(data, (height, width))
cv2.imshow("Calibration", cv_image)
c = cv2.waitKey(10)
if 'c' == chr(c & 255):
img_name = folder_path + "{}.jpg".format(frame_id)
frame_id = frame_id + 1
cv2.imwrite(img_name, cv_image)
if 'q' == chr(c & 255):
break
os.close(pipein)
if ((int(argv[1]) == 1) or (int(argv[1]) == 2)):
files = []
for r, d, f in os.walk(folder_path):
for file in f:
if '.jpg' in file:
files.append(os.path.join(r, file))
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
objp = np.zeros((4*6,3), np.float32)
objp[:,:2] = np.mgrid[0:6,0:4].T.reshape(-1,2)
objpoints = []
imgpoints = []
for f in files:
img = cv2.imread(f)
gray = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (6,4),cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE)
if ret == True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
imgpoints.append(corners2)
img = cv2.drawChessboardCorners(img, (6,4), corners2,ret)
cv2.imshow('img',img)
cv2.waitKey(50)
cv2.destroyAllWindows()
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
h, w = img.shape[:2]
fovx, fovy, focalLength, principalPoint, aspectRatio = cv2.calibrationMatrixValues(mtx, (w, h), 0.0036, 0.0036)
print("himax fovx {} fovy {}".format(fovx, fovy))
cv_file = cv2.FileStorage("calibration.yaml", cv2.FILE_STORAGE_WRITE)
cv_file.write("k", mtx)
cv_file.write("D", dist)
cv_file.write("size", np.array([h, w]))
cv_file.release()
def main():
parser = argparse.ArgumentParser(description='Camera Calibrator - OpenCV and Emanuele Ruffaldi SSSA 2014-2015')
parser.add_argument('path', help='path where images can be found (png or jpg)',nargs="+")
parser.add_argument('--save', help='name of output calibration in YAML otherwise prints on console')
parser.add_argument('--verbose',action="store_true")
parser.add_argument('--ir',action='store_true')
parser.add_argument('--threshold',type=int,default=0)
parser.add_argument('--calib',type=str,help="default calib for guess or nocalibrate mode")
parser.add_argument('--flipy',action='store_true')
parser.add_argument('--flipx',action='store_true')
parser.add_argument('--side',help="side: all,left,right",default="all")
#parser.add_argument('--load',help="read intrinsics from file")
#parser.add_argument('--nocalibrate',action="store_true",help="performs only reprojection")
#parser.add_argument('--noextract',action="store_true",help="assumes features already computed (using yaml files and not the images)")
parser.add_argument('--debug',help="debug dir for chessboard markers",default="")
parser.add_argument('--pattern_size',default=(6,9),help="pattern as (w,h)",type=lambda s: coords(s,'Pattern',int))
parser.add_argument('--square_size2',default=(0,0),help="alt square size",type=lambda s: coords(s,'Square Size',float))
parser.add_argument('--grid_offset',default=(0,0),help="grid offset",type=lambda s: coords(s,'Square Size',float))
parser.add_argument('--target_size',default=None,help="target image as (w,h) pixels",type=lambda s: coords(s,'Target Image',int), nargs=2)
parser.add_argument('--aperture',default=None,help="sensor size in m as (w,h)",type=lambda s: coords(s,'Aperture',float), nargs=2)
parser.add_argument('--square_size',help='square size in m',type=float,default=0.025)
parser.add_argument('--nodistortion',action="store_true");
parser.add_argument('--outputpath',help="path for output yaml files");
parser.add_argument('--nocalibrate',action="store_true")
args = parser.parse_args()
# From documentation http://docs.opencv.org/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html
# C default of cvFindChessboardCorners is ADAPT+NORM
# Python default of cvFindChessboardCorners is ADAPT
if args.ir:
eflags = cv2.CALIB_CB_ADAPTIVE_THRESH + cv2.CALIB_CB_NORMALIZE_IMAGE
else:
eflags = cv2.CALIB_CB_ADAPTIVE_THRESH
#eflags += cv2.CALIB_CB_FAST_CHECK #+ cv2.CV_CALIB_CB_FILTER_QUADS
#CV_CALIB_CB_FILTER_QUADS
if False:
if args.intrinsics != None:
# load yaml
pass
if args.nocalibrate:
pass
if args.noextract:
pass
if args.calib:
calib = loadcalib(args.calib)
else:
calib = dict(camera_matrix=None,dist=None)
img_names = []
for p in args.path:
img_names.extend(glob(p))
debug_dir = args.debug
square_size = args.square_size
print "square_size is",square_size
pattern_size_cols_rows = (args.pattern_size[0],args.pattern_size[1])
pattern_points = np.zeros( (np.prod(pattern_size_cols_rows), 3), np.float32 )
pattern_points[:,:2] = np.indices(pattern_size_cols_rows).T.reshape(-1, 2)
if args.flipy:
for i in range(0,pattern_points.shape[0]):
pattern_points[i,1] = args.pattern_size[1] - pattern_points[i,1] - 1
if args.flipx:
for i in range(0,pattern_points.shape[0]):
pattern_points[i,0] = args.pattern_size[0] - pattern_points[i,0] - 1
# Non square patterns, broadcast product making a non-square grid
if args.square_size2[0] != 0:
pattern_points *= np.array([args.square_size2[0],args.square_size2[1],0.0])
else:
pattern_points *= args.square_size
if args.grid_offset[0] != 0 or args.grid_offset[1] != 0:
pattern_points[:,0] += args.grid_offset[0]
pattern_points[:,1] += args.grid_offset[1]
obj_points = []
img_points = []
yaml_done = []
target = args.target_size
h, w = 0, 0
lastsize = None
criteriasub = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
criteriacal = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 120, 0.001)
#giacomo
#for both sub and cal cv::TermCriteria term_criteria(cv::TermCriteria::COUNT + cv::TermCriteria::EPS, 50, DBL_EPSILON);
print "images",img_names
j = 0
img_namesr = []
for fn in (sorted(img_names)):
if os.path.isdir(fn):
for y in os.listdir(fn):
if y.endswith(".jpg") or y.endswith(".png"):
img_namesr.append(os.path.join(fn,y))
else:
img_namesr.append(fn)
img_names = img_namesr
for fn in (sorted(img_names)):
if fn.endswith(".yaml"):
continue
j = j +1
print fn,'processing',j
img = cv2.imread(fn,-1)
if img is None:
print fn,"failed to load"
continue
h, w = img.shape[:2]
if args.side == "left":
if len(img.shape)== 3:
img = img[0:h,0:w/2,:]
else:
img = img[0:h,0:w/2]
w = w/2
elif args.side == "right":
if len(img.shape)== 3:
img = img[0:h,w/2:,:]
else:
img = img[0:h,w/2:]
w = w/2
if target is not None and (h,w) != target:
print fn, (h,w),"->",target
img = cv2.resize(img,target)
h,w = target
else:
if lastsize is None:
lastsize = (h,w)
print "using",(h,w)
else:
if lastsize != (h,w):
print fn, "all images should be the same size, enforcing"
target = lastsize
img = cv2.resize(img,target)
h,w = target
if len(img.shape) == 3 and img.shape[2] == 3:
img = cv2.cvtColor(img,cv2.COLOR_BGR2GRAY)
else:
pattern_size_cols_rows
if args.threshold > 0:
retval,img = cv2.threshold(img, args.threshold, 255, cv2.THRESH_BINARY);
print "thresholded ",img.shape,gray.dtype
cv2.imshow("ciao",img)
cv2.waitKey(0)
#255-gray if we flipped it
found, corners = cv2.findChessboardCorners(img, pattern_size_cols_rows,flags=eflags)
if found:
# Giacomo (11,11)
cv2.cornerSubPix(img, corners, (5, 5), (-1, -1), criteriasub)
if not found:
print fn,'chessboard not found'
continue
if args.outputpath:
yamlfile = os.path.join(args.outputpath,s.path.splitext(os.path.split(fn)[1])[0]+".yaml")
else:
yamlfile = os.path.splitext(fn)[0]+".yaml"
info = dict(width=w,height=h,image_points=corners.reshape(-1,2).tolist(),world_points=pattern_points.tolist())
yaml.dump(info,open(yamlfile,"wb"))
print "\tgenerated yaml"
img_points.append(corners.reshape(-1, 2))
obj_points.append(pattern_points)
yaml_done.append(yamlfile)
if debug_dir is not None:
vis = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR)
cv2.drawChessboardCorners(vis, pattern_size_cols_rows, corners, found)
path,name = os.path.split(fn)
name,ext = os.path.splitext(name)
dd = '%s/%s_chess.png' % (debug_dir, name)
cv2.imwrite(dd, vis)
print "\twriting debug",dd
if not args.nocalibrate:
#CV_CALIB_USE_INTRINSIC_GUESS
if len(obj_points) == 0:
print "cannot find corners"
return
flags = 0
if args.nodistortion:
flags = cv2.CALIB_FIX_K1 | cv2.CALIB_FIX_K2 | cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5 | cv2.CALIB_FIX_K6 | cv2.CALIB_ZERO_TANGENT_DIST
if args.calib:
flags = flags | cv2.CV_CALIB_USE_INTRINSIC_GUESS
print "calibrating...",len(img_points),"images"
rms, camera_matrix, dist_coefs, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, (w, h), calib["camera_matrix"], calib["dist"],criteria=criteriacal,flags=flags)
print "error:", rms
print "camera matrix:\n", camera_matrix
print "distortion coefficients:", dist_coefs.transpose()
#cv2.destroyAllWindows()
#apertureWidth
#apertureHeight
if args.aperture:
fovx,fovy,focalLength,principalPoint,aspectRatio = cv2.calibrationMatrixValues(camera_matrix,(w,h),args.aperture[0],args.aperture[1])
outname = args.save
if outname is not None:
ci = dict(image_width=w,image_height=h,pattern_size=list(pattern_size_cols_rows),rms=rms,camera_matrix=camera_matrix.tolist(),dist=dist_coefs.ravel().tolist(),square_size=square_size)
print ci
yaml.dump(ci,open(outname,"wb"))
for i,y in enumerate(yaml_done):
o = yaml.load(open(y,"rb"))
o["rms"] = float(recaberror(img_points[i],obj_points[i],rvecs[i],tvecs[i],camera_matrix,dist_coefs))
o["rvec"] = rvecs[i].tolist()
o["tvec"] = tvecs[i].tolist()
yaml.dump(o,open(y,"wb"))
elif calib["camera_matrix"] is not None:
for i,y in enumerate(yaml_done):
retval,rvec,tvec = cv2.solvePnP(obj_points[i], img_points[i], calib["camera_matrix"],calib["dist"])
o = yaml.load(open(y,"rb"))
o["rms"] = float(recaberror(img_points[i],obj_points[i],rvec,tvec,calib["camera_matrix"],calib["dist"]))
o["rvec"] = rvec.tolist()
o["tvec"] = tvec.tolist()
yaml.dump(o,open(y,"wb"))
