def StereoCalibrate(imgList):
print("Begin calibrating...")
calib_flags = \
cv2.CALIB_FIX_ASPECT_RATIO + \
cv2.CALIB_ZERO_TANGENT_DIST + \
cv2.CALIB_USE_INTRINSIC_GUESS + \
cv2.CALIB_SAME_FOCAL_LENGTH + \
cv2.CALIB_RATIONAL_MODEL + \
cv2.CALIB_FIX_K3 + cv2.CALIB_FIX_K4 + cv2.CALIB_FIX_K5
imgPointsList1, imgPointsList2, objPointsList = FindChessboardCornersList(imgList)
imageSize = (640, 480)
# retval, cameraMatrix1, distCoeffs1, _, _ = cv2.calibrateCamera(np.asarray(objPointsList), np.asarray(imgPointsList1), (640, 480), None, None)
# retval, cameraMatrix2, distCoeffs2, _, _ = cv2.calibrateCamera(np.asarray(objPointsList), np.asarray(imgPointsList2), (640, 480), None, None)
cameraMatrix1 = cv2.initCameraMatrix2D(np.asarray(objPointsList), np.asarray(imgPointsList1), imageSize, 0)
cameraMatrix2 = cv2.initCameraMatrix2D(np.asarray(objPointsList), np.asarray(imgPointsList2), imageSize, 0)
rmserror, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(objPointsList, imgPointsList1, imgPointsList2,
cameraMatrix1, None, cameraMatrix2, None, imageSize,
flags=calib_flags)
print("Result")
print("RMS Error: ", rmserror)
print("CameraMatrix1: ", cameraMatrix1)
print("CameraMatrix2: ", cameraMatrix2)
print("distCoeffs1: ", distCoeffs1)
print("distCoeffs2: ", distCoeffs2)
print("R: ", R)
print("T: ", T)
print("E: ", E)
print("F: ", F)
print("Begin rectification...")
R1, R2, P1, P2, Q, validPixROI1, validPixROI2 = \
cv2.stereoRectify(cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, imageSize, R, T)
print("Rectification complete.")
print("Result: ")
print("R1: ", R1)
print("R2: ", R2)
print("P1: ", P1)
print("P2: ", P2)
print("Tx: ", P2[0,3]/P1[0,0])
print("Q: ", Q)
mapL1, mapL2 = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imageSize, cv2.CV_16SC2)
mapR1, mapR2 = cv2.initUndistortRectifyMap(cameraMatrix1, distCoeffs1, R1, P1, imageSize, cv2.CV_16SC2)
imgListL, imgListR = tuple(zip(*imgList))
for (img, fileName) in imgListL:
rectL = cv2.remap(img, mapL1, mapL2, cv2.INTER_LINEAR)
cv2.imwrite("../result/rectified/left/" + fileName, rectL)
for (img, fileName) in imgListR:
rectR = cv2.remap(img, mapR1, mapR2, cv2.INTER_LINEAR)
cv2.imwrite("../result/rectified/right/" + fileName, rectR)
def getCalibratefromStereoImage(objpoints, l_imgpoints, r_imgpoints):
# from OpenCV docs: if any of CV_CALIB_FIX_ASPECT_RATIO... are specified, the matrix components must be initialized.
cameraMatrix1 = cv2.initCameraMatrix2D(objpoints, l_imgpoints, (640, 480), 0);
cameraMatrix2 = cv2.initCameraMatrix2D(objpoints, r_imgpoints, (640, 480), 0);
distCoeffs1 = None
distCoeffs2 = None
# directly call stereCalibrate from OpenCV library
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(objpoints, l_imgpoints, r_imgpoints, \
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, (640, 480), \
criteria=stereocalib_criteria, flags=stereocalib_flags)
return retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F
def getCalibratefromStereoImage(objpoints, l_imgpoints, r_imgpoints):
# from OpenCV docs: if any of CV_CALIB_FIX_ASPECT_RATIO... are specified, the matrix components must be initialized.
cameraMatrix1 = cv2.initCameraMatrix2D(objpoints, l_imgpoints, (640, 480),
0)
cameraMatrix2 = cv2.initCameraMatrix2D(objpoints, r_imgpoints, (640, 480),
0)
distCoeffs1 = None
distCoeffs2 = None
# directly call stereCalibrate from OpenCV library
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(objpoints, l_imgpoints, r_imgpoints, \
cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, (640, 480), \
criteria=stereocalib_criteria, flags=stereocalib_flags)
return retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F
def get_next_camera_matrix(image_points, model_points, image_width,
image_height):
img_points = nparray_2D_from_point_list(image_points)
mod_points = nparray_3D_from_point_list(model_points)
objpoints = []
imgpoints = []
objpoints.append(mod_points)
imgpoints.append(img_points)
camera_matrix = cv2.initCameraMatrix2D(objpoints, imgpoints,
(image_width, image_height), 1)
return camera_matrix
def cameraCalib(pts2d, pts3d):
obj_pts = [numpy.array(pts3d).reshape(-1,3).astype('float32')]
img_pts = [numpy.array(pts2d).reshape(-1,2).astype('float32')]
initCamera = cv2.initCameraMatrix2D(obj_pts, img_pts, (1024, 768))
ret, cameraMat, distCoeff, rvecs, tvecs = cv2.calibrateCamera(
obj_pts,
img_pts,
(1024, 768),
initCamera,
numpy.zeros(4, 'float32'),
flags = cv2.CALIB_USE_INTRINSIC_GUESS)
return [cameraMat, rvecs, tvecs]
def get_camera_matrix(image_points, chess_field_size, image_width,
image_height):
img_points = nparray_2D_from_point_list(image_points)
mod_points = np.array([[0.0, 0.0, 0.0], [chess_field_size, 0.0, 0.0],
[0.0, chess_field_size, 0.0],
[chess_field_size, chess_field_size, 0.0]],
np.float32)
objpoints = []
imgpoints = []
objpoints.append(mod_points)
imgpoints.append(img_points)
camera_matrix = cv2.initCameraMatrix2D(objpoints, imgpoints,
(image_width, image_height), 1)
return camera_matrix
def main():
src = 'Images/view1.png'
img = cv2.imread(src, cv2.IMREAD_COLOR)
size = img.shape
input_corners = np.array(calibrate.extractCorners(calibrate.extractFieldLines(img)))
output_corners = np.array([[0, 500.0, 0], [800.0, 500.0, 0], [800.0, 0, 0], [0, 0, 0]])
input_corners = input_corners.astype('float32')
output_corners = output_corners.astype('float32')
cam_mat = cv2.initCameraMatrix2D([output_corners], [input_corners], size)
# cam_mat = np.array([[2200, 0, 750], [0, 2200, 750.0], [0, 0, 1.0]])
# dist_coefs = np.zeros(4)
# projMat = cv2.calibrateCamera([output_corners], [input_corners], size, cam_mat, dist_coefs, flags=cv2.CALIB_USE_INTRINSIC_GUESS)
imshow(img)
plt.show()
def main():
src = 'Images/view1.png'
img = cv2.imread(src, cv2.IMREAD_COLOR)
size = img.shape
input_corners = np.array(
calibrate.extractCorners(calibrate.extractFieldLines(img)))
output_corners = np.array([[0, 500.0, 0], [800.0, 500.0, 0], [800.0, 0, 0],
[0, 0, 0]])
input_corners = input_corners.astype('float32')
output_corners = output_corners.astype('float32')
cam_mat = cv2.initCameraMatrix2D([output_corners], [input_corners], size)
# cam_mat = np.array([[2200, 0, 750], [0, 2200, 750.0], [0, 0, 1.0]])
# dist_coefs = np.zeros(4)
# projMat = cv2.calibrateCamera([output_corners], [input_corners], size, cam_mat, dist_coefs, flags=cv2.CALIB_USE_INTRINSIC_GUESS)
imshow(img)
plt.show()
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 calc_affine_projection_matrix(landmarks, vertices, image):
"""
returns the 3x4 camera matrix based on the given locations of the landmarks
and matching 3d vertices.
"""
cv2.initCameraMatrix2D(vertices, landmarks, image.shape)
def CalFromDisc():
global rows
global columns
global params
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((columns*rows,3), np.float32)
objp[:,:2] = np.mgrid[0:rows,0:columns].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
images = glob.glob(imgFolder + '/*.jpg')
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (rows,columns), cv2.CALIB_CB_FILTER_QUADS)
# If found, add object points, image points (after refining them)
if ret == True:
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
imgpoints.append(corners2)
# Draw and display the corners
img = cv2.drawChessboardCorners(img, (rows,columns), corners2, ret)
cv2.imshow('Proyecto2: Calibracion de camara en modo almacenamiento. (Chessboard)',img)
cv2.waitKey(500)
cv2.destroyAllWindows()
camera_matrix = camera_matrix = cv2.initCameraMatrix2D(objpoints, imgpoints, gray.shape[::-1])
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], camera_matrix, None, flags = cv2.CALIB_USE_INTRINSIC_GUESS)
np.savez('camera', mtx = mtx, dist = dist, rvecs = rvecs, tvecs = tvecs)
params['camera'].append({
'K': mtx.tolist(),
'dist': dist.tolist()
})
with open('params.txt', 'w') as outfile:
json.dump(params, outfile)
img = cv2.imread(imgFolder + '/' + imgBaseNm + str(imgBaseIdx) + '.jpg')
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y : y + h, x : x + w]
cv2.imwrite('calibresult.png', dst)
# Load previously saved data
with np.load('camera.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
print("Press s in camera window for saving image.\n")
for fname in glob.glob(imgFolder + '/' + imgBaseNm + '*.jpg'):
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (rows, columns), cv2.CALIB_CB_FILTER_QUADS)
if ret == True:
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
# Find the rotation and translation vectors.
ret,rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, dist)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img, corners2, imgpts)
cv2.imshow('Proyecto2: Calibracion de camara en modo almacenamiento. (Proyeccion)', img)
k = cv2.waitKey(0) & 0xFF
if k == ord('s'):
cv2.imwrite(fname[:6]+'.png', img)
cv2.destroyAllWindows()
import cv2
import numpy as np
assert float(cv2.__version__.rsplit(
'.', 1)[0]) >= 3, 'OpenCV version 3 or newer required.'
"""K = np.array([[ 689.21, 0. , 1295.56],
[ 0. , 690.48, 942.17],
[ 0. , 0. , 1. ]])"""
K = np.array([[404.41 / 1.3, 0., 486 / 2], [0., 302.89 / 1.3, 364 / 2],
[0., 0., 1.]])
# zero distortion coefficients work well for this image
D = np.array([0., 0., 0., 0.])
# use Knew to scale the output
Knew = K.copy()
Knew[(0, 1), (0, 1)] = 0.4 * Knew[(0, 1), (0, 1)]
"""warped = imread('warped.png')
unwarped = imread('unwarped.png')
cv2.initCameraMatrix2D()"""
img = cv2.imread('Capture4.png')
cv2.imshow('test', img)
cv2.waitKey(2000)
img_undistorted = cv2.fisheye.undistortImage(img, K, D=D, Knew=Knew)
h, w, _ = img_undistorted.shape
img_undistorted = img_undistorted[(h / 3 - 30):(h / 3 - 50) + h / 2,
w / 4:w / 4 + w / 2]
cv2.imwrite('undistorted.jpg', img_undistorted)
cv2.imshow('undistorted', img_undistorted)
cv2.waitKey(3000)
def captureTarget(self, image_set, camera_info_set=None):
logging.debug("Checking for Target in %d Images", len(image_set))
centers_set = []
for i, image in enumerate(image_set):
if (image is None):
centers_set += [None]
else:
image_array = np.asarray(image)
if (len(image_array.shape) > 2 and image_array.shape[2] == 3):
image_array = cv2.cvtColor(image_array, cv2.COLOR_BGR2GRAY)
cv2.imwrite("testout.png", image_array)
print(self._target_shape)
if (self._target_type == 'circles'
or self._target_type == 'acircles'):
[ret_val,
centers] = cv2.findCirclesGrid(image_array,
self._target_shape,
flags=self._target_flags)
elif (self._target_type == 'chessboard'):
width = image_array.shape[0]
height = image_array.shape[1]
scale = math.sqrt((height * width) / (640. * 480.))
if (scale > 1.0):
scaled_image = cv2.resize(
image_array,
(int(height / scale), int(width / scale)))
else:
scaled_image = image_array
x_scale = float(height) / scaled_image.shape[1]
y_scale = float(width) / scaled_image.shape[0]
[ret_val, centers
] = cv2.findChessboardCorners(scaled_image,
self._target_shape,
flags=self._target_flags)
if (ret_val):
logging.debug("Chessboard found in %s scale Image %d",
scale, i)
if (scale > 1.0):
centers[:, :, 0] *= x_scale
centers[:, :, 1] *= y_scale
radius = int(math.ceil(scale) * 5)
else:
radius = 5
criteria = (cv2.TERM_CRITERIA_EPS +
cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001)
logging.debug(
"Corners refined using %d pixel radius in Image %d",
radius, i)
cv2.cornerSubPix(image_array, centers,
(radius, radius), (-1, -1), criteria)
else:
logging.error("Invalid Target Type: %s", self._target_type)
raise NameError("Invalid Target Type: ", self._target_type)
return
if (not ret_val):
logging.debug("Centers not found in Image %d", i)
centers_set += [None]
########################################################################################################################################
# return [None]*self._num_cameras, [None]*self._num_cameras
else:
print("Target Found in Image %d" % i)
centers_set += [centers]
params_set = []
novel_centers_set = []
centers_idxs = [
idx for idx, val in enumerate(centers_set) if val is not None
]
for i, centers in enumerate(centers_set):
if (centers is None):
params_set += [None]
novel_centers_set += [None]
else:
if (camera_info_set is not None
and camera_info_set[i] is not None
and camera_info_set[i].camera_matrix is not None):
camera_matrix = camera_info_set[i].camera_matrix
else:
model_centers = []
model_centers.append(
np.array(self._target_points, dtype=np.float32))
image_centers = []
image_centers.append(np.array(centers, dtype=np.float32))
camera_matrix = cv2.initCameraMatrix2D(
model_centers, image_centers, image_array.shape)
if (camera_info_set is not None
and camera_info_set[i].distortion_coefficients
is not None):
dist_coeffs = camera_info_set[i].distortion_coefficients
else:
dist_coeffs = None
[ret_pnp, r_vec,
t_vec] = cv2.solvePnP(self._target_points, centers,
camera_matrix, dist_coeffs)
if (not ret_pnp):
logging.debug("PNP Failed on Image %d", i)
params_set += [None]
novel_centers_set += [None]
continue
[r_mat, r_jac] = cv2.Rodrigues(r_vec)
norm_vec = -r_mat[:, 2]
norm_vec = norm_vec / np.linalg.norm(norm_vec)
target_col = np.mean(centers[:, :, 0]) / image_array.shape[0]
target_row = np.mean(centers[:, :, 1]) / image_array.shape[1]
target_area = self.boardStats(centers)
target_area = target_area / (image_array.shape[0] *
image_array.shape[1])
target_pitch = norm_vec[1]
target_yaw = norm_vec[0]
logging.debug(
"Target Params in Image %d: col: %f row: %f area: %f pitch: %f yaw: %f",
i, target_col, target_row, target_area, target_pitch,
target_yaw)
target_params = [
target_col, target_row, target_area, target_pitch,
target_yaw
]
is_novel = False
for j in centers_idxs:
if (self.isViewNovel(target_params,
self._target_params[i][j],
self._param_diff_threshold)):
is_novel = True
self._target_params[i][j] += [target_params]
if (is_novel):
logging.debug("Image %d added", i)
novel_centers_set += [centers]
params_set += [target_params]
else:
logging.debug("Image %d is not novel enough", i)
novel_centers_set += [None]
params_set += [None]
if (len(filter(lambda x: x != None, novel_centers_set)) > 0):
if (camera_info_set is None):
camera_info_set = [None] * len(image_set)
for i, (image, centers, camera_info) in enumerate(
zip(image_set, novel_centers_set, camera_info_set)):
self._target_image_set[i].append(image)
self._target_centers_set[i].append(centers)
self._model_centers_set.append(self._target_points)
self._initial_camera_calibration_set[i].append(camera_info)
return centers_set, params_set
def stereo_calibration(self,
write_yaml_flag=False,
draw_flag=False,
show_flag=False,
save_flag=False,
mono_calib=False,
load_num=-1):
"""name
if not calibrated:
need to calibrate monocular camera first
need to find points together
cause we need the points
Args:
Returns:
"""
left_path = os.path.join(STEREOIMGPATH, 'left')
right_path = os.path.join(STEREOIMGPATH, 'right')
self.camera_left.load_images(left_path,
'Calibration',
load_num=load_num)
self.camera_right.load_images(right_path,
'Calibration',
load_num=load_num)
self.camera_left.chess_board_size = np.array(CHESSBOARDSIZE)
self.camera_right.chess_board_size = np.array(CHESSBOARDSIZE)
# find the points!
if not self.stereo_pts_flag:
self.__stereo_find_points()
if mono_calib:
# monocular calibration
self.camera_left.calibrate_camera(draw_flag, show_flag, save_flag)
self.camera_right.calibrate_camera(draw_flag, show_flag, save_flag)
if write_yaml_flag:
self.camera_left.write_yaml('_stereo_need')
self.camera_right.write_yaml('_stereo_need')
else:
self.camera_left.IntP = cv2.initCameraMatrix2D(
np.asarray(self.obj_pts), np.asarray(self.img_pts_l),
tuple(self.camera_left.gary_img_shape), 0)
self.camera_right.IntP = cv2.initCameraMatrix2D(
np.asarray(self.obj_pts), np.asarray(self.img_pts_r),
tuple(self.camera_left.gary_img_shape), 0)
logging.info('Start Stereo Calibration')
stereocalib_criteria = (cv2.TERM_CRITERIA_MAX_ITER +
cv2.TERM_CRITERIA_EPS, 30, 1e-5)
flags = 0
flags |= cv2.CALIB_FIX_INTRINSIC
# flags |= cv2.CALIB_FIX_PRINCIPAL_POINT
# flags |= cv2.CALIB_USE_INTRINSIC_GUESS
# flags |= cv2.CALIB_FIX_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_ASPECT_RATIO
# flags |= cv2.CALIB_ZERO_TANGENT_DIST
# flags |= cv2.CALIB_RATIONAL_MODEL
# flags |= cv2.CALIB_SAME_FOCAL_LENGTH
# flags |= cv2.CALIB_FIX_K3
# flags |= cv2.CALIB_FIX_K4
# flags |= cv2.CALIB_FIX_K5
self.stereo_calib_err, self.camera_left.IntP, self.camera_left.DisP, \
self.camera_right.IntP, self.camera_right.DisP, \
self.R_relate, self.t_relate, self.E_calib, self.F_calib = cv2.stereoCalibrate(
self.obj_pts, self.img_pts_l, self.img_pts_r,
self.camera_left.IntP, self.camera_left.DisP,
self.camera_right.IntP, self.camera_right.DisP,
tuple(self.camera_left.gary_img_shape),
criteria=stereocalib_criteria, flags=flags)
# self.stereo_calib_err, self.camera_left.IntP, self.camera_left.DisP, \
# self.camera_right.IntP, self.camera_right.DisP, \
# self.R_relate, self.t_relate, self.E_calib, self.F_calib = cv2.stereoCalibrate(
# self.obj_pts, self.img_pts_l, self.img_pts_r,
# None,None,None,None,
# tuple(self.camera_left.gary_img_shape),
# None, None)
self.stereo_calib_flag = True
logging.info('Stereo Calibration Done')
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 30, 0.01)
cv2.cornerSubPix(image, corners, (11, 11), (-1, -1), term) # 精定位角点
image_points.append(corners.reshape(-1, 2))
image_lists.append(image)
# 2. 构建标定板的点坐标，objectPoints
object_points = np.zeros((np.prod(board_size), 3), np.float32)
object_points[:, :2] = np.indices(board_size).T.reshape(-1, 2)
object_points *= square_Size
object_points = [object_points] * image_number
# 3. 分别得到两个相机的初始CameraMatrix
h, w = image_lists[0].shape
camera_matrix = list()
camera_matrix.append(cv2.initCameraMatrix2D(object_points, image_points[:image_number], (w, h), 0))
camera_matrix.append(cv2.initCameraMatrix2D(object_points, image_points[image_number:], (w, h), 0))
# 4. 双目视觉进行标定
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 100, 1e-5)
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(object_points, image_points[:image_number], image_points[image_number:], camera_matrix[0],
None, camera_matrix[1], None, (w, h),
flags=cv2.CALIB_FIX_ASPECT_RATIO | cv2.CALIB_ZERO_TANGENT_DIST | cv2.CALIB_USE_INTRINSIC_GUESS |
cv2.CALIB_SAME_FOCAL_LENGTH | cv2.CALIB_RATIONAL_MODEL | cv2.CALIB_FIX_K3 | cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5,
criteria=term)
# 5. 矫正一张图像看看，是否完成了极线矫正
start_time = time.time()
fname1 = './images/left/left01.jpg'
fname2 = './images/right/right01.jpg'
ax.plot(all_commonpoints[side][[0, -1], :][num, 0], all_commonpoints[side][[0, -1], :][num, 1],
'ro', ms = 10, mec = 'red', mfc = 'None')
plt.xlim(xmin = x0, xmax = x1)
plt.ylim(ymin = y0, ymax = y1)
plt.show()
# <codecell>
cv2.getPerspectiveTransform()
# <codecell>
cv2.initCameraMatrix2D()
# <codecell>
# cv2.stereoCalibrate(objectPoints, imagePoints1, imagePoints2, imageSize[, cameraMatrix1[, distCoeffs1[,
# cameraMatrix2[, distCoeffs2[, R[, T[, E[, F[, criteria[, flags]]]]]]]]]])
# -> retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F
allflags = [# cv2.CALIB_FIX_INTRINSIC, # fix cameraMatrix? and distCoeffs?, only calculate R, T, E, F matrices
cv2.CALIB_USE_INTRINSIC_GUESS, # provide initial values
# cv2.CALIB_FIX_PRINCIPAL_POINT,
# cv2.CALIB_FIX_FOCAL_LENGTH,
# cv2.CALIB_FIX_ASPECT_RATIO,
# cv2.CALIB_SAME_FOCAL_LENGTH,
# cv2.CALIB_FIX_K2, # corresponding radial distortion coefficient is not changed
# cv2.CALIB_FIX_K3, # should be fixed unless fish-eye lens used; usually very small
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)
import cv2
import numpy
f = open('ptcorrs/00000001.ptcorr')
pts3d = []
pts2d = []
for line in f.readlines():
line = line.split()
p3 = [float(line[0]), float(line[1]), float(line[2])]
p2 = [float(line[3]), float(line[4])]
pts3d.append(p3)
pts2d.append(p2)
obj_pts = [numpy.array(pts3d).reshape(-1,3).astype('float32')]
img_pts = [numpy.array(pts2d).reshape(-1,2).astype('float32')]
initCamera = cv2.initCameraMatrix2D(obj_pts, img_pts, (1024, 768))
ret, cameraMat, distCoeff, rvecs, tvecs = cv2.calibrateCamera(
obj_pts,
img_pts,
(1024, 768),
initCamera,
numpy.zeros(4, 'float32'),
flags = cv2.CALIB_USE_INTRINSIC_GUESS)
print cameraMat
res = cv2.Rodrigues(numpy.array(rvecs).reshape(1,3).astype('float32'))
print "rvec", rvecs
print "center", -res[0].T.dot(tvecs)
print res[0]
cv2.drawChessboardCorners(img2, pattern_size, corners2, found)
path, name, ext = splitfn(fn2)
outfile = debug_dir + 'points' + name + '.jpg'
cv2.imwrite(outfile, img2)
if found:
img_names_undistort2.append(outfile)
img_points2.append(corners2.reshape(-1, 2))
obj_points2.append(pattern_points2)
############################################################################################
############################################################################################
#Se obtiene la matriz de cada camara (parametros intrinsecos)
camera_matrix1 = cv2.initCameraMatrix2D(obj_points, img_points, (w, h))
camera_matrix2 = cv2.initCameraMatrix2D(obj_points2, img_points2, (w2, h2))
term_crit = (cv2.TERM_CRITERIA_MAX_ITER + cv2.TERM_CRITERIA_EPS, 100, 1e-5)
#Se obtienen Parametros extrinsecos R y T de cada camara
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
def CalFromCam():
global rows
global columns
global params
idxCam = 0
# termination criteria
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001)
# prepare object points, like (0,0,0), (1,0,0), (2,0,0) ....,(6,5,0)
objp = np.zeros((columns*rows,3), np.float32)
objp[:,:2] = np.mgrid[0:rows,0:columns].T.reshape(-1,2)
# Arrays to store object points and image points from all the images.
objpoints = [] # 3d point in real world space
imgpoints = [] # 2d points in image plane.
cap = cv2.VideoCapture(0)
while(True):
# Capture frame-by-frame
ret, frame = cap.read()
img = frame
# Our operations on the frame come here
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Display the resulting frame
cv2.imshow('Proyecto2: Calibracion de camara capturando de camara. (Camara)', frame)
key = cv2.waitKey(2)
if key & 0xFF == ord('d'):
break
if key & 0xFF == ord('c'):
# Find the chess board corners
ret, corners = cv2.findChessboardCorners(gray, (rows,columns), cv2.CALIB_CB_FILTER_QUADS)
# If found, add object points, image points (after refining them)
if ret == True:
# Save original image for future used
cv2.imwrite(imgFolder + "/chessCam" + str(idxCam) + ".jpg", frame)
#Increase camera index for files
idxCam += 1
objpoints.append(objp)
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
imgpoints.append(corners2)
# Draw and display the corners
img = cv2.drawChessboardCorners(img, (rows,columns), corners2, ret)
#Show chessboard with found points
cv2.imshow('Proyecto2: Calibracion de camara capturando de camara. (Chessboard)', img)
cv2.waitKey(500)
else:
print("Chessboard corners not found! Please try again.\n")
cv2.destroyAllWindows()
camera_matrix = camera_matrix = cv2.initCameraMatrix2D(objpoints, imgpoints, gray.shape[::-1])
ret, mtx, dist, rvecs, tvecs = cv2.calibrateCamera(objpoints, imgpoints, gray.shape[::-1], camera_matrix, None, flags = cv2.CALIB_USE_INTRINSIC_GUESS)
np.savez('camera', mtx = mtx, dist = dist, rvecs = rvecs, tvecs = tvecs)
params['camera'].append({
'K': mtx.tolist(),
'dist': dist.tolist()
})
with open('params.txt', 'w') as outfile:
json.dump(params, outfile)
img = cv2.imread(imgFolder + '/' + imgBaseNm + str(imgBaseIdx) + '.jpg')
h, w = img.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1, (w, h))
# undistort
dst = cv2.undistort(img, mtx, dist, None, newcameramtx)
# crop the image
x, y, w, h = roi
dst = dst[y : y + h, x : x + w]
cv2.imwrite('calibresult.png', dst)
# Load previously saved data
with np.load('camera.npz') as X:
mtx, dist, _, _ = [X[i] for i in ('mtx','dist','rvecs','tvecs')]
axis = np.float32([[3,0,0], [0,3,0], [0,0,-3]]).reshape(-1,3)
print("Press d in camera window when you are done projecting.\n")
while(True):
# Capture frame-by-frame
ret, frame = cap.read()
img = frame
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
ret, corners = cv2.findChessboardCorners(gray, (rows, columns), cv2.CALIB_CB_FILTER_QUADS)
if ret == True:
corners2 = cv2.cornerSubPix(gray, corners, (11,11), (-1,-1), criteria)
# Find the rotation and translation vectors.
ret,rvecs, tvecs = cv2.solvePnP(objp, corners2, mtx, dist)
# project 3D points to image plane
imgpts, jac = cv2.projectPoints(axis, rvecs, tvecs, mtx, dist)
img = draw(img, corners2, imgpts)
key = cv2.waitKey(20)
if key & 0xFF == ord('d'):
break
cv2.imshow('Proyecto2: Calibracion de camara en modo almacenamiento. (Proyeccion)', img)
# When everything done, release the capture
cap.release()
cv2.destroyAllWindows()
def calibrate(self):
imgLL, imgRL = [], []
for name in glob.glob(self.images_dir + "/left*"):
imgLL.append(name)
for name in glob.glob(self.images_dir + "/right*"):
imgRL.append(name)
assert len(imgLL) == len(imgRL), "Number of the left images must be same with the right ones."
imgLL.sort()
imgRL.sort()
# keypoints params
scales = [1, 2]
square_size = 20. # Set this to your actual square size
# find corners
print("finding corners...")
good_image_pairs = []
image_points = [[], []]
image_shape = None
for i in range(len(imgLL)):
imgL = cv2.imread(imgLL[i], 0)
if image_shape is None:
image_shape = imgL.shape
else:
if image_shape != imgL.shape:
print("the shape of left image {} is {}, not same with {}.".format(
i, imgL.shape, image_shape))
continue
cornersL = findCornerMultiScale(imgL, self.border_size, scales)
if cornersL is None: continue
imgR = cv2.imread(imgRL[i], 0)
if image_shape is None:
image_shape = imgR.shape
else:
if image_shape != imgR.shape:
print("the shape of left image {} is {}, not same with {}.".format(
i, imgR.shape, image_shape))
continue
cornersR = findCornerMultiScale(imgR, self.border_size, scales)
if cornersR is None: continue
good_image_pairs.append([imgL, imgR])
image_points[0].append(cornersL)
image_points[1].append(cornersR)
n_image_pairs = len(good_image_pairs)
assert n_image_pairs > 1, "Error: number of image pairs is {}, " \
"which is too little pairs to run the calibration".format(n_image_pairs)
print("find {} good image pairs.".format(n_image_pairs))
objp = np.zeros((np.prod(self.border_size), 3), np.float32)
objp[:, :2] = np.indices(self.border_size).T.reshape(-1, 2)
objp *= square_size
object_points = [objp] * n_image_pairs
# initialize camera matrix
h, w = image_shape
camera_matrix = list()
camera_matrix.append(cv2.initCameraMatrix2D(object_points, image_points[0], (w, h), 0))
camera_matrix.append(cv2.initCameraMatrix2D(object_points, image_points[1], (w, h), 0))
# calibration
print("calibrating...")
term = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_COUNT, 100, 1e-5)
ret, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = \
cv2.stereoCalibrate(object_points, image_points[0], image_points[1], camera_matrix[0],
None, camera_matrix[1], None, (w, h),
flags=cv2.CALIB_FIX_ASPECT_RATIO | cv2.CALIB_ZERO_TANGENT_DIST | cv2.CALIB_USE_INTRINSIC_GUESS |
cv2.CALIB_SAME_FOCAL_LENGTH | cv2.CALIB_RATIONAL_MODEL | cv2.CALIB_FIX_K3 |
cv2.CALIB_FIX_K4 | cv2.CALIB_FIX_K5,
criteria=term)
# save calibration params
# print("cam1 {}, dist1 {}".format(cameraMatrix1, distCoeffs1))
# print('R={}, T={}, E={}, F={}.'.format(R, T, E, F))
self.stereo_camera_params = dict(cameraMatrix1=cameraMatrix1, distCoeffs1=distCoeffs1,
cameraMatrix2=cameraMatrix2, distCoeffs2=distCoeffs2,
R=R, T=T, E=E, F=F)
def estimate_intrinsics(point2d_coord,
point2d_fid,
model_point3d_coord,
img_shape,
estimate_dist=True,
dist_complexity=2,
verbose=1):
""" Estimates intrinsic parameters for each camera from the given 2D point correspondences.
Input:
point2d_coord: Nx2 np.array, Array containing 2D coordinates of N points.
point2d_fid: Nx2 np.array, Array containing the frame id for each of the N points.
model_point3d_coord: Nx3 np.array, Array containing the 3D coordinates in a marker based coordinate system for each of the N points.
img_shape: uple of two int, Shapes of the images (height1, width1).
estimate_dist: bool, If the distortion parameters should be estimated or not.
dist_complexity: int, Level of complexity of the distortion model; 1 = K1, 2 = K1 + K2, else = K1 + K2 + K3
Returns:
cam_intrinsic: list of 3x3 np.array, Intrinsic calibration of each camera.
cam_dist: list of 1x5 np.array, Distortion coefficients following the OpenCV pinhole camera model.
"""
assert point2d_coord.shape[0] == point2d_fid.shape[0], "Shape mismatch."
assert point2d_coord.shape[0] == model_point3d_coord.shape[
0], "Shape mismatch."
assert point2d_coord.shape[1] == 2, "Shape mismatch."
assert model_point3d_coord.shape[1] == 3, "Shape mismatch."
if verbose > 0:
print('------------')
print('- Estimating intrinsic parameters')
if verbose > 0:
startTime = time.time()
# accumulate points
img_points, object_points = list(), list()
pts_count, normal_list, hull_points_list = list(), list(), list()
for fid in np.unique(point2d_fid).tolist():
mask = np.array(point2d_fid) == fid
if np.sum(mask) < 4:
continue
# subset of visible 2d/3d points
points2d_obs = point2d_coord[mask, :]
model_point3d_coord_obs = model_point3d_coord[mask, :]
img_points.append(points2d_obs)
object_points.append(model_point3d_coord_obs)
# Coarse K approximation: It uses the relation between FOV alpha, sensor size w and focal length f:
# f = 1/ (2* tan[alpha/2]) * w
# assuming a FOV of 60deg -> f = 0.86 * w
# Another assumption is that the principal point is in the middle of the image
K_guess = np.array([[img_shape[1] * 0.86, 0.0, img_shape[1] * 0.5],
[0.0, img_shape[0] * 0.86, img_shape[0] * 0.5],
[0.0, 0.0, 1.0]])
success, r_rel, _ = cv2.solvePnP(np.expand_dims(
model_point3d_coord_obs, 1),
np.expand_dims(points2d_obs, 1),
K_guess,
distCoeffs=np.zeros((5, )),
flags=cv2.SOLVEPNP_ITERATIVE)
# get normal vector from tag rotation
R, _ = cv2.Rodrigues(r_rel)
n = np.matmul(R, np.array([0.0, 0.0, 1.0]))
normal_list.append(n)
# figure out non axis aligned bounding box
hull = ConvexHull(points2d_obs)
hull_points = points2d_obs[hull.vertices]
hull_points /= np.array([[img_shape[1], img_shape[0]]],
dtype=np.float32)
hull_points_list.append(hull_points)
pts_count.append(np.sum(mask))
if verbose > 0:
print('- For estimating intrinsic there are %d'
' 2D->3D correspondences' %
(sum([x.shape[0] for x in img_points])))
# parameters of the selection algorithm
img_size = 100
normal_thresh = 10.0 # when normals differ more than that angle we add it (angle in deg)
area_thresh = 0.1 # when new area is more than that we add it (percentage of image area)
def _score_normals(n1, n2):
""" Calculates the angle between two normals. """
return np.arccos(np.dot(n1, n2))
def _score_area(area_covered, hullpts, tmp_img_size=img_size):
""" Calculates the percentage of new area hullpts are adding to the current area covered. """
# scale points to tmp size
hullpts = hullpts.copy() * tmp_img_size
# draw tmp images
hullpts1 = hullpts.astype(np.int32).reshape([-1]).tolist()
map = Image.new('L', (tmp_img_size, tmp_img_size), 0)
ImageDraw.Draw(map).polygon(hullpts1, outline=1, fill=1)
# calculate how much is new
percentage_new = 1.0 - np.sum(np.logical_and(
map, area_covered)) / (np.sum(map) + 1e-6)
return percentage_new
def _hull2mask(hullpts, tmp_img_size=100):
""" Converts a convex hull (set of points) into an binary mask. """
# scale points to tmp size
hullpts = hullpts.copy() * tmp_img_size
# draw tmp images
hullpts1 = hullpts.astype(np.int32).reshape([-1]).tolist()
mask = Image.new('L', (tmp_img_size, tmp_img_size), 0)
ImageDraw.Draw(mask).polygon(hullpts1, outline=1, fill=1)
return mask
# select a good subset from the images we have
ind_selected = list() # subset of views we use
# sort by number of points visibile in view
sort_ind = np.argsort(pts_count)[::-1]
# greedily pick views
area_covered = np.zeros((img_size, img_size))
for i in sort_ind.tolist():
if pts_count[i] < 8:
# when there are too little point on the plane the normal estimation usually is bad
continue
# check how close this normal is to any we already selected
if len(ind_selected) > 0:
score_n = min([
_score_normals(normal_list[j], normal_list[i])
for j in ind_selected
]) * 180.0 / np.pi
else:
score_n = 0.0
# print('Difference to closest angle in selected set', score_n)
if score_n > normal_thresh:
# print('Added %d due to normal condition' % i)
ind_selected.append(i)
area_covered = np.logical_or(area_covered,
_hull2mask(hull_points_list[i]))
else:
score_a = _score_area(area_covered, hull_points_list[i])
# print('percentage_new', score_a)
if score_a > area_thresh:
# print('Added %d due to area condition' % i)
ind_selected.append(i)
area_covered = np.logical_or(area_covered,
_hull2mask(hull_points_list[i]))
# find out largest angle difference
angle_max = 0.0
for k1, ind1 in enumerate(ind_selected):
for k2 in range(k1 + 1, len(ind_selected)):
ind2 = ind_selected[k2]
angle_max = max(
angle_max,
_score_normals(normal_list[ind1], normal_list[ind2]))
angle_max *= 180.0 / np.pi
# stop when image is mostly covered and there is some minimal angular difference
if (np.mean(area_covered) > 0.8) and (angle_max > 30.0):
break
if verbose > 0:
print('- Estimating intrinsic for from subset of %d views yielding %d'
' 2D->3D correspondences' %
(len(ind_selected), sum([pts_count[i] for i in ind_selected])))
# take subset
object_points = [object_points[i].astype(np.float32) for i in ind_selected]
img_points = [img_points[i].astype(np.float32) for i in ind_selected]
if verbose > 0:
print('- Cam: For estimating intrinsic there are %d'
' 2D->3D correspondences' %
(sum([x.shape[0] for x in img_points])))
# find initial solution
K_init = cv2.initCameraMatrix2D(object_points, img_points,
(img_shape[1], img_shape[0]))
if verbose > 2:
print('- Cam K_init:')
print(K_init)
# estimate intrinsics for this cam
# scaling error up with number of points; otherwise this function takes forever when there are many points
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.001
) # accuracy should scale with number of points used
flags = cv2.CALIB_USE_INTRINSIC_GUESS
# disable most of the distortion parameters by default, because estimation is usually bad
if dist_complexity == 1:
flags += cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_FIX_K2 + cv2.CALIB_FIX_K3 + \
cv2.CALIB_FIX_K4 + cv2.CALIB_FIX_K5 + cv2.CALIB_FIX_K6
elif dist_complexity == 2:
flags += cv2.CALIB_ZERO_TANGENT_DIST + cv2.CALIB_FIX_K3 + \
cv2.CALIB_FIX_K4 + cv2.CALIB_FIX_K5 + cv2.CALIB_FIX_K6
else:
flags += cv2.CALIB_ZERO_TANGENT_DIST + \
cv2.CALIB_FIX_K4 + cv2.CALIB_FIX_K5 + cv2.CALIB_FIX_K6
if not estimate_dist:
# also turn off all factors
flags = cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_ZERO_TANGENT_DIST + \
cv2.CALIB_FIX_K1 + cv2.CALIB_FIX_K2 + cv2.CALIB_FIX_K3 + \
cv2.CALIB_FIX_K4 + cv2.CALIB_FIX_K5 + cv2.CALIB_FIX_K6
error, K, dist, _, _ = cv2.calibrateCamera(object_points,
img_points,
(img_shape[1], img_shape[0]),
K_init,
None,
criteria=criteria,
flags=flags)
if verbose > 2:
print('K')
print(K, '\n')
print('dist')
print(dist, '\n')
if verbose > 2:
print('- Estimating intrinsics took %.2f seconds' %
(time.time() - startTime))
if verbose > 0:
print('- Cam: Reprojection error over %d points: %.3f' %
(sum([x.shape[0] for x in img_points]), error))
print('------------')
return K, dist
(-1, -1), criteria)
imgpts_leftcam.append(corners_refined_l)
corners_refined_r = cv.cornerSubPix(gray_r, corners_r, (11, 11),
(-1, -1), criteria)
imgpts_rightcam.append(corners_refined_r)
count += 1
print(str(count) + " pairs successfully detected.")
'''2. Get single camera intrinsic matrix and distortion coefficients'''
criteria = (cv.TERM_CRITERIA_EPS + cv.TERM_CRITERIA_MAX_ITER, 100, 1e-05)
flag = cv.CALIB_FIX_ASPECT_RATIO + cv.CALIB_ZERO_TANGENT_DIST + cv.CALIB_USE_INTRINSIC_GUESS + \
cv.CALIB_FIX_K3 + cv.CALIB_FIX_K4 + cv.CALIB_FIX_K5
print("Calibrate left camera.")
retval1 = cv.initCameraMatrix2D(objpts, imgpts_leftcam, gray_l.shape[::-1], 0)
ret_l, mtx_l, dist_l, rvecs_l, tvecs_l = cv.calibrateCamera(objpts,
imgpts_leftcam,
gray_l.shape[::-1],
retval1,
None,
flags=flag,
criteria=criteria)
print("Calibrate right camera.")
retval2 = cv.initCameraMatrix2D(objpts, imgpts_leftcam, gray_l.shape[::-1], 0)
ret_r, mtx_r, dist_r, rvecs_r, tvecs_r = cv.calibrateCamera(objpts,
imgpts_rightcam,
gray_r.shape[::-1],
retval2,
None,
flags=flag,
#corners2 = cv2.cornerSubPix(gray,centres,(20, 20),(-1,-1), criteria)
#imgpoints.append(corners2)
# Draw and display the corners
# img2= cv2.drawChessboardCorners(img, (m, n), centres, ret)
#cv2.imwrite(fname[:-4]+'-pts.png', img2)
#cv2.waitKey(1000)
else:
print('Not found')
img_path = cwd + '/data/161025-2cam/test/'
images = glob.glob(img_path + 're*.png')
#ret, cameraMatrix0, distCoeffs0, rvecs, tvecs = cv2.calibrateCamera(objpoints_c0, imgpoints_c0, (640, 480), None, None)
cameraMatrix0_int = cv2.initCameraMatrix2D(objpoints_c0, imgpoints_c0,
(640, 480), 1.33)
scale0, cameraMatrix0, distCoeffs0, r0, t0 = cv2.calibrateCamera(
objpoints_c0, imgpoints_c0, (640, 480), cameraMatrix0_int, None, None)
#print (scale0, cameramatrix0, distCoeffs0, r0, t0)
for fname in images:
print(fname)
img = cv2.imread(fname)
ret, centres = cv2.findCirclesGrid(img, (m, n),
term_crit) #, blobDetector)
# If found, add object points, image points (after refining them)
if ret == True:
print('found')
def run(self):
idxrange = range(1, 27)
img0 = cv2.imread(
os.path.join(thisdir,
'../data/castle/castle.%03d.jpg' % (idxrange[0], )))
w, h = getsize(img0)
prev_keypoints, prev_descrs = self.detector.detectAndCompute(
img0.copy(), None)
self.matcher.add([prev_descrs.astype(np.uint8)])
for imgidx in idxrange:
print('-------------------------------------------------------')
print(
os.path.join(thisdir, '../data/castle/castle.%03d.jpg' %
(imgidx, )) + '\n' +
os.path.join(thisdir, '../data/castle/castle.%03d.jpg' %
(imgidx + 1, )))
img1 = cv2.imread(
os.path.join(thisdir,
'../data/castle/castle.%03d.jpg' % (imgidx, )))
img2 = cv2.imread(
os.path.join(thisdir, '../data/castle/castle.%03d.jpg' %
(imgidx + 1, )))
if img1 is None or img2 is None:
raise Exception('Fail to open images.')
# Detect and match keypoints
prev_keypoints, prev_descrs = self.detector.detectAndCompute(
img1.copy(), None)
curr_keypoints, curr_descrs = self.detector.detectAndCompute(
img2.copy(), None)
self.matcher.clear()
self.matcher.add([prev_descrs.astype(np.uint8)])
matches = self.matcher.knnMatch(curr_descrs, k=2)
matches = [
m[0] for m in matches
if len(m) == 2 and m[0].distance < m[1].distance * 0.75
]
print('%d matches.' % len(matches))
p0 = [prev_keypoints[m.trainIdx].pt for m in matches]
p1 = [curr_keypoints[m.queryIdx].pt for m in matches]
p0, p1 = np.float32((p0, p1))
# Skip first two frames
# if imgidx<2: continue
# Estimate homography
H, status = cv2.findHomography(p0, p1, cv2.RANSAC, 13.0)
status = status.ravel() != 0
print('inliner percentage: %.1f %%' %
(status.mean().item(0) * 100., ))
if status.sum() < MIN_MATCH_COUNT:
continue
p0, p1 = p0[status], p1[status]
# Display inliners
imgpair = cv2.addWeighted(img2, .5, img1, .5, 0)
draw_matches(imgpair, p0, p1)
cv2.imshow('keypoint matches', imgpair)
# Estimate fundamental matrix
F, status = cv2.findFundamentalMat(p0, p1, cv2.FM_8POINT, 3, .99)
# Estimate camera matrix
p0 = np.hstack((p0, np.ones((p0.shape[0], 1)))).astype(np.float32)
K = cv2.initCameraMatrix2D([p0], [p1], (w, h))
p0 = p0[:, :2]
# Estimate essential matrix
E, status = cv2.findEssentialMat(p0, p1, cameraMatrix=K)
ret, R, t, status = cv2.recoverPose(E,
p0,
p1,
cameraMatrix=K,
mask=status)
rvec, jacobian = cv2.Rodrigues(R)
print('(R, t)=', rvec.T, '\n', t.T)
# Dense rectification
retval, H1, H2 = cv2.stereoRectifyUncalibrated(p0, p1, F, (w, h))
# Triangulation
projMat1 = np.hstack((np.eye(3), np.zeros((3, 1))))
projMat2 = get_P_prime_from_F(F)
points4D = cv2.triangulatePoints(projMat1, projMat2, p0.T, p1.T)
# Plot triangulation results
objectPoints = points4D.T[:, :3].astype(np.float32)
print(objectPoints)
np.savetxt('pts.txt', objectPoints, fmt='%.5f')
plt.close()
fig, axarr = plt.subplots(2, 2)
axarr[0, 0].plot(p0[:, 0], 480 - p0[:, 1], '.')
axarr[0, 1].plot(p1[:, 0], 480 - p1[:, 1], '.')
axarr[1, 0].plot(-objectPoints[:, 2], -objectPoints[:, 1], '.')
axarr[1, 1].plot(-objectPoints[:, 2], objectPoints[:, 0], '.')
plt.ion()
plt.draw()
plt.waitforbuttonpress(1)
retval, rvec, tvec, inliners = cv2.solvePnPRansac(
objectPoints, p1.astype(np.float32), K, np.zeros((1, 4)))
print('rvec, tvec = ', rvec.T, '\n', tvec.T)
rectified = np.zeros((h, w, 3), np.uint8)
disparity = np.zeros((h, w, 3), np.float32)
warpSize = (int(w * .9), int(h * .9))
img1_warp = cv2.warpPerspective(img1, H1, warpSize)
img2_warp = cv2.warpPerspective(img2, H2, warpSize)
rectified = cv2.addWeighted(img1_warp, .5, img2_warp, .5, 0)
disparity = self.stereoMatcher.compute(
cv2.cvtColor(img1_warp, cv2.COLOR_BGR2GRAY),
cv2.cvtColor(img2_warp, cv2.COLOR_BGR2GRAY))
disparity, buf = cv2.filterSpeckles(disparity, -self.maxDiff,
pow(20, 2), self.maxDiff)
rectified = cv2.addWeighted(img1_warp, .5, img2_warp, .5, 0)
cv2.imshow('rectified', rectified)
cv2.imshow('disparity', ((disparity + 50) * .5).astype(np.uint8))
[exit(0) if cv2.waitKey() & 0xff == 27 else None]
prev_keypoints, prev_descrs = curr_keypoints, curr_descrs
def singleCamCalib(umeas, xworld, planeData, cameraData):
"""
Inputs
-------------------------------------------
umeas 2 x nX*nY*nplanes x ncams array of image points in each camera
planeData struct of image related parameters
cameraData struct of camera related parameters
Returns
-------------------------------------------
cameraMats 3 x 3 x ncams: camera matrices for individual cameras
boardRotMats 3 x 3 x nplanes x ncams: board rotation matrix per image per camera
boardTransVecs 3 x nplanes x ncams: board translational vector per image per camera
"""
log.info("Single camera calibrations")
# set up implementation variables
nX = planeData.nX
nY = planeData.nY
nplanes = planeData.ncalplanes
ncams = cameraData.ncams
imageSize = (cameraData.sX, cameraData.sY)
aspectRatio = cameraData.sX / cameraData.sY
# set up storage variables
cameraMats = np.zeros([3, 3, ncams])
boardRotMats = np.zeros([3, 3, nplanes, ncams])
boardTransVecs = np.zeros([3, nplanes, ncams])
# cycle through each camera
log.info("Cycle through each camera")
for c in range(0, ncams):
log.VLOG(2, "Calibrating camera %d", c)
# calculate initial camera matrix for this camera
# use all world points but only image points in this camera
camUmeas = np.transpose(umeas[:, :, c]).astype('float32')
camMatrix = cv2.initCameraMatrix2D([xworld], [camUmeas], imageSize,
aspectRatio)
log.VLOG(4, "with Intrinsic Parameter Matrix %s", camMatrix)
# iterate through each board placement to find rotation matrix and rotation vector
for n in range(0, nplanes):
log.VLOG(3, "%%% on image {}".format(n))
# isolate only points related to this board's placement
currWorld = xworld[nX * nY * n:nX * nY * (n + 1), :]
currImage = camUmeas[nX * nY * n:nX * nY * (n + 1), :]
# find the board's rotation matrix and translation vector
_, rotvec, transVector = cv2.solvePnP(
currWorld, currImage, camMatrix,
np.zeros((8, 1), dtype='float32'))
# Convert rotation vector to rotation matrix.
rotMatrix, _ = cv2.Rodrigues(rotvec)
log.VLOG(4, "Rotation matrix for camera %d image %d: %s", c, n,
rotMatrix)
log.VLOG(4, "Translational vector for camera %d image %d: %s", c,
n, transVector)
# add board values to storage variables
boardRotMats[:, :, n, c] = rotMatrix
boardTransVecs[:, n, c] = transVector.ravel()
# add camera matrix to storage variable
cameraMats[:, :, c] = camMatrix
return [cameraMats, boardRotMats, boardTransVecs]
