def calibrate_camera_interactive(images, objp, boardSize):
# Arrays to store object points and image points from all the images.
objectPoints = [] # 3d point in real world space
imagePoints = [] # 2d points in image plane
test_image = cv2.imread(images[0])
imageSize = (test_image.shape[1], test_image.shape[0])
# Read images
for fname in images:
img = cv2.imread(fname)
ret, corners = cvh.extractChessboardFeatures(img, boardSize)
# If chessboard corners are found, add object points and image points
if ret == True:
objectPoints.append(objp)
imagePoints.append(corners)
# Draw and display the corners
cv2.drawChessboardCorners(
img, boardSize, corners, ret )
cv2.imshow("img", img)
cv2.waitKey(100)
# Calibration
reproj_error, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(
objectPoints, imagePoints, imageSize )
distCoeffs = distCoeffs.reshape((-1)) # convert to vector
return reproj_error, cameraMatrix, distCoeffs, rvecs, tvecs, \
objectPoints, imagePoints, imageSize
def calibrate_camera_interactive(images, objp, boardSize):
# Arrays to store object points and image points from all the images.
objectPoints = [] # 3d point in real world space
imagePoints = [] # 2d points in image plane
test_image = cv2.imread(images[0])
imageSize = (test_image.shape[1], test_image.shape[0])
# Read images
for fname in images:
img = cv2.imread(fname)
ret, corners = cvh.extractChessboardFeatures(img, boardSize)
# If chessboard corners are found, add object points and image points
if ret == True:
objectPoints.append(objp)
imagePoints.append(corners)
# Draw and display the corners
cv2.drawChessboardCorners(img, boardSize, corners, ret)
cv2.imshow("img", img)
cv2.waitKey(100)
# Calibration
reproj_error, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.calibrateCamera(
objectPoints, imagePoints, imageSize)
distCoeffs = distCoeffs.reshape((-1)) # convert to vector
return reproj_error, cameraMatrix, distCoeffs, rvecs, tvecs, \
objectPoints, imagePoints, imageSize
def calibrate_relative_poses_interactive(image_sets, cameraMatrixs, distCoeffss, imageSizes, boardSizes, board_scales, board_rvecs, board_tvecs):
"""
Make an estimate of the relative poses (as 4x4 projection matrices) between many cameras.
Base these relative poses to the first camera.
Each camera should be looking at its own chessboard,
use different sizes of chessboards if a camera sees chessboard that are not associated with that camera.
'board_scales' scales the chessboard-units to world-units.
'board_rvecs' and 'board_tvecs' transform the rescaled local chessboard-coordinates to world-coordinates.
The inverse of the reprojection error is used for weighting.
"""
num_cams = len(image_sets)
num_images = len(image_sets[0])
reproj_error_max = 0
# Preprocess object-points of the different boards
board_objps = []
for boardSize, board_scale, board_rvec, board_tvec in zip(
boardSizes, board_scales, board_rvecs, board_tvecs ):
objp = np.ones((np.prod(boardSize), 4))
objp[:, 0:3] = calibration_tools.grid_objp(boardSize) * board_scale
objp = objp.dot(trfm.P_from_R_and_t(cvh.Rodrigues(board_rvec), np.array(board_tvec).reshape(3, 1))[0:3, :].T)
board_objps.append(objp)
# Calculate all absolute poses
Ps = np.zeros((num_images, num_cams, 4, 4))
weights = np.zeros((num_images, 1, 1, 1))
for i, images in enumerate(zip(*image_sets)):
reproj_error = 0
for c, (image, cameraMatrix, distCoeffs, imageSize, boardSize, board_objp) in enumerate(zip(
images, cameraMatrixs, distCoeffss, imageSizes, boardSizes, board_objps )):
img = cv2.imread(image)
ret, corners = cvh.extractChessboardFeatures(img, boardSize)
if not ret:
print ("Error: Image '%s' didn't contain a chessboard of size %s." % (image, boardSize))
return False, None
# Draw and display the corners
cv2.drawChessboardCorners(
img, boardSize, corners, ret )
cv2.imshow("img", img)
cv2.waitKey(100)
ret, rvec, tvec = cv2.solvePnP(board_objp, corners, cameraMatrix, distCoeffs)
Ps[i, c, :, :] = trfm.P_from_R_and_t(cvh.Rodrigues(rvec), tvec)
reproj_error = max(reprojection_error( # max: worst case
[board_objp], [corners], cameraMatrix, distCoeffs, [rvec], [tvec] )[1], reproj_error)
reproj_error_max = max(reproj_error, reproj_error_max)
weights[i] = 1. / reproj_error
# Apply weighting on Ps, and rebase against first camera
Ps *= weights / weights.sum()
Ps = Ps.sum(axis=0)
Pref_inv = trfm.P_inv(Ps[0, :, :]) # use first cam as reference
return True, [P.dot(Pref_inv) for P in Ps], reproj_error_max
def calibrate_relative_poses_interactive(image_sets, cameraMatrixs,
distCoeffss, imageSizes, boardSizes,
board_scales, board_rvecs,
board_tvecs):
"""
Make an estimate of the relative poses (as 4x4 projection matrices) between many cameras.
Base these relative poses to the first camera.
Each camera should be looking at its own chessboard,
use different sizes of chessboards if a camera sees chessboard that are not associated with that camera.
'board_scales' scales the chessboard-units to world-units.
'board_rvecs' and 'board_tvecs' transform the rescaled local chessboard-coordinates to world-coordinates.
The inverse of the reprojection error is used for weighting.
"""
num_cams = len(image_sets)
num_images = len(image_sets[0])
reproj_error_max = 0
# Preprocess object-points of the different boards
board_objps = []
for boardSize, board_scale, board_rvec, board_tvec in zip(
boardSizes, board_scales, board_rvecs, board_tvecs):
objp = np.ones((np.prod(boardSize), 4))
objp[:, 0:3] = calibration_tools.grid_objp(boardSize) * board_scale
objp = objp.dot(
trfm.P_from_R_and_t(cvh.Rodrigues(board_rvec),
np.array(board_tvec).reshape(3, 1))[0:3, :].T)
board_objps.append(objp)
# Calculate all absolute poses
Ps = np.zeros((num_images, num_cams, 4, 4))
weights = np.zeros((num_images, 1, 1, 1))
for i, images in enumerate(zip(*image_sets)):
reproj_error = 0
for c, (image, cameraMatrix, distCoeffs, imageSize, boardSize,
board_objp) in enumerate(
zip(images, cameraMatrixs, distCoeffss, imageSizes,
boardSizes, board_objps)):
img = cv2.imread(image)
ret, corners = cvh.extractChessboardFeatures(img, boardSize)
if not ret:
print(
"Error: Image '%s' didn't contain a chessboard of size %s."
% (image, boardSize))
return False, None
# Draw and display the corners
cv2.drawChessboardCorners(img, boardSize, corners, ret)
cv2.imshow("img", img)
cv2.waitKey(100)
ret, rvec, tvec = cv2.solvePnP(board_objp, corners, cameraMatrix,
distCoeffs)
Ps[i, c, :, :] = trfm.P_from_R_and_t(cvh.Rodrigues(rvec), tvec)
reproj_error = max(
reprojection_error( # max: worst case
[board_objp], [corners], cameraMatrix, distCoeffs, [rvec],
[tvec])[1],
reproj_error)
reproj_error_max = max(reproj_error, reproj_error_max)
weights[i] = 1. / reproj_error
# Apply weighting on Ps, and rebase against first camera
Ps *= weights / weights.sum()
Ps = Ps.sum(axis=0)
Pref_inv = trfm.P_inv(Ps[0, :, :]) # use first cam as reference
return True, [P.dot(Pref_inv) for P in Ps], reproj_error_max
def triangl_pose_est_interactive(img_left, img_right, cameraMatrix, distCoeffs,
imageSize, objp, boardSize, nonplanar_left,
nonplanar_right):
""" (TODO: remove debug-prints)
Triangulation and relative pose estimation will be performed from LEFT to RIGHT image.
Both images have to contain the whole chessboard,
in order to make a decent estimation of the relative pose based on 'solvePnP'.
Then the user can manually create matches between non-planar objects.
If the user omits this step, only triangulation of the chessboard corners will be performed,
and they will be compared to the real 3D points.
Otherwise the coordinates of the triangulated points of the manually matched points will be printed,
and a relative pose estimation will be performed (using the essential matrix),
this pose estimation will be compared with the decent 'solvePnP' estimation.
In that case you will be asked whether you want to mute the chessboard corners in all calculations,
note that this requires at least 8 pairs of matches corresponding with non-planar geometry.
During manual matching process,
switching between LEFT and RIGHT image will be done in a zigzag fashion.
To stop selecting matches, press SPACE.
Additionally, you can also introduce predefined non-planar geometry,
this is e.g. useful if you don't want to select non-planar geometry manually.
"""
### Setup initial state, and calc some very accurate poses to compare against with later
K_left = cameraMatrix
K_right = cameraMatrix
K_inv_left = cvh.invert(K_left)
K_inv_right = cvh.invert(K_right)
distCoeffs_left = distCoeffs
distCoeffs_right = distCoeffs
# Extract chessboard features, together they form a set of 'planar' points
ret_left, planar_left = cvh.extractChessboardFeatures(img_left, boardSize)
ret_right, planar_right = cvh.extractChessboardFeatures(
img_right, boardSize)
if not ret_left or not ret_right:
print("Chessboard is not (entirely) in sight, aborting.")
return
# Save exact 2D and 3D points of the chessboard
num_planar = planar_left.shape[0]
planar_left_orig, planar_right_orig = planar_left, planar_right
objp_orig = objp
# Calculate P matrix of left pose, in reality this one is known
ret, rvec_left, tvec_left = cv2.solvePnP(objp, planar_left, K_left,
distCoeffs_left)
P_left = trfm.P_from_R_and_t(cvh.Rodrigues(rvec_left), tvec_left)
# Calculate P matrix of right pose, in reality this one is unknown
ret, rvec_right, tvec_right = cv2.solvePnP(objp, planar_right, K_right,
distCoeffs_right)
P_right = trfm.P_from_R_and_t(cvh.Rodrigues(rvec_right), tvec_right)
# Load previous non-planar inliers, if desired
if nonplanar_left.size and not raw_input(
"Type 'no' if you don't want to use previous non-planar inliers? "
).strip().lower() == "no":
nonplanar_left = map(tuple, nonplanar_left)
nonplanar_right = map(tuple, nonplanar_left)
else:
nonplanar_left = []
nonplanar_right = []
### Create predefined non-planar 3D geometry, to enable automatic selection of non-planar points
if raw_input(
"Type 'yes' if you want to create and select predefined non-planar geometry? "
).strip().lower() == "yes":
# A cube
cube_coords = np.array([(0., 0., 0.), (1., 0., 0.), (1., 1., 0.),
(0., 1., 0.), (0., 0., 1.), (1., 0., 1.),
(1., 1., 1.), (0., 1., 1.)])
cube_coords *= 2 # scale
cube_edges = np.array([(0, 1), (1, 2), (2, 3), (3, 0), (4, 5), (5, 6),
(6, 7), (7, 4), (0, 4), (1, 5), (2, 6), (3, 7)])
# An 8-point circle
s2 = 1. / np.sqrt(2)
circle_coords = np.array([(1., 0., 0.), (s2, s2, 0.), (0., 1., 0.),
(-s2, s2, 0.), (-1., 0., 0.), (-s2, -s2, 0.),
(0., -1., 0.), (s2, -s2, 0.)])
circle_edges = np.array([(i, (i + 1) % 8) for i in range(8)])
# Position 2 cubes and 2 circles in the scene
cube1 = np.array(cube_coords)
cube1[:, 1] -= 1
cube2 = np.array(cube_coords)
cube2 = cvh.Rodrigues((0., 0., pi / 4)).dot(cube2.T).T
cube2[:, 0] += 4
cube2[:, 1] += 3
cube2[:, 2] += 2
circle1 = np.array(circle_coords)
circle1 *= 2
circle1[:, 1] += 5
circle1[:, 2] += 2
circle2 = np.array(circle_coords)
circle2 = cvh.Rodrigues((pi / 2, 0., 0.)).dot(circle2.T).T
circle2[:, 1] += 5
circle2[:, 2] += 2
# Print output to be used in Blender (see "calibrate_pose_visualization.blend")
print()
print("Cubes")
print("edges_cube = \\\n", map(list, cube_edges))
print("coords_cube1 = \\\n", map(list, cube1))
print("coords_cube2 = \\\n", map(list, cube2))
print()
print("Circles")
print("edges_circle = \\\n", map(list, circle_edges))
print("coords_circle1 = \\\n", map(list, circle1))
print("coords_circle2 = \\\n", map(list, circle2))
print()
color = rgb(0, 200, 150)
for verts, edges in zip(
[cube1, cube2, circle1, circle2],
[cube_edges, cube_edges, circle_edges, circle_edges]):
out_left = cvh.wireframe3DGeometry(img_left, verts, edges, color,
rvec_left, tvec_left, K_left,
distCoeffs_left)
out_right = cvh.wireframe3DGeometry(img_right, verts, edges, color,
rvec_right, tvec_right,
K_right, distCoeffs_right)
valid_match_idxs = [
i for i, (pl, pr) in enumerate(zip(out_left, out_right))
if 0 <= min(pl[0], pr[0]) <= max(pl[0], pr[0]) < imageSize[0]
and 0 <= min(pl[1], pr[1]) <= max(pl[1], pr[1]) < imageSize[1]
]
nonplanar_left += map(tuple,
out_left[valid_match_idxs]) # concatenate
nonplanar_right += map(tuple,
out_right[valid_match_idxs]) # concatenate
nonplanar_left = np.rint(nonplanar_left).astype(int)
nonplanar_right = np.rint(nonplanar_right).astype(int)
### User can manually create matches between non-planar objects
class ManualMatcher:
def __init__(self, window_name, images, points):
self.window_name = window_name
self.images = images
self.points = points
self.img_idx = 0 # 0: left; 1: right
cv2.namedWindow(window_name)
cv2.setMouseCallback(window_name, self.onMouse)
def onMouse(self, event, x, y, flags, userdata):
if event == cv2.EVENT_LBUTTONDOWN:
self.points[self.img_idx].append((x, y))
elif event == cv2.EVENT_LBUTTONUP:
# Switch images in a ping-pong fashion
if len(self.points[0]) != len(self.points[1]):
self.img_idx = 1 - self.img_idx
def run(self):
print("Select your matches. Press SPACE when done.")
while True:
img = cv2.drawKeypoints(self.images[self.img_idx], [
cv2.KeyPoint(p[0], p[1], 7.)
for p in self.points[self.img_idx]
],
color=rgb(0, 0, 255))
cv2.imshow(self.window_name, img)
key = cv2.waitKey(50) & 0xFF
if key == ord(' '): break # finish if SPACE is pressed
num_points_diff = len(self.points[0]) - len(self.points[1])
if num_points_diff:
del self.points[num_points_diff < 0][-abs(num_points_diff):]
print("Selected", len(self.points[0]), "pairs of matches.")
# Execute the manual matching
nonplanar_left, nonplanar_right = list(nonplanar_left), list(
nonplanar_right)
ManualMatcher("Select match-points of non-planar objects",
[img_left, img_right],
[nonplanar_left, nonplanar_right]).run()
num_nonplanar = len(nonplanar_left)
has_nonplanar = (num_nonplanar > 0)
if 0 < num_nonplanar < 8:
print("Warning: you've selected less than 8 pairs of matches.")
nonplanar_left = np.array(nonplanar_left).reshape(num_nonplanar, 2)
nonplanar_right = np.array(nonplanar_right).reshape(num_nonplanar, 2)
mute_chessboard_corners = False
if num_nonplanar >= 8 and raw_input(
"Type 'yes' if you want to exclude chessboard corners from calculations? "
).strip().lower() == "yes":
mute_chessboard_corners = True
num_planar = 0
planar_left = np.zeros((0, 2))
planar_right = np.zeros((0, 2))
objp = np.zeros((0, 3))
print("Chessboard corners muted.")
has_prev_triangl_points = not mute_chessboard_corners # normally in this example, this should always be True, unless user forces False
### Undistort points => normalized coordinates
allfeatures_left = np.concatenate((planar_left, nonplanar_left))
allfeatures_right = np.concatenate((planar_right, nonplanar_right))
allfeatures_nrm_left = cv2.undistortPoints(np.array([allfeatures_left]),
K_left, distCoeffs_left)[0]
allfeatures_nrm_right = cv2.undistortPoints(np.array([allfeatures_right]),
K_right, distCoeffs_right)[0]
planar_nrm_left, nonplanar_nrm_left = allfeatures_nrm_left[:planar_left.shape[
0]], allfeatures_nrm_left[planar_left.shape[0]:]
planar_nrm_right, nonplanar_nrm_right = allfeatures_nrm_right[:planar_right.shape[
0]], allfeatures_nrm_right[planar_right.shape[0]:]
### Calculate relative pose using the essential matrix
# Only do pose estimation if we've got 2D points of non-planar geometry
if has_nonplanar:
# Determine inliers by calculating the fundamental matrix on all points,
# except when mute_chessboard_corners is True: only use nonplanar_nrm_left and nonplanar_nrm_right
F, status = cv2.findFundamentalMat(
allfeatures_nrm_left, allfeatures_nrm_right, cv2.FM_RANSAC,
0.006 * np.amax(allfeatures_nrm_left),
0.99) # threshold from [Snavely07 4.1]
# OpenCV BUG: "status" matrix is not initialized with zeros in some cases, reproducable with 2 sets of 8 2D points equal to (0., 0.)
# maybe because "_mask.create()" is not called:
# https://github.com/Itseez/opencv/blob/7e2bb63378dafb90063af40caff20c363c8c9eaf/modules/calib3d/src/ptsetreg.cpp#L185
# Workaround to test on outliers: use "!= 1", instead of "== 0"
print(status.T)
inlier_idxs = np.where(status == 1)[0]
print("Removed", allfeatures_nrm_left.shape[0] - inlier_idxs.shape[0],
"outlier(s).")
num_planar = np.where(inlier_idxs < num_planar)[0].shape[0]
num_nonplanar = inlier_idxs.shape[0] - num_planar
print("num chessboard inliers:", num_planar)
allfeatures_left, allfeatures_right = allfeatures_left[
inlier_idxs], allfeatures_right[inlier_idxs]
allfeatures_nrm_left, allfeatures_nrm_right = allfeatures_nrm_left[
inlier_idxs], allfeatures_nrm_right[inlier_idxs]
if not mute_chessboard_corners:
objp = objp[inlier_idxs[:num_planar]]
planar_left, planar_right = allfeatures_left[:
num_planar], allfeatures_right[:
num_planar]
planar_nrm_left, planar_nrm_right = allfeatures_nrm_left[:
num_planar], allfeatures_nrm_right[:
num_planar]
nonplanar_nrm_left, nonplanar_nrm_right = allfeatures_nrm_left[
num_planar:], allfeatures_nrm_right[num_planar:]
# Calculate first solution of relative pose
if allfeatures_nrm_left.shape[0] >= 8:
F, status = cv2.findFundamentalMat(allfeatures_nrm_left,
allfeatures_nrm_right,
cv2.FM_8POINT)
print("F:")
print(F)
else:
print(
"Error: less than 8 pairs of inliers found, I can't perform the 8-point algorithm."
)
#E = (K_right.T) .dot (F) .dot (K_left) # "Multiple View Geometry in CV" by Hartley&Zisserman (9.12)
E = F # K = I because we already normalized the coordinates
print("Correct determinant of essential matrix?",
(abs(cv2.determinant(E)) <= 1e-7))
w, u, vt = cv2.SVDecomp(E, flags=cv2.SVD_MODIFY_A)
print(w)
if ((w[0] < w[1] and w[0] / w[1]) or
(w[1] < w[0] and w[1] / w[0]) or 0) < 0.7:
print(
"Essential matrix' 'w' vector deviates too much from expected")
W = np.array([
[0., -1., 0.], # Hartley&Zisserman (9.13)
[1., 0., 0.],
[0., 0., 1.]
])
R = (u).dot(W).dot(vt) # Hartley&Zisserman result 9.19
det_R = cv2.determinant(R)
print("Coherent rotation?", (abs(det_R) - 1 <= 1e-7))
if det_R - (
-1
) < 1e-7: # http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid
# E *= -1:
vt *= -1 # svd(-E) = u * w * (-v).T
R *= -1 # => u * W * (-v).T = -R
print("det(R) == -1, compensated.")
t = u[:, 2:3] # Hartley&Zisserman result 9.19
P = trfm.P_from_R_and_t(R, t)
# Select right solution where the (center of the) 3d points are/is in front of both cameras,
# when has_prev_triangl_points == True, we can do it a lot faster.
test_point = np.ones((4, 1)) # 3D point to test the solutions with
if has_prev_triangl_points:
print("Using advantage of already triangulated points' position")
print(P)
# Select the closest already triangulated cloudpoint idx to the center of the cloud
center_of_mass = objp.sum(axis=0) / objp.shape[0]
center_objp_idx = np.argmin(
((objp - center_of_mass)**2).sum(axis=1))
print("center_objp_idx:", center_objp_idx)
# Select the corresponding image points
center_imgp_left = planar_nrm_left[center_objp_idx]
center_imgp_right = planar_nrm_right[center_objp_idx]
# Select the corresponding 3D point
test_point[0:3, :] = objp[center_objp_idx].reshape(3, 1)
print("test_point:")
print(test_point)
test_point = P_left.dot(
test_point
) # set the reference axis-system to the one of the left camera, note that are_points_in_front_of_left_camera is automatically True
center_objp_triangl, triangl_status = iterative_LS_triangulation(
center_imgp_left.reshape(1, 2), np.eye(4),
center_imgp_right.reshape(1, 2), P)
print("triangl_status:", triangl_status)
if (center_objp_triangl).dot(test_point[0:3, 0:1]) < 0:
P[0:3, 3:4] *= -1 # do a baseline reversal
print(P, "fixed triangulation inversion")
if P[0:3, :].dot(test_point)[
2, 0] < 0: # are_points_in_front_of_right_camera is False
P[0:3, 0:3] = (u).dot(W.T).dot(
vt) # use the other solution of the twisted pair ...
P[0:3, 3:4] *= -1 # ... and also do a baseline reversal
print(P, "fixed camera projection inversion")
elif num_nonplanar > 0:
print(
"Doing all ambiguity checks since there are no already triangulated points"
)
print(P)
for i in range(4): # check all 4 solutions
objp_triangl, triangl_status = iterative_LS_triangulation(
nonplanar_nrm_left, np.eye(4), nonplanar_nrm_right, P)
print("triangl_status:", triangl_status)
center_of_mass = objp_triangl.sum(axis=0) / len(
objp_triangl
) # select the center of the triangulated cloudpoints
test_point[0:3, :] = center_of_mass.reshape(3, 1)
print("test_point:")
print(trfm.P_inv(P_left).dot(test_point))
if np.eye(3, 4).dot(test_point)[2, 0] > 0 and P[0:3, :].dot(
test_point
)[2, 0] > 0: # are_points_in_front_of_cameras is True
break
if i % 2:
P[0:3, 0:3] = (u).dot(W.T).dot(
vt) # use the other solution of the twisted pair
print(P, "using the other solution of the twisted pair")
else:
P[0:3, 3:4] *= -1 # do a baseline reversal
print(P, "doing a baseline reversal")
are_points_in_front_of_left_camera = (np.eye(
3, 4).dot(test_point)[2, 0] > 0)
are_points_in_front_of_right_camera = (P[0:3, :].dot(test_point)[2, 0]
> 0)
print("are_points_in_front_of_cameras?",
are_points_in_front_of_left_camera,
are_points_in_front_of_right_camera)
if not (are_points_in_front_of_left_camera
and are_points_in_front_of_right_camera):
print("No valid solution found!")
P_left_result = trfm.P_inv(P).dot(P_right)
P_right_result = P.dot(P_left)
print("P_left")
print(P_left)
print("P_rel")
print(P)
print("P_right")
print(P_right)
print(
"=> error:",
reprojection_error([objp_orig] * 2,
[planar_left_orig, planar_right_orig],
cameraMatrix, distCoeffs,
[rvec_left, rvec_right],
[tvec_left, tvec_right])[1])
print("P_left_result")
print(P_left_result)
print("P_right_result")
print(P_right_result)
# We use "P_left" instead of "P_left_result" because the latter depends on the unknown "P_right"
print(
"=> error:",
reprojection_error([objp_orig] * 2,
[planar_left_orig, planar_right_orig],
cameraMatrix, distCoeffs, [
cvh.Rodrigues(P_left[0:3, 0:3]),
cvh.Rodrigues(P_right_result[0:3, 0:3])
], [P_left[0:3, 3], P_right_result[0:3, 3]])[1])
### Triangulate
objp_result = np.zeros((0, 3))
if has_prev_triangl_points:
# Do triangulation of all points
# NOTICE: in a real case, we should only use not yet triangulated points that are in sight
objp_result, triangl_status = iterative_LS_triangulation(
allfeatures_nrm_left, P_left, allfeatures_nrm_right, P_right)
print("triangl_status:", triangl_status)
elif num_nonplanar > 0:
# We already did the triangulation during the pose estimation, but we still need to backtransform them from the left camera axis-system
objp_result = trfm.P_inv(P_left).dot(
np.concatenate((objp_triangl.T, np.ones((1, len(objp_triangl))))))
objp_result = objp_result[0:3, :].T
print(objp_triangl)
print("objp:")
print(objp)
print(
"=> error:",
reprojection_error([objp_orig] * 2,
[planar_left_orig, planar_right_orig], cameraMatrix,
distCoeffs, [rvec_left, rvec_right],
[tvec_left, tvec_right])[1])
print("objp_result of chessboard:")
print(objp_result[:num_planar, :])
if has_nonplanar:
print("objp_result of non-planar geometry:")
print(objp_result[num_planar:, :])
if num_planar + num_nonplanar == 0:
print("=> error: undefined")
else:
print(
"=> error:",
reprojection_error([objp_result] * 2,
[allfeatures_left, allfeatures_right],
cameraMatrix, distCoeffs,
[rvec_left, rvec_right],
[tvec_left, tvec_right])[1])
### Print total combined reprojection error
# We only have both pose estimation and triangulation if we've got 2D points of non-planar geometry
if has_nonplanar:
if num_planar + num_nonplanar == 0:
print("=> error: undefined")
else:
# We use "P_left" instead of "P_left_result" because the latter depends on the unknown "P_right"
print(
"Total combined error:",
reprojection_error(
[objp_result] * 2, [allfeatures_left, allfeatures_right],
cameraMatrix, distCoeffs, [
cvh.Rodrigues(P_left[0:3, 0:3]),
cvh.Rodrigues(P_right_result[0:3, 0:3])
], [P_left[0:3, 3], P_right_result[0:3, 3]])[1])
"""
Further things we can do (in a real case):
1. solvePnP() on inliers (including previously already triangulated points),
or should we use solvePnPRansac() (in that case what to do with the outliers)?
2. re-triangulate on new pose estimation
"""
### Print summary to be used in Blender to visualize (see "calibrate_pose_visualization.blend")
print("Camera poses:")
if has_nonplanar:
print("Left")
print_pose(rvec_left, tvec_left)
print()
print("Left_result")
print_pose(cvh.Rodrigues(P_left_result[0:3, 0:3]), P_left_result[0:3,
3:4])
print()
print("Right")
print_pose(rvec_right, tvec_right)
print()
print("Right_result")
print_pose(cvh.Rodrigues(P_right_result[0:3, 0:3]),
P_right_result[0:3, 3:4])
print()
else:
print("<skipped: no non-planar objects have been selected>")
print()
print("Points:")
print("Chessboard")
print("coords = \\\n", map(list, objp_result[:num_planar, :]))
print()
if has_nonplanar:
print("Non-planar geometry")
print("coords_nonplanar = \\\n", map(list,
objp_result[num_planar:, :]))
print()
### Return to remember last manually matched successful non-planar imagepoints
return nonplanar_left, nonplanar_right
def realtime_pose_estimation(device_id, filename_base_extrinsics, cameraMatrix,
distCoeffs, objp, boardSize):
"""
This interactive demo will track a chessboard in realtime using a webcam,
and the WORLD axis-system will be drawn on it: [X Y Z] = [red green blue]
Further on you will see some data in the bottom-right corner,
this indicates both the pose of the current image w.r.t. the WORLD axis-system,
as well as the pose of the current image w.r.t. the previous keyframe pose.
To create a new keyframe while running, press SPACE.
Each time a new keyframe is generated,
the corresponding image and data (in txt-format) is written to the 'filename_base_extrinsics' folder.
All poses are defined in the WORLD axis-system,
the rotation notation follows axis-angle representation: '<unit vector> * <magnitude (degrees)>'.
To quit, press ESC.
"""
cv2.namedWindow("Image (with axis-system)")
fontFace = cv2.FONT_HERSHEY_DUPLEX
fontScale = 0.5
mlt = cvh.MultilineText()
cap = cv2.VideoCapture(device_id)
imageNr = 0 # keyframe image id
rvec_prev = np.zeros((3, 1))
rvec = None
tvec_prev = np.zeros((3, 1))
tvec = None
# Loop until 'q' or ESC pressed
last_key_pressed = 0
while not last_key_pressed in (ord('q'), 27):
ret_, img = cap.read()
ret, corners = cvh.extractChessboardFeatures(img, boardSize)
# If valid features found, solve for 'rvec' and 'tvec'
if ret == True:
ret, rvec, tvec = cv2.solvePnP( # TODO: use Ransac version for other types of features
objp, corners, cameraMatrix, distCoeffs)
# Draw axis-system
cvh.drawAxisSystem(img, cameraMatrix, distCoeffs, rvec, tvec)
# OpenCV's 'rvec' and 'tvec' seem to be defined as follows:
# 'rvec': rotation transformation: transforms points from "WORLD axis-system -> CAMERA axis-system"
# 'tvec': translation of "CAMERA center -> WORLD center", all defined in the "CAMERA axis-system"
rvec *= -1 # convert to: "CAMERA axis-system -> WORLD axis-system", equivalent to rotation of CAMERA axis-system w.r.t. WORLD axis-system
tvec = cvh.Rodrigues(rvec).dot(
tvec) # bring to "WORLD axis-system", ...
tvec *= -1 # ... and change direction to "WORLD center -> CAMERA center"
# Calculate pose relative to last keyframe
rvec_rel = trfm.delta_rvec(rvec, rvec_prev)
tvec_rel = tvec - tvec_prev
# Extract axis and angle, to enhance representation
rvec_axis, rvec_angle = trfm.axis_and_angle_from_rvec(rvec)
rvec_rel_axis, rvec_rel_angle = trfm.axis_and_angle_from_rvec(
rvec_rel)
# Draw pose information
texts = []
texts.append("Current pose:")
texts.append(" Rvec: %s * %.1fdeg" %
(format3DVector(rvec_axis), degrees(rvec_angle)))
texts.append(" Tvec: %s" % format3DVector(tvec))
texts.append("Relative to previous pose:")
texts.append(
" Rvec: %s * %.1fdeg" %
(format3DVector(rvec_rel_axis), degrees(rvec_rel_angle)))
texts.append(" Tvec: %s" % format3DVector(tvec_rel))
mlt.text(texts[0],
fontFace,
fontScale * 1.5,
rgb(150, 0, 0),
thickness=2)
mlt.text(texts[1], fontFace, fontScale, rgb(255, 0, 0))
mlt.text(texts[2], fontFace, fontScale, rgb(255, 0, 0))
mlt.text(texts[3],
fontFace,
fontScale * 1.5,
rgb(150, 0, 0),
thickness=2)
mlt.text(texts[4], fontFace, fontScale, rgb(255, 0, 0))
mlt.text(texts[5], fontFace, fontScale, rgb(255, 0, 0))
mlt.putText(img, (img.shape[1],
img.shape[0])) # put text in bottom-right corner
# Show Image
cv2.imshow("Image (with axis-system)", img)
mlt.clear()
# Save keyframe image when SPACE is pressed
last_key_pressed = cv2.waitKey(1) & 0xFF
if last_key_pressed == ord(' ') and ret:
filename = filename_base_extrinsics + str(imageNr)
cv2.imwrite(filename + ".jpg", img) # write image to jpg-file
textTotal = '\n'.join(texts)
open(filename + ".txt",
'w').write(textTotal) # write data to txt-file
print("Saved keyframe image+data to", filename, ":")
print(textTotal)
imageNr += 1
rvec_prev = rvec
tvec_prev = tvec
def main():
# Initially known data
boardSize = (8, 6)
objp = calibration_tools.grid_objp(boardSize)
cameraMatrix, distCoeffs, imageSize = calibration_tools.load_camera_intrinsics(
os.path.join("..", "..", "datasets", "webcam", "camera_intrinsics.txt")
)
# Initiate 2d 3d arrays
objectPoints = []
imagePoints = []
# Select working (or 'testing') set
from glob import glob
images = sorted(glob(os.path.join("..", "..", "datasets", "webcam", "captures2", "*.jpeg")))
def main_loop(left_points, left_gray, left_img, right_img, triangl_idxs, chessboard_idxs):
# Detect right (key)points
right_keypoints = fast.detect(right_img)
right_FAST_points = np.array([kp.pt for kp in right_keypoints], dtype=np.float32)
# Visualize right_FAST_points
print("Visualize right_FAST_points")
cv2.imshow(
"img", cv2.drawKeypoints(right_img, [cv2.KeyPoint(p[0], p[1], 7.0) for p in right_FAST_points], color=blue)
) # blue markers with size 7
cv2.waitKey()
# Calculate optical flow (= 'OF') field from left to right
right_gray = cv2.cvtColor(right_img, cv2.COLOR_BGR2GRAY)
right_OF_points, status_OF, err_OF = cv2.calcOpticalFlowPyrLK(
left_gray, right_gray, left_points
) # points to start from
err_OF = err_OF.reshape(-1)
def match_OF_based(
right_OF_points,
right_FAST_points,
err_OF,
status_OF,
max_radius_OF_to_FAST,
max_dist_ratio,
left_point_idxs=None,
): # if not None, left_point_idxs specifies mask
# Filter out the OF points with high error
right_OF_points, right_OF_to_left_idxs = zip(
*[
(p, i)
for i, p in enumerate(right_OF_points)
if status_OF[i]
and err_OF[i] < max_OF_error # only include correct OF-points
and (left_point_idxs == None or i in left_point_idxs) # error should be low enough
]
) # apply mask
right_OF_points = np.array(right_OF_points)
# Visualize right_OF_points
print("Visualize right_OF_points")
cv2.imshow(
"img",
cv2.drawKeypoints(right_img, [cv2.KeyPoint(p[0], p[1], 7.0) for p in right_OF_points], color=blue),
) # blue markers with size 7
cv2.waitKey()
# Align right_OF_points with right_FAST_points by matching them
matches_twoNN = matcher.radiusMatch(
right_OF_points, right_FAST_points, max_radius_OF_to_FAST # query points # train points
)
# Filter out ambiguous matches by a ratio-test, and prevent duplicates
best_dist_matches_by_trainIdx = {} # duplicate prevention: trainIdx -> match_best_dist
for query_matches in matches_twoNN:
# Ratio test
if not (
len(query_matches) == 1
or ( # only one match, probably a good one
len(query_matches) > 1
and query_matches[0].distance # if more than one, first two shouldn't lie too close
/ query_matches[1].distance
< max_dist_ratio
)
):
continue
# Relink match to use 'left_point' indices
match = cv2.DMatch(
right_OF_to_left_idxs[query_matches[0].queryIdx], # queryIdx: left_points
query_matches[0].trainIdx, # trainIdx: right_FAST_points
query_matches[0].distance,
)
# Duplicate prevention
if (
not match.trainIdx in best_dist_matches_by_trainIdx
or err_OF[match.queryIdx] # no duplicate found
< err_OF[ # replace duplicate if inferior, based on err_OF
best_dist_matches_by_trainIdx[match.trainIdx].queryIdx
]
):
best_dist_matches_by_trainIdx[match.trainIdx] = match
return best_dist_matches_by_trainIdx
# Match between FAST -> FAST features
matches_by_trainIdx = match_OF_based(
right_OF_points, right_FAST_points, err_OF, status_OF, max_radius_OF_to_FAST["FAST"], max_dist_ratio["FAST"]
)
if allow_chessboard_matcher_and_refiner and chessboard_idxs:
# Match between chessboard -> chessboard features
matches_by_trainIdx_chessboard = match_OF_based(
right_OF_points,
right_FAST_points,
err_OF,
status_OF,
max_radius_OF_to_FAST["chessboard"],
max_dist_ratio["chessboard"],
chessboard_idxs,
) # set mask
# Overwrite FAST -> FAST feature matches by chessboard -> chessboard feature matches
matches_by_trainIdx.update(matches_by_trainIdx_chessboard)
# Refine chessboard features
chessboard_corners_idxs = list(matches_by_trainIdx_chessboard)
chessboard_corners = right_FAST_points[chessboard_corners_idxs]
cv2.cornerSubPix(
right_gray,
chessboard_corners,
(11, 11), # window
(-1, -1), # deadzone
(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30, 0.001),
) # termination criteria
right_FAST_points[chessboard_corners_idxs] = chessboard_corners
# Update chessboard_idxs
chessboard_idxs = set(matches_by_trainIdx_chessboard)
print("Amount of chessboard features tracked in the new image:", len(chessboard_idxs))
# Calculate average filtered OF vector
trainIdxs = list(matches_by_trainIdx)
queryIdxs = [matches_by_trainIdx[trainIdx].queryIdx for trainIdx in trainIdxs]
mean_OF_vector = (right_FAST_points[trainIdxs] - left_points[queryIdxs]).mean(axis=0)
mean_OF_vector_length = np.linalg.norm(mean_OF_vector)
print(
"mean_OF_vector (from LEFT to RIGHT):", mean_OF_vector, "; mean_OF_vector_length:", mean_OF_vector_length
)
# Partition matches to make a distinction between previously triangulated points and non-triangl.
matches_left_triangl_to_right_FAST = []
matches_left_non_triangl_to_right_FAST = []
for trainIdx in matches_by_trainIdx:
match = matches_by_trainIdx[trainIdx]
if matches_by_trainIdx[trainIdx].queryIdx in triangl_idxs:
matches_left_triangl_to_right_FAST.append(match)
else:
matches_left_non_triangl_to_right_FAST.append(match)
# and all matches together
matches_left_to_right_FAST = matches_left_triangl_to_right_FAST + matches_left_non_triangl_to_right_FAST
# Visualize (previously triangulated) left_points of corresponding outlier-filtered right_FAST_points
print("Visualize LEFT points (previously triangulated)")
cv2.imshow(
"img",
cv2.drawKeypoints(
left_img,
[
cv2.KeyPoint(left_points[m.queryIdx][0], left_points[m.queryIdx][1], 7.0)
for m in matches_left_triangl_to_right_FAST
],
color=blue,
),
) # blue markers with size 7
cv2.waitKey()
# Visualize (previously triangulated) outlier-filtered right_FAST_points
print("Visualize RIGHT points (previously triangulated)")
cv2.imshow(
"img",
cv2.drawKeypoints(
right_img,
[
cv2.KeyPoint(right_FAST_points[m.trainIdx][0], right_FAST_points[m.trainIdx][1], 7.0)
for m in matches_left_triangl_to_right_FAST
],
color=blue,
),
) # blue markers with size 7
cv2.waitKey()
# Visualize (not yet triangulated) outlier-filtered right_FAST_points
print("Visualize LEFT points (not yet triangulated)")
cv2.imshow(
"img",
cv2.drawKeypoints(
left_img,
[
cv2.KeyPoint(left_points[m.queryIdx][0], left_points[m.queryIdx][1], 7.0)
for m in matches_left_non_triangl_to_right_FAST
],
color=blue,
),
) # blue markers with size 7
cv2.waitKey()
# Visualize (not yet triangulated) outlier-filtered right_FAST_points
print("Visualize RIGHT points (not yet triangulated)")
cv2.imshow(
"img",
cv2.drawKeypoints(
right_img,
[
cv2.KeyPoint(right_FAST_points[m.trainIdx][0], right_FAST_points[m.trainIdx][1], 7.0)
for m in matches_left_non_triangl_to_right_FAST
],
color=blue,
),
) # blue markers with size 7
cv2.waitKey()
# Pose estimation and Triangulation
# ...
# Update triangl_idxs TODO: filter using outlier-removal by epipolar constraints
triangl_idxs = set(matches_by_trainIdx)
return right_FAST_points, right_gray, triangl_idxs, chessboard_idxs
###----------------------------- Frame 0 (init) -----------------------------###
print("###---------------------- Frame 0 (init) ----------------------###")
left_img = cv2.imread(images[0])
left_gray = cv2.cvtColor(left_img, cv2.COLOR_BGR2GRAY)
right_img = cv2.imread(images[1])
# Detect left (key)points
ret, left_points = cvh.extractChessboardFeatures(left_img, boardSize)
if not ret:
raise Exception("No chessboard features detected.")
# Set masks to alter matches' priority
chessboard_idxs = set(range(len(left_points))) # all chessboard corners are in sight
triangl_idxs = set(chessboard_idxs) # the chessboard feature points are already triangulated
# Invoke main loop
right_FAST_points, right_gray, triangl_idxs, chessboard_idxs = main_loop(
left_points, left_gray, left_img, right_img, triangl_idxs, chessboard_idxs
)
for i in range(2, 14):
###----------------------------- Frame i -----------------------------###
print("###---------------------- Frame %s ----------------------###" % i)
# Update data for new frame
left_img = right_img
left_gray = right_gray
right_img = cv2.imread(images[i])
# Use previous feature points
left_points = right_FAST_points
# Invoke main loop
right_FAST_points, right_gray, triangl_idxs, chessboard_idxs = main_loop(
left_points, left_gray, left_img, right_img, triangl_idxs, chessboard_idxs
)
def main():
global boardSize
global cameraMatrix, distCoeffs, imageSize
global max_OF_error, max_lost_tracks_ratio
global keypoint_coverage_radius #, min_keypoint_coverage
global target_amount_keypoints, corner_quality_level, corner_min_dist
global homography_condition_threshold
global max_solvePnP_reproj_error, max_2nd_solvePnP_reproj_error, max_fundMat_reproj_error
global max_solvePnP_outlier_ratio, max_2nd_solvePnP_outlier_ratio, solvePnP_use_extrinsic_guess
# Initially known data
boardSize = (8, 6)
color_palette, color_palette_size = prepare_color_palette(
2, 3, 4) # used to identify 3D point group ids
cameraMatrix, distCoeffs, imageSize = \
load_camera_intrinsics(os.path.join("..", "..", "datasets", "webcam", "camera_intrinsics.txt"))
#cameraMatrix, distCoeffs, imageSize = \
#load_camera_intrinsics(os.path.join("..", "..", "..", "ARDrone2_tests", "camera_calibration", "live_video", "camera_intrinsics_front.txt"))
# Select working (or 'testing') set
from glob import glob
images = sorted(
glob(
os.path.join("..", "..", "datasets", "webcam", "captures2",
"*.jpeg")))
#images = sorted(glob(os.path.join("..", "..", "..", "ARDrone2_tests", "flying_front", "lowres", "drone0", "*.jpg")))
# Setup some visualization helpers
composite2D_painter = Composite2DPainter("composite 2D", imageSize)
composite3D_painter = Composite3DPainter(
"composite 3D",
trfm.P_from_R_and_t(cvh.Rodrigues((pi, 0., 0.)),
np.array([[0., 0., 40.]]).T), (1280, 720))
### Tweaking parameters ###
# OF calculation
max_OF_error = 12.
max_lost_tracks_ratio = 0.5
# keypoint_coverage
keypoint_coverage_radius = int(max_OF_error)
#min_keypoint_coverage = 0.2
# goodFeaturesToTrack
target_amount_keypoints = int(
round(
(imageSize[0] * imageSize[1]) /
(pi * keypoint_coverage_radius**2))) # target is entire image full
print "target_amount_keypoints:", target_amount_keypoints
corner_quality_level = 0.01
corner_min_dist = keypoint_coverage_radius
# keyframe_test
homography_condition_threshold = 500 # defined as ratio between max and min singular values
# reprojection error
max_solvePnP_reproj_error = 4. #0.5 # TODO: revert to a lower number
max_2nd_solvePnP_reproj_error = max_solvePnP_reproj_error / 2 # be more strict in a 2nd iteration, used after 1st pass of triangulation
max_fundMat_reproj_error = 2.0
# solvePnP
max_solvePnP_outlier_ratio = 0.3
max_2nd_solvePnP_outlier_ratio = 1. # used in 2nd iteration, after 1st pass of triangulation
solvePnP_use_extrinsic_guess = False # TODO: set to True and see whether the 3D results are better
# Init
imgs = []
imgs_gray = []
objp = [] # 3D points
objp_groups = [
] # 3D point group ids, each new batch of detected points is put in a separate group
group_id = 0 # current 3D point group id
rvecs, rvecs_keyfr = [], []
tvecs, tvecs_keyfr = [], []
# Start frame requires special treatment
# Start frame : read image and detect 2D points
imgs.append(cv2.imread(images[0]))
base_img = imgs[0]
imgs_gray.append(cv2.cvtColor(imgs[0], cv2.COLOR_BGR2GRAY))
ret, new_imgp = cvh.extractChessboardFeatures(imgs[0], boardSize)
if not ret:
print "First image must contain the entire chessboard!"
return
# Start frame : define a priori 3D points
objp = prepare_object_points(boardSize)
objp_groups = np.zeros((objp.shape[0]), dtype=np.int)
group_id += 1
# Start frame : setup linking data-structures
base_imgp = new_imgp # 2D points
all_idxs_tmp = np.arange(len(base_imgp))
triangl_idxs = set(all_idxs_tmp)
nontriangl_idxs = set()
imgp_to_objp_idxs = np.array(sorted(triangl_idxs), dtype=int)
# Start frame : get absolute pose
ret, rvec, tvec = cv2.solvePnP( # assume first frame is a proper frame with chessboard fully in-sight
objp, new_imgp, cameraMatrix, distCoeffs)
print "solvePnP reproj_error init:", reprojection_error(
objp, new_imgp, rvec, tvec, cameraMatrix, distCoeffs)[0]
rvecs.append(rvec)
tvecs.append(tvec)
rvec_keyfr = rvec
tvec_keyfr = tvec
rvecs_keyfr.append(rvec_keyfr)
tvecs_keyfr.append(tvec_keyfr)
# Start frame : add other points
mask_img = keypoint_mask(new_imgp)
to_add = target_amount_keypoints - len(new_imgp)
imgp_extra = cv2.goodFeaturesToTrack(imgs_gray[0], to_add,
corner_quality_level, corner_min_dist,
None, mask_img).reshape((-1, 2))
cv2.imshow(
"img",
cv2.drawKeypoints(imgs[0],
[cv2.KeyPoint(p[0], p[1], 7.) for p in imgp_extra],
color=rgb(0, 0, 255)))
cv2.waitKey()
print "added:", len(imgp_extra)
base_imgp, new_imgp, imgp_to_objp_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp = idxs_rebase_and_add_imgp(
imgp_extra, base_imgp, new_imgp, imgp_to_objp_idxs, triangl_idxs,
nontriangl_idxs, all_idxs_tmp)
ret = 2 # indicate keyframe
# Draw 3D points info of current frame
print "Drawing composite 2D image"
composite2D_painter.draw(imgs[0], rvec, tvec, ret, cameraMatrix,
distCoeffs, triangl_idxs, nontriangl_idxs,
all_idxs_tmp, new_imgp, imgp_to_objp_idxs, objp,
objp_groups, group_id, color_palette,
color_palette_size)
# Draw 3D points info of all frames
print "Drawing composite 3D image (keys: LEFT/RIGHT/UP/DOWN/PAGEUP/PAGEDOWN/HOME/END)"
composite3D_painter.draw(rvec, tvec, ret, triangl_idxs, imgp_to_objp_idxs,
objp, objp_groups, color_palette,
color_palette_size)
for i in range(1, len(images)):
# Frame[i-1] -> Frame[i]
print "\nFrame[%s] -> Frame[%s]" % (i - 1, i)
print " processing '", images[i], "':"
cur_img = cv2.imread(images[i])
imgs.append(cur_img)
imgs_gray.append(cv2.cvtColor(imgs[-1], cv2.COLOR_BGR2GRAY))
ret, base_imgp, new_imgp, triangl_idxs, nontriangl_idxs, imgp_to_objp_idxs, all_idxs_tmp, objp, objp_groups, group_id, rvec, tvec, rvec_keyfr, tvec_keyfr, base_img = \
handle_new_frame(base_imgp, new_imgp, imgs[-2], imgs_gray[-2], imgs[-1], imgs_gray[-1], triangl_idxs, nontriangl_idxs, imgp_to_objp_idxs, all_idxs_tmp, objp, objp_groups, group_id, rvec_keyfr, tvec_keyfr, base_img)
if ret:
rvecs.append(rvec)
tvecs.append(tvec)
if ret == 2: # frame is a keyframe
rvecs_keyfr.append(rvec_keyfr)
tvecs_keyfr.append(tvec_keyfr)
else: # frame rejected
del imgs[-1]
del imgs_gray[-1]
# Draw 3D points info of current frame
print "Drawing composite 2D image"
composite2D_painter.draw(cur_img, rvec, tvec, ret, cameraMatrix,
distCoeffs, triangl_idxs, nontriangl_idxs,
all_idxs_tmp, new_imgp, imgp_to_objp_idxs,
objp, objp_groups, group_id, color_palette,
color_palette_size)
# Draw 3D points info of all frames
print "Drawing composite 3D image (keys: LEFT/RIGHT/UP/DOWN/PAGEUP/PAGEDOWN)"
composite3D_painter.draw(rvec, tvec, ret, triangl_idxs,
imgp_to_objp_idxs, objp, objp_groups,
color_palette, color_palette_size)
def triangl_pose_est_interactive(img_left, img_right, cameraMatrix, distCoeffs, imageSize, objp, boardSize, nonplanar_left, nonplanar_right):
""" (TODO: remove debug-prints)
Triangulation and relative pose estimation will be performed from LEFT to RIGHT image.
Both images have to contain the whole chessboard,
in order to make a decent estimation of the relative pose based on 'solvePnP'.
Then the user can manually create matches between non-planar objects.
If the user omits this step, only triangulation of the chessboard corners will be performed,
and they will be compared to the real 3D points.
Otherwise the coordinates of the triangulated points of the manually matched points will be printed,
and a relative pose estimation will be performed (using the essential matrix),
this pose estimation will be compared with the decent 'solvePnP' estimation.
In that case you will be asked whether you want to mute the chessboard corners in all calculations,
note that this requires at least 8 pairs of matches corresponding with non-planar geometry.
During manual matching process,
switching between LEFT and RIGHT image will be done in a zigzag fashion.
To stop selecting matches, press SPACE.
Additionally, you can also introduce predefined non-planar geometry,
this is e.g. useful if you don't want to select non-planar geometry manually.
"""
### Setup initial state, and calc some very accurate poses to compare against with later
K_left = cameraMatrix
K_right = cameraMatrix
K_inv_left = cvh.invert(K_left)
K_inv_right = cvh.invert(K_right)
distCoeffs_left = distCoeffs
distCoeffs_right = distCoeffs
# Extract chessboard features, together they form a set of 'planar' points
ret_left, planar_left = cvh.extractChessboardFeatures(img_left, boardSize)
ret_right, planar_right = cvh.extractChessboardFeatures(img_right, boardSize)
if not ret_left or not ret_right:
print ("Chessboard is not (entirely) in sight, aborting.")
return
# Save exact 2D and 3D points of the chessboard
num_planar = planar_left.shape[0]
planar_left_orig, planar_right_orig = planar_left, planar_right
objp_orig = objp
# Calculate P matrix of left pose, in reality this one is known
ret, rvec_left, tvec_left = cv2.solvePnP(
objp, planar_left, K_left, distCoeffs_left )
P_left = trfm.P_from_R_and_t(cvh.Rodrigues(rvec_left), tvec_left)
# Calculate P matrix of right pose, in reality this one is unknown
ret, rvec_right, tvec_right = cv2.solvePnP(
objp, planar_right, K_right, distCoeffs_right )
P_right = trfm.P_from_R_and_t(cvh.Rodrigues(rvec_right), tvec_right)
# Load previous non-planar inliers, if desired
if nonplanar_left.size and not raw_input("Type 'no' if you don't want to use previous non-planar inliers? ").strip().lower() == "no":
nonplanar_left = map(tuple, nonplanar_left)
nonplanar_right = map(tuple, nonplanar_left)
else:
nonplanar_left = []
nonplanar_right = []
### Create predefined non-planar 3D geometry, to enable automatic selection of non-planar points
if raw_input("Type 'yes' if you want to create and select predefined non-planar geometry? ").strip().lower() == "yes":
# A cube
cube_coords = np.array([(0., 0., 0.), (1., 0., 0.), (1., 1., 0.), (0., 1., 0.),
(0., 0., 1.), (1., 0., 1.), (1., 1., 1.), (0., 1., 1.)])
cube_coords *= 2 # scale
cube_edges = np.array([(0, 1), (1, 2), (2, 3), (3, 0),
(4, 5), (5, 6), (6, 7), (7, 4),
(0, 4), (1, 5), (2, 6), (3, 7)])
# An 8-point circle
s2 = 1. / np.sqrt(2)
circle_coords = np.array([( 1., 0., 0.), ( s2, s2, 0.),
( 0., 1., 0.), (-s2, s2, 0.),
(-1., 0., 0.), (-s2, -s2, 0.),
( 0., -1., 0.), ( s2, -s2, 0.)])
circle_edges = np.array([(i, (i+1) % 8) for i in range(8)])
# Position 2 cubes and 2 circles in the scene
cube1 = np.array(cube_coords)
cube1[:, 1] -= 1
cube2 = np.array(cube_coords)
cube2 = cvh.Rodrigues((0., 0., pi/4)).dot(cube2.T).T
cube2[:, 0] += 4
cube2[:, 1] += 3
cube2[:, 2] += 2
circle1 = np.array(circle_coords)
circle1 *= 2
circle1[:, 1] += 5
circle1[:, 2] += 2
circle2 = np.array(circle_coords)
circle2 = cvh.Rodrigues((pi/2, 0., 0.)).dot(circle2.T).T
circle2[:, 1] += 5
circle2[:, 2] += 2
# Print output to be used in Blender (see "calibrate_pose_visualization.blend")
print ()
print ("Cubes")
print ("edges_cube = \\\n", map(list, cube_edges))
print ("coords_cube1 = \\\n", map(list, cube1))
print ("coords_cube2 = \\\n", map(list, cube2))
print ()
print ("Circles")
print ("edges_circle = \\\n", map(list, circle_edges))
print ("coords_circle1 = \\\n", map(list, circle1))
print ("coords_circle2 = \\\n", map(list, circle2))
print ()
color = rgb(0, 200, 150)
for verts, edges in zip([cube1, cube2, circle1, circle2],
[cube_edges, cube_edges, circle_edges, circle_edges]):
out_left = cvh.wireframe3DGeometry(
img_left, verts, edges, color, rvec_left, tvec_left, K_left, distCoeffs_left )
out_right = cvh.wireframe3DGeometry(
img_right, verts, edges, color, rvec_right, tvec_right, K_right, distCoeffs_right )
valid_match_idxs = [i for i, (pl, pr) in enumerate(zip(out_left, out_right))
if 0 <= min(pl[0], pr[0]) <= max(pl[0], pr[0]) < imageSize[0] and
0 <= min(pl[1], pr[1]) <= max(pl[1], pr[1]) < imageSize[1]
]
nonplanar_left += map(tuple, out_left[valid_match_idxs]) # concatenate
nonplanar_right += map(tuple, out_right[valid_match_idxs]) # concatenate
nonplanar_left = np.rint(nonplanar_left).astype(int)
nonplanar_right = np.rint(nonplanar_right).astype(int)
### User can manually create matches between non-planar objects
class ManualMatcher:
def __init__(self, window_name, images, points):
self.window_name = window_name
self.images = images
self.points = points
self.img_idx = 0 # 0: left; 1: right
cv2.namedWindow(window_name)
cv2.setMouseCallback(window_name, self.onMouse)
def onMouse(self, event, x, y, flags, userdata):
if event == cv2.EVENT_LBUTTONDOWN:
self.points[self.img_idx].append((x, y))
elif event == cv2.EVENT_LBUTTONUP:
# Switch images in a ping-pong fashion
if len(self.points[0]) != len(self.points[1]):
self.img_idx = 1 - self.img_idx
def run(self):
print ("Select your matches. Press SPACE when done.")
while True:
img = cv2.drawKeypoints(self.images[self.img_idx], [cv2.KeyPoint(p[0],p[1], 7.) for p in self.points[self.img_idx]], color=rgb(0,0,255))
cv2.imshow(self.window_name, img)
key = cv2.waitKey(50) & 0xFF
if key == ord(' '): break # finish if SPACE is pressed
num_points_diff = len(self.points[0]) - len(self.points[1])
if num_points_diff:
del self.points[num_points_diff < 0][-abs(num_points_diff):]
print ("Selected", len(self.points[0]), "pairs of matches.")
# Execute the manual matching
nonplanar_left, nonplanar_right = list(nonplanar_left), list(nonplanar_right)
ManualMatcher("Select match-points of non-planar objects", [img_left, img_right], [nonplanar_left, nonplanar_right]).run()
num_nonplanar = len(nonplanar_left)
has_nonplanar = (num_nonplanar > 0)
if 0 < num_nonplanar < 8:
print ("Warning: you've selected less than 8 pairs of matches.")
nonplanar_left = np.array(nonplanar_left).reshape(num_nonplanar, 2)
nonplanar_right = np.array(nonplanar_right).reshape(num_nonplanar, 2)
mute_chessboard_corners = False
if num_nonplanar >= 8 and raw_input("Type 'yes' if you want to exclude chessboard corners from calculations? ").strip().lower() == "yes":
mute_chessboard_corners = True
num_planar = 0
planar_left = np.zeros((0, 2))
planar_right = np.zeros((0, 2))
objp = np.zeros((0, 3))
print ("Chessboard corners muted.")
has_prev_triangl_points = not mute_chessboard_corners # normally in this example, this should always be True, unless user forces False
### Undistort points => normalized coordinates
allfeatures_left = np.concatenate((planar_left, nonplanar_left))
allfeatures_right = np.concatenate((planar_right, nonplanar_right))
allfeatures_nrm_left = cv2.undistortPoints(np.array([allfeatures_left]), K_left, distCoeffs_left)[0]
allfeatures_nrm_right = cv2.undistortPoints(np.array([allfeatures_right]), K_right, distCoeffs_right)[0]
planar_nrm_left, nonplanar_nrm_left = allfeatures_nrm_left[:planar_left.shape[0]], allfeatures_nrm_left[planar_left.shape[0]:]
planar_nrm_right, nonplanar_nrm_right = allfeatures_nrm_right[:planar_right.shape[0]], allfeatures_nrm_right[planar_right.shape[0]:]
### Calculate relative pose using the essential matrix
# Only do pose estimation if we've got 2D points of non-planar geometry
if has_nonplanar:
# Determine inliers by calculating the fundamental matrix on all points,
# except when mute_chessboard_corners is True: only use nonplanar_nrm_left and nonplanar_nrm_right
F, status = cv2.findFundamentalMat(allfeatures_nrm_left, allfeatures_nrm_right, cv2.FM_RANSAC, 0.006 * np.amax(allfeatures_nrm_left), 0.99) # threshold from [Snavely07 4.1]
# OpenCV BUG: "status" matrix is not initialized with zeros in some cases, reproducable with 2 sets of 8 2D points equal to (0., 0.)
# maybe because "_mask.create()" is not called:
# https://github.com/Itseez/opencv/blob/7e2bb63378dafb90063af40caff20c363c8c9eaf/modules/calib3d/src/ptsetreg.cpp#L185
# Workaround to test on outliers: use "!= 1", instead of "== 0"
print (status.T)
inlier_idxs = np.where(status == 1)[0]
print ("Removed", allfeatures_nrm_left.shape[0] - inlier_idxs.shape[0], "outlier(s).")
num_planar = np.where(inlier_idxs < num_planar)[0].shape[0]
num_nonplanar = inlier_idxs.shape[0] - num_planar
print ("num chessboard inliers:", num_planar)
allfeatures_left, allfeatures_right = allfeatures_left[inlier_idxs], allfeatures_right[inlier_idxs]
allfeatures_nrm_left, allfeatures_nrm_right = allfeatures_nrm_left[inlier_idxs], allfeatures_nrm_right[inlier_idxs]
if not mute_chessboard_corners:
objp = objp[inlier_idxs[:num_planar]]
planar_left, planar_right = allfeatures_left[:num_planar], allfeatures_right[:num_planar]
planar_nrm_left, planar_nrm_right = allfeatures_nrm_left[:num_planar], allfeatures_nrm_right[:num_planar]
nonplanar_nrm_left, nonplanar_nrm_right = allfeatures_nrm_left[num_planar:], allfeatures_nrm_right[num_planar:]
# Calculate first solution of relative pose
if allfeatures_nrm_left.shape[0] >= 8:
F, status = cv2.findFundamentalMat(allfeatures_nrm_left, allfeatures_nrm_right, cv2.FM_8POINT)
print ("F:")
print (F)
else:
print ("Error: less than 8 pairs of inliers found, I can't perform the 8-point algorithm.")
#E = (K_right.T) .dot (F) .dot (K_left) # "Multiple View Geometry in CV" by Hartley&Zisserman (9.12)
E = F # K = I because we already normalized the coordinates
print ("Correct determinant of essential matrix?", (abs(cv2.determinant(E)) <= 1e-7))
w, u, vt = cv2.SVDecomp(E, flags=cv2.SVD_MODIFY_A)
print (w)
if ((w[0] < w[1] and w[0]/w[1]) or (w[1] < w[0] and w[1]/w[0]) or 0) < 0.7:
print ("Essential matrix' 'w' vector deviates too much from expected")
W = np.array([[0., -1., 0.], # Hartley&Zisserman (9.13)
[1., 0., 0.],
[0., 0., 1.]])
R = (u) .dot (W) .dot (vt) # Hartley&Zisserman result 9.19
det_R = cv2.determinant(R)
print ("Coherent rotation?", (abs(det_R) - 1 <= 1e-7))
if det_R - (-1) < 1e-7: # http://en.wikipedia.org/wiki/Essential_matrix#Showing_that_it_is_valid
# E *= -1:
vt *= -1 # svd(-E) = u * w * (-v).T
R *= -1 # => u * W * (-v).T = -R
print ("det(R) == -1, compensated.")
t = u[:, 2:3] # Hartley&Zisserman result 9.19
P = trfm.P_from_R_and_t(R, t)
# Select right solution where the (center of the) 3d points are/is in front of both cameras,
# when has_prev_triangl_points == True, we can do it a lot faster.
test_point = np.ones((4, 1)) # 3D point to test the solutions with
if has_prev_triangl_points:
print ("Using advantage of already triangulated points' position")
print (P)
# Select the closest already triangulated cloudpoint idx to the center of the cloud
center_of_mass = objp.sum(axis=0) / objp.shape[0]
center_objp_idx = np.argmin(((objp - center_of_mass)**2).sum(axis=1))
print ("center_objp_idx:", center_objp_idx)
# Select the corresponding image points
center_imgp_left = planar_nrm_left[center_objp_idx]
center_imgp_right = planar_nrm_right[center_objp_idx]
# Select the corresponding 3D point
test_point[0:3, :] = objp[center_objp_idx].reshape(3, 1)
print ("test_point:")
print (test_point)
test_point = P_left.dot(test_point) # set the reference axis-system to the one of the left camera, note that are_points_in_front_of_left_camera is automatically True
center_objp_triangl, triangl_status = iterative_LS_triangulation(
center_imgp_left.reshape(1, 2), np.eye(4),
center_imgp_right.reshape(1, 2), P )
print ("triangl_status:", triangl_status)
if (center_objp_triangl) .dot (test_point[0:3, 0:1]) < 0:
P[0:3, 3:4] *= -1 # do a baseline reversal
print (P, "fixed triangulation inversion")
if P[0:3, :].dot(test_point)[2, 0] < 0: # are_points_in_front_of_right_camera is False
P[0:3, 0:3] = (u) .dot (W.T) .dot (vt) # use the other solution of the twisted pair ...
P[0:3, 3:4] *= -1 # ... and also do a baseline reversal
print (P, "fixed camera projection inversion")
elif num_nonplanar > 0:
print ("Doing all ambiguity checks since there are no already triangulated points")
print (P)
for i in range(4): # check all 4 solutions
objp_triangl, triangl_status = iterative_LS_triangulation(
nonplanar_nrm_left, np.eye(4),
nonplanar_nrm_right, P )
print ("triangl_status:", triangl_status)
center_of_mass = objp_triangl.sum(axis=0) / len(objp_triangl) # select the center of the triangulated cloudpoints
test_point[0:3, :] = center_of_mass.reshape(3, 1)
print ("test_point:")
print (trfm.P_inv(P_left) .dot (test_point))
if np.eye(3, 4).dot(test_point)[2, 0] > 0 and P[0:3, :].dot(test_point)[2, 0] > 0: # are_points_in_front_of_cameras is True
break
if i % 2:
P[0:3, 0:3] = (u) .dot (W.T) .dot (vt) # use the other solution of the twisted pair
print (P, "using the other solution of the twisted pair")
else:
P[0:3, 3:4] *= -1 # do a baseline reversal
print (P, "doing a baseline reversal")
are_points_in_front_of_left_camera = (np.eye(3, 4).dot(test_point)[2, 0] > 0)
are_points_in_front_of_right_camera = (P[0:3, :].dot(test_point)[2, 0] > 0)
print ("are_points_in_front_of_cameras?", are_points_in_front_of_left_camera, are_points_in_front_of_right_camera)
if not (are_points_in_front_of_left_camera and are_points_in_front_of_right_camera):
print ("No valid solution found!")
P_left_result = trfm.P_inv(P).dot(P_right)
P_right_result = P.dot(P_left)
print ("P_left")
print (P_left)
print ("P_rel")
print (P)
print ("P_right")
print (P_right)
print ("=> error:", reprojection_error(
[objp_orig] * 2, [planar_left_orig, planar_right_orig],
cameraMatrix, distCoeffs,
[rvec_left, rvec_right],
[tvec_left, tvec_right] )[1])
print ("P_left_result")
print (P_left_result)
print ("P_right_result")
print (P_right_result)
# We use "P_left" instead of "P_left_result" because the latter depends on the unknown "P_right"
print ("=> error:", reprojection_error(
[objp_orig] * 2, [planar_left_orig, planar_right_orig],
cameraMatrix, distCoeffs,
[cvh.Rodrigues(P_left[0:3, 0:3]), cvh.Rodrigues(P_right_result[0:3, 0:3])],
[P_left[0:3, 3], P_right_result[0:3, 3]] )[1])
### Triangulate
objp_result = np.zeros((0, 3))
if has_prev_triangl_points:
# Do triangulation of all points
# NOTICE: in a real case, we should only use not yet triangulated points that are in sight
objp_result, triangl_status = iterative_LS_triangulation(
allfeatures_nrm_left, P_left,
allfeatures_nrm_right, P_right )
print ("triangl_status:", triangl_status)
elif num_nonplanar > 0:
# We already did the triangulation during the pose estimation, but we still need to backtransform them from the left camera axis-system
objp_result = trfm.P_inv(P_left) .dot (np.concatenate((objp_triangl.T, np.ones((1, len(objp_triangl))))))
objp_result = objp_result[0:3, :].T
print (objp_triangl)
print ("objp:")
print (objp)
print ("=> error:", reprojection_error(
[objp_orig] * 2,
[planar_left_orig, planar_right_orig],
cameraMatrix, distCoeffs, [rvec_left, rvec_right], [tvec_left, tvec_right] )[1])
print ("objp_result of chessboard:")
print (objp_result[:num_planar, :])
if has_nonplanar:
print ("objp_result of non-planar geometry:")
print (objp_result[num_planar:, :])
if num_planar + num_nonplanar == 0:
print ("=> error: undefined")
else:
print ("=> error:", reprojection_error(
[objp_result] * 2,
[allfeatures_left, allfeatures_right],
cameraMatrix, distCoeffs, [rvec_left, rvec_right], [tvec_left, tvec_right] )[1])
### Print total combined reprojection error
# We only have both pose estimation and triangulation if we've got 2D points of non-planar geometry
if has_nonplanar:
if num_planar + num_nonplanar == 0:
print ("=> error: undefined")
else:
# We use "P_left" instead of "P_left_result" because the latter depends on the unknown "P_right"
print ("Total combined error:", reprojection_error(
[objp_result] * 2,
[allfeatures_left, allfeatures_right],
cameraMatrix, distCoeffs,
[cvh.Rodrigues(P_left[0:3, 0:3]), cvh.Rodrigues(P_right_result[0:3, 0:3])],
[P_left[0:3, 3], P_right_result[0:3, 3]] )[1])
"""
Further things we can do (in a real case):
1. solvePnP() on inliers (including previously already triangulated points),
or should we use solvePnPRansac() (in that case what to do with the outliers)?
2. re-triangulate on new pose estimation
"""
### Print summary to be used in Blender to visualize (see "calibrate_pose_visualization.blend")
print ("Camera poses:")
if has_nonplanar:
print ("Left")
print_pose(rvec_left, tvec_left)
print ()
print ("Left_result")
print_pose(cvh.Rodrigues(P_left_result[0:3, 0:3]), P_left_result[0:3, 3:4])
print ()
print ("Right")
print_pose(rvec_right, tvec_right)
print ()
print ("Right_result")
print_pose(cvh.Rodrigues(P_right_result[0:3, 0:3]), P_right_result[0:3, 3:4])
print ()
else:
print ("<skipped: no non-planar objects have been selected>")
print ()
print ("Points:")
print ("Chessboard")
print ("coords = \\\n", map(list, objp_result[:num_planar, :]))
print ()
if has_nonplanar:
print ("Non-planar geometry")
print ("coords_nonplanar = \\\n", map(list, objp_result[num_planar:, :]))
print ()
### Return to remember last manually matched successful non-planar imagepoints
return nonplanar_left, nonplanar_right
def realtime_pose_estimation(device_id, filename_base_extrinsics, cameraMatrix, distCoeffs, objp, boardSize):
"""
This interactive demo will track a chessboard in realtime using a webcam,
and the WORLD axis-system will be drawn on it: [X Y Z] = [red green blue]
Further on you will see some data in the bottom-right corner,
this indicates both the pose of the current image w.r.t. the WORLD axis-system,
as well as the pose of the current image w.r.t. the previous keyframe pose.
To create a new keyframe while running, press SPACE.
Each time a new keyframe is generated,
the corresponding image and data (in txt-format) is written to the 'filename_base_extrinsics' folder.
All poses are defined in the WORLD axis-system,
the rotation notation follows axis-angle representation: '<unit vector> * <magnitude (degrees)>'.
To quit, press ESC.
"""
cv2.namedWindow("Image (with axis-system)")
fontFace = cv2.FONT_HERSHEY_DUPLEX
fontScale = 0.5
mlt = cvh.MultilineText()
cap = cv2.VideoCapture(device_id)
imageNr = 0 # keyframe image id
rvec_prev = np.zeros((3, 1))
rvec = None
tvec_prev = np.zeros((3, 1))
tvec = None
# Loop until 'q' or ESC pressed
last_key_pressed = 0
while not last_key_pressed in (ord('q'), 27):
ret_, img = cap.read()
ret, corners = cvh.extractChessboardFeatures(img, boardSize)
# If valid features found, solve for 'rvec' and 'tvec'
if ret == True:
ret, rvec, tvec = cv2.solvePnP( # TODO: use Ransac version for other types of features
objp, corners, cameraMatrix, distCoeffs )
# Draw axis-system
cvh.drawAxisSystem(img, cameraMatrix, distCoeffs, rvec, tvec)
# OpenCV's 'rvec' and 'tvec' seem to be defined as follows:
# 'rvec': rotation transformation: transforms points from "WORLD axis-system -> CAMERA axis-system"
# 'tvec': translation of "CAMERA center -> WORLD center", all defined in the "CAMERA axis-system"
rvec *= -1 # convert to: "CAMERA axis-system -> WORLD axis-system", equivalent to rotation of CAMERA axis-system w.r.t. WORLD axis-system
tvec = cvh.Rodrigues(rvec).dot(tvec) # bring to "WORLD axis-system", ...
tvec *= -1 # ... and change direction to "WORLD center -> CAMERA center"
# Calculate pose relative to last keyframe
rvec_rel = trfm.delta_rvec(rvec, rvec_prev)
tvec_rel = tvec - tvec_prev
# Extract axis and angle, to enhance representation
rvec_axis, rvec_angle = trfm.axis_and_angle_from_rvec(rvec)
rvec_rel_axis, rvec_rel_angle = trfm.axis_and_angle_from_rvec(rvec_rel)
# Draw pose information
texts = []
texts.append("Current pose:")
texts.append(" Rvec: %s * %.1fdeg" % (format3DVector(rvec_axis), degrees(rvec_angle)))
texts.append(" Tvec: %s" % format3DVector(tvec))
texts.append("Relative to previous pose:")
texts.append(" Rvec: %s * %.1fdeg" % (format3DVector(rvec_rel_axis), degrees(rvec_rel_angle)))
texts.append(" Tvec: %s" % format3DVector(tvec_rel))
mlt.text(texts[0], fontFace, fontScale*1.5, rgb(150,0,0), thickness=2)
mlt.text(texts[1], fontFace, fontScale, rgb(255,0,0))
mlt.text(texts[2], fontFace, fontScale, rgb(255,0,0))
mlt.text(texts[3], fontFace, fontScale*1.5, rgb(150,0,0), thickness=2)
mlt.text(texts[4], fontFace, fontScale, rgb(255,0,0))
mlt.text(texts[5], fontFace, fontScale, rgb(255,0,0))
mlt.putText(img, (img.shape[1], img.shape[0])) # put text in bottom-right corner
# Show Image
cv2.imshow("Image (with axis-system)", img)
mlt.clear()
# Save keyframe image when SPACE is pressed
last_key_pressed = cv2.waitKey(1) & 0xFF
if last_key_pressed == ord(' ') and ret:
filename = filename_base_extrinsics + str(imageNr)
cv2.imwrite(filename + ".jpg", img) # write image to jpg-file
textTotal = '\n'.join(texts)
open(filename + ".txt", 'w').write(textTotal) # write data to txt-file
print ("Saved keyframe image+data to", filename, ":")
print (textTotal)
imageNr += 1
rvec_prev = rvec
tvec_prev = tvec
def main():
global boardSize
global cameraMatrix, distCoeffs, imageSize
global max_OF_error, max_lost_tracks_ratio
global keypoint_coverage_radius#, min_keypoint_coverage
global target_amount_keypoints, corner_quality_level, corner_min_dist
global homography_condition_threshold
global max_solvePnP_reproj_error, max_2nd_solvePnP_reproj_error, max_fundMat_reproj_error
global max_solvePnP_outlier_ratio, max_2nd_solvePnP_outlier_ratio, solvePnP_use_extrinsic_guess
# Initially known data
boardSize = (8, 6)
color_palette, color_palette_size = prepare_color_palette(2, 3, 4) # used to identify 3D point group ids
cameraMatrix, distCoeffs, imageSize = \
load_camera_intrinsics(os.path.join("..", "..", "datasets", "webcam", "camera_intrinsics.txt"))
#cameraMatrix, distCoeffs, imageSize = \
#load_camera_intrinsics(os.path.join("..", "..", "..", "ARDrone2_tests", "camera_calibration", "live_video", "camera_intrinsics_front.txt"))
# Select working (or 'testing') set
from glob import glob
images = sorted(glob(os.path.join("..", "..", "datasets", "webcam", "captures2", "*.jpeg")))
#images = sorted(glob(os.path.join("..", "..", "..", "ARDrone2_tests", "flying_front", "lowres", "drone0", "*.jpg")))
# Setup some visualization helpers
composite2D_painter = Composite2DPainter("composite 2D", imageSize)
composite3D_painter = Composite3DPainter(
"composite 3D", trfm.P_from_R_and_t(cvh.Rodrigues((pi, 0., 0.)), np.array([[0., 0., 40.]]).T), (1280, 720) )
### Tweaking parameters ###
# OF calculation
max_OF_error = 12.
max_lost_tracks_ratio = 0.5
# keypoint_coverage
keypoint_coverage_radius = int(max_OF_error)
#min_keypoint_coverage = 0.2
# goodFeaturesToTrack
target_amount_keypoints = int(round((imageSize[0] * imageSize[1]) / (pi * keypoint_coverage_radius**2))) # target is entire image full
print "target_amount_keypoints:", target_amount_keypoints
corner_quality_level = 0.01
corner_min_dist = keypoint_coverage_radius
# keyframe_test
homography_condition_threshold = 500 # defined as ratio between max and min singular values
# reprojection error
max_solvePnP_reproj_error = 4.#0.5 # TODO: revert to a lower number
max_2nd_solvePnP_reproj_error = max_solvePnP_reproj_error / 2 # be more strict in a 2nd iteration, used after 1st pass of triangulation
max_fundMat_reproj_error = 2.0
# solvePnP
max_solvePnP_outlier_ratio = 0.3
max_2nd_solvePnP_outlier_ratio = 1. # used in 2nd iteration, after 1st pass of triangulation
solvePnP_use_extrinsic_guess = False # TODO: set to True and see whether the 3D results are better
# Init
imgs = []
imgs_gray = []
objp = [] # 3D points
objp_groups = [] # 3D point group ids, each new batch of detected points is put in a separate group
group_id = 0 # current 3D point group id
rvecs, rvecs_keyfr = [], []
tvecs, tvecs_keyfr = [], []
# Start frame requires special treatment
# Start frame : read image and detect 2D points
imgs.append(cv2.imread(images[0]))
base_img = imgs[0]
imgs_gray.append(cv2.cvtColor(imgs[0], cv2.COLOR_BGR2GRAY))
ret, new_imgp = cvh.extractChessboardFeatures(imgs[0], boardSize)
if not ret:
print "First image must contain the entire chessboard!"
return
# Start frame : define a priori 3D points
objp = prepare_object_points(boardSize)
objp_groups = np.zeros((objp.shape[0]), dtype=np.int)
group_id += 1
# Start frame : setup linking data-structures
base_imgp = new_imgp # 2D points
all_idxs_tmp = np.arange(len(base_imgp))
triangl_idxs = set(all_idxs_tmp)
nontriangl_idxs = set()
imgp_to_objp_idxs = np.array(sorted(triangl_idxs), dtype=int)
# Start frame : get absolute pose
ret, rvec, tvec = cv2.solvePnP( # assume first frame is a proper frame with chessboard fully in-sight
objp, new_imgp, cameraMatrix, distCoeffs )
print "solvePnP reproj_error init:", reprojection_error(objp, new_imgp, rvec, tvec, cameraMatrix, distCoeffs)[0]
rvecs.append(rvec)
tvecs.append(tvec)
rvec_keyfr = rvec
tvec_keyfr = tvec
rvecs_keyfr.append(rvec_keyfr)
tvecs_keyfr.append(tvec_keyfr)
# Start frame : add other points
mask_img = keypoint_mask(new_imgp)
to_add = target_amount_keypoints - len(new_imgp)
imgp_extra = cv2.goodFeaturesToTrack(imgs_gray[0], to_add, corner_quality_level, corner_min_dist, None, mask_img).reshape((-1, 2))
cv2.imshow("img", cv2.drawKeypoints(imgs[0], [cv2.KeyPoint(p[0],p[1], 7.) for p in imgp_extra], color=rgb(0,0,255)))
cv2.waitKey()
print "added:", len(imgp_extra)
base_imgp, new_imgp, imgp_to_objp_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp = idxs_rebase_and_add_imgp(
imgp_extra, base_imgp, new_imgp, imgp_to_objp_idxs, triangl_idxs, nontriangl_idxs, all_idxs_tmp )
ret = 2 # indicate keyframe
# Draw 3D points info of current frame
print "Drawing composite 2D image"
composite2D_painter.draw(imgs[0], rvec, tvec, ret,
cameraMatrix, distCoeffs, triangl_idxs, nontriangl_idxs, all_idxs_tmp, new_imgp, imgp_to_objp_idxs, objp, objp_groups, group_id, color_palette, color_palette_size)
# Draw 3D points info of all frames
print "Drawing composite 3D image (keys: LEFT/RIGHT/UP/DOWN/PAGEUP/PAGEDOWN/HOME/END)"
composite3D_painter.draw(rvec, tvec, ret,
triangl_idxs, imgp_to_objp_idxs, objp, objp_groups, color_palette, color_palette_size)
for i in range(1, len(images)):
# Frame[i-1] -> Frame[i]
print "\nFrame[%s] -> Frame[%s]" % (i-1, i)
print " processing '", images[i], "':"
cur_img = cv2.imread(images[i])
imgs.append(cur_img)
imgs_gray.append(cv2.cvtColor(imgs[-1], cv2.COLOR_BGR2GRAY))
ret, base_imgp, new_imgp, triangl_idxs, nontriangl_idxs, imgp_to_objp_idxs, all_idxs_tmp, objp, objp_groups, group_id, rvec, tvec, rvec_keyfr, tvec_keyfr, base_img = \
handle_new_frame(base_imgp, new_imgp, imgs[-2], imgs_gray[-2], imgs[-1], imgs_gray[-1], triangl_idxs, nontriangl_idxs, imgp_to_objp_idxs, all_idxs_tmp, objp, objp_groups, group_id, rvec_keyfr, tvec_keyfr, base_img)
if ret:
rvecs.append(rvec)
tvecs.append(tvec)
if ret == 2: # frame is a keyframe
rvecs_keyfr.append(rvec_keyfr)
tvecs_keyfr.append(tvec_keyfr)
else: # frame rejected
del imgs[-1]
del imgs_gray[-1]
# Draw 3D points info of current frame
print "Drawing composite 2D image"
composite2D_painter.draw(cur_img, rvec, tvec, ret,
cameraMatrix, distCoeffs, triangl_idxs, nontriangl_idxs, all_idxs_tmp, new_imgp, imgp_to_objp_idxs, objp, objp_groups, group_id, color_palette, color_palette_size)
# Draw 3D points info of all frames
print "Drawing composite 3D image (keys: LEFT/RIGHT/UP/DOWN/PAGEUP/PAGEDOWN)"
composite3D_painter.draw(rvec, tvec, ret,
triangl_idxs, imgp_to_objp_idxs, objp, objp_groups, color_palette, color_palette_size)
def main():
# Initially known data
boardSize = (8, 6)
objp = calibration_tools.grid_objp(boardSize)
cameraMatrix, distCoeffs, imageSize = calibration_tools.load_camera_intrinsics(
os.path.join("..", "..", "datasets", "webcam",
"camera_intrinsics.txt"))
# Initiate 2d 3d arrays
objectPoints = []
imagePoints = []
# Select working (or 'testing') set
from glob import glob
images = sorted(
glob(
os.path.join("..", "..", "datasets", "webcam", "captures2",
"*.jpeg")))
def main_loop(left_points, left_gray, left_img, right_img, triangl_idxs,
chessboard_idxs):
# Detect right (key)points
right_keypoints = fast.detect(right_img)
right_FAST_points = np.array([kp.pt for kp in right_keypoints],
dtype=np.float32)
# Visualize right_FAST_points
print("Visualize right_FAST_points")
cv2.imshow(
"img",
cv2.drawKeypoints(
right_img,
[cv2.KeyPoint(p[0], p[1], 7.) for p in right_FAST_points],
color=blue)) # blue markers with size 7
cv2.waitKey()
# Calculate optical flow (= 'OF') field from left to right
right_gray = cv2.cvtColor(right_img, cv2.COLOR_BGR2GRAY)
right_OF_points, status_OF, err_OF = cv2.calcOpticalFlowPyrLK(
left_gray, right_gray, left_points) # points to start from
err_OF = err_OF.reshape(-1)
def match_OF_based(right_OF_points,
right_FAST_points,
err_OF,
status_OF,
max_radius_OF_to_FAST,
max_dist_ratio,
left_point_idxs=None
): # if not None, left_point_idxs specifies mask
# Filter out the OF points with high error
right_OF_points, right_OF_to_left_idxs = \
zip(*[ (p, i) for i, p in enumerate(right_OF_points)
if status_OF[i] and # only include correct OF-points
err_OF[i] < max_OF_error and # error should be low enough
(left_point_idxs == None or i in left_point_idxs) ]) # apply mask
right_OF_points = np.array(right_OF_points)
# Visualize right_OF_points
print("Visualize right_OF_points")
cv2.imshow(
"img",
cv2.drawKeypoints(
right_img,
[cv2.KeyPoint(p[0], p[1], 7.) for p in right_OF_points],
color=blue)) # blue markers with size 7
cv2.waitKey()
# Align right_OF_points with right_FAST_points by matching them
matches_twoNN = matcher.radiusMatch(
right_OF_points, # query points
right_FAST_points, # train points
max_radius_OF_to_FAST)
# Filter out ambiguous matches by a ratio-test, and prevent duplicates
best_dist_matches_by_trainIdx = {
} # duplicate prevention: trainIdx -> match_best_dist
for query_matches in matches_twoNN:
# Ratio test
if not (
len(query_matches) == 1
or # only one match, probably a good one
(
len(query_matches) > 1
and # if more than one, first two shouldn't lie too close
query_matches[0].distance / query_matches[1].distance <
max_dist_ratio)):
continue
# Relink match to use 'left_point' indices
match = cv2.DMatch(
right_OF_to_left_idxs[
query_matches[0].queryIdx], # queryIdx: left_points
query_matches[0].trainIdx, # trainIdx: right_FAST_points
query_matches[0].distance)
# Duplicate prevention
if (not match.trainIdx in best_dist_matches_by_trainIdx
or # no duplicate found
err_OF[match.queryIdx]
< # replace duplicate if inferior, based on err_OF
err_OF[best_dist_matches_by_trainIdx[
match.trainIdx].queryIdx]):
best_dist_matches_by_trainIdx[match.trainIdx] = match
return best_dist_matches_by_trainIdx
# Match between FAST -> FAST features
matches_by_trainIdx = match_OF_based(right_OF_points,
right_FAST_points, err_OF,
status_OF,
max_radius_OF_to_FAST["FAST"],
max_dist_ratio["FAST"])
if allow_chessboard_matcher_and_refiner and chessboard_idxs:
# Match between chessboard -> chessboard features
matches_by_trainIdx_chessboard = match_OF_based(
right_OF_points, right_FAST_points, err_OF, status_OF,
max_radius_OF_to_FAST["chessboard"],
max_dist_ratio["chessboard"], chessboard_idxs) # set mask
# Overwrite FAST -> FAST feature matches by chessboard -> chessboard feature matches
matches_by_trainIdx.update(matches_by_trainIdx_chessboard)
# Refine chessboard features
chessboard_corners_idxs = list(matches_by_trainIdx_chessboard)
chessboard_corners = right_FAST_points[chessboard_corners_idxs]
cv2.cornerSubPix(
right_gray,
chessboard_corners,
(11, 11), # window
(-1, -1), # deadzone
(cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 30,
0.001)) # termination criteria
right_FAST_points[chessboard_corners_idxs] = chessboard_corners
# Update chessboard_idxs
chessboard_idxs = set(matches_by_trainIdx_chessboard)
print("Amount of chessboard features tracked in the new image:",
len(chessboard_idxs))
# Calculate average filtered OF vector
trainIdxs = list(matches_by_trainIdx)
queryIdxs = [
matches_by_trainIdx[trainIdx].queryIdx for trainIdx in trainIdxs
]
mean_OF_vector = (right_FAST_points[trainIdxs] -
left_points[queryIdxs]).mean(axis=0)
mean_OF_vector_length = np.linalg.norm(mean_OF_vector)
print("mean_OF_vector (from LEFT to RIGHT):", mean_OF_vector,
"; mean_OF_vector_length:", mean_OF_vector_length)
# Partition matches to make a distinction between previously triangulated points and non-triangl.
matches_left_triangl_to_right_FAST = []
matches_left_non_triangl_to_right_FAST = []
for trainIdx in matches_by_trainIdx:
match = matches_by_trainIdx[trainIdx]
if matches_by_trainIdx[trainIdx].queryIdx in triangl_idxs:
matches_left_triangl_to_right_FAST.append(match)
else:
matches_left_non_triangl_to_right_FAST.append(match)
# and all matches together
matches_left_to_right_FAST = matches_left_triangl_to_right_FAST + matches_left_non_triangl_to_right_FAST
# Visualize (previously triangulated) left_points of corresponding outlier-filtered right_FAST_points
print("Visualize LEFT points (previously triangulated)")
cv2.imshow("img",
cv2.drawKeypoints(left_img, [
cv2.KeyPoint(left_points[m.queryIdx][0],
left_points[m.queryIdx][1], 7.)
for m in matches_left_triangl_to_right_FAST
],
color=blue)) # blue markers with size 7
cv2.waitKey()
# Visualize (previously triangulated) outlier-filtered right_FAST_points
print("Visualize RIGHT points (previously triangulated)")
cv2.imshow("img",
cv2.drawKeypoints(right_img, [
cv2.KeyPoint(right_FAST_points[m.trainIdx][0],
right_FAST_points[m.trainIdx][1], 7.)
for m in matches_left_triangl_to_right_FAST
],
color=blue)) # blue markers with size 7
cv2.waitKey()
# Visualize (not yet triangulated) outlier-filtered right_FAST_points
print("Visualize LEFT points (not yet triangulated)")
cv2.imshow("img",
cv2.drawKeypoints(left_img, [
cv2.KeyPoint(left_points[m.queryIdx][0],
left_points[m.queryIdx][1], 7.)
for m in matches_left_non_triangl_to_right_FAST
],
color=blue)) # blue markers with size 7
cv2.waitKey()
# Visualize (not yet triangulated) outlier-filtered right_FAST_points
print("Visualize RIGHT points (not yet triangulated)")
cv2.imshow("img",
cv2.drawKeypoints(right_img, [
cv2.KeyPoint(right_FAST_points[m.trainIdx][0],
right_FAST_points[m.trainIdx][1], 7.)
for m in matches_left_non_triangl_to_right_FAST
],
color=blue)) # blue markers with size 7
cv2.waitKey()
# Pose estimation and Triangulation
# ...
# Update triangl_idxs TODO: filter using outlier-removal by epipolar constraints
triangl_idxs = set(matches_by_trainIdx)
return right_FAST_points, right_gray, triangl_idxs, chessboard_idxs
###----------------------------- Frame 0 (init) -----------------------------###
print("###---------------------- Frame 0 (init) ----------------------###")
left_img = cv2.imread(images[0])
left_gray = cv2.cvtColor(left_img, cv2.COLOR_BGR2GRAY)
right_img = cv2.imread(images[1])
# Detect left (key)points
ret, left_points = cvh.extractChessboardFeatures(left_img, boardSize)
if not ret:
raise Exception("No chessboard features detected.")
# Set masks to alter matches' priority
chessboard_idxs = set(range(
len(left_points))) # all chessboard corners are in sight
triangl_idxs = set(
chessboard_idxs
) # the chessboard feature points are already triangulated
# Invoke main loop
right_FAST_points, right_gray, triangl_idxs, chessboard_idxs = \
main_loop(left_points, left_gray, left_img, right_img, triangl_idxs, chessboard_idxs)
for i in range(2, 14):
###----------------------------- Frame i -----------------------------###
print("###---------------------- Frame %s ----------------------###" %
i)
# Update data for new frame
left_img = right_img
left_gray = right_gray
right_img = cv2.imread(images[i])
# Use previous feature points
left_points = right_FAST_points
# Invoke main loop
right_FAST_points, right_gray, triangl_idxs, chessboard_idxs = \
main_loop(left_points, left_gray, left_img, right_img, triangl_idxs, chessboard_idxs)
