def print_pose(rvec, tvec):
"""
Some debug printing of the camera pose projection matrix,
the printed output can be used in Blender to visualize this camera pose,
using the "create_pose_camera()" function.
"rvec", "tvec" : defining the camera pose, compatible with OpenCV's output
"""
import cv2
import transforms as trfm
ax, an = trfm.axis_and_angle_from_rvec(-rvec)
print ("axis, angle = \\")
print (list(ax.reshape(-1)), an) # R^(-1)
print ("pos = \\")
print (list(-cv2.Rodrigues(-rvec)[0].dot(tvec).reshape(-1))) # -R^(-1) * t
def print_pose(rvec, tvec):
"""
Some debug printing of the camera pose projection matrix,
the printed output can be used in Blender to visualize this camera pose,
using the "create_pose_camera()" function.
"rvec", "tvec" : defining the camera pose, compatible with OpenCV's output
"""
import cv2
import transforms as trfm
ax, an = trfm.axis_and_angle_from_rvec(-rvec)
print("axis, angle = \\")
print(list(ax.reshape(-1)), an) # R^(-1)
print("pos = \\")
print(list(-cv2.Rodrigues(-rvec)[0].dot(tvec).reshape(-1))) # -R^(-1) * t
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 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
