def findM(self, img_names, side):
'''Determine the Camera Matrix and update the
StereoCam's values for the camera matrix, M
@img_names = individual camera calibration images
@side = specify which camera (1 = left, 2 = right)
'''
# Specify the calibration checkerboard square and pattern size
pattern_size = (9, 6)
square_size = 1.0
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
# locate the object and image points of the checker board in the
# calibration images
obj_points = []
img_points = []
h, w = 0, 0
for fn in img_names:
img1 = cv2.imread(fn)
img = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
if img is None:
print "Failed to load", fn
continue
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 not found:
print 'chessboard not found'
continue
img_points.append(corners.reshape(-1, 2))
obj_points.append(pattern_points)
# Calibrate the camera
rms, camera_matrix, dist_coefs, rvecs, tvecs = \
cv2.calibrateCamera(obj_points, img_points, (w, h), None, None)
# undistort images
K = camera_matrix
d = np.array([dist_coefs[0][0], dist_coefs[0][1], 0, 0, 0])
img = cv2.imread('09152015/left/l1.JPG')
h, w = img.shape[:2]
# Update the StereoCam camera matrix with the optimal camera matrix
if side == 1:
self.M1, _ = cv2.getOptimalNewCameraMatrix(K, d, (w, h), 0)
if side == 2:
self.M2, _ = cv2.getOptimalNewCameraMatrix(K, d, (w, h), 0)
def get_view_matrix(self, alpha):
"""
Returns camera matrix for handling image and coordinates distortion and undistortion. Based on alpha,
up to all pixels of the distorted image can be visible in the undistorted image.
:param alpha: Free scaling parameter between 0 (when all the pixels in the undistorted image are valid) and 1
(when all the source image pixels are retained in the undistorted image). For convenience for -1
returns custom camera matrix self.Kundistortion and None returns self.K.
:type alpha: float or None
:return: camera matrix for a view defined by alpha
:rtype: array, shape=(3, 3)
"""
if alpha == -1:
Kundistortion = self.Kundistortion
elif alpha is None:
Kundistortion = self.K
elif self.calibration_type == 'opencv':
Kundistortion, _ = cv2.getOptimalNewCameraMatrix(self.K, self.opencv_dist_coeff, tuple(self.size_px), alpha)
elif self.calibration_type == 'opencv_fisheye':
Kundistortion = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(self.K, self.opencv_dist_coeff,
tuple(self.size_px), self.R,
balance=alpha)
else:
# TODO
assert False, 'not implemented'
return Kundistortion
def recent_events(self, events):
frame = events.get('frame')
if not frame:
return
if self.collect_new:
img = frame.img
status, grid_points = cv2.findCirclesGrid(img, (4,11), flags=cv2.CALIB_CB_ASYMMETRIC_GRID)
if status:
self.img_points.append(grid_points)
self.obj_points.append(self.obj_grid)
self.collect_new = False
self.count -=1
self.button.status_text = "{:d} to go".format(self.count)
if self.count<=0 and not self.calculated:
self.calculate()
self.button.status_text = ''
if self.window_should_close:
self.close_window()
if self.show_undistortion:
adjusted_k,roi = cv2.getOptimalNewCameraMatrix(cameraMatrix= self.camera_intrinsics[0], distCoeffs=self.camera_intrinsics[1], imageSize=self.camera_intrinsics[2], alpha=0.5,newImgSize=self.camera_intrinsics[2],centerPrincipalPoint=1)
self.undist_img = cv2.undistort(frame.img, self.camera_intrinsics[0], self.camera_intrinsics[1],newCameraMatrix=adjusted_k)
def correctDistortion(image):
size = image.shape[1],image.shape[0]
corners = findCorners(image)
patternPoints = np.zeros( (np.prod(boardSize), 3), np.float32 )
patternPoints[:,:2] = np.indices(boardSize).T.reshape(-1, 2)
imagePoints = np.array([corners.reshape(-1, 2)])
objectPoints = np.array([patternPoints])
cameraMatrix = np.zeros((3, 3))
distCoefs = np.zeros(4)
rc,cameraMatrix,distCoeffs,rvecs,tvecs = cv2.calibrateCamera(
objectPoints,
imagePoints,
boardSize,
cameraMatrix,
distCoefs
)
newCameraMatrix,newExtents = cv2.getOptimalNewCameraMatrix(cameraMatrix,distCoeffs,size,0.0)
mapx, mapy = cv2.initUndistortRectifyMap(
cameraMatrix,
distCoeffs,
None,
cameraMatrix,
size,
cv2.CV_32FC1
)
newImage = cv2.remap( image, mapx, mapy, cv2.INTER_LANCZOS4 )
return newImage
def undistort_image(img, cameraMatrix, distCoeffs, imageSize):
"""
Undistort image "img",
shot with a camera with intrinsics ("cameraMatrix", "distCoeffs", and "imageSize" (w, h)).
Apart from the undistorted image, a region-of-interest will also be returned.
See OpenCV's doc about the format of "cameraMatrix" and "distCoeffs".
"""
# Refine cameraMatrix, and calculate RegionOfInterest
cameraMatrix_new, roi = cv2.getOptimalNewCameraMatrix(
cameraMatrix, distCoeffs, imageSize,
1 ) # all source image pixels retained in undistorted image
# Undistort
mapX, mapY = cv2.initUndistortRectifyMap(
cameraMatrix, distCoeffs,
None, # optional rectification transformation
cameraMatrix_new, imageSize,
5 ) # type of the first output map (CV_32FC1)
img_undistorted = cv2.remap(
img, mapX, mapY, cv2.INTER_LINEAR )
# Crop the image
x,y, w,h = roi
img_undistorted = img_undistorted[y:y+h, x:x+w]
return img_undistorted, roi
def calibrate(self,images):
# 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.
w,h=0,0
for fname in images:
gray = cv2.imread(fname,0)
ret,objpoints,imgpoints = self.findMarkers(gray,objpoints,imgpoints)
h,w=gray.shape[:2]
#print len(objpoints),len(imgpoints),w,h
rms, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, (w,h), None, None)
print 25*'-'
print "RMS:", rms
print "camera matrix:\n", mtx
print "distortion coefficients: ", dist.ravel()
print 25*'-'
# not sure of the value of this
alpha = 0.5
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),alpha)
# newcameramtx = 0
# roi = 0
self.data = {'camera_matrix': mtx, 'dist_coeff': dist, 'newcameramtx': newcameramtx, 'rms': rms, 'rvecs': rvecs, 'tvecs': tvecs}
def calibrate(filenames):
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((6*7,3), np.float32)
objp[:,:2] = np.mgrid[0:7,0:6].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = []
for filename in filenames:
# Find the chess board corners. If found, add object points, image points (after refining them)
img = cv2.imread(filename)
if img != None:
print "Loaded " + repr(filename)
else:
print "Unable to load image " + repr(filename)
continue
images.append(img)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (7,6), None)
if ret == True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
imgpoints.append(corners2)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None,None)
print "Loaded all images and calbulated calibration"
for i, img in enumerate(images):
img = images[i]
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w,h), 1, (w,h))
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
x, y, w, h = roi
cv2.imwrite( 'calibrated/out_' + str(i) + '.png', dst[ y : y+h, x : x+w ])
print "Outputted calibrated image: 'calibrated/out_" + str(i) + ".png'"
def warpImage(self):
# Undistort the frame
#img = cv2.cvtColor(self.image, cv2.COLOR_BGR2GRAY)
img = self.image
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(self.cameraMatrix, self.distCoefs, (w, h), 1, (w, h))
dst = cv2.undistort(img, self.cameraMatrix, self.distCoefs, None, newcameramtx)
self.image = dst
print "roi:", roi
# crop the image
x, y, w, h = roi
if w > 0 and h > 0:
dst = dst[y:y+h, x:x+w]
self.image = dst
self.height = h
self.width = w
#self.image = cv2.cvtColor(dst, cv2.COLOR_GRAY2BGR)
# if self.image is not None:
# self.height, self.width, self.depth = self.image.shape
# Warp the image to be the optimal size
# warpedimage = zeros((self.warpheight, self.warpwidth, 3), uint8)
# dsize = (self.warpwidth, self.warpheight)
# cv2.warpPerspective(self.image, self.M, dsize, dst=warpedimage, borderMode=cv2.BORDER_TRANSPARENT)
# self.image = warpedimage
# self.height,self.width,self.depth = self.image.shape
# self.warped = True
return
def __init__(self, fname, log, F, dist, pitch):
"""
Creates a new MonoRectify object to read video from video file fname
using attitude data from LogReader log, using camera matrix F and
distortion coefficients dist. The camera is assumed to be pitched down
by pitch degrees.
"""
self.log = log
self.F = F
self.dist = dist
self.fname = fname
self.pitch = pitch
# Open video for reading
self.cap = cv2.VideoCapture(fname)
assert(self.cap.isOpened())
self.fps = self.cap.get(cv2.cv.CV_CAP_PROP_FPS)
self.w = int(self.cap.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH))
self.h = int(self.cap.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT))
# Get new camera matrix for post-distortion removal transformations
self.newF, self.roi=cv2.getOptimalNewCameraMatrix(self.F, self.dist,
(self.w,self.h),0)
print self.F
print self.newF
#self.newF[0,0] = self.newF[0,0]*.9
#self.newF[1,1] = self.newF[1,1]*.9
print self.newF
def main(argv): camera = np.load(argv[0]) images_path = argv[1] output_dir = argv[2] if not os.path.exists(output_dir): os.makedirs(output_dir) images = glob.glob(images_path + '/*.jpg') for fname in images: img = cv2.imread(fname) h, w = img.shape[:2] newcameramtx, roi = cv2.getOptimalNewCameraMatrix( camera['mtx'], camera['dist'], (w,h), 0, # 0.6 (w,h) ) mapx,mapy = cv2.initUndistortRectifyMap( camera['mtx'], camera['dist'], None, newcameramtx, (w,h), 5 ) dst = cv2.remap(img,mapx,mapy,cv2.INTER_LINEAR) cv2.imwrite(output_dir + '/' + os.path.basename(fname), dst)
def __init__(self, args):
self.outputChannel = args.output_channel
self.videoCapture = self.setup(args)
self.frame = None
self.lcmobj = lcm.LCM()
self.saveImages = args.save_images
# Get all calibration files and use the latest to get the data
# returned by internal calibration
# NOTE: See drivers/general_camera_tests/test_calibration.py for
# explanations about these variables
calibrationResults = pickle.load(
open(navigation.getLatestIntrinsicCalibration(), "rb"))
self.retval = calibrationResults["retval"]
self.matrix = calibrationResults["matrix"]
self.distCoeffs = calibrationResults["distCoeffs"]
self.rvecs = calibrationResults["rvecs"]
self.tvecs = calibrationResults["tvecs"]
self.newCameraMatrix, self.roi = cv2.getOptimalNewCameraMatrix(
cameraMatrix=self.matrix,
distCoeffs=self.distCoeffs,
imageSize=(int(self.width), int(self.height)),
alpha=1,
newImgSize=(int(self.width), int(self.height))
)
# Overwrite the camera's width and height with the ROI width/height,
# b/c that will be the final output
self.width = self.roi[2]
self.height = self.roi[3]
self.requestSub = self.lcmobj.subscribe(args.request_channel,
self.handleRequest)
def undistort(mtx, dist, objpoints, imgpoints, rvecs, tvecs, img):
#read in an image of choice
#usefule for the reading in and segmenting of board to make hough lines easier
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('calibresult.png', dst)
# undistort
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('calibresult.png', dst)
mean_error = 0
for i in xrange(len(objpoints)):
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
mean_error += error
print "total error: ", mean_error / len(objpoints)
def get_transform():
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
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 = []
w,h = 0,0
for fname in os.listdir('chessboard'):
if fname[0] == '.': continue
img = cv2.imread('chessboard/' + fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
h, w = img.shape[:2]
found, corners = cv2.findChessboardCorners(gray, pattern_size)
print found
if found:
corners2 = cv2.cornerSubPix(gray,corners,(11,11),(-1,-1),criteria)
img_points.append(corners.reshape(-1, 2))
obj_points.append(pattern_points)
vis = cv2.cvtColor(gray, cv2.COLOR_GRAY2BGR)
cv2.drawChessboardCorners(vis, pattern_size, corners, found)
# cv2.imshow('img', vis)
# cv2.waitKey(500)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(obj_points, img_points, (w,h), None, None)
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),1,(w,h))
return mtx, roi, newcameramtx, dist
def _cleanImage(image): h, w = image.shape[:2] newcameramtx, roi=cv2.getOptimalNewCameraMatrix(camera_matrix,dist_coefs,(w,h),1,(w,h)) dst = cv2.undistort(image, camera_matrix, dist_coefs, None, newcameramtx) x,y,w,h = roi dst = dst[y:y+h, x:x+w] return dst
def main(file_list, outdir):
# copy parameters to arrays
K = np.array([[1743.23312, 0, 2071.06177], [0, 1741.57626, 1476.48298], [0, 0, 1]])
d = np.array([-0.307412748, 0.300929683, 0, 0, 0]) # just use first two terms (no translation)
logging.debug("Starting loop")
for image in file_list:
logging.debug("var %s", image)
imgname = image.split("/")[-1]
new_image_path = os.path.join(outdir, imgname)
if not os.path.exists(new_image_path):
logging.debug("Undistorting %s . . . ", imgname)
# read one of your images
img = cv2.imread(image)
h, w = img.shape[:2]
# un-distort
newcamera, roi = cv2.getOptimalNewCameraMatrix(K, d, (w, h), 0)
newimg = cv2.undistort(img, K, d, None, newcamera)
# cv2.imwrite("original.jpg", img)
cv2.imwrite(new_image_path, newimg)
# Write metadata
old_meta = pyexiv2.ImageMetadata(image)
new_meta = pyexiv2.ImageMetadata(new_image_path)
old_meta.read()
new_meta.read()
old_meta.copy(new_meta)
new_meta.write()
else:
logging.debug("Image already processed")
def undistort(path, imagesArr, K, d):
print '\n-------- Undistort Images ---------'
for fname in imagesArr:
print 'Undistorting', os.path.basename(fname),
img = cv2.imread(fname)
if img is None:
print ' - Failed to load.'
continue
h, w = img.shape[:2]
# Calculate new optimal matrix to avoid losing pixels in the edges after undistortion
new_matrix, roi = cv2.getOptimalNewCameraMatrix(K, d, (w, h), 1)
# Generate undistorted image
#newimg = cv2.undistort(img, K, d, None, new_matrix)
# Alternative undistortion via remapping
mapx, mapy = cv2.initUndistortRectifyMap(K, d, None, new_matrix, (w, h), cv2.CV_32FC1)
newimg = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# Output undistorted image to the same location with postfix '_undistorted'
f = os.path.basename(fname).split('.')
newimg_path = os.path.join(path, ".".join(f[:-1]) + '_undistorted.' + f[-1])
cv2.imwrite(newimg_path, newimg)
print '----->', newimg_path
print 'Undistorted', len(imagesArr), 'images.'
print '-------- End of Undistortion ---------\n'
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 dewarp(imagedir):
# Loading from json file
C = CameraParams.fromfile(os.path.join(imagedir, "params.json"))
K = C.K
D = C.D
print("Loaded camera parameters from " + os.path.join(imagedir, "params.json"))
for f in file_list(imagedir, ['jpg', 'jpeg', 'png']):
print(f)
colour = cv2.imread(f)
grey = cv2.cvtColor(colour, cv2.COLOR_BGR2GRAY)
h, w = grey.shape[:2]
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(K, D, (w,h), 1, (w,h))
mapx, mapy = cv2.initUndistortRectifyMap(K, D, None, newcameramtx, (w,h), 5)
dewarped = cv2.remap(grey, mapx, mapy, cv2.INTER_LINEAR)
x, y, w, h = roi
dewarped = dewarped[y:y+h, x:x+w]
grey = cv2.resize(grey, (0,0), fx=0.5, fy=0.5)
dewarped = cv2.resize(dewarped, (0,0), fx=0.5, fy=0.5)
cv2.imshow("Original", grey )
cv2.imshow("Dewarped", dewarped)
cv2.waitKey(-1)
def getImage():
# initialize the camera and grab a reference to the raw camera capture
camera = PiCamera()
camera.resolution = (640, 480)
camera.framerate = 32
rawCapture = PiRGBArray(camera, size=(640, 480))
# print("setting is end!")
# allow the camera to warmup
# time.sleep(0.1)
dst = None
# capture frames from the camera
for frame in camera.capture_continuous(rawCapture, format="bgr", use_video_port=True):
# grab the raw NumPy array representing the image, then initialize the timestamp
# and occupied/unoccupied text
image = frame.array
mtx =np.array([[100, 0, 320], [0, 100, 240], [0, 0, 1]], dtype=np.float32)
dist = np.array([-0.009, 0.0, 0.0, 0.0], dtype=np.float32)
h, w = image.shape[:2]
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(mtx,dist,(w,h),1,(w,h))
# undistort
dst = cv2.undistort(image, mtx, dist, None, newcameramtx)
break
return dst
def update(self, queue):
self.camera = PiCamera()
self.image = None
self.camera.resolution = (w, h)
self.camera.framerate = 60
self.camera.brightness = 70
self.camera.contrast = 100
self.rawCapture = PiRGBArray(self.camera, size=(w, h))
time.sleep(0.1)
mtx = np.matrix([[313.1251541, 0., 157.36763381],
[0., 311.84837219, 130.36209271],
[0., 0., 1.]])
dist = np.matrix([[-0.42159111, 0.44966352, -0.00877638, 0.00070653, -0.43508731]])
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 0, (w, h))
self.mapx, self.mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
self.stream = self.camera.capture_continuous(self.rawCapture, format="bgr", use_video_port=True)
self.stopped = False
for f in self.stream:
self.image = cv2.remap(f.array, self.mapx, self.mapy, cv2.INTER_LINEAR)
self.rawCapture.truncate(0)
if not q.full():
queue.put(self.image, False)
if self.stopped:
self.stream.close()
self.rawCapture.close()
self.camera.close()
return
def optimiseMatrix(self, img):
"""
:param img: The source image to take as reference
"""
h, w = img.shape[:2]
optimalCameraMatrix, roi = cv2.getOptimalNewCameraMatrix(self.cameraMatrix, self.distortionCoeffs, (w,h), 1)
self.optimalCameraMatrix = optimalCameraMatrix
return optimalCameraMatrix
def change_resize_undist_mode(val):
self.resize_distord = val
if val: #find the right value to crop
self.affine_roi()
else: #find the original roi
_, self.roi = cv2.getOptimalNewCameraMatrix(cameraMatrix= self.camera_intrinsics[0], distCoeffs=self.camera_intrinsics[1], imageSize=self.camera_intrinsics[2], alpha=0.7,newImgSize=self.camera_intrinsics[2],centerPrincipalPoint=1)
self.roi += 0., 0.
init_prev_img()
def __init__(self, calib_data) :
self.h = calib_data.h
self.w = calib_data.w
self.rms = calib_data.rms
self.camera_matrix = calib_data.camera_matrix
self.dist_coefs = calib_data.dist_coefs
self.newcameramtx, self.roi = cv2.getOptimalNewCameraMatrix(self.camera_matrix, self.dist_coefs, (self.w, self.h), 1, (self.w, self.h))
def refineImage1(img): global newcameramtx1 global roi1 global mtx1 global dist1 global imgShape1 # h, w = img.shape[:2] # zamenjeno jednostavnim imgShape jer je to uvek onstantno newcameramtx1, roi1 = cv2.getOptimalNewCameraMatrix(mtx1, dist1, imgShape1, 1, imgShape1)
def refineImage(img): global newcameramtx global roi global mtx global dist global imgShape # h, w = img.shape[:2] # zamenjeno jednostavnim imgShape jer je to uvek konstantno, samo ga trebas azurirati nakon kalibracije posto se tu menja newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, imgShape, 1, imgShape)
def undistortPoints(self, points):
s = self.img.shape
cam = self.coeffs['cameraMatrix']
d = self.coeffs['distortionCoeffs']
(newCameraMatrix, _) = cv2.getOptimalNewCameraMatrix(cam,
d, (s[1], s[0]), 1,
(s[1], s[0]))
return cv2.undistortPoints(points,cam, d, P=newCameraMatrix)
def undistort(img, mtx, dist):
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
return dst
def main():
# ファイル名宣言
imreadName = "mov0.jpg" # 読み込む画像ファイル名
imwriteName = "image_correction.jpg" # 歪み補正済み画像ファイル名
jsonName = "calibrateParameter.json" # 読み込むJSONファイル名
# パターン宣言
square_size = 23.0 # パターン1マスの1辺サイズ[mm]
pattern_size = (10, 7) # パターンの行列数
# Object Points(ワールド座標系), Image 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)
# 内部パラメータの読み込み
inputfile = open(jsonName, "r")
data = json.load(inputfile)
inputfile.close()
cameraMatrix = np.array(data["CameraMatrix"])
distCoeffs =np.array(data["DistortCoeffs"])
# 画像の取得
image = cv2.imread(imreadName, 0)
# チェスボードのコーナーを検出
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"
return
image_points.append(corners.reshape(-1, 2))
object_points.append(pattern_points)
imagePoints = np.array(image_points)
objectPoints = np.array(object_points)
# solvePnP
retval, rvec, tvec = cv2.solvePnP(objectPoints, imagePoints, cameraMatrix, distCoeffs)
print "retval: ", retval
print "rvec: ", rvec
print "tvec:", tvec
# 歪み補正
newcamera, roi = cv2.getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, image.shape, 0)
image_correction = cv2.undistort(image, cameraMatrix, distCoeffs, None, newcamera)
# 補正した画像の保存
cv2.imwrite(imwriteName, image_correction)
def undistortImage(self, cv_image):
h, w = cv_image.shape[:2]
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(self.camera_matrix,self.distortion,(w,h),1,(w,h))
dst = cv2.undistort(cv_image, self.camera_matrix, self.distortion, None, newcameramtx)
# crop the image, I could get roi from camera intrinsics or from cv2.getOptimalNewCameraMatrix. Think its not needed though?
x,y,w,h = roi
dst = np.array(dst[y:y+h, x:x+w])
return dst
def undistort(image, cameraMatrix, distCoefs):
"""Undistort image"""
h, w = image.shape[:2]
newCameraMtx, roi = cv2.getOptimalNewCameraMatrix(cameraMatrix, distCoefs, (w, h), 1, (w, h))
# Undistort
dst = cv2.undistort(image, cameraMatrix, distCoefs, None, newCameraMtx)
# Crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
return dst
def find_markers(frame):
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# cv2.imwrite(file_out, frame)
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, DICTIONARY, parameters=PARAMETERS)
rvecs, tvecs, _objPoints = cv2.aruco.estimatePoseSingleMarkers(corners, MARKER_EDGE, CAMERA_MATRIX, DIST_COEFFS)
h,w = frame.shape[:2] # frame - original frame captured from camera
newCameraMatrix, roi = cv2.getOptimalNewCameraMatrix(CAMERA_MATRIX, DIST_COEFFS, (w,h), 1, (w,h))
### testing to get the undistorted image
### -> fail
# newcameramtx, roi = cv2.getOptimalNewCameraMatrix(CAMERA_MATRIX, DIST_COEFFS, (w,h), 1, (w,h))
# dst = cv2.undistort(gray, CAMERA_MATRIX, DIST_COEFFS, None, newcameramtx)
# x, y, w, h = roi
# # dst = dst[y:y+h, x:x+w]
# cv2.imwrite(file_out, dst)
result = set()
if ids is None:
return result
for i in range(0, len(ids)):
try:
id = str(ids[i][0])
if id > 20:
return
marker = corners[i]
undistortedMarker = cv2.undistortPoints(marker, CAMERA_MATRIX, DIST_COEFFS, P=newCameraMatrix)
# print(marker)
# print("------------")
# print(undistortedMarker[0][i][0])
# print(tvecs[i][0][0])
# print(undistortedMarker[0][i])
# print(undistortedMarker[0])
x1 = marker[0][0][0]
x2 = marker[0][2][0]
y1 = marker[0][0][1]
y2 = marker[0][2][1]
xCenter = (x1 + x2)/2
yCenter = (y1 + y2)/2
# print("Original %s:" % ids[i][0])
# print(marker)
#
# print('Undistorted %s:' % ids[i][0])
# print(undistortedMarker)
# x = tvecs[i][0][0]
# y = tvecs[i][0][1]
x = xCenter
y = yCenter
bearing = calc_bearing(rvecs[i][0])
result.add((id, x,y,bearing))
except Exception:
traceback.print_exc()
return result
def startCalibration(self):
global mybalance
if flag_fisheye_calibrate == 0:
N_OK = len(self.objpoints)
self.camera_matrix = np.zeros((3, 3))
self.distCoeffs = np.zeros((4, 1))
self.rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
self.tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
self.ret, _, _, _, _ = \
cv2.calibrateCamera(
objectPoints=self.objpoints, imagePoints=self.imgpoints, imageSize=(self.width, self.height),
cameraMatrix=self.camera_matrix, distCoeffs=self.distCoeffs, rvecs=self.rvecs, tvecs=self.tvecs,
flags=(cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW),
criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# check the result of calibration
print('RMS re-projection error is ', self.ret)
print('distort Coeffs is ')
print(self.distCoeffs)
print('camera matrix is ')
print(self.camera_matrix)
# https://docs.opencv.org/3.0-beta/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#getoptimalnewcameramatrix
self.new_camera_matrix, self.roi = cv2.getOptimalNewCameraMatrix(self.camera_matrix,
self.distCoeffs,
(self.width, self.height), 1.0,
(self.width, self.height))
print("self.roi is ", self.roi)
## self.roi or (self.width, self.height) ??
self.map1, self.map2 = cv2.initUndistortRectifyMap(self.camera_matrix, self.distCoeffs, np.eye(3),
self.new_camera_matrix,
(self.width, self.height),
cv2.CV_16SC2)
else:
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(self.objpoints, self.imgpoints, (self.width, self.height), None, None)
# you should write all the cv2.fisheye.CALIB_..3 things .. then it works
# # The result is same with originally works
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_FIX_SKEW,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# No good results at all
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_CHECK_COND,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# No good results at all
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_FIX_INTRINSIC,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# Does not work at all
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None)
# originally works
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# originally works
print('before calibartion, width_train, height_train is ', self.width_train, self.height_train)
N_OK = len(self.objpoints)
self.camera_matrix = np.zeros((3, 3))
self.distCoeffs = np.zeros((4, 1))
self.rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
self.tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
self.ret, _, _, _, _ = cv2.fisheye.calibrate(
objectPoints=self.objpoints,
imagePoints=self.imgpoints,
image_size=(self.width_train, self.height_train),
K=self.camera_matrix,
D=self.distCoeffs,
rvecs=None, # self.rvecs,
tvecs=None, # self.tvecs,
flags=(cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_FIX_SKEW + cv2.fisheye.CALIB_CHECK_COND), # cv2.fisheye.CALIB_FIX_PRINCIPAL_POINT
criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 200, 1e-6))
# check the result of calibration
print('camera matrix is ')
print(self.camera_matrix)
# print('self.rvecs is ')
# print(self.rvecs)
# print('self.tvecs is ')
# print(self.tvecs)
print('distort Coeffs is ')
print(self.distCoeffs)
print('RMS re-projection error is ', self.ret)
print('balance is ', mybalance)
print("calibration complete")
# Save camera calibration information for later
np.savez(save_path,
cam_matrix=cam_matrix,
distortion_coeff=distortion_coeff,
rotation_vecs=rotation_vecs,
translation_vecs=translation_vecs)
# Calculate the re-projection error
mean_error = 0
for i in range(len(obj_points)):
img_points2, _ = cv2.projectPoints(obj_points[i], rotation_vecs[i], translation_vecs[i],
cam_matrix, distortion_coeff)
error = cv2.norm(img_points[i], img_points2, cv2.NORM_L2) / len(img_points2)
mean_error += error
print('Total Error: {:.4f}%'.format((100*mean_error) / len(obj_points)))
img = cv2.imread('images/calibration/leftsample/frame1.png')
h, w = img.shape[:2]
new_cam_matrix, roi = cv2.getOptimalNewCameraMatrix(cam_matrix, distortion_coeff, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, cam_matrix, distortion_coeff, None, new_cam_matrix)
# crop the image
x, y, w, h = roi
dst = dst[y:y+h, x:x+w]
cv2.imwrite('undistorted_left.png', dst)
#cv2.imwrite('{}.corners.jpg'.format(fnameR), imgR)
else:
print("NO: {}".format(fname))
#os.remove(fname)
#os.remove(fnameR)
# sys.exit(0)
# Calibrate individual images.
retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(
objpoints, imgpointsL, grayL.shape[::-1], None, None)
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(
objpoints, imgpointsR, grayR.shape[::-1], None, None)
h, w = grayL.shape[:2]
newcameramtxL, roiL = cv2.getOptimalNewCameraMatrix(mtxL, distL, (w, h), 1,
(w, h))
newcameramtxR, roiR = cv2.getOptimalNewCameraMatrix(mtxR, distR, (w, h), 1,
(w, h))
# Rectify image pair.
img = cv2.imread("img/good/pi2_25_good.jpg")
dst = cv2.undistort(img, mtxL, distL, None, newcameramtxL)
x, y, w, h = roiL
print("{} {} {} {}".format(x, y, w, h))
dst = dst[y:y + h, x:x + w]
cv2.imwrite("img/pi2_25_rectified.jpg", dst)
img = cv2.imread("img/good/pi1_25_good.jpg")
dst = cv2.undistort(img, mtxR, distR, None, newcameramtxR)
print("{} {} {} {}".format(x, y, w, h))
dst = dst[y:y + h, x:x + w]
K = np.load('./camera_params2/K.npy')
dist = np.load('./camera_params2/dist.npy')
#Specify image paths
img_path1 = './construct_samples/9l.jpg'
img_path2 = './construct_samples/9r.jpg'
#Load pictures
img_1 = cv2.imread(img_path1)
img_2 = cv2.imread(img_path2)
#Get height and width. Note: It assumes that both pictures are the same size. They HAVE to be same size and height.
h,w = img_2.shape[:2]
#Get optimal camera matrix for better undistortion
new_camera_matrix, roi = cv2.getOptimalNewCameraMatrix(K,dist,(w,h),1,(w,h))
#Undistort images
img_1_undistorted = cv2.undistort(img_1, K, dist, None, new_camera_matrix)
img_2_undistorted = cv2.undistort(img_2, K, dist, None, new_camera_matrix)
#Downsample each image 3 times (because they're too big)
img_1_downsampled = downsample_image(img_1_undistorted,1)
img_2_downsampled = downsample_image(img_2_undistorted,1)
#cv2.imwrite('undistorted_left.jpg', img_1_downsampled)
#cv2.imwrite('undistorted_right.jpg', img_2_downsampled)
#Set disparity parameters
#Note: disparity range is tuned according to specific parameters obtained through trial and error.
FPS = video.get(cv2.CAP_PROP_FPS)
width = video.get(cv2.CAP_PROP_FRAME_WIDTH)
height = video.get(cv2.CAP_PROP_FRAME_HEIGHT)
size = (int(width), int(height))
total_frames = video.get(cv2.CAP_PROP_FRAME_COUNT)
frame_lapse = (1/FPS)*1000
#Initializes the export video file
codec = cv2.VideoWriter_fourcc(*'DIVX')
video_out = cv2.VideoWriter(str(filename[:-4])+'_undistored.avi', codec, FPS, size, 1)
#Initializes the frame counter
current_frame = 0
start = time.clock()
newMat, ROI = cv2.getOptimalNewCameraMatrix(intrinsic_matrix, distCoeff, size, alpha = crop, centerPrincipalPoint = 1)
mapx, mapy = cv2.initUndistortRectifyMap(intrinsic_matrix, distCoeff, None, newMat, size, m1type = cv2.CV_32FC1)
while current_frame < total_frames:
success, image = video.read()
current_frame = video.get(cv2.CAP_PROP_POS_FRAMES)
dst = cv2.remap(image, mapx, mapy, cv2.INTER_LINEAR)
#dst = cv2.undistort(image, intrinsic_matrix, distCoeff, None)
video_out.write(dst)
video.release()
video_out.release()
duration = (time.clock()-float(start))/60
import cv2
import numpy as np
from new_find import make
# 导入前面fix.py 计算的镜头内外参数
u = np.load('./fix_camera/parameter/u.npy')
v = np.load('./fix_camera/parameter/v.npy')
mtx = np.load('./fix_camera/parameter/mtx.npy')
dist = np.load('./fix_camera/parameter/dist.npy')
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (u, v), 0, (u, v))
#打开摄像机
camera = cv2.VideoCapture(1)
camera.set(3, 1980)
camera.set(4, 1080)
frame_num = 1
x1 = 0
fps = camera.get(cv2.CAP_PROP_FPS)
while True:
(grabbed, frame) = camera.read()
h1, w1 = frame.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (u, v), 0,
(u, v))
if frame_num >= 0:
# 纠正畸变
dst1 = cv2.undistort(frame, mtx, dist, None, newcameramtx)
#dst2 = cv2.undistort(frame, mtx, dist, None, newcameramtx)
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx,
(w1, h1), 5)
dst2 = cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)
def detect_image_orb(img1, mtx, dist):
# Número mínimo de pontos que devem ser encontrados na câmera para determinar se o poster está ali ou não
MIN_MATCHES = 32
# Usar o algoritmo ORB (Oriented FAST and Rotated BRIEF) para detectar características-chave das imagens
# nfeatures indica o número de pontos-chave da imagem do poster que serão usados para caracterizá-lo,
# enquanto scoreType indica o critério de escolha dos pontos
orb = cv2.ORB_create(nfeatures=1000, scoreType=cv2.ORB_FAST_SCORE)
# Usa o detector ORB para identificar os pontos-chave e descritores do poster
kp1, des1 = orb.detectAndCompute(img1, None)
# Webcam
cap = cv2.VideoCapture(0)
# A partir das dimensões do primeiro frame e da matriz de correção, é gerada uma matriz para a câmera e a região de interesse (roi)
_, img = cap.read()
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# mapx, mapy são os fatores de correção que serão aplicados em cada frame para eliminar a distorção
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
while True:
# A cada iteração, captura um frame
_, img2 = cap.read()
# Retorna o frame corrigido a partir dos parâmetros 'mapx' e 'mapy'
corr = cv2.remap(img2, mapx, mapy, cv2.INTER_LINEAR)
# Limita a imagem apenas à região de interesse
x, y, w, h = roi
img2 = corr[y:y + h, x:x + w]
# Usa o detector ORB para identificar os pontos-chave e descritores da imagem da webcam
kp2, des2 = orb.detectAndCompute(img2, None)
# Para comparar as características-chave (vetores "descritors") do poster com as características da imagem da câmera,
# Será usado um algoritmo FLANN (Fast approximate nearest neighbour) para essa comparação
# index_params são parâmetros recomendados pela documentação do OpenCV
index_params = dict(algorithm=6, # Pontos serão associados com Locality Sensitivy Hashing
table_number=6, # O número de tabelas de hashing
key_size=12, # O tamanho da chave nas tabelas
multi_probe_level=2) # O número de níves em que o algoritmo será executado
# A função pede um argumento para parâmetros de pesquisa, que neste caso será deixado em branco
search_params = {}
# Configura o comparador FLANN com os parâmetros acima
flann = cv2.FlannBasedMatcher(index_params, search_params)
# Usa o comparador FLANN para relacionar pontos da câmera com pontos do poster
matches = flann.knnMatch(des1, des2, k=2) # Error on blurred
# Filtra os pontos encontrados usando o teste de razão de Lowe
good_matches = []
matches = [x for x in matches if x and len(x) == 2] # Garantir que só haverão pontos (x,y) no vetor
for m, n in matches:
if m.distance < 0.75 * n.distance:
good_matches.append(m)
# Se o número de pontos encontrados for maior do que o mínimo arbitrado, consideramos que a imagem foi encontrada
if len(good_matches) > MIN_MATCHES:
# Bloco try para detectar erros na transformação de perspectiva
try:
# Extrair os pontos da câmera e da imagem identificados anteriormente
src_pts = np.float32([kp1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2)
dst_pts = np.float32([kp2[m.trainIdx].pt for m in good_matches]).reshape(-1, 1, 2)
# Define a região da imagem identificada na câmera (homografia da imagem original)
M, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
# Com as dimensões da imagem original, define uma matriz de pontos que constituem o contorno dela
h, w = img1.shape
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], [w - 1, 0]]).reshape(-1, 1, 2)
# Converte a matriz do contorno da imagem para a perspectiva da câmera
dst = cv2.perspectiveTransform(pts, M)
# Desenha uma linha em volta da imagem identificada na webcam
img2 = cv2.polylines(img2, [np.int32(dst)], True, 255, 3, cv2.LINE_AA)
# Se ocorrer, imprime o erro e apenas mostra a imagem da câmera
except Exception as e:
print(e)
# Mostra a variável img2
cv2.imshow('Reconhecido', img2)
# Encerra o programa quando aperta a tecla 'q'
if cv2.waitKey(1) & 0xFF == ord('q'):
break
def detect_image_surf(img1, mtx, dist):
# Número mínimo de pontos-chave criados na imagem do poster para caracterizá-lo
minHessian = 500
# Número mínimo de pontos que devem ser encontrados na câmera para determinar se o poster está ali ou não
minMatches = 80
# Usar o algoritmo SURF (Speeded-Up Robust Features) para detectar características-chave das imagens
detector = cv2.xfeatures2d_SURF.create(hessianThreshold=minHessian)
# Usa o detector SURF para identificar os pontos-chave e descritores do poster
keypoints1, descriptors1 = detector.detectAndCompute(img1, None)
# Para comparar as características-chave (vetores "descritors") do poster com as características da imagem da câmera,
# Será usado um algoritmo FLANN (Fast approximate nearest neighbour) para essa comparação
matcher = cv2.DescriptorMatcher_create(cv2.DescriptorMatcher_FLANNBASED)
# Webcam
cap = cv2.VideoCapture(0)
# A partir das dimensões do primeiro frame e da matriz de correção, é gerada uma matriz para a câmera e a região de interesse (roi)
_, img = cap.read()
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# mapx, mapy são os fatores de correção que serão aplicados em cada frame para eliminar a distorção
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h), 5)
while True:
# A cada iteração, captura um frame
_, img2 = cap.read()
# Retorna o frame corrigido a partir dos parâmetros 'mapx' e 'mapy'
img2 = cv2.remap(img2, mapx, mapy, cv2.INTER_LINEAR)
# Limita a imagem apenas à região de interesse
x, y, w, h = roi
img2 = img2[y:y + h, x:x + w]
# Usa o detector SURF para identificar os pontos-chave e descritores da imagem da webcam
keypoints2, descriptors2 = detector.detectAndCompute(img2, None)
# Usa o comparador FLANN para relacionar pontos da câmera com pontos do poster
knn_matches = matcher.knnMatch(descriptors1, descriptors2, 2)
# Filtra os pontos encontrados usando o teste de razão de Lowe
good_matches = []
for m, n in knn_matches:
if m.distance < 0.7 * n.distance:
good_matches.append(m)
# Se o número de pontos encontrados for maior do que o mínimo arbitrado, consideramos que a imagem foi encontrada
if len(good_matches) > minMatches:
# Bloco try para detectar erros na transformação de perspectiva
try:
# Extrair os pontos da câmera e da imagem identificados anteriormente
src_pts = np.float32([keypoints1[m.queryIdx].pt for m in good_matches]).reshape(-1, 1, 2)
dst_pts = np.float32([keypoints2[m.trainIdx].pt for m in good_matches]).reshape(-1, 1, 2)
# Define a região da imagem identificada na câmera (homografia da imagem original)
M, _ = cv2.findHomography(src_pts, dst_pts, cv2.RANSAC, 5.0)
# Com as dimensões da imagem original, define uma matriz de pontos que constituem o contorno dela
h, w = img1.shape
pts = np.float32([[0, 0], [0, h - 1], [w - 1, h - 1], [w - 1, 0]]).reshape(-1, 1, 2)
# Converte a matriz do contorno da imagem para a perspectiva da câmera
dst = cv2.perspectiveTransform(pts, M)
# Desenha uma linha em volta da imagem identificada na webcam
img2 = cv2.polylines(img2, [np.int32(dst)], True, 255, 3, cv2.LINE_AA)
# Se ocorrer, imprime o erro e apenas mostra a imagem da câmera
except Exception as e:
print(e)
# Mostra a variável img2
cv2.imshow('Reconhecido', img2)
# Encerra o programa quando aperta a tecla 'q'
if cv2.waitKey(1) & 0xFF == ord('q'):
break
R = np.eye(3, dtype=np.float64)
ret, M1, d1, M2, d2, R, T, E, F = cv2.stereoCalibrate(
objpoints,
imgpoints_l,
imgpoints_r,
M1,
d1,
M2,
d2,
img_shape,
criteria=stereocalib_criteria,
flags=flags)
# get new optimal camera matrix
newCamMtx1, roi1 = cv2.getOptimalNewCameraMatrix(M1, d1, img_shape, 1,
img_shape)
newCamMtx2, roi2 = cv2.getOptimalNewCameraMatrix(M2, d2, img_shape, 1,
img_shape)
# rectification and undistortion maps which can be used directly to correct the stereo pair
(
rectification_l, rectification_r, projection_l, projection_r,
disparityToDepthMap, ROI_l, ROI_r
) = cv2.stereoRectify(
M1,
d1,
M2,
d2,
img_shape,
R,
T,
def test_calibrate():
flags = (cv2.CALIB_CB_EXHAUSTIVE + cv2.CALIB_CB_ACCURACY +
cv2.CALIB_CB_MARKER) # for findChessboardCornersSB
obj_points = []
img_points_r = []
img_points_l = []
img_shape = None
chess_img_r = None
chess_img_l = None
corners_l = None
corners_r = None
objp = np.zeros((np.prod(CHESSBOARD_SIZE), 3), dtype=np.float32)
objp[:, :2] = np.mgrid[0:CHESSBOARD_SIZE[0],
0:CHESSBOARD_SIZE[1]].T.reshape(-1, 2)
for i in range(1, 113):
t = str(i)
chessboard_l_found = False
chessboard_r_found = False
corners_l = None
corners_r = None
chess_img_l = cv2.imread('{0}/chessboard-L{1}.png'.format(DIR_NAME, t),
0)
chess_img_r = cv2.imread('{0}/chessboard-R{1}.png'.format(DIR_NAME, t),
0)
img_shape = chess_img_r.shape[:2]
try:
chessboard_l_found, corners_l = cv2.findChessboardCornersSB(
chess_img_l, CHESSBOARD_SIZE, flags=flags)
chessboard_r_found, corners_r = cv2.findChessboardCornersSB(
chess_img_r, CHESSBOARD_SIZE, flags=flags)
except Exception as ex:
print('error finding chessboard {0}'.format(t))
if chessboard_l_found == True & chessboard_r_found == True:
print('Found {0}'.format(t))
obj_points.append(objp)
img_points_l.append(corners_l)
img_points_r.append(corners_r)
# calibrate cameras
ret_l, K_l, dist_l, rvecs_l, tvecs_l = cv2.calibrateCamera(
obj_points, img_points_l, chess_img_l.shape[::-1], None, None)
ret_r, K_r, dist_r, rvecs_r, tvecs_r = cv2.calibrateCamera(
obj_points, img_points_r, chess_img_r.shape[::-1], None, None)
new_camera_matrix_l, roi_l = cv2.getOptimalNewCameraMatrix(
K_l, dist_l, img_shape, 1, img_shape)
new_camera_matrix_r, roi_r = cv2.getOptimalNewCameraMatrix(
K_r, dist_r, img_shape, 1, img_shape)
left_camera = (new_camera_matrix_l, K_l, dist_l)
with open('camera_matrix_l.pickle', 'wb') as handle:
pickle.dump(left_camera, handle, protocol=HIGHEST_PROTOCOL)
right_camera = (new_camera_matrix_r, K_r, dist_r)
with open('camera_matrix_r.pickle', 'wb') as handle:
pickle.dump(right_camera, handle, protocol=HIGHEST_PROTOCOL)
# stereo calibrate
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS,
100, 1e-5)
retS, matrix_l, dist_coef_l, matrix_r, dist_coef_r, R, T, E, F = cv2.stereoCalibrate(
obj_points,
img_points_l,
img_points_r,
new_camera_matrix_l,
dist_l,
new_camera_matrix_r,
dist_r,
img_shape,
criteria=stereocalib_criteria,
flags=cv2.CALIB_FIX_INTRINSIC)
rot_l, rot_r, proj_l, proj_r, Q, roi_l, roi_r = cv2.stereoRectify(
matrix_l, dist_coef_l, matrix_r, dist_coef_r, img_shape, R, T)
print('retS.stereoCalibrate - ', retS, ' retL - ', ret_l, ' retR - ',
ret_r)
print('R: ', R)
print('T:', T)
left_map = cv2.initUndistortRectifyMap(matrix_l, dist_coef_l, rot_l,
proj_l, img_shape, cv2.CV_16SC2)
right_map = cv2.initUndistortRectifyMap(matrix_r, dist_coef_r, rot_r,
proj_r, img_shape, cv2.CV_16SC2)
stereo_map = (left_map, right_map)
with open('stereo_map.pickle', 'wb') as handle:
pickle.dump(stereo_map, handle, protocol=HIGHEST_PROTOCOL)
cameras = Cameras()
while True:
frame_l, frame_r = cameras.grab_one()
res = np.hstack((frame_l, frame_r))
cv2.imshow('original', res)
print('roi_l:', roi_l, ' - roi_r:', roi_r)
ret = cv2.getValidDisparityROI(roi_l, roi_r, 1, 10, 5)
print(ret)
left_rectified = cv2.remap(frame_l,
left_map[0],
left_map[1],
interpolation=cv2.INTER_LANCZOS4,
borderMode=cv2.BORDER_CONSTANT)
right_rectified = cv2.remap(frame_r,
right_map[0],
right_map[1],
interpolation=cv2.INTER_LANCZOS4,
borderMode=cv2.BORDER_CONSTANT)
cv2.imshow('left_rectified', left_rectified)
cv2.imshow('right_rectified', right_rectified)
img_to_use_l = left_rectified
img_to_use_r = right_rectified
chessboard_l_found, corners_l = cv2.findChessboardCorners(
img_to_use_l, CHESSBOARD_SIZE, None)
chessboard_r_found, corners_r = cv2.findChessboardCorners(
img_to_use_r, CHESSBOARD_SIZE, None)
print(chessboard_l_found, chessboard_r_found)
if chessboard_l_found == True & chessboard_r_found == True:
print('point_l: ', corners_l[0][0][0])
print('point_r: ', corners_r[0][0][0])
disparity_maybe = corners_l[0][0][0] - corners_r[0][0][0]
disp_may = (corners_l[0][0][0], corners_l[0][0][1],
disparity_maybe)
np_disp = np.array(
[[[corners_l[0][0][0], corners_l[0][0][1], disparity_maybe]]],
dtype=np.float32)
new_coord = cv2.perspectiveTransform(np_disp, Q)
print('new_coord: ', new_coord)
k = cv2.waitKey(0) & 0xFF
if k == 115:
cv2.imwrite('{0}/img-L.png'.format(DIR_NAME), frame_l)
cv2.imwrite('{0}/img-R.png'.format(DIR_NAME), frame_r)
if k == 27:
break
else:
print(" no points found.")
# cv2.destroyAllWindows()
assert gray is not None
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
img = cv2.imread('IMG_0035.JPG')
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx,
dist,
imageSize=(w, h),
alpha=0,
newImgSize=(w, h))
# undistort
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h),
5)
dst = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# crop the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
cv2.imwrite('calibresult.png', dst)
tot_error = 0
import cv2
import numpy as np
from calib_store import load_coefficients
if __name__ == '__main__':
image = cv2.imread("../data/image22.png", 1)
K, D = load_coefficients("./cam_calib.yml")
K = np.array(K, dtype=np.float64).reshape((3, 3))
D = np.array(D, dtype=np.float64).reshape((1, 5))
h, w = image.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(K, D, (w, h), 1, (w, h))
mapx, mapy = cv2.initUndistortRectifyMap(K, D, None, newcameramtx, (w, h),
5)
dst = cv2.remap(image, mapx, mapy, cv2.INTER_LINEAR)
dst = dst[90:h - 90, 160:w - 160, :]
cv2.imshow("image", dst)
cv2.waitKey(10000)
# positions, _, _, rvec, tvec = getPositions(frame, mtx, dist, planebots.MarkerDictionary, planebots.DetectionParameters,
# calIds, calPositions, rvec, tvec)
# pts, jac = cv2.projectPoints(positions, rvec, tvec, mtx, dist)
# drawGridPoints(frame, pts)
#
# # defining the corner points
# framevertices = np.array([[[0, 0], frame.shape[1::-1]]], np.float32)
#
#
corners, ids, rej = cv2.aruco.detectMarkers(frame,
planebots.MarkerDictionary,
None, None,
planebots.DetectionParameters,
None)
newmtx, newroi = cv2.getOptimalNewCameraMatrix(mtx, dist,
frame.shape[1::-1], 1,
frame.shape[1::-1])
# newpts = cv2.undistortPoints(framevertices, newmtx, dist)
n = 5
m = 6
x, y = np.meshgrid(np.linspace(0, frame.shape[1], n),
np.linspace(0, frame.shape[0], m))
# # gr = np.ogrid()
# gridpoints = np.array([x, y]).swapaxes(0, 2)
# drawGridPoints(frame, gridpoints, color=(255, 0, 122))
#
# cv2.imwrite('detection1.png', frame)
# newmtx, newroi = cv2.getOptimalNewCameraMatrix(mtx, dist, frame.shape[1::-1], 1, frame.shape[1::-1])
flatpoints = np.array([[x.flatten(), y.flatten()]],
np.float64).swapaxes(1, 2)
imgpoints.append(corners)
# Draw and display the corners
cv2.drawChessboardCorners(img, PATTERN_SIZE, corners, ret)
cv2.imshow('img', img)
cv2.waitKey(500)
cv2.destroyAllWindows()
ret, mtxl, distl, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints,
gray.shape[::-1], None,
None)
img = cv2.imread(random.choice(imagesl))
h, w = img.shape[:2]
newcameramtxl, roi = cv2.getOptimalNewCameraMatrix(mtxl, distl, (w, h), 1,
(w, h))
calibration_data['mtxl'] = mtxl
calibration_data['distl'] = distl
calibration_data['newcameramtxl'] = newcameramtxl
print("mtxl: {}".format(mtxl))
print("distl: {}".format(distl))
print("newcameramtxl: {}".format(newcameramtxl))
# undistort
dst = cv2.undistort(img, mtxl, distl, None, newcameramtxl)
cv2.imshow('calibresult', dst)
cv2.waitKey(0)
t += 1
if (True == retR) & (True == retL):
time.sleep(0.50)
cv2.imshow('left', leftFrame)
cv2.imshow('right', rightFrame)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
print(
"Fin de la prise de vue du damier de calibration, début du calcul des données de calibration"
)
hR, wR = rightFrame.shape[:2]
retR, mtxR, distR, rvecsR, tvecsR = cv2.calibrateCamera(
objpoints, imgpointsR, (hR, wR), None, None)
OmtxR, roiR = cv2.getOptimalNewCameraMatrix(mtxR, distR, (wR, hR), 1, (wR, hR))
hL, wL = leftFrame.shape[:2]
retL, mtxL, distL, rvecsL, tvecsL = cv2.calibrateCamera(
objpoints, imgpointsL, (hL, wL), None, None)
OmtxL, roiL = cv2.getOptimalNewCameraMatrix(mtxL, distL, (wL, hL), 1, (wL, hL))
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
retS, MLS, dLS, MRS, dRS, R, T, E, F = cv2.stereoCalibrate(
objpoints, imgpointsL, imgpointsR, mtxL, distL, mtxR, distR, (hR, wR),
criteria_stereo, flags)
rectify_scale = 1
RL, RR, PL, PR, Q, roiL, roiR = cv2.stereoRectify(MLS, dLS, MRS, dRS, (hR, wR),
R, T, rectify_scale, (0, 0))
Left_Stereo_Map = cv2.initUndistortRectifyMap(MLS, dLS, RL, PL, (wR, hR),
cv2.CV_16SC2)
Right_Stereo_Map = cv2.initUndistortRectifyMap(MRS, dRS, RR, PR, (wR, hR),
intrinsic = np.load("intrinsic.npy")
distortion = np.load("distortion.npy")
#all the markers that we know of so far
markers = set()
#the relative tvec locations between markers
frames = 0
arucoDict = cv2.aruco.Dictionary_get(cv2.aruco.DICT_5X5_1000)
#get optimal camera matrix to undistort the image
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(intrinsic, distortion,
(1920, 1080), 1,
(1920, 1080))
#create aruco detector
aruco_detector = ArUcoDetector(intrinsic,
distortion,
arucoDict,
square_length=0.05)
curr_figure = None
#setting up plotting for the tvecs
aruco_fig = plt.figure()
aruco_tvec_ax = aruco_fig.add_axes(projection="3d")
print("\nRMS:", rms)
print("camera matrix:\n", camera_matrix)
print("distortion coefficients: ", dist_coefs.ravel())
# undistort the image with the calibration
print('')
for fn in img_names if debug_dir else []:
path, name, ext = splitfn(fn)
img_found = os.path.join(debug_dir, name + '_chess.png')
outfile = os.path.join(debug_dir, name + '_undistorted.png')
img = cv.imread(img_found)
if img is None:
continue
h, w = img.shape[:2]
newcameramtx, roi = cv.getOptimalNewCameraMatrix(
camera_matrix, dist_coefs, (w, h), 1, (w, h))
dst = cv.undistort(img, camera_matrix, dist_coefs, None, newcameramtx)
# crop and save the image
x, y, w, h = roi
dst = dst[y:y + h, x:x + w]
print('Undistorted image written to: %s' % outfile)
cv.imwrite(outfile, dst)
cv.destroyAllWindows()
def CalibrateCamera(directory, visualize=False):
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# here we only care about computing the intrinsic parameters of the camera
# and not the true positions of the checkerboard, so we can do everything
# up to a scale factor, this means we can prepare our object points as
# (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0) representing the coordinates
# of the corners in the checkerboard's local coordinate frame
# if we cared about exact position we would need to scale these according
# to the true size of the checkerboard
objp = np.zeros((9 * 6, 3), np.float32)
objp[:, :2] = np.mgrid[0:9, 0:6].T.reshape(-1, 2)
# Set up arrays to store object points and image points from all the images
# Here the image points are the 2d positions of the corners in each image
# the object points are the true 3d positions of the corners in the
# checkerboards coordinate frame
objpoints = [] # 3d point in the checkerboard's coordinate frame
imgpoints = [] # 2d points in image plane.
# Grab all images in the directory
images = glob.glob(directory + '/*.jpg')
for fname in images:
# read the image
img = cv2.imread(fname)
img = cv2.resize(img,
None,
fx=0.19,
fy=0.19,
interpolation=cv2.INTER_AREA)
#w,h = cv2.GetSize(img)
#print(w,h)
# convert to grayscale (this simplifies corner detection)
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)
# This ensures that only images where all corners could be detected are
# used for calibration
if ret == True:
# the object points in the checkerboard frame are always the same
# so we just duplicate them for each image
objpoints.append(objp)
# refine corner locations, initial corner detection is performed by
# sliding a convultional filter across the image, so accuracy is
# at best 1 pixel, but from the image gradient we can compute the
# location of the corner at sub-pixel accuracy
corners2 = \
cv2.cornerSubPix(gray,corners, (11,11), (-1,-1), criteria)
# append the refined pixel location to our image points array
imgpoints.append(corners2)
# if visualization is enabled, draw and display the corners
if visualize == True:
cv2.drawChessboardCorners(img, (9, 6), corners2, ret)
cv2.imshow('Display', img)
cv2.waitKey(500)
# Perform camera calibration
# Here I have fixed K3, the highest order distortion coefficient
# This simplifies camera calibration and makes it easier to get a good
# result, however this is only works under the assumption that your camera
# does not have too much distortion, if your camera is very wide field of
# view, you should remove this flag
ret, intrinsics, distortion, rvecs, tvecs = \
cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, \
None,flags=cv2.CALIB_FIX_K3,criteria=criteria)
# print error if calibration unsuccessful
if not ret:
print("Calibration failed, recollect images and try again")
# if successful, compute an print reprojection error, this is a good metric
# for how good the calibration is. If your result is greater than 1px you
# should probably recalibrate
total_error = 0
for i in range(len(objpoints)):
# project the object points onto each camera image and compare
# against the detected image positions
imgpoints2, _ = cv2.projectPoints(objpoints[i], rvecs[i], tvecs[i], \
intrinsics, distortion)
error = cv2.norm(imgpoints[i], imgpoints2,
cv2.NORM_L2) / len(imgpoints2)
total_error += error
print("mean error: {}".format(total_error / len(objpoints)))
# compute the region for where we have full information and the resulting
# intrinsic calibration matrix
h, w = img.shape[:2]
new_intrinsics, roi = cv2.getOptimalNewCameraMatrix(
intrinsics, distortion, (w, h), 1, (w, h))
# return only the information we will need going forward
cv2.destroyAllWindows()
return intrinsics, distortion, roi, new_intrinsics
corners2 = cv2.cornerSubPix(gray, corners, (11, 11), (-1, -1), criteria)
imgpoints.append(corners2)
img = cv2.drawChessboardCorners(img, (5,9), corners2, ret)
cv2.imshow('img',img)
cv2.waitKey(500)
else:
print("ret: {}".format(ret))
cv2.imshow('img',img)
cv2.waitKey(500)
#cv2.destroyAllWindows()
print("Successful count: {}".format(cnt))
print("calibrating...")
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1],None, None)
h,w = first_match.shape[:2]
newmtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist,(w,h),0,(w,h))
print("Calibration values:")
print("mtx: {}".format(mtx))
print("dist: {}".format(dist))
print("rvecs: {}".format(rvecs))
print("tvecs: {}".format(tvecs))
print("newmtx: {}".format(newmtx))
print("roi: {}".format(roi))
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newmtx, (w,h), 5)
dst = cv2.remap(first_match, mapx, mapy, cv2.INTER_LINEAR)
#x,y,w,h = roi
#dst = dst[y:y+h, x:x+w]
cv2.imshow('img', dst)
while True:
save_json({
'camera_matrix': camera_matrix.tolist(),
'dist_coeffs': dist_coeffs.tolist(),
'err': err
})
print('...DONE')
except Exception as e:
print(e)
success = False
# Generate the corrections
new_camera_matrix, valid_pix_roi = cv2.getOptimalNewCameraMatrix(
camera_matrix,
dist_coeffs,
resolution,
0)
mapx, mapy = cv2.initUndistortRectifyMap(
camera_matrix,
dist_coeffs,
None,
new_camera_matrix,
resolution,
5)
while True:
frame = stream.read()
if mapx is not None and mapy is not None:
frame = cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)
cv2.imshow('frame', frame)
#CALIBRATION
retval, cameraMatrix, distCoeffs, rotvecs, transvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], None, None)
#UNDISTORTION
for x in range(0, 29):
print x
image1 = cv2.imread('/home/thor/Desktop/FYP/test_data/%s.jpg' %x) #distorted images
image2 = cv2.resize(image1, (0, 0), fx=0.25, fy=0.25)
image = cv2.cvtColor(image2,cv2.COLOR_BGR2GRAY)
h, w = image.shape[:2]
#print h
#print w
newcameramtx, roi=cv2.getOptimalNewCameraMatrix(cameraMatrix, distCoeffs, (w, h), 1, (w, h))
#print cameraMatrix
#print newcameramtx
#print roi
# undistort using cv2.undistort()
dst = cv2.undistort(image, cameraMatrix, distCoeffs, None, newcameramtx)
# crop the image
x, y, w, h = rois
dst = dst[y:y+h, x:x+w]
cv2.imshow('img', dst)
cv2.waitKey(5000)
cv2.imwrite('/home/thor/Desktop/FYP/undistortion_result/calibresul%s.jpg' %x, dst)
########################
try:
namedWindow('win')
createTrackbar('alpha', 'win', 100, 100, getAlpha)
W = int(cap.get(CAP_PROP_FRAME_WIDTH))
H = int(cap.get(CAP_PROP_FRAME_HEIGHT))
u = zeros((H * 2, W, 3), dtype='uint8') # pre-initialize display
while 1:
# Interface
cv_imshow('win', u)
press = waitKey(20)
if press in (27, 81, 113):
break
# Update Camera Matrix
ncm, roi = getOptimalNewCameraMatrix(calib.cameraMatrix,
calib.distCoeffs, (W, H),
alpha_val)
x, y, w, h = roi
# Read frames and correct
r, f = cap.read()
if not r:
logging.warning('Unable to read frame')
continue
_u1 = undistort(f,
calib.cameraMatrix,
calib.distCoeffs,
None,
newCameraMatrix=ncm)
u1 = zeros((H, W, 3), dtype='uint8')
u1[y:y + h, x:x + w] = _u1[y:y + h, x:x + w]
R_mtxs.append(rotation_mtx)
extrinsic_mtxs.append(extrinsic_mtx)
print("外参：")
print(extrinsic_mtx)
calib_result_extrinsic_fname = fname.replace('.png', 'extrinsic_mtx.npy')
save_dict = {'tvec': tvec, 'rvec': rvec, 'extrinsic_mtx': extrinsic_mtx, 'rotation_mtx': rotation_mtx}
np.save(calib_result_extrinsic_fname, save_dict)
# print("done")
# 去畸变
img2 = cv2.imread(root_dir + '115.png')
h, w = img2.shape[:2]
dst = cv2.undistort(img2, mtx, dist)
cv2.imshow("distort", dst)
# cv2.imwrite('calibresult.png',dst)
# rgb相机去畸
rgb_image = img2
cv2.imshow("rgb", rgb_image)
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, rgb_image.shape[1::-1], 1)
map1, map2 = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, rgb_image.shape[1::-1], cv2.CV_32FC1)
rgb_image_undistort = cv2.remap(rgb_image, map1, map2, cv2.INTER_NEAREST)
rgb_intrinsic_mtx = newcameramtx
print("new rgb_camera mtx:\n", newcameramtx)
cv2.imshow("rgb_undistort", rgb_image_undistort)
cv2.waitKey(0)
# print("Object points do not match")
# sys.exit(1)
objectPoints = leftObjectPoints
print("Calibrating left camera...")
_, leftCameraMatrix, leftDistortionCoefficients, _, _ = cv2.calibrateCamera(
leftObjectPoints, leftImagePoints, imageSize, None, None)
leftImage = glob.glob("/Users/admin/Desktop/Robot/EIIC/left/*.png")
leftImage = sorted(leftImage)
saveLeft = "/Users/admin/Desktop/Robot/EIIC/outputCalibratedL/{:06d}.png"
frameLeft = 1
for i in leftImage:
img = cv2.imread(i)
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
leftCameraMatrix, leftDistortionCoefficients, (w, h), 1, (w, h))
# undistort
leftDistortionCoefficients = cv2.undistort(img, leftCameraMatrix,
leftDistortionCoefficients,
None, newcameramtx)
# crop the image
x, y, w, h = roi
leftDistortionCoefficients = leftDistortionCoefficients[y:y + h, x:x + w]
cv2.imwrite(saveLeft.format(frameLeft), leftDistortionCoefficients)
frameLeft += 1
print('Hello')
print("Calibrating right camera...")
_, rightCameraMatrix, rightDistortionCoefficients, _, _ = cv2.calibrateCamera(
rightObjectPoints, rightImagePoints, imageSize, None, None)
config = dict()
with open("config.json", "r") as config_file:
config = json.load(config_file)
mtx = np.array(config["mtx"])
newcameramtx = np.array(config["newcameramtx"])
dist = np.array(config["dist"])
while (True):
ret, frame = cap.read()
if ret:
cv2.imshow("frame", frame)
h, w = frame.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(
mtx, dist, (w, h), 1, (w, h))
x, y, w, h = roi
img = cv2.undistort(frame, mtx, dist, None, newcameramtx)[y:y + h,
x:x + w]
cv2.imshow("undistorted", img)
key = cv2.waitKey(100 // 60)
if key == 27: break
cv2.destroyAllWindows()
def calibrate(self, images_left_path, images_right_path, pattern_type='chessboard', pattern_size=(7,6)):
""" Compute stereo parameters like a fundamental matrix,
rotation and traslation matrix and esential matrix.
Arguments:
images_left_path (str): folder where is saved left calibration pattern photos.
images_right_path (str): folder where is saved right calibration pattern photos.
pattern_type (str): It can be "circles" pattern or "chessboard" pattern (default).
pattern_size (tuple): If pattern_type is "chessboard" this the Number of inner corners per a chessboard row and column. But If pattern_type is "circles" this will be the number of circles per row and column.
Raises:
OSError: If didn't find photos on images_left_path or images_right_path folders.
"""
# adjust image path
images_left, _ = os.path.splitext(images_left_path)
images_right, _ = os.path.splitext(images_right_path)
# available formats
formats = ['*.svg', '*.png', '*.jpg', '*.bmp', '*.jpeg', '*.raw']
# get files names
filesL = [glob.glob((images_left + '/' + e) if len(images_left) > 0 else e) for e in formats]
filesR = [glob.glob((images_right + '/' + e) if len(images_right) > 0 else e) for e in formats]
# flatting files list
imagesL = [item for sublist in filesL for item in sublist]
imagesR = [item for sublist in filesR for item in sublist]
# termination criteria
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((pattern_size[1]*pattern_size[0],3), np.float32)
objp[:,:2] = np.mgrid[0:pattern_size[0],0:pattern_size[1]].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpointsL = [] # 2d points in image plane.
imgpointsR = [] # 2d points in image plane.
try:
if len(imagesL) == 0 or len(imagesR) == 0:
raise OSError
for imgLeft, imgRight in zip(imagesL, imagesR):
imgL = cv.imread(imgLeft)
imgR = cv.imread(imgRight)
grayL = cv.cvtColor(imgL, cv.COLOR_BGR2GRAY)
grayR = cv.cvtColor(imgR, cv.COLOR_BGR2GRAY)
# Find the chess board corners
if pattern_type == 'chessboard':
retL, cornersL = cv.findChessboardCorners(grayL, pattern_size, None)
retR, cornersR = cv.findChessboardCorners(grayR, pattern_size, None)
elif pattern_type == 'circles':
retL, cornersL = cv.findCirclesGrid(grayL, pattern_size, None)
retR, cornersR = cv.findCirclesGrid(grayR, pattern_size, None)
# If found, add object points, image points (after refining them)
if retL and retR == True:
objpoints.append(objp)
cornersL = cv.cornerSubPix(grayL, cornersL, (11,11), (-1,-1), criteria)
imgpointsL.append(cornersL)
cornersR = cv.cornerSubPix(grayR, cornersR, (11,11), (-1,-1), criteria)
imgpointsR.append(cornersR)
# Draw and display the corners
# cv.drawChessboardCorners(imgL, pattern_size, cornersL, retL)
# cv.namedWindow('img left', cv.WINDOW_NORMAL)
# cv.imshow('img left', imgL)
# cv.drawChessboardCorners(imgR, pattern_size, cornersR, retR)
# cv.namedWindow('img right', cv.WINDOW_NORMAL)
# cv.imshow('img right', imgR)
# cv.waitKey(1000)
flags = 0
flags |= cv.CALIB_FIX_INTRINSIC
# Here we fix the intrinsic camara matrixes so that only Rot, Trns, Emat and Fmat are calculated.
# Hence intrinsic parameters are the same
criteria_stereo= (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 30, 0.001)
try:
# This step is performed to transformation between the two cameras and calculate Essential and Fundamenatl matrix
print("Calibrating estereo...")
self.ret_stereo, self.camL.matrix, self.camL.dist_coeffs, self.camR.matrix, self.camR.dist_coeffs, self.rot, self.trans, self.e_matrix, self.f_matrix = cv.stereoCalibrate(objpoints, imgpointsL, imgpointsR, self.camL.matrix, self.camL.dist_coeffs, self.camR.matrix, self.camR.dist_coeffs, grayL.shape[::-1], criteria_stereo, flags)
print("system calibrated")
except AttributeError:
print("Camera parameters hasn't fixed")
print("Fixing left camera...")
self.camL.ret, self.camL.matrix, self.camL.dist_coeffs, self.camL.rvecs, self.camL.tvecs = cv.calibrateCamera(objpoints, imgpointsL, grayL.shape[::-1], None, None)
heightL, widthL = imgL.shape[:2]
self.camL.matrix, self.camL.roi = cv.getOptimalNewCameraMatrix(self.camL.matrix, self.camL.dist_coeffs, (widthL, heightL), 1, (widthL, heightL))
errorL = re_projection_error(objpoints, self.camL.rvecs, self.camL.tvecs, self.camL.matrix, imgpointsL, self.camL.dist_coeffs)
print("Left camera error {}".format(errorL))
print("Fixing Right camera...")
self.camR.ret, self.camR.matrix, self.camR.dist_coeffs, self.camR.rvecs, self.camR.tvecs = cv.calibrateCamera(objpoints, imgpointsR, grayR.shape[::-1], None, None)
heightR, widthR = imgL.shape[:2]
self.camR.matrix, self.camR.roi = cv.getOptimalNewCameraMatrix(self.camR.matrix, self.camR.dist_coeffs, (widthR, heightR), 1, (widthR, heightR))
errorR = re_projection_error(objpoints, self.camR.rvecs, self.camR.tvecs, self.camR.matrix, imgpointsR, self.camR.dist_coeffs)
print("Right camera error {}".format(errorR))
print("Calibrating estereo...")
# This step is performed to transformation between the two cameras and calculate Essential and Fundamenatl matrix
self.ret_stereo, self.camL.matrix, self.camL.dist_coeffs, self.camR.matrix, self.camR.dist_coeffs, self.rot, self.trans, self.e_matrix, self.f_matrix = cv.stereoCalibrate(objpoints, imgpointsL, imgpointsR, self.camL.matrix, self.camL.dist_coeffs, self.camR.matrix, self.camR.dist_coeffs, grayL.shape[::-1], criteria_stereo, flags)
print("system calibrated")
except OSError:
if len(imagesL) == 0:
print("Could not find any image in {}".format(images_left_path))
if len(imagesR) == 0:
print("Could not find any image in {}".format(images_right_path))
imgpoints.append(corners2)
op.append(objp)
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(op, imgpoints,
b.shape[::-1], None, None)
tot_error = 0
for i in range(len(op)):
imgpoints2, _ = cv2.projectPoints(op[i], rvecs[i], tvecs[i], mtx, dist)
error = cv2.norm(imgpoints[i], imgpoints2, cv2.NORM_L2) / len(imgpoints2)
tot_error += error
# print(ret, (tot_error / len(op))**0.5)
# cv2.namedWindow('winname', cv2.WINDOW_NORMAL)
# cv2.imshow('winname', a)
# cv2.waitKey(0)
"""下面是校正部分"""
h, w = a.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (h, w), 1)
dst = cv2.undistort(a, mtx, dist, None)
# undistort
mapx, mapy = cv2.initUndistortRectifyMap(mtx, dist, None, newcameramtx, (w, h),
5)
dst = cv2.remap(a, mapx, mapy, cv2.INTER_LINEAR)
# crop the image
# x, y, w, h = roi
# dst = dst[y:y + h, x:x + w]
print(mapx.shape)
print(mapy)
np.savez("outfile", mtx, dist)
a = cv2.cvtColor(a, cv2.COLOR_BGR2RGB)
dst = cv2.cvtColor(dst, cv2.COLOR_BGR2RGB)
# print(roi)
plt.subplot(121), plt.imshow(a), plt.title('source')
def startCalibration(self):
global mybalance
if flag_fisheye_calibrate == 0:
self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.calibrateCamera(
self.objpoints, self.imgpoints, (self.width, self.height), None, None)
# check the result of calibration
print("self.camera_matrix=np.array(" + str(self.camera_matrix.tolist()) + ")")
print("self.distCoeffs=np.array(" + str(self.distCoeffs.tolist()) + ")")
# https://docs.opencv.org/3.0-beta/modules/calib3d/doc/camera_calibration_and_3d_reconstruction.html#getoptimalnewcameramatrix
self.new_camera_matrix, self.roi = cv2.getOptimalNewCameraMatrix(self.camera_matrix,
self.distCoeffs,
(self.width, self.height), mybalance,
(self.width, self.height))
print("self.roi is ", self.roi)
## self.roi or (self.width, self.height) ??
self.map1, self.map2 = cv2.initUndistortRectifyMap(self.camera_matrix, self.distCoeffs, np.eye(3),
self.new_camera_matrix,
(self.width, self.height),
cv2.CV_16SC2)
else:
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(self.objpoints, self.imgpoints, (self.width, self.height), None, None)
# you should write all the cv2.fisheye.CALIB_..3 things .. then it works
# # The result is same with originally works
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_FIX_SKEW,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# No good results at all
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_CHECK_COND,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# No good results at all
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_FIX_INTRINSIC,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# Does not work at all
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None)
# originally works
# self.ret, self.camera_matrix, self.distCoeffs, self.rvecs, self.tvecs = cv2.fisheye.calibrate(
# self.objpoints, self.imgpoints, (self.width, self.height), None, None, None, None,
# cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC + cv2.fisheye.CALIB_CHECK_COND + cv2.fisheye.CALIB_FIX_SKEW,
# (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 1e-6))
# originally works
N_OK = len(self.objpoints)
self.camera_matrix = np.zeros((3, 3))
self.distCoeffs = np.zeros((4, 1))
self.rvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
self.tvecs = [np.zeros((1, 1, 3), dtype=np.float64) for i in range(N_OK)]
self.ret, _, _, _, _ = cv2.fisheye.calibrate(
objectPoints=self.objpoints,
imagePoints=self.imgpoints,
image_size=(self.width, self.height),
K=self.camera_matrix,
D=self.distCoeffs,
rvecs=None,#self.rvecs,
tvecs=None,#self.tvecs,
flags=(cv2.fisheye.CALIB_RECOMPUTE_EXTRINSIC),#+ cv2.fisheye.CALIB_FIX_SKEW), # +cv2.fisheye.CALIB_FIX_PRINCIPAL_POINT cv2.fisheye.CALIB_CHECK_COND
criteria=(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 200, 1e-6))
# check the result of calibration
print('camera matrix is ')
print(self.camera_matrix)
# print('new camera Matrix is ')
# print(self.new_camera_matrix)
# print('self.rvecs is ')
# print(self.rvecs)
# print('self.tvecs is ')
# print(self.tvecs)
print('distort Coeffs is ')
print(self.distCoeffs)
print('RMS re-projection error is ', self.ret)
print('balance is ', mybalance)
print('f**k')
# self.distCoeffs = np.zeros((4, 1))
self.roi = [0,0,0,0]
# check the result of calibration
print('camera matrix is ')
print(self.camera_matrix)
# print('new camera Matrix is ')
# print(self.new_camera_matrix)
print('distort Coeffs is ')
print(self.distCoeffs)
# print('self.roi is ', self.roi)
print('fuck2')
self.new_camera_matrix = self.camera_matrix
self.map1 = 0
self.map2 = 0
# self.new_camera_matrix = cv2.fisheye.estimateNewCameraMatrixForUndistortRectify(K=self.camera_matrix,
# D=self.distCoeffs,
# image_size=(self.width, self.height),
# R=np.eye(3),
# P=None,
# balance=mybalance,
# new_size=(self.width, self.height),
# fov_scale=1.0
# )
# # check the result of calibration
# print('camera matrix is ')
# print(self.camera_matrix)
# print('new camera Matrix is ')
# print(self.new_camera_matrix)
# print('distort Coeffs is ')
# print(self.distCoeffs)
#
# # https://medium.com/@kennethjiang/calibrate-fisheye-lens-using-opencv-part-2-13990f1b157f
# self.map1, self.map2 = cv2.fisheye.initUndistortRectifyMap(K=self.camera_matrix,
# D=self.distCoeffs,
# R=np.eye(3),
# P=self.new_camera_matrix,
# size=(self.width, self.height),
# m1type=cv2.CV_32FC1)
tmp = np.array(self.map1)
print("self.map1 is ", tmp.shape)
tmp = np.array(self.map2)
print("self.map2 is ", tmp.shape)
# http://opencv-users.1802565.n2.nabble.com/Re-what-is-CV-32FC1-td6684091.html
# CV_<bit_depth>(S|U|F)C<number_of_channels>
# bit_depth is number of bits per element in matrix.supported bit. depth are 8, 16, 32, and 84.
# S = signed, U = unsigned, F = float
# number_of_channels is of course number of channels, in term of matrix you can think of this as third dimension
print("calibration complete")
import numpy as np
import cv2
from matplotlib import pyplot as plt
# Define camera matrix K
K = np.array([[4479.94, 0., 2727.48], [0., 4374.51, 1811.92], [0., 0., 1.]])
# Define distortion coefficients d
d = np.array([-0.0196948, 0.00652119, 0.000237354, 0.00108494, 0.0909295])
# Read an example image and acquire its size
img = cv2.imread("C:\Tal's DATA\Testim\BGU Campus\BackYard\DJI_0008.JPG")
h, w = img.shape[:2]
# Generate new camera matrix from parameters
newcameramatrix, roi = cv2.getOptimalNewCameraMatrix(K, d, (w, h), 0)
# Generate look-up tables for remapping the camera image
mapx, mapy = cv2.initUndistortRectifyMap(K, d, None, newcameramatrix, (w, h),
5)
# Remap the original image to a new image
newimg = cv2.remap(img, mapx, mapy, cv2.INTER_LINEAR)
# Display old and new image
fig, (oldimg_ax, newimg_ax) = plt.subplots(1, 2)
oldimg_ax.imshow(img)
oldimg_ax.set_title('Original image')
newimg_ax.imshow(newimg)
newimg_ax.set_title('Unwarped image')
plt.show()
