def get_new_MM_posers(camera_parameters, mmap_low, mmap_high):
low_pose = aruco.MarkerMapPoseTracker()
high_pose = aruco.MarkerMapPoseTracker()
low_pose.setParams(camera_parameters, mmap_low)
high_pose.setParams(camera_parameters, mmap_high)
return aruco.MarkerDetector(), low_pose, high_pose
def findImageArucoParams(imgName):
# create markerDetector object
markerdetector = aruco.MarkerDetector()
# Load an color image in grayscale
img = cv2.imread(imgName, cv2.IMREAD_COLOR)
markers = markerdetector.detect(img, camparam)
return markers
'''
def detect_goal(frame):
detector = aruco.MarkerDetector()
markers = detector.detect(frame)
m10x, m10y, m10w, m10h, m10d = -1, -1, -1, -1, -1
m11x, m11y, m11w, m11h, m11d = -1, -1, -1, -1, -1
for marker in markers:
if marker.id == 10:
m10x = int(round((marker[1][0] + marker[2][0]) / 2))
m10y = int(round((marker[2][1] + marker[3][1]) / 2))
m10w = int(round((marker[0][0] + marker[3][0]) / 2 - m10x))
m10h = int(round((marker[0][1] + marker[1][1]) / 2 - m10y))
m10d = int(round(abs(marker[2][1] - marker[3][1])))
if marker.id == 11:
m11x = int(round((marker[1][0] + marker[2][0]) / 2))
m11y = int(round((marker[2][1] + marker[3][1]) / 2))
m11w = int(round((marker[0][0] + marker[3][0]) / 2 - m11x))
m11h = int(round((marker[0][1] + marker[1][1]) / 2 - m11y))
m11d = int(round(abs(marker[2][1] - marker[3][1])))
else:
print "seeing marker", marker.id
goal_rect = [-1, -1, -1, -1]
if m10x != -1 and m10y != -1 and m11x != -1 and m11y != -1:
goal_rect[0] = m11x + m11w
goal_rect[1] = m11y
goal_rect[2] = m10x - m11x - m11w
goal_rect[3] = (m10h + m11h) / 2
x, y, w, h = goal_rect[0], goal_rect[1], goal_rect[2], goal_rect[3]
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
elif m10x != -1 and m10y != -1:
goal_rect[0] = int(round(m10x - m10w * 0.75))
goal_rect[1] = int(round(m10y + 0.75 * m10d))
goal_rect[2] = int(round(m10w * 0.75))
goal_rect[3] = m10h
x, y, w, h = goal_rect[0], goal_rect[1], goal_rect[2], goal_rect[3]
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
elif m11x != -1 and m11y != -1:
goal_rect[0] = m11x + m11w
goal_rect[1] = int(round(m11y + 0.75 * m11d))
goal_rect[2] = int(round(m11w * 0.75))
goal_rect[3] = m11h
x, y, w, h = goal_rect[0], goal_rect[1], goal_rect[2], goal_rect[3]
cv2.rectangle(frame, (x, y), (x + w, y + h), (0, 255, 0), 2)
#~ cv2.imshow("",frame)
#~ k = cv2.waitKey(5) & 0xFF
return goal_rect
def detectMarker(img, param_file): markers = []; detector = aruco.MarkerDetector() detector.setMinMaxSize(0.001) cam_param = aruco.CameraParameters() cam_param.readFromXMLFile(param_file) marker_set = detector.detect(img, cam_param, 1.0); for marker in marker_set: m = AR_Marker(marker) for i in range(4): m.points[i] = (int(marker[i][0]), int(marker[i][1])) markers.append(m) return markers
@author: fehlfarbe
'''
import sys
import cv2
import numpy as np
import aruco
if __name__ == '__main__':
# load board and camera parameters
#boardconfig = aruco.BoardConfiguration("chessboardinfo_small_meters.yml")
camparam = aruco.CameraParameters()
camparam.readFromXMLFile("dfk72_6mm_param2.yml")
# create detector and set parameters
detector = aruco.MarkerDetector()
params = detector.getParams()
#detector.setParams(camparam)
# set minimum marker size for detection
#markerdetector = detector.getMarkerDetector()
#markerdetector.setMinMaxSize(0.01)
# load video
cap = cv2.VideoCapture('example.mp4')
ret, frame = cap.read()
if not ret:
print("can't open video!")
sys.exit(-1)
def findRotationAndTranslation():
# load board and camera parameters
#boardconfig = aruco.BoardConfiguration("chessboardinfo_small_meters.yml")
'''
Create the image to project from projector with two markers on it (id 5 and 6).
'''
dictionary = aruco.Dictionary_loadPredefined(aruco.Dictionary.ARUCO)
markerToProject = np.full([800, 1280], 0, dtype=np.uint8)
#print(markerToProject.shape) (800, 1280)
for i in [400, 700]:
mi = cv2.resize(dictionary.getMarkerImage_id(5 if i == 400 else 6, 3),
None,
fx=5,
fy=5,
interpolation=cv2.INTER_NEAREST)
white_image = np.full([mi.shape[0] + 40, mi.shape[1] + 40],
255,
dtype=np.uint8)
markerToProject[380:20 + 400 + mi.shape[0],
i - 20:i + 20 + mi.shape[1]] = white_image
#markerToProject[400:400+mi.shape[0],i:i+mi.shape[1]] = np.invert(mi, dtype=np.uint8)
markerToProject[400:400 + mi.shape[0], i:i + mi.shape[1]] = mi
markerToProject[markerToProject == 255] = 160
print("marker size ", mi.shape[0], mi.shape[1])
cv2.namedWindow("marker projection", cv2.WND_PROP_FULLSCREEN)
cv2.setWindowProperty("marker projection", cv2.WND_PROP_FULLSCREEN,
cv2.cv.CV_WINDOW_FULLSCREEN)
cv2.imshow("marker projection", markerToProject)
with open("camera_calibration.yaml", 'r') as stream:
try:
data = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
exit()
camparam = aruco.CameraParameters()
camparam.setParams(
np.array(data["camera_matrix"]),
np.array(data["distortion_coefficients"]),
#np.array([ data["image_width"], data["image_height"] ]))
np.array([1280, 720]))
print("camera matrix", camparam.CameraMatrix)
# create detector and set parameters
detector = aruco.MarkerDetector()
params = detector.getParams()
#open first camera
'''
Tried to set the width and height as 1280x720 but it didn't work so using a
recorded video rather than using the camera directly.
'''
os.system("v4l2-ctl --set-fmt-video=width=1280,height=720,pixelformat=1")
# v = cv2.VideoCapture(0)
v = cv2.VideoCapture("marker.avi")
'''
if not v.isOpened():
print "unable to open video stream"
exit()
# This will help in recording the frames to a video.
ret = v.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 1280)
ret = v.set(cv2.cv.CV_CAP_PROP_FRAME_WIDTH, 960)
print("return value", ret)
w = v.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH);
h = v.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT);
print("width and height", w, h)
fourcc = cv2.cv.CV_FOURCC(*'XVID')
out = cv2.VideoWriter('output.avi',fourcc, 25.0, (int(w),int(h)))
'''
projpoints1 = np.array([[400], [400]], dtype=np.float32)
projpoints2 = np.array([[400 + 105], [400]], dtype=np.float32)
projpoints3 = np.array([[400], [400 + 105]], dtype=np.float32)
projpoints4 = np.array([[400 + 105], [400 + 105]], dtype=np.float32)
imageSize = None
count = 0
while True:
ret, img = v.read()
imageSize = img.shape[::-1]
print("Image size ", imageSize)
orig = np.copy(img)
markers = detector.detect(img)
for marker in markers:
marker.draw(img, np.array([255, 255, 255]), 2)
# for all markers except 5 and 6
img = drawCube(markers, camparam, img)
img = drawCubeForProjectedMarkers(markers, camparam, img)
#out.write(img)
# show frame
cv2.imshow("frame", cv2.resize(img, None, fx=0.5, fy=0.5))
key = cv2.waitKey(30)
if key == 1048689 or key == 1048603: # ESC or 'q'
break
if key == 1048608: #space
cv2.imwrite("out.png", img)
#out.release()
'''
For stereo calibration, I have assumed the distortion and intrinsic matrix
so that it would help in the process.
'''
distortionPr = np.array([[0.0, 0.0, 0.0, 0.0, 0.0]], dtype=np.float32)
cameraMatrixPr = np.matrix(
'1000.0 0.0 1280; 0.0 1000.0 200.0; 0.0 0.0 1.0')
cameraMatrixPr = np.array(cameraMatrixPr, dtype=np.float32)
imageSize = (1280, 720)
'''
# Look at our old code "projector_camera_calibration_aruco_old_code.py" in the same
# directory to find out how to fill these structures (ImagePointsP, ImagePointsC, ObjectPoints)
# required for stereo calibration.
print("Image size ************************************ ")
print(imageSize)
print("Object Points ********************************* ", len(ObjectPoints))
print(ObjectPoints)
print("imageC points ********************************* ", len(ImagePointsC))
print(ImagePointsC)
print("imageP points ********************************* ", len(ImagePointsP))
print(ImagePointsP)
retval = 0
cameraMatrix1 = []
cameraMatrix2 = []
distCoeffs1 = []
distCoeffs2 = []
R = []
T = []
E = []
F = []
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(ObjectPoints, ImagePointsP, ImagePointsC, imageSize, cameraMatrix, distortion, cameraMatrixPr, distortionPr, None, None, None, None, (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT,30,1e-6), cv2.CALIB_USE_INTRINSIC_GUESS)
print("----------ret----------", ret)
print("########## Camera matrix 1")
print(cameraMatrix1)
print("########## Camera matrix 2")
print(cameraMatrix2)
print("########## Dist coeff 1")
print(distCoeffs1)
print("########## Dist coeff 2")
print(distCoeffs2)
print("########## Rotation matrix")
print(np.matrix(R))
print("########## Translation vector")
print(np.matrix(T))
print("##########")
data = {"camera_matrix1": cameraMatrix1.tolist(), "camera_matrix2": cameraMatrix2.tolist(),
"distortion_coefficients1": distCoeffs1.tolist(),"distortion_coefficients2": distCoeffs2.tolist(),
"rotation" : R.tolist(), "translation":T.tolist()}
with open("stereo_calibration.yaml", "w") as f:
yaml.dump(data, f)
'''
R = []
T = []
return R, T
def __init__(self, camera_file, z_up=False):
self.camparams = aruco.CameraParameters()
self.camparams.readFromXMLFile(camera_file)
self.aruco_detector = aruco.MarkerDetector()
self.z_up = z_up
def findRotationAndTranslation():
# load board and camera parameters
#boardconfig = aruco.BoardConfiguration("chessboardinfo_small_meters.yml")
dictionary = aruco.Dictionary_loadPredefined(aruco.Dictionary.ARUCO)
markerToProject = np.full([800, 1280], 0, dtype=np.uint8)
#print(markerToProject.shape) (800, 1280)
for i in [400, 700]:
mi = cv2.resize(dictionary.getMarkerImage_id(5 if i == 400 else 6, 3),
None,
fx=5,
fy=5,
interpolation=cv2.INTER_NEAREST)
white_image = np.full([mi.shape[0] + 40, mi.shape[1] + 40],
255,
dtype=np.uint8)
markerToProject[380:20 + 400 + mi.shape[0],
i - 20:i + 20 + mi.shape[1]] = white_image
#markerToProject[400:400+mi.shape[0],i:i+mi.shape[1]] = np.invert(mi, dtype=np.uint8)
markerToProject[400:400 + mi.shape[0], i:i + mi.shape[1]] = mi
markerToProject[markerToProject == 255] = 160
print("marker size ", mi.shape[0], mi.shape[1])
cv2.namedWindow("marker projection", cv2.WND_PROP_FULLSCREEN)
cv2.setWindowProperty("marker projection", cv2.WND_PROP_FULLSCREEN,
cv2.cv.CV_WINDOW_FULLSCREEN)
cv2.imshow("marker projection", markerToProject)
with open("camera_calibration.yaml", 'r') as stream:
try:
data = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
exit()
camparam = aruco.CameraParameters()
camparam.setParams(
np.array(data["camera_matrix"]),
np.array(data["distortion_coefficients"]),
#np.array([ data["image_width"], data["image_height"] ]))
np.array([1280, 720]))
# create detector and set parameters
detector = aruco.MarkerDetector()
params = detector.getParams()
#open first camera
#v = cv2.VideoCapture("stereo.avi")
v = cv2.VideoCapture(0)
if not v.isOpened():
print "unable to open video stream"
exit()
'''
w = v.get(cv2.cv.CV_CAP_PROP_FRAME_WIDTH);
h = v.get(cv2.cv.CV_CAP_PROP_FRAME_HEIGHT);
fourcc = cv2.cv.CV_FOURCC(*'XVID')
out = cv2.VideoWriter('output.avi',fourcc, 25.0, (int(w),int(h)))
'''
ImagePointsP = []
ImagePointsC = []
ObjectPoints = []
projpoints1 = np.array([[400], [400]], dtype=np.float32)
projpoints2 = np.array([[400 + 105], [400]], dtype=np.float32)
projpoints3 = np.array([[400], [400 + 105]], dtype=np.float32)
projpoints4 = np.array([[400 + 105], [400 + 105]], dtype=np.float32)
imageSize = None
count = 0
while True:
ret, img = v.read()
imageSize = img.shape[::-1]
orig = np.copy(img)
markers = detector.detect(img)
for marker in markers:
# print marker ID and point positions
#print("Marker: {:d}".format(marker.id))
#for i, point in enumerate(marker):
# print("\t{:d} {}".format(i, str(point)))
marker.draw(img, np.array([255, 255, 255]), 2)
# calculate marker extrinsics for marker size of 30mm
marker.calculateExtrinsics(0.030, camparam)
#print("Marker extrinsics:{:d} iteration {:d} \n{:s}\n{:s}".format(marker.id, count, marker.Tvec, marker.Rvec))
#print("detected ids: {}".format(", ".join(str(m.id) for m in markers)))
# Try all the corners of marker5
marker1 = None
marker5 = None
for marker in markers:
if marker.id == 1:
marker1 = marker
if marker.id == 5: #marker id is changed.. for sanity check. revert it to 5 later
marker5 = marker
marker5corners = []
objPoints = []
if marker1 != None and marker5 != None:
for i, point in enumerate(marker5):
marker5corners.append(point)
print(marker5corners)
count = count + 1
for i in range(len(marker5corners)):
#pixelValue = marker5.getCenter()
pixelValue = marker5corners[i]
cameraMatrix = camparam.CameraMatrix
distortion = camparam.Distorsion
test = np.zeros((1, 1, 2), dtype=np.float32)
test[0][0][0] = pixelValue[0]
test[0][0][1] = pixelValue[1]
print("***************** start *****************")
print("before correction Pixel values ")
print(test)
pixelCoordinates = cv2.undistortPoints(test, cameraMatrix,
distortion)
print("corrected Pixel values ")
print(pixelCoordinates)
pixelCoordinates = np.array(
[pixelCoordinates[0][0][0], pixelCoordinates[0][0][1], 1])
inversePixelValues = calculatePixelValues(
pixelCoordinates, cameraMatrix)
print("Inverse pixel values... calculate")
print(inversePixelValues)
marker1.calculateExtrinsics(0.030, camparam)
rvec = marker1.Rvec
tvec = marker1.Tvec
print("marker translation vector")
print(tvec)
print("rvec from marker")
print(rvec)
rotationMatrix, jacb = cv2.Rodrigues(rvec)
print("rotation matrix")
print(rotationMatrix)
print("row vector since we are using a transpose.")
print(rotationMatrix[2])
normalVector = rotationMatrix[2]
#####################################################
unityMatrix = np.identity(3)
#rvecForRepr = np.array([[0.0], [0.0], [0.0]], dtype=np.float32)
rvecForRepr, jacb = cv2.Rodrigues(unityMatrix)
print("rvecForRepr", rvecForRepr)
tvecForRepr = np.array([[0.0], [0.0], [0.0]], dtype=np.float32)
#####################################################
newTvec = np.matrix(rotationMatrix) * np.matrix(tvec)
print("newTvec", newTvec)
newTvec = -1 * newTvec
newTvec = np.array(newTvec)
print("newTvec", newTvec)
#####################################################
projectedImagePoints, jacb = cv2.projectPoints(
np.array([newTvec]), rvecForRepr, tvecForRepr,
cameraMatrix, distortion)
print("projected newTvec back on the camera plane")
print(projectedImagePoints)
print("projected newTvec -> marker1 getcenter")
print(marker1.getCenter())
computed3Dpoints = computeRayPlaneIntersection(
newTvec, normalVector, pixelCoordinates)
print("computed 3d points")
print(computed3Dpoints)
projectedImagePoints, jacb = cv2.projectPoints(
np.array([computed3Dpoints]), rvecForRepr, tvecForRepr,
cameraMatrix, distortion)
print("projected 3d points on image plane c")
print(projectedImagePoints)
''' (not using it anymore)
print("marker translation vector ", tvec, type(tvec))
rotationMatrix, jacb = cv2.Rodrigues(rvec)
print("rotation matrix")
print(rotationMatrix)
print("normal vector c")
print(rotationMatrix[:, 2])
normalVector1 = rotationMatrix[:,2]
print("normal vector r")
print(rotationMatrix[2])
normalVector2 = rotationMatrix[2]
#####################################################
rvecForRepr = np.array([[0.0], [0.0], [0.0]], dtype=np.float32)
tvecForRepr = np.array([[0.0], [0.0], [0.0]], dtype=np.float32)
#####################################################
computed3Dpoints = computeRayPlaneIntersection(tvec, normalVector1, pixelCoordinates)
print("computed 3d points c")
print(computed3Dpoints)
projectedImagePoints, jacb = cv2.projectPoints(np.array([computed3Dpoints]), rvecForRepr, tvecForRepr, cameraMatrix, distortion)
print("projected 3d points on image plane c")
print(projectedImagePoints)
computed3Dpoints = computeRayPlaneIntersection(tvec, normalVector2, pixelCoordinates)
print("computed 3d points r")
print(computed3Dpoints)
projectedImagePoints, jacb = cv2.projectPoints(np.array([computed3Dpoints]), rvecForRepr, tvecForRepr, cameraMatrix, distortion)
print("projected 3d points on image plane r")
print(projectedImagePoints)
print("***************** end *****************")
'''
objPoints.append(computed3Dpoints)
ImagePointsP.append(
np.array([projpoints1, projpoints2, projpoints3, projpoints4]))
ImagePointsC.append(np.array(marker5corners))
ObjectPoints.append(np.array(objPoints))
if count == 50:
break
#out.write(img)
# show frame
cv2.imshow("frame", cv2.resize(img, None, fx=0.5, fy=0.5))
key = cv2.waitKey(30)
if key == 1048689 or key == 1048603: # ESC or 'q'
break
if key == 1048608: #space
cv2.imwrite("out.png", img)
#out.release()
distortionPr = np.array([[0.0, 0.0, 0.0, 0.0, 0.0]], dtype=np.float32)
cameraMatrixPr = np.matrix(
'1000.0 0.0 1280; 0.0 1000.0 100.0; 0.0 0.0 1.0')
cameraMatrixPr = np.array(cameraMatrixPr, dtype=np.float32)
imageSize = (1280, 720)
print("Image size ************************************ ")
print(imageSize)
print("Object Points ********************************* ",
len(ObjectPoints))
print(ObjectPoints)
print("imageC points ********************************* ",
len(ImagePointsC))
print(ImagePointsC)
print("imageP points ********************************* ",
len(ImagePointsP))
print(ImagePointsP)
retval = 0
cameraMatrix1 = []
cameraMatrix2 = []
distCoeffs1 = []
distCoeffs2 = []
R = []
T = []
E = []
F = []
retval, cameraMatrix1, distCoeffs1, cameraMatrix2, distCoeffs2, R, T, E, F = cv2.stereoCalibrate(
ObjectPoints, ImagePointsP, ImagePointsC, imageSize, cameraMatrix,
distortion, cameraMatrixPr, distortionPr, None, None, None, None,
(cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 1e-6),
cv2.CALIB_USE_INTRINSIC_GUESS)
print("----------ret----------", ret)
print("########## Camera matrix 1")
print(cameraMatrix1)
print("########## Camera matrix 2")
print(cameraMatrix2)
print("########## Dist coeff 1")
print(distCoeffs1)
print("########## Dist coeff 2")
print(distCoeffs2)
print("########## Rotation matrix")
print(np.matrix(R))
print("########## Translation vector")
print(np.matrix(T))
print("##########")
data = {
"camera_matrix1": cameraMatrix1.tolist(),
"camera_matrix2": cameraMatrix2.tolist(),
"distortion_coefficients1": distCoeffs1.tolist(),
"distortion_coefficients2": distCoeffs2.tolist(),
"rotation": R.tolist(),
"translation": T.tolist()
}
with open("stereo_calibration.yaml", "w") as f:
yaml.dump(data, f)
return R, T
def __init__(self):
self.detector = aruco.MarkerDetector()
print("inited ArUco Finder")
