def main():
global boardSize, filename_base_chessboards, filename_intrinsics, filename_distorted, filename_triangl_pose_est_left, filename_triangl_pose_est_right, filename_base_extrinsics, filenames_extra_chessboards, filenames_extra_intrinsics, extra_boardSizes, extra_board_scales, extra_board_rvecs, extra_board_tvecs, device_id
boardSize = (8, 6)
filename_base_chessboards = join_path(
"data", "chessboards",
"chessboard*.jpg") # calibration images of the base camera
filename_intrinsics = join_path("results", "camera_intrinsics.txt")
filename_distorted = join_path(
"data", "chessboards", "chessboard07.jpg") # a randomly chosen image
#filename_triangl_pose_est_left = join_path("data", "chessboards", "chessboard07.jpg") # a randomly chosen image
#filename_triangl_pose_est_right = join_path("data", "chessboards", "chessboard08.jpg") # a randomly chosen image
filename_triangl_pose_est_left = join_path(
"data", "chessboards_and_nonplanar",
"image-0001.jpeg") # a randomly chosen image
filename_triangl_pose_est_right = join_path(
"data", "chessboards_and_nonplanar",
"image-0056.jpeg") # a randomly chosen image
filename_base_extrinsics = join_path("results", "chessboards_extrinsic",
"chessboard")
filenames_extra_chessboards = (
join_path(
"data", "chessboards_front",
"front-*.jpg"), # sets of calibration images of extra cameras
join_path("data", "chessboards_bottom", "bottom-*.jpg"))
filenames_extra_intrinsics = (join_path("results",
"camera_intrinsics_front.txt"),
join_path("results",
"camera_intrinsics_bottom.txt")
) # intrinsics of extra cameras
extra_boardSizes = ((8, 6), (8, 6))
extra_board_scales = (1., 1.)
extra_board_rvecs = ((0., 0., 0.), (0., -pi / 2, 0.))
extra_board_tvecs = ((0., 0., 0.), (6., 0., (1200. - 193.) / 27.6 + 1.))
device_id = 1 # webcam
nonplanar_left = nonplanar_right = np.zeros((0, 2))
help_text = """\
Choose between: (in order)
1: prepare_object_points (required)
2: calibrate_camera_interactive (required for "reprojection_error")
3: save_camera_intrinsics
4: load_camera_intrinsics (required)
5: undistort_image
6: reprojection_error
7: triangl_pose_est_interactive
8: realtime_pose_estimation (recommended)
9: calibrate_relative_poses_interactive
q: quit
Info: Sometimes you will be prompted: 'someVariable [defaultValue]: ',
in that case you can type a new value,
or simply press ENTER to preserve the default value.
"""
from textwrap import dedent
print(dedent(help_text))
inp = ""
while inp.lower() != "q":
inp = raw_input("\n: ").strip()
if inp == "1":
get_variable("boardSize", lambda x: eval("(%s)" % x))
print() # add new-line
objp = calibration_tools.grid_objp(boardSize)
elif inp == "2":
get_variable("filename_base_chessboards")
from glob import glob
images = sorted(glob(filename_base_chessboards))
print() # add new-line
reproj_error, cameraMatrix, distCoeffs, rvecs, tvecs, objectPoints, imagePoints, imageSize = \
calibrate_camera_interactive(images, objp, boardSize)
print("cameraMatrix:\n", cameraMatrix)
print("distCoeffs:\n", distCoeffs)
print("reproj_error:", reproj_error)
cv2.destroyAllWindows()
elif inp == "3":
get_variable("filename_intrinsics")
print() # add new-line
calibration_tools.save_camera_intrinsics(filename_intrinsics,
cameraMatrix, distCoeffs,
imageSize)
elif inp == "4":
get_variable("filename_intrinsics")
print() # add new-line
cameraMatrix, distCoeffs, imageSize = \
calibration_tools.load_camera_intrinsics(filename_intrinsics)
elif inp == "5":
get_variable("filename_distorted")
img = cv2.imread(filename_distorted)
print() # add new-line
img_undistorted, roi = calibration_tools.undistort_image(
img, cameraMatrix, distCoeffs, imageSize)
cv2.imshow("Original Image", img)
cv2.imshow("Undistorted Image", img_undistorted)
print("Press any key to continue.")
cv2.waitKey()
cv2.destroyAllWindows()
elif inp == "6":
mean_error, square_error = reprojection_error(
objectPoints, imagePoints, cameraMatrix, distCoeffs, rvecs,
tvecs)
print("mean absolute error:", mean_error)
print("square error:", square_error)
elif inp == "7":
print(triangl_pose_est_interactive.__doc__)
get_variable("filename_triangl_pose_est_left")
img_left = cv2.imread(filename_triangl_pose_est_left)
get_variable("filename_triangl_pose_est_right")
img_right = cv2.imread(filename_triangl_pose_est_right)
print() # add new-line
nonplanar_left, nonplanar_right = \
triangl_pose_est_interactive(img_left, img_right, cameraMatrix, distCoeffs, imageSize, objp, boardSize, nonplanar_left, nonplanar_right)
cv2.destroyAllWindows()
elif inp == "8":
print(realtime_pose_estimation.__doc__)
get_variable("device_id", int)
get_variable("filename_base_extrinsics")
print() # add new-line
realtime_pose_estimation(device_id, filename_base_extrinsics,
cameraMatrix, distCoeffs, objp, boardSize)
cv2.destroyAllWindows()
elif inp == "9":
print(calibrate_relative_poses_interactive.__doc__)
get_variable("filenames_extra_chessboards",
lambda x: eval("(%s)" % x))
from glob import glob
image_sets = [
sorted(glob(images)) for images in filenames_extra_chessboards
]
get_variable("filenames_extra_intrinsics",
lambda x: eval("(%s)" % x))
cameraMatrixs, distCoeffss, imageSizes = zip(
*map(calibration_tools.load_camera_intrinsics,
filenames_extra_intrinsics))
get_variable("extra_boardSizes", lambda x: eval("(%s)" % x))
get_variable("extra_board_scales", lambda x: eval("(%s)" % x))
get_variable("extra_board_rvecs", lambda x: eval("(%s)" % x))
get_variable("extra_board_tvecs", lambda x: eval("(%s)" % x))
print() # add new-line
ret, Ps, reproj_error_max = \
calibrate_relative_poses_interactive(image_sets, cameraMatrixs, distCoeffss, imageSizes,
extra_boardSizes, extra_board_scales, extra_board_rvecs, extra_board_tvecs)
if ret:
print("Ps:")
for P in Ps:
print(P)
print("reproj_error_max:", reproj_error_max)
def main():
"""
A file with the initial pose and 3D points of the SVO dataset, will be created.
"""
num_features = 100
num_iterations = 53
#corner_quality_level = 0.4805294789864505
print "Searching for features ( max", num_features, ")..."
# Load first image
img = cv2.cvtColor(
cv2.imread(join_path("sin2_tex2_h1_v8_d", "img", "frame_000002_0.png")),
cv2.COLOR_BGR2GRAY )
# Find just enough features, iterate using bisection to find the best "corner_quality_level" value
corner_min_dist = 0.
lower = 0.
upper = 1.
for i in range(num_iterations):
corner_quality_level = (lower + upper) / 2
imgp = cv2.goodFeaturesToTrack(img, num_features, corner_quality_level, corner_min_dist)
if imgp == None or len(imgp) < num_features:
upper = corner_quality_level
else:
lower = corner_quality_level
corner_quality_level = lower if lower else corner_quality_level
imgp = cv2.goodFeaturesToTrack(img, num_features, corner_quality_level, corner_min_dist).reshape((-1, 2))
print len(imgp), "features found, corner_quality_level:", corner_quality_level
# Load camera intrinsics
cameraMatrix, distCoeffs, imageSize = calibration_tools.load_camera_intrinsics(
join_path("camera_intrinsics.txt") )
# Load and save first pose
timestps, locations, quaternions = dataset_tools.load_cam_trajectory_TUM(
join_path("sin2_tex2_h1_v8_d", "traj_groundtruth.txt") )
P = trfm.P_from_pose_TUM(quaternions[0], locations[0])
np.savetxt(join_path("sin2_tex2_h1_v8_d", "init_pose.txt"), P)
# Generate 3D points, knowing that they lay at the plane z=0:
# for each 2D point, the following system of equations is solved for X:
# A * X = b
# where:
# X = [x, y, s]^T
# [x, y, 0] the 3D point's coords
# s is an unknown scalefactor of the normalized 2D point
# A = [[ 1 0 | ],
# [ 0 1 | Pu ],
# [ 0 0 | ]]
# Pu = - R^(-1) * p
# p = [u, v, 1]^T the 2D point's homogeneous coords
# b = - R^(-1) * t
# R, t = 3x3 rotation matrix and 3x1 translation vector of 3x4 pose matrix P
# The system of equations is solved in bulk for all points using broadcasted backsubstitution
objp = np.empty((len(imgp), 3)).T
objp[2:3, :] = 0. # z = 0
imgp_nrml = cv2.undistortPoints(np.array([imgp]), cameraMatrix, distCoeffs)[0]
imgp_nrml = np.concatenate((imgp_nrml, np.ones((len(imgp), 1))), axis=1) # to homogeneous coords
P_inv = trfm.P_inv(P)
Pu = P_inv[0:3, 0:3].dot(imgp_nrml.T)
scale = -P_inv[2:3, 3:4] / Pu[2:3, :]
objp[0:2, :] = P_inv[0:2, 3:4] + scale * Pu[0:2, :] # x and y components
objp = objp.T
# Save 3D points
dataset_tools.save_3D_points_to_pcd_file(
join_path("sin2_tex2_h1_v8_d", "init_points.pcd"),
objp )
print "Done."
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, filename_base_chessboards, filename_intrinsics, filename_distorted, filename_triangl_pose_est_left, filename_triangl_pose_est_right, filename_base_extrinsics, filenames_extra_chessboards, filenames_extra_intrinsics, extra_boardSizes, extra_board_scales, extra_board_rvecs, extra_board_tvecs, device_id
boardSize = (8, 6)
filename_base_chessboards = join_path("data", "chessboards", "chessboard*.jpg") # calibration images of the base camera
filename_intrinsics = join_path("results", "camera_intrinsics.txt")
filename_distorted = join_path("data", "chessboards", "chessboard07.jpg") # a randomly chosen image
#filename_triangl_pose_est_left = join_path("data", "chessboards", "chessboard07.jpg") # a randomly chosen image
#filename_triangl_pose_est_right = join_path("data", "chessboards", "chessboard08.jpg") # a randomly chosen image
filename_triangl_pose_est_left = join_path("data", "chessboards_and_nonplanar", "image-0001.jpeg") # a randomly chosen image
filename_triangl_pose_est_right = join_path("data", "chessboards_and_nonplanar", "image-0056.jpeg") # a randomly chosen image
filename_base_extrinsics = join_path("results", "chessboards_extrinsic", "chessboard")
filenames_extra_chessboards = (join_path("data", "chessboards_front", "front-*.jpg"), # sets of calibration images of extra cameras
join_path("data", "chessboards_bottom", "bottom-*.jpg"))
filenames_extra_intrinsics = (join_path("results", "camera_intrinsics_front.txt"),
join_path("results", "camera_intrinsics_bottom.txt")) # intrinsics of extra cameras
extra_boardSizes = ((8, 6), (8, 6))
extra_board_scales = (1., 1.)
extra_board_rvecs = ((0., 0., 0.), (0., -pi/2, 0.))
extra_board_tvecs = ((0., 0., 0.), (6., 0., (1200.-193.)/27.6+1.))
device_id = 1 # webcam
nonplanar_left = nonplanar_right = np.zeros((0, 2))
help_text = """\
Choose between: (in order)
1: prepare_object_points (required)
2: calibrate_camera_interactive (required for "reprojection_error")
3: save_camera_intrinsics
4: load_camera_intrinsics (required)
5: undistort_image
6: reprojection_error
7: triangl_pose_est_interactive
8: realtime_pose_estimation (recommended)
9: calibrate_relative_poses_interactive
q: quit
Info: Sometimes you will be prompted: 'someVariable [defaultValue]: ',
in that case you can type a new value,
or simply press ENTER to preserve the default value.
"""
from textwrap import dedent
print (dedent(help_text))
inp = ""
while inp.lower() != "q":
inp = raw_input("\n: ").strip()
if inp == "1":
get_variable("boardSize", lambda x: eval("(%s)" % x))
print () # add new-line
objp = calibration_tools.grid_objp(boardSize)
elif inp == "2":
get_variable("filename_base_chessboards")
from glob import glob
images = sorted(glob(filename_base_chessboards))
print () # add new-line
reproj_error, cameraMatrix, distCoeffs, rvecs, tvecs, objectPoints, imagePoints, imageSize = \
calibrate_camera_interactive(images, objp, boardSize)
print ("cameraMatrix:\n", cameraMatrix)
print ("distCoeffs:\n", distCoeffs)
print ("reproj_error:", reproj_error)
cv2.destroyAllWindows()
elif inp == "3":
get_variable("filename_intrinsics")
print () # add new-line
calibration_tools.save_camera_intrinsics(
filename_intrinsics, cameraMatrix, distCoeffs, imageSize )
elif inp == "4":
get_variable("filename_intrinsics")
print () # add new-line
cameraMatrix, distCoeffs, imageSize = \
calibration_tools.load_camera_intrinsics(filename_intrinsics)
elif inp == "5":
get_variable("filename_distorted")
img = cv2.imread(filename_distorted)
print () # add new-line
img_undistorted, roi = calibration_tools.undistort_image(
img, cameraMatrix, distCoeffs, imageSize )
cv2.imshow("Original Image", img)
cv2.imshow("Undistorted Image", img_undistorted)
print ("Press any key to continue.")
cv2.waitKey()
cv2.destroyAllWindows()
elif inp == "6":
mean_error, square_error = reprojection_error(
objectPoints, imagePoints, cameraMatrix, distCoeffs, rvecs, tvecs )
print ("mean absolute error:", mean_error)
print ("square error:", square_error)
elif inp == "7":
print (triangl_pose_est_interactive.__doc__)
get_variable("filename_triangl_pose_est_left")
img_left = cv2.imread(filename_triangl_pose_est_left)
get_variable("filename_triangl_pose_est_right")
img_right = cv2.imread(filename_triangl_pose_est_right)
print () # add new-line
nonplanar_left, nonplanar_right = \
triangl_pose_est_interactive(img_left, img_right, cameraMatrix, distCoeffs, imageSize, objp, boardSize, nonplanar_left, nonplanar_right)
cv2.destroyAllWindows()
elif inp == "8":
print (realtime_pose_estimation.__doc__)
get_variable("device_id", int)
get_variable("filename_base_extrinsics")
print () # add new-line
realtime_pose_estimation(device_id, filename_base_extrinsics, cameraMatrix, distCoeffs, objp, boardSize)
cv2.destroyAllWindows()
elif inp == "9":
print (calibrate_relative_poses_interactive.__doc__)
get_variable("filenames_extra_chessboards", lambda x: eval("(%s)" % x))
from glob import glob
image_sets = [sorted(glob(images)) for images in filenames_extra_chessboards]
get_variable("filenames_extra_intrinsics", lambda x: eval("(%s)" % x))
cameraMatrixs, distCoeffss, imageSizes = zip(*map(
calibration_tools.load_camera_intrinsics, filenames_extra_intrinsics ))
get_variable("extra_boardSizes", lambda x: eval("(%s)" % x))
get_variable("extra_board_scales", lambda x: eval("(%s)" % x))
get_variable("extra_board_rvecs", lambda x: eval("(%s)" % x))
get_variable("extra_board_tvecs", lambda x: eval("(%s)" % x))
print () # add new-line
ret, Ps, reproj_error_max = \
calibrate_relative_poses_interactive(image_sets, cameraMatrixs, distCoeffss, imageSizes,
extra_boardSizes, extra_board_scales, extra_board_rvecs, extra_board_tvecs)
if ret:
print ("Ps:")
for P in Ps: print (P)
print ("reproj_error_max:", reproj_error_max)
def main():
"""
A file with the initial pose and 3D points of the SVO dataset, will be created.
"""
num_features = 100
num_iterations = 53
#corner_quality_level = 0.4805294789864505
print("Searching for features ( max", num_features, ")...")
# Load first image
img = cv2.cvtColor(
cv2.imread(join_path("sin2_tex2_h1_v8_d", "img",
"frame_000002_0.png")), cv2.COLOR_BGR2GRAY)
# Find just enough features, iterate using bisection to find the best "corner_quality_level" value
corner_min_dist = 0.
lower = 0.
upper = 1.
for i in range(num_iterations):
corner_quality_level = (lower + upper) / 2
imgp = cv2.goodFeaturesToTrack(img, num_features, corner_quality_level,
corner_min_dist)
if imgp == None or len(imgp) < num_features:
upper = corner_quality_level
else:
lower = corner_quality_level
corner_quality_level = lower if lower else corner_quality_level
imgp = cv2.goodFeaturesToTrack(img, num_features, corner_quality_level,
corner_min_dist).reshape((-1, 2))
print(len(imgp), "features found, corner_quality_level:",
corner_quality_level)
# Load camera intrinsics
cameraMatrix, distCoeffs, imageSize = calibration_tools.load_camera_intrinsics(
join_path("camera_intrinsics.txt"))
# Load and save first pose
timestps, locations, quaternions = dataset_tools.load_cam_trajectory_TUM(
join_path("sin2_tex2_h1_v8_d", "traj_groundtruth.txt"))
P = trfm.P_from_pose_TUM(quaternions[0], locations[0])
np.savetxt(join_path("sin2_tex2_h1_v8_d", "init_pose.txt"), P)
# Generate 3D points, knowing that they lay at the plane z=0:
# for each 2D point, the following system of equations is solved for X:
# A * X = b
# where:
# X = [x, y, s]^T
# [x, y, 0] the 3D point's coords
# s is an unknown scalefactor of the normalized 2D point
# A = [[ 1 0 | ],
# [ 0 1 | Pu ],
# [ 0 0 | ]]
# Pu = - R^(-1) * p
# p = [u, v, 1]^T the 2D point's homogeneous coords
# b = - R^(-1) * t
# R, t = 3x3 rotation matrix and 3x1 translation vector of 3x4 pose matrix P
# The system of equations is solved in bulk for all points using broadcasted backsubstitution
objp = np.empty((len(imgp), 3)).T
objp[2:3, :] = 0. # z = 0
imgp_nrml = cv2.undistortPoints(np.array([imgp]), cameraMatrix,
distCoeffs)[0]
imgp_nrml = np.concatenate((imgp_nrml, np.ones((len(imgp), 1))),
axis=1) # to homogeneous coords
P_inv = trfm.P_inv(P)
Pu = P_inv[0:3, 0:3].dot(imgp_nrml.T)
scale = -P_inv[2:3, 3:4] / Pu[2:3, :]
objp[0:2, :] = P_inv[0:2, 3:4] + scale * Pu[0:2, :] # x and y components
objp = objp.T
# Save 3D points
dataset_tools.save_3D_points_to_pcd_file(
join_path("sin2_tex2_h1_v8_d", "init_points.pcd"), objp)
print("Done.")
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)
