def draw(self):
# Get the ROI.
roi = self.image[self.roiPt0[1]:self.roiPt1[1],
self.roiPt0[0]:self.roiPt1[0]]
# Draw detected markers.
aruco.drawDetectedMarkers(roi, self.corners)
# Put the ROI back.
self.image[self.roiPt0[1]:self.roiPt1[1],
self.roiPt0[0]:self.roiPt1[0]] = roi
#Draw the detected Charuco corners.
aruco.drawDetectedCornersCharuco(self.image,
self.charucoCorners,
cornerColor=self.COLOR_BLUE)
#Draw the detected calibration points.
for idx, corners in enumerate(self.calibrationCorners):
#print idx
#aruco.drawDetectedCornersCharuco(self.image, self.calibrationCorners[idx], self.calibrationIds[idx], self.COLOR_LBLUE)
aruco.drawDetectedCornersCharuco(self.image,
self.calibrationCorners[idx],
cornerColor=self.COLOR_LBLUE)
return
#Draw region of interest
cv2.rectangle(self.image, self.roiPt0, self.roiPt1, self.COLOR_PURPLE,
2)
def detect_board():
vs = VideoStream(usePiCamera=True, resolution=res).start()
fps = FPS().start()
time.sleep(1)
freq = 0
i=0
target_x = []
target_y = []
while(i<5000):
tf = time.time()
# Capture frame-by-frame
frame = vs.read()
# Our operations on the frame come here
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parameters = aruco.DetectorParameters_create()
#print(parameters))
status = "No Marker Detected"
position_dict = {}
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict, parameters=parameters)
if ids is not None:
ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(corners, ids, gray, board )
# if there are enough corners to get a reasonable result
if( ret > 5 ):
aruco.drawDetectedCornersCharuco(frame,ch_corners,ch_ids,(0,0,255) )
retval, rvec, tvec = aruco.estimatePoseCharucoBoard( ch_corners, ch_ids, board, camera_matrix, dist_coeff )
# if a pose could be estimated
if( retval ) :
frame = aruco.drawAxis(frame,camera_matrix,dist_coeff,rvec,tvec,0.1)
rmat = cv2.Rodrigues(rvec)[0]
position_camera = -np.matrix(rmat).T * np.matrix(tvec)
position_dict[ids[0][0]] = position_camera
status = ("x: %.2f, y: %.2f, z: %.2f")%(position_camera[0],position_camera[1],position_camera[2])
# Display the resulting frame
cv2.putText(frame, status, (20,30), cv2.FONT_HERSHEY_SIMPLEX, .5, (0,0,255),2)
out.write(frame)
#cv2.imshow('frame',gray)
#if cv2.waitKey(1) & 0xFF == ord('q'):
# break
i += 1
freq = .7*freq + .3 * (1/(time.time()-tf))
fps.update()
#print("Operating frequency: %.2f Hz"%freq)
# When everything done, release the capture
out.release()
vs.stop()
cv2.destroyAllWindows()
def calib_charucoboard(nrow,
ncolumn,
aruco_markerdict=aruco.DICT_6X6_250,
square_markersize=25,
imgs_path='./',
img_format='png',
save_name='mycam_charuco_data.yaml'):
"""
:param nrow:
:param ncolumn:
:param marker_dict:
:param imgs_path:
:param save_name:
:return:
author: weiwei
date: 20190420
"""
# read images and detect cornders
aruco_dict = aruco.Dictionary_get(aruco_markerdict)
allCorners = []
allIds = []
# SUB PIXEL CORNER DETECTION CRITERION
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 27, 0.0001)
board = aruco.CharucoBoard_create(ncolumn, nrow, square_markersize, .57 * square_markersize, aruco_dict)
print(imgs_path)
images = glob.glob(imgs_path + '*.' + img_format)
candfiles=[]
for fname in images:
img = cv2.imread(fname)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, aruco_dict)
if len(corners) > 0:
# SUB PIXEL DETECTION
for corner in corners:
cv2.cornerSubPix(gray, corner, winSize=(2, 2), zeroZone=(-1, -1), criteria=criteria)
res2 = cv2.aruco.interpolateCornersCharuco(corners, ids, gray, board)
# require len(res2[1]) > nrow*ncolumn/2 at least half of the corners are detected
if res2[1] is not None and res2[2] is not None and len(res2[1]) > (nrow-1)*(ncolumn-1)/2:
allCorners.append(res2[1])
allIds.append(res2[2])
candfiles.append((fname.split("/")[-1]).split("\\")[-1])
imaxis = aruco.drawDetectedMarkers(img, corners, ids)
imaxis = aruco.drawDetectedCornersCharuco(imaxis, res2[1], res2[2], (255,255,0))
cv2.imshow('img', imaxis)
cv2.waitKey(100)
# The calibratedCameraCharucoExtended function additionally estimate calibration errors
# Thus, stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors are returned
# We dont use them here though
# see https://docs.opencv.org/3.4.6/d9/d6a/group__aruco.html for details
(ret, mtx, dist, rvecs, tvecs,
stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors) = \
cv2.aruco.calibrateCameraCharucoExtended(charucoCorners=allCorners, charucoIds=allIds, board=board,
imageSize=gray.shape, cameraMatrix=None, distCoeffs=None,
flags=cv2.CALIB_RATIONAL_MODEL,
criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9))
print(ret, mtx, dist, rvecs, tvecs, candfiles)
if ret:
with open(save_name, "w") as f:
yaml.dump([mtx, dist, rvecs, tvecs, candfiles], f)
def calibrate_charucoboard(self, image, display_image):
# ChAruco board variables
CHARUCOBOARD_ROWCOUNT = 5
CHARUCOBOARD_COLCOUNT = 5
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_50)
SQUARE_LENGTH = (1 + 5.0 / 8) * 0.0254
MARKER_LENGTH = (1 + 9.0 / 32) * 0.0254
# Create constants to be passed into OpenCV and Aruco methods
print(CHARUCOBOARD_COLCOUNT > 1, CHARUCOBOARD_ROWCOUNT > 1,
SQUARE_LENGTH > MARKER_LENGTH)
CHARUCO_BOARD = aruco.CharucoBoard_create(
squaresX=CHARUCOBOARD_COLCOUNT,
squaresY=CHARUCOBOARD_ROWCOUNT,
squareLength=SQUARE_LENGTH,
markerLength=MARKER_LENGTH,
dictionary=ARUCO_DICT)
# Create the arrays and variables we'll use to store info like corners and IDs from images processed
corners_all = [] # Corners discovered in all images processed
ids_all = [] # Aruco ids corresponding to corners discovered
image_size = None # Determined at runtime
# Grayscale the image
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
# Find aruco markers in the query image
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=ARUCO_DICT)
# Outline the aruco markers found in our query image
display_image = aruco.drawDetectedMarkers(image=display_image,
corners=corners)
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=CHARUCO_BOARD)
# If a Charuco board was found, let's collect image/corner points
# Requiring at least 20 squares
if response > 5:
# Add these corners and ids to our calibration arrays
corners_all.append(charuco_corners)
ids_all.append(charuco_ids)
# Draw the Charuco board we've detected to show our calibrator the board was properly detected
display_image = aruco.drawDetectedCornersCharuco(
image=display_image,
charucoCorners=charuco_corners,
charucoIds=charuco_ids)
return [], corners_all, display_image
else:
print("Not able to detect a charuco board in image")
return [], [], display_image
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
(rows, cols, channels) = cv_image.shape
if cols > 60 and rows > 60:
corners, ids, rejectedImgPoints = aruco.detectMarkers(
cv_image, self.aruco_dict, parameters=self.parameters)
#print("aruco")
#print(corners)
if len(corners) > 0:
retval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, cv_image, self.board)
#print(diamondCorners)
img = aruco.drawDetectedCornersCharuco(cv_image,
charucoCorners)
if self.camera_info_read and (not charucoCorners is None):
retval, rvec, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners, charucoIds, self.board,
self.camera_matrix, self.dist_coeffs)
#print(retval)
if not rvec is None:
img = aruco.drawAxis(img, self.camera_matrix,
self.dist_coeffs, rvec, tvec, 0.1)
marker_msg = MarkerArray()
marker_msg.header = data.header
marker_msg.markers = []
#print(corners)
if not ids is None:
if len(ids) != 0:
for i in range(len(ids)):
marker_msg.markers.append(Marker())
marker_msg.markers[i].id = int(ids[i])
marker_msg.markers[i].corners = []
for j in range((corners[i]).shape[1]):
marker_msg.markers[i].corners.append(Point2D())
marker_msg.markers[i].corners[j].x = corners[i][0,
j,
0]
marker_msg.markers[i].corners[j].y = corners[i][0,
j,
1]
#cv2.imshow("Image window", cv_image)
#cv2.waitKey(3)
try:
self.image_pub.publish(self.bridge.cv2_to_imgmsg(cv_image, "bgr8"))
self.marker_pub.publish(marker_msg)
except CvBridgeError as e:
print(e)
def findPointsAndSaveImages(img, img_save_prefex, img_nr):
imgName = img_save_prefex+str(img_nr)
cv2.imwrite(save_path+imgName+".png", img)
corners, ids, rejected = aruco.detectMarkers(img, BoardInfo.charucoBoard.dictionary)
cimg = aruco.drawDetectedMarkers(img.copy(), corners, ids)
cv2.imwrite(save_path+"detected__"+imgName+".png", cimg)
if len(ids) < 10:
return cimg, None, None
numCorners, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(corners, ids, img, BoardInfo.charucoBoard)
cimg = aruco.drawDetectedCornersCharuco(img.copy(), charucoCorners, charucoIds)
cv2.imwrite(save_path+"corners__"+imgName+".png", cimg)
if numCorners <= 0:
return cimg, None, None
return cimg, charucoCorners, charucoIds
def get_corners(self, cv2_img, cam_id):
gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=aruco_dict)
if ids is not None and len(ids) > 5:
_, Chcorners, Chids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
self.current_markers[cam_id] = len(Chcorners)
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=aruco_dict)
_, Chcorners, Chids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
QueryImg = aruco.drawDetectedCornersCharuco(
gray, Chcorners, Chids, (0, 0, 255))
# Display our image
#cv2.imshow('QueryImage', QueryImg)
print("{} markers detected".format(len(ids)))
a = raw_input()
def est_board_pose(self, cv2_img):
gray = cv2.cvtColor(cv2_img, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=aruco_dict)
if ids is not None and len(ids) > 5:
_, Chcorners, Chids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
retval, rvecs, tvecs = aruco.estimatePoseCharucoBoard(
Chcorners, Chids, charuco_board, self.camera_matrix[:, :],
self.distCoeffs[:, :])
#######################
X_C_M = np.eye(4)
X_C_M[0, 3] = tvecs[0]
X_C_M[1, 3] = tvecs[1]
X_C_M[2, 3] = tvecs[2]
R = cv2.Rodrigues(rvecs[:])[0]
X_C_M[0:3, 0:3] = R
X_C_E = X_C_M.dot(self.X_M_E)
tvecs_new = X_C_E[:3, 3]
rvecs_new = cv2.Rodrigues(X_C_E[0:3, 0:3])[0]
# #####################
QueryImg = aruco.drawDetectedCornersCharuco(
cv2_img, Chcorners, Chids, (0, 0, 255))
if (retval):
QueryImg = aruco.drawAxis(cv2_img, self.camera_matrix[:, :],
self.distCoeffs[:, :], rvecs, tvecs,
0.032)
QueryImg = aruco.drawAxis(QueryImg, self.camera_matrix[:, :],
self.distCoeffs[:, :], rvecs_new,
tvecs_new, 0.032)
# Display our image
# cv2.imshow('QueryImage', QueryImg)
# cv2.waitKey(0)
else:
rvecs = np.zeros((3, 1))
tvecs = np.zeros((3, 1))
cv2.imshow('QueryImage', cv2_img)
print("board not detected")
cv2.waitKey(1)
#print("{} markers detected".format(len(ids)))
self.tvecs = tvecs
self.rvecs = rvecs
def manually_verify_board_detection(self, image, corners, ids=None):
height, width = image.shape[:2]
image = aruco.drawDetectedCornersCharuco(image, corners, ids)
cv2.putText(image, '(a) Accept (d) Reject',
(int(width / 1.35), int(height / 16)),
cv2.FONT_HERSHEY_SIMPLEX, 0.5, 255, 1, cv2.LINE_AA)
cv2.imshow('verify_detection', image)
while 1:
key = cv2.waitKey(0) & 0xFF
if key == ord('a'):
cv2.putText(image, 'Accepted!',
(int(width / 2.5), int(height / 1.05)),
cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2, cv2.LINE_AA)
cv2.imshow('verify_detection', image)
cv2.waitKey(100)
return True
elif key == ord('d'):
cv2.putText(image, 'Rejected!',
(int(width / 2.5), int(height / 1.05)),
cv2.FONT_HERSHEY_SIMPLEX, 1, 255, 2, cv2.LINE_AA)
cv2.imshow('verify_detection', image)
cv2.waitKey(100)
return False
def generateCalibrationImg(self, rows=7, columns=5, lengthSquare=0.0354, lengthMarker=0.0177):
# Create charuco board with actual measured dimensions from print out
board = aruco.CharucoBoard_create(
squaresX=columns,
squaresY=rows,
squareLength=lengthSquare,
markerLength=lengthMarker,
dictionary=self.arucoDict)
# Get image paths from calibration folder
paths = glob.glob(self.calibrationDir + '*' + self.imgExtension)
# Image list
data = []
# Counter
ii = 0
# Loop through all images
for filePath in paths:
# Read image and convert to gray
img = cv2.imread(filePath)
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find aruco markers in the query image
corners, ids, _ = aruco.detectMarkers(image=gray, dictionary=self.arucoDict)
# Outline the aruco markers found in the query image
aruco.drawDetectedMarkers(image=img, corners=corners)
# Get charuco corners and ids from detected aruco markers
response, charucoCorners, charucoIDs = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=board)
# If at least 20 corners were found
if response > 20:
# Draw the Charuco board detected to show calibration results
aruco.drawDetectedCornersCharuco(image=img, charucoCorners=charucoCorners, charucoIds=charucoIDs)
# Resize image and save to list
data.append(img)
# Incrememnt counter
ii += 1
else:
# Error message
print('Error in: ' + str(filePath))
# Exit condition
if (ii >= 9):
break
# Create matrix image
A = np.concatenate((data[0], data[1], data[2], data[3]), axis=1)
B = np.concatenate((data[4], data[5], data[6], data[7]), axis=1)
D = np.concatenate((A, B), axis=0)
# Display and save matrix image
cv2.imshow('Photo Matrix', D)
cv2.imwrite('PhotoMatrix.png', D)
cv2.waitKey(0)
cv2.destroyAllWindows()
def startcamerastreaming(self, c, ReturnFlag, n_sq_y, n_sq_x, squareLen,
markerLen, aruco_dict):
count = 0
# start realsense camera streaming
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
profile = pipeline.start(config)
# get Realsense stream intrinsics
intr = profile.get_stream(
rs.stream.color).as_video_stream_profile().get_intrinsics()
mtx = np.array([[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy],
[0, 0, 1]])
dist = np.array(intr.coeffs)
#create charuco board with specified number of squares given marker length and square length and dictionary type.
charuco_board = aruco.CharucoBoard_create(n_sq_x, n_sq_y, squareLen,
markerLen, aruco_dict)
rvec = np.zeros((1, 3)) # , np.float32)
tvec = np.zeros((1, 3)) # , np.float32)
calibDone = False
MLRSSum = np.zeros((4, 4))
while (True):
# start streaming from Realsense
if calibDone != True:
print("came here")
# receive the marker 1 pose in AR headset space.
dataRecv = c.recv(64)
MLArr = np.frombuffer(dataRecv, dtype=np.float32)
print(MLArr)
frames = pipeline.wait_for_frames()
# if the marker 1 pose is not None, proceed to get marker 1 pose in Realsense space
if MLArr[3] != 0:
color_frame = frames.get_color_frame()
input_img = np.asanyarray(color_frame.get_data())
# cv2.imshow('input',input_img)
# operations on the frame - conversion to grayscale
gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY)
# detector parameters can be set here (List of detection parameters[3])
parameters = aruco.DetectorParameters_create()
parameters.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
# font for displaying text (below)
font = cv2.FONT_HERSHEY_SIMPLEX
# check if the ids list is not empty
# if no check is added the code will crash
if (ids is not None):
ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
# if there are enough corners to get a reasonable result
if (ret > 3):
aruco.drawDetectedCornersCharuco(
input_img, ch_corners, ch_ids, (0, 0, 255))
retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(
ch_corners, ch_ids, charuco_board, mtx, dist,
None, None)
# if the board pose could be estimated, continue with remaining steps
if (retval):
print("board pose", rvec, tvec)
rvec = np.reshape(rvec, (1, 3))
tvec = np.reshape(tvec, (1, 3))
print("board pose", rvec, tvec)
# draw charuco board axis
aruco.drawAxis(input_img, mtx, dist, rvec,
tvec, 0.1)
print(MLArr, rvec, tvec)
# compute the calibration matrix between realsense space and AR headset space for the 'i'th sample of marker 1 pose in realsense and AR headset space.
MLRSi = self.computecalibmatrix(
MLArr, rvec, tvec, n_sq_y, n_sq_x,
squareLen)
print("MLRSi", MLRSi)
# convert MLRSi to euler
#R_M1_RS_reshape = np.array(MLRSi[:3,:3]).reshape([3,3])
R_M1_RS = R.from_rotvec(rvec)
R_M1_RS_euler = R_M1_RS.as_euler('xyz',
degrees=True)
# save the MLRSi values in csv
with open(file_name_TM1RS, 'a+',
newline='') as write_obj:
csv_writer = writer(write_obj)
csv_writer.writerow(
[R_M1_RS_euler,
tvec.reshape([1, 3])])
MLRSSum = MLRSSum + MLRSi
count = count + 1
cv2.imshow('current frame', input_img)
#if the specified number of samples for homography are taken, compute the transformation matrix
if count >= n_samples:
print("count", count)
MLRSFinal = MLRSSum / count
#once n_samples homography matrices are computed, their average is computed and returned as the final transformation matrix from Realsense to AR headset space.
print("MLRSFinal", MLRSFinal)
print("calib done")
calibDone = True
if ReturnFlag == 1:
return MLRSFinal
if cv2.waitKey(1) & 0xFF == ord('q'):
break
# When everything done, release the capture
#cap.release()
cv2.destroyAllWindows()
def cal():
# ChAruco board variables
CHARUCOBOARD_ROWCOUNT = 9
CHARUCOBOARD_COLCOUNT = 6
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_250)
# Create constants to be passed into OpenCV and Aruco methods
CHARUCO_BOARD = aruco.CharucoBoard_create(
squaresX=CHARUCOBOARD_COLCOUNT,
squaresY=CHARUCOBOARD_ROWCOUNT,
squareLength=0.028,
markerLength=0.014,
dictionary=ARUCO_DICT)
# Create the arrays and variables we'll use to store info like corners and IDs from images processed
corners_all = [] # Corners discovered in all images processed
ids_all = [] # Aruco ids corresponding to corners discovered
image_size = None # Determined at runtime
# This requires a set of images or a video taken with the camera you want to calibrate
# I'm using a set of images taken with the camera with the naming convention:
# 'camera-pic-of-charucoboard-<NUMBER>.jpg'
# All images used should be the same size, which if taken with the same camera shouldn't be a problem
images = glob.glob(f'{FOLDER}/*.{IM_TYPE}')
# Loop through images glob'ed
for iname in images:
# Open the image
img = cv2.imread(iname)
img = cv2.flip(img, 1)
# Grayscale the image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find aruco markers in the query image
corners, ids, _ = aruco.detectMarkers(
image=gray,
dictionary=ARUCO_DICT)
# Outline the aruco markers found in our query image
img = aruco.drawDetectedMarkers(
image=img,
corners=corners)
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=CHARUCO_BOARD)
# If a Charuco board was found, let's collect image/corner points
# Requiring at least 20 squares
if response > 20:
# Add these corners and ids to our calibration arrays
corners_all.append(charuco_corners)
ids_all.append(charuco_ids)
# Draw the Charuco board we've detected to show our calibrator the board was properly detected
img = aruco.drawDetectedCornersCharuco(
image=img,
charucoCorners=charuco_corners,
charucoIds=charuco_ids)
# If our image size is unknown, set it now
if not image_size:
image_size = gray.shape[::-1]
# Reproportion the image, maxing width or height at 1000
proportion = max(img.shape) / 1000.0
img = cv2.resize(
img, (int(img.shape[1]/proportion), int(img.shape[0]/proportion)))
# Pause to display each image, waiting for key press
cv2.imshow('Charuco board', img)
cv2.waitKey(0)
else:
print("Not able to detect a charuco board in image: {}".format(iname))
# Destroy any open CV windows
cv2.destroyAllWindows()
# Make sure at least one image was found
if len(images) < 1:
# Calibration failed because there were no images, warn the user
print("Calibration was unsuccessful. No images of charucoboards were found. Add images of charucoboards and use or alter the naming conventions used in this file.")
# Exit for failure
exit()
# Make sure we were able to calibrate on at least one charucoboard by checking
# if we ever determined the image size
if not image_size:
# Calibration failed because we didn't see any charucoboards of the PatternSize used
print("Calibration was unsuccessful. We couldn't detect charucoboards in any of the images supplied. Try changing the patternSize passed into Charucoboard_create(), or try different pictures of charucoboards.")
# Exit for failure
exit()
# Now that we've seen all of our images, perform the camera calibration
# based on the set of points we've discovered
calibration, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco(
charucoCorners=corners_all,
charucoIds=ids_all,
board=CHARUCO_BOARD,
imageSize=image_size,
cameraMatrix=None,
distCoeffs=None)
return cameraMatrix, distCoeffs, rvecs, tvecs
frame, map1, map2, cv2.INTER_LINEAR,
cv2.BORDER_CONSTANT) # for fisheye remapping
frame_remapped_gray = cv2.cvtColor(
frame_remapped,
cv2.COLOR_BGR2GRAY) # aruco.detectMarkers() requires gray image
corners, ids, rejectedImgPoints = aruco.detectMarkers(
frame_remapped_gray, aruco_dict,
parameters=arucoParams) # First, detect markers
aruco.refineDetectedMarkers(frame_remapped_gray, board, corners, ids,
rejectedImgPoints)
if ids != None: # if there is at least one marker detected
charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, frame_remapped_gray, board)
im_with_charuco_board = aruco.drawDetectedCornersCharuco(
frame_remapped, charucoCorners, charucoIds, (0, 255, 0))
retval, rvec, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners, charucoIds, board, camera_matrix,
dist_coeffs) # posture estimation from a charuco board
if retval == True:
im_with_charuco_board = aruco.drawAxis(
im_with_charuco_board, camera_matrix, dist_coeffs, rvec,
tvec, 100
) # axis length 100 can be changed according to your requirement
else:
im_with_charuco_left = frame_remapped
cv2.imshow("charucoboard", im_with_charuco_board)
if cv2.waitKey(2) & 0xFF == ord('q'):
break
def calibrate_camera():
# Generate Charuco board
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
squareSize = 0.03907 #0.03
markerSize = squareSize / 1.5 #0.02
board = aruco.CharucoBoard_create(7, 5, squareSize, markerSize, dictionary)
img = board.draw((int(1000 * 1.414), 1000))
cv2.imwrite('charuco.png', img)
cap = cv2.VideoCapture(2)
# Camera resolution
res = (int(cap.get(cv2.CAP_PROP_FRAME_WIDTH)),
int(cap.get(cv2.CAP_PROP_FRAME_HEIGHT)))
ret, frame = cap.read()
print(frame.shape)
allCharucoCorners = []
allCharucoIds = []
frame_idx = 0
frame_spacing = 20 # Only check every fifth frame
required_count = 60
success = False
while True:
# Capture and image
ret, frame = cap.read()
# Convert to gray scale
grayframe = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
# Detect markers
marker_corners, marker_ids, _ = aruco.detectMarkers(
grayframe, dictionary)
# If detected any markers
if len(marker_corners) > 0 and frame_idx % frame_spacing == 0:
aruco.drawDetectedMarkers(grayframe, marker_corners, marker_ids)
# Detect corners
ret, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
marker_corners, marker_ids, grayframe, board)
#If detected any corners
if charuco_corners is not None and charuco_ids is not None and len(
charuco_corners) > 3:
allCharucoCorners.append(charuco_corners)
allCharucoIds.append(charuco_ids)
# Draw detected corners
aruco.drawDetectedCornersCharuco(grayframe, charuco_corners,
charuco_ids)
cv2.imshow('grayframe', grayframe)
key = cv2.waitKey(1) & 0xFF
if key == ord('q'):
break
frame_idx += 1
print("Found: " + str(len(allCharucoIds)) + "/" + str(required_count))
if len(allCharucoIds) > required_count:
success = True
break
if success:
print("Finished")
try:
err, camera_matrix, dist_coeffs, rvecs, tvecs = aruco.calibrateCameraCharuco(
allCharucoCorners, allCharucoIds, board, res, None, None)
print('Calibrated with error', err)
print(camera_matrix)
save_json({
'camera_matrix': camera_matrix.tolist(),
'dist_coeffs': dist_coeffs.tolist(),
'err': err
})
except:
success = False
print("failed!")
return
# Generate the corrections
new_camera_matrix, valid_pix_roi = cv2.getOptimalNewCameraMatrix(
camera_matrix, dist_coeffs, res, 0)
#new_camera_matrix = camera_matrix
mapx, mapy = cv2.initUndistortRectifyMap(camera_matrix, dist_coeffs,
None, new_camera_matrix, res,
5)
while True:
ret, frame = cap.read()
if mapx is not None and mapy is not None:
frame = cv2.remap(frame, mapx, mapy, cv2.INTER_LINEAR)
cv2.imshow('frame', frame)
if cv2.waitKey(1) & 255 == ord('q'):
break
cap.release()
cv2.destroyAllWindows()
def callback(self, data):
try:
cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
gray = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=ARUCO_DICT)
if ids is not None:
cv_image = aruco.drawDetectedMarkers(image=cv_image,
corners=corners)
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=CHARUCO_BOARD)
# Pause to display each image, waiting for key press
cv2.imshow('Charuco board', cv_image)
key = cv2.waitKey(1)
if key & 0xFF == ord('q') or key == 27:
rospy.loginfo('Total {} capture images for Calibration'.format(
self.capturecount))
cv2.destroyAllWindows()
rospy.signal_shutdown('Program terminate')
elif key == ord('s'):
if response > 20:
# Add these corners and ids to our calibration arrays
self.corners_all.append(charuco_corners)
self.ids_all.append(charuco_ids)
# Draw the Charuco board we've detected to show our calibrator the board was properly detected
cv_image = aruco.drawDetectedCornersCharuco(
image=cv_image,
charucoCorners=charuco_corners,
charucoIds=charuco_ids)
# If our image size is unknown, set it now
if not self.image_size:
self.image_size = gray.shape[::-1]
# Reproportion the image, maxing width or height at 1000
proportion = max(cv_image.shape) / 1000.0
cv_image = cv2.resize(
cv_image, (int(cv_image.shape[1] / proportion),
int(cv_image.shape[0] / proportion)))
self.capturecount += 1
rospy.loginfo('Detected 20 markers and SAVE : {}'.format(
self.capturecount))
else:
cv2.imshow('Charuco board', cv_image)
key = cv2.waitKey(1)
if key & 0xFF == ord('q') or key == 27:
rospy.loginfo('Total {} capture images for Calibration'.format(
self.capturecount))
cv2.destroyAllWindows()
rospy.signal_shutdown('Program terminate')
assert ret, "Cannot connect with camera"
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find aruco markers in the query image
corners, ids, _ = arc.detectMarkers(image=gray, dictionary=dictionary)
# Outline the aruco markers found in our query image
img = arc.drawDetectedMarkers(image=img, corners=corners)
if len(corners) > 10:
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = arc.interpolateCornersCharuco(
markerCorners=corners, markerIds=ids, image=gray, board=board)
# Draw the Charuco board we've detected to show our calibrator the board was properly detected
img = arc.drawDetectedCornersCharuco(image=img,
charucoCorners=charuco_corners,
charucoIds=charuco_ids)
if cv2.waitKey(1) & 0xFF == ord('s'):
# Add these corners and ids to our calibration arrays
cornerpoints.append(charuco_corners)
aruco_ids.append(charuco_ids)
img_list.append(img)
print("Saved image")
image_count += 1
cv2.putText(img, f"Images taken: {image_count}", (10, 50), font, 0.8,
(0, 0, 255), 2, cv2.LINE_AA)
cv2.imshow(
"Frame",
cv2.resize(img, (int(0.6 * height), int(0.6 * width)),
def startTracking(kalman_flag, marker_type, MLRSTr, n_sq_y, n_sq_x, squareLen,
markerLen, aruco_dict, socket_connect, c):
RSTr = None
img_idx = 0
img_sent = 0
reinit_thresh = 10
skip_frame_reinit = 120 #after every 150 frames, reinitialize detection
skip_frame_send = 4
#if Charuco board is the marker type, execute following if statement
if marker_type == 1:
rvec = np.zeros((1, 3))
tvec = np.zeros((1, 3))
objectPoints = np.zeros((1, 3))
#configure Realsense camera stream
pipeline = rs.pipeline()
config = rs.config()
config.enable_stream(rs.stream.color, 640, 480, rs.format.bgr8, 30)
profile = pipeline.start(config)
#get realsense stream intrinsics
intr = profile.get_stream(rs.stream.color).as_video_stream_profile(
).get_intrinsics() # profile.as_video_stream_profile().get_intrinsics()
mtx = np.array([[intr.fx, 0, intr.ppx], [0, intr.fy, intr.ppy], [0, 0, 1]])
dist = np.array(intr.coeffs)
while (True):
#start streaming
frames = pipeline.wait_for_frames()
#get color frame
color_frame = frames.get_color_frame()
input_img = np.asanyarray(color_frame.get_data())
# operations on the frame - convert to grayscale
gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY)
img_idx = img_idx + 1
# detector parameters can be set here.
parameters = aruco.DetectorParameters_create()
parameters.adaptiveThreshConstant = 10
# lists of ids and the corners belonging to each id
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=parameters)
# font for displaying text (below)
font = cv2.FONT_HERSHEY_SIMPLEX
if marker_type == 1:
# check if the ids list is not empty
if (ids is not None):
# create Charudo board
charuco_board = aruco.CharucoBoard_create(
n_sq_x, n_sq_y, squareLen, markerLen, aruco_dict)
ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
# if there are enough corners to get a reasonable result
if (ret > 3):
aruco.drawDetectedCornersCharuco(input_img, ch_corners,
ch_ids, (0, 0, 255))
#estimate Charuco board pose
retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(
ch_corners, ch_ids, charuco_board, mtx, dist, None,
None) # , useExtrinsicGuess=False)
print("pose", retval, rvec, tvec)
# if a pose could be estimated
if (retval):
# draw axis showing pose of board
rvec = np.reshape(rvec, (1, 3))
tvec = np.reshape(tvec, (3, 1))
aruco.drawAxis(input_img, mtx, dist, rvec, tvec, 0.1)
#convert to rotation matrix from rotation vector
R_rvec = R.from_rotvec(rvec)
R_rotmat = R_rvec.as_matrix()
# get transformation matrix combining tvec and rotation matrix
RSTr = np.hstack([R_rotmat.reshape((3, 3)), tvec])
RSTr = np.vstack([RSTr, [0, 0, 0, 1]])
print("RSTr", RSTr)
else:
# show 'No Ids' when no markers are found
cv2.putText(input_img, "No Ids", (0, 64), font, 1, (0, 255, 0),
2, cv2.LINE_AA)
# following part is executed if single Aruco marker is selected
else:
if np.all(ids != None):
# estimate pose of each marker and return the values
rvec, tvec, _ = aruco.estimatePoseSingleMarkers(
corners, markerLen, mtx, dist)
# (rvec-tvec).any() # get rid of that nasty numpy value array error
for i in range(0, ids.size):
# draw axis for the aruco markers
aruco.drawAxis(input_img, mtx, dist, rvec[i], tvec[i], 0.1)
# draw a square around the markers
aruco.drawDetectedMarkers(input_img, corners)
#select only the first marker in the list (assumes single Aruco marker is in the scene)
#convert to rotation matrix from rotation vector
R_rvec = R.from_rotvec(rvec[0])
R_rotmat = R_rvec.as_matrix()
# get transformation matrix combining tvec and rotation matrix
RSTr = np.hstack([R_rotmat[0], tvec[0].transpose()])
RSTr = np.vstack([RSTr, [0, 0, 0, 1]])
tvec = tvec.reshape(1, 3)
print(tvec)
# Send the transformed pose to the AR Headset only if the marker detected is within 0.8 m
if (tvec[0][2] != 0 and tvec[0][2] < 0.8):
##print("img_idx img_sent", img_idx, img_sent)
if kalman_flag == 0 or img_idx - img_sent > reinit_thresh or img_idx % skip_frame_reinit == 0:
#convert to rotation matrix from rotation vector
R_rvec = R.from_rotvec(rvec[0])
R_rotmat = R_rvec.as_matrix()
# get transformation matrix combining tvec and rotation matrix
RSTr = np.hstack([R_rotmat[0], tvec.transpose()])
RSTr = np.vstack([RSTr, [0, 0, 0, 1]])
img_sent = img_idx
else:
# execute the following if kalman filter to be applied
rvec = rvec.reshape((3, 1))
rvec = rvec.reshape((1, 3))
current_measurement = np.array([
np.float32(tvec[0][0]),
np.float32(tvec[0][1]),
np.float32(tvec[0][2]),
np.float32(rvec[0][0]),
np.float32(rvec[0][1]),
np.float32(rvec[0][2])
]).reshape([1, 6])
current_prediction = kalman.predict()
current_prediction = np.array(current_prediction,
np.float32)
current_prediction = current_prediction.transpose()[0]
# predicted rotation
rvec_P = current_prediction[3:6]
# predicted tvec
tvec_P = np.array(current_prediction[:3]).reshape(
[1, 3])
# convert to rotation matrix using r function
R_rvec = R.from_rotvec(rvec_P)
R_RotMat = R_rvec.as_matrix()
tvec = tvec_P
# update the estimate of the kalman filter
RSTr = np.hstack([R_rotmat[0], tvec.transpose()])
RSTr = np.vstack([RSTr, [0, 0, 0, 1]])
kalman.correct(np.transpose(current_measurement))
if socket_connect == 1 and RSTr is not None:
# transform the pose from OpenCV's right-handed rule to Unity's left-handed rule
RSTr_LH = np.array([
RSTr[0][0], RSTr[0][2], RSTr[0][1], RSTr[0][3], RSTr[2][0],
RSTr[2][2], RSTr[2][1], RSTr[2][3], RSTr[1][0], RSTr[1][2],
RSTr[1][1], RSTr[1][3], RSTr[3][0], RSTr[3][1], RSTr[3][2],
RSTr[3][3]
]) # converting to left handed coordinate system
RSTr_LH = RSTr_LH.reshape(4, 4)
# compute transformed pose to send to MagicLeap
PoseTr = np.matmul(MLRSTr, RSTr_LH)
# Head Pose matrix in the form of array to be sent
ArrToSend = np.array([
PoseTr[0][0], PoseTr[0][1], PoseTr[0][2], PoseTr[0][3],
PoseTr[1][0], PoseTr[1][1], PoseTr[1][2], PoseTr[1][3],
PoseTr[2][0], PoseTr[2][1], PoseTr[2][2], PoseTr[2][3],
PoseTr[3][0], PoseTr[3][1], PoseTr[3][2], PoseTr[3][3]
])
# pack the array to be sent before sending
dataTosend = struct.pack('f' * len(ArrToSend), *ArrToSend)
if socket_connect == 1 and img_idx % skip_frame_send == 0: #
img_sent = img_idx
c.send(dataTosend)
# display the resulting frame
cv2.imshow('frame', input_img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
calids = []
for im in imgs:
gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
# corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary, parameters=parameters)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, dictionary)
# print('ids 1:', len(ids))
# if ids were found, then
if len(ids) > 0:
# print(corners, ids, rejectedImgPoints)
ret, chcorners, chids = aruco.interpolateCornersCharuco(corners, ids, gray, board)
calcorners.append(chcorners)
calids.append(chids)
im2 = im.copy()
aruco.drawDetectedCornersCharuco(im2, chcorners, chids)
# aruco.drawDetectedMarkers(im2, rejectedImgPoints, borderColor=(100, 0, 240))
cv2.imshow('markers', im2)
cv2.waitKey()
ret, cameraMatrix, distCoeffs, rvecs, tvecs = aruco.calibrateCameraCharuco(calcorners, calids, board, gray.shape[::-1], None, None)
print('rms', ret)
print(cameraMatrix)
print(distCoeffs)
cam_params = {
'marker_type': 'aruco',
'cameraMatrix': cameraMatrix,
'distCoeffs': distCoeffs,
'image_size': imgs[0].shape[:2],
im_gray = cv2.cvtColor(img_charuco, cv2.COLOR_RGB2GRAY)
h, w = im_gray.shape[:2]
dst = cv2.undistort(im_gray, camera_matrix, distortion_coff, None,
newcameramtx)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
dst, aruco_dict, parameters=arucoParams)
cv2.imshow("original", im_gray)
if not corners:
print("pass")
else:
aruco.refineDetectedMarkers(im_gray, board, corners, ids,
rejectedImgPoints)
charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, im_gray, board)
image_with_charuco_board = aruco.drawDetectedCornersCharuco(
img_charuco, charucoCorners, charucoIds, (0, 255, 0))
status, rvec, tvec = aruco.estimatePoseCharucoBoard(
charucoCorners, charucoIds, board, camera_matrix, distortion_coff)
if status != 0:
img_aruco = aruco.drawAxis(image_with_charuco_board, newcameramtx,
distortion_coff, rvec, tvec, 20)
else:
print('no markers detected')
if cv2.waitKey(0) & 0xFF == ord('q'):
cv2.imwrite(base_folder + 'detected charuco pattern.jpg', img_charuco)
break
cv2.imshow("World co-ordinate frame axes", img_charuco)
camera.release()
camera.getFrames()
cv2.imshow("image", camera.color_img)
# convert frame to grayscale
gray = cv2.cvtColor(camera.color_img, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, dictionary, parameters=arucoParams) # First, detect markers
aruco.refineDetectedMarkers(gray, charucoBoard, corners, ids,
rejectedImgPoints)
if ids is not None: # if there is at least one marker detected
charucoretval, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, gray, charucoBoard)
im_with_charuco_board = aruco.drawDetectedCornersCharuco(
gray, charucoCorners, charucoIds, (0, 255, 0))
rvec, tvec = np.zeros(3), np.zeros(3)
retval, _, _ = aruco.estimatePoseCharucoBoard(
charucoCorners, charucoIds, charucoBoard, cameraMatrix,
distCoeffs, rvec,
tvec) # posture estimation from a charuco board
if retval == True:
im_with_charuco_board = aruco.drawAxis(
im_with_charuco_board, cameraMatrix, distCoeffs, rvec,
tvec, 0.05
) # axis length 100 can be changed according to your requirement
camera.close()
cv2.destroyAllWindows()
def collect_charuco_detections(capture, aruco_dict, charuco_board):
# Create the arrays and variables we'll use to store info like corners and IDs from images processed
corners_all = [] # Corners discovered in all images processed
ids_all = [] # Aruco ids corresponding to corners discovered
image_size = None # Determined at runtime
good = 0
def show(frame):
# resize
proportion = max(frame.shape) / 800.0
im = cv2.resize(frame, (int(
frame.shape[1] / proportion), int(frame.shape[0] / proportion)))
# add text
txt = f'good frames so far: {good}'
im = cv2.putText(im, txt, (50, 50), cv2.FONT_HERSHEY_SIMPLEX, 1,
(255, 0, 0), 2, cv2.LINE_AA)
# show the debug image
cv2.imshow('calibration', im)
while True:
more, img = capture.read()
if not more:
break
# Grayscale the image
gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY)
# Find aruco markers in the query image
corners, ids, _ = aruco.detectMarkers(image=gray,
dictionary=aruco_dict)
# Outline the aruco markers found in our query image
img = aruco.drawDetectedMarkers(image=img, corners=corners)
try:
# Get charuco corners and ids from detected aruco markers
response, charuco_corners, charuco_ids = aruco.interpolateCornersCharuco(
markerCorners=corners,
markerIds=ids,
image=gray,
board=charuco_board)
# If a Charuco board was found, let's collect image/corner points
# Requiring at least 20 squares
if response > 20:
# Add these corners and ids to our calibration arrays
corners_all.append(charuco_corners)
ids_all.append(charuco_ids)
# Draw the Charuco board we've detected to show our calibrator the board was properly detected
img = aruco.drawDetectedCornersCharuco(
image=img,
charucoCorners=charuco_corners,
charucoIds=charuco_ids)
# If our image size is unknown, set it now
if not image_size:
image_size = gray.shape[::-1]
good += 1
show(img)
else:
show(img)
except cv2.error as e:
show(img)
if cv2.waitKey(1) & 0xFF == ord('q'):
break
return corners_all, ids_all, image_size
corners, ids, rejected = aruco.detectMarkers(img, BoardInfo.aurcoDict)
cimg = aruco.drawDetectedMarkers(img.copy(), corners, ids)
cv2.imwrite(outPath + "DetectedMarkers.png", cimg)
charucoCorners, charucoIds = [], []
if len(ids) > 0:
numCorners, charucoCorners, charucoIds = aruco.interpolateCornersCharuco(
corners, ids, img, BoardInfo.charucoBoard)
if len(charucoIds) < 0:
continue
all_charco_corners_camera.append(charucoCorners.copy())
all_charco_ids_camera.append(charucoIds.copy())
cimg = aruco.drawDetectedCornersCharuco(img.copy(), charucoCorners,
charucoIds)
cv2.imwrite(outPath + "DetectedCorners.png", cimg)
valid_points, new_points_cam, new_points_projector = getCameraCoordinates(
img, validV, validH, coordsV, coordsH, charucoCorners)
charucoIds = charucoIds[valid_points]
if len(charucoIds) < 0:
continue
all_charco_corners_camera_2.append(new_points_cam)
all_real_points.append(
BoardInfo.charucoBoard.chessboardCorners[charucoIds[:, 0]])
print(new_points_projector)
all_charco_corners_projector.append(new_points_projector)
ret, frame = cap.read()
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = aruco.detectMarkers(gray, aruco_dict,
parameters=aruco_parameters)
# if enough markers were detected
# then process the board
if( ids is not None ):
#gray = aruco.drawDetectedMarkers(gray, corners)
ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(corners, ids, gray, charuco_board )
# if there are enough corners to get a reasonable result
if( ret > 5 ):
aruco.drawDetectedCornersCharuco(frame,ch_corners,ch_ids,(0,0,255) )
retval, rvec, tvec = aruco.estimatePoseCharucoBoard( ch_corners, ch_ids, charuco_board, camera_matrix, dist_coeffs )
# if a pose could be estimated
if( retval ) :
frame = aruco.drawAxis(frame,camera_matrix,dist_coeffs,rvec,tvec,0.032)
# imshow and waitKey are required for the window
# to open on a mac.
cv2.imshow('frame', frame)
if( cv2.waitKey(1) & 0xFF == ord('q') ):
break
cap.release()
cv2.destroyAllWindows()
color_frame = frames.get_color_frame()
input_img = np.asanyarray(color_frame.get_data())
# convert frame to grayscale
gray = cv2.cvtColor(input_img, cv2.COLOR_BGR2GRAY)
# detect Aruco markers
corners, ids, rejectedImgPoints = aruco.detectMarkers(
gray, aruco_dict, parameters=aruco_parameters)
# if enough markers were detected, then process the board
if (ids is not None):
ret, ch_corners, ch_ids = aruco.interpolateCornersCharuco(
corners, ids, gray, charuco_board)
# if there are enough corners to get a reasonable result, proceed to estimate board pose
if (ret > 3):
aruco.drawDetectedCornersCharuco(input_img, ch_corners, ch_ids,
(0, 0, 255))
#estimate charuco board pose. The axis is centered at one of the corners of the Charuco board.
retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(
ch_corners, ch_ids, charuco_board, mtx, dist, None,
None) #, useExtrinsicGuess=False)
print("pose", retval, rvec, tvec)
#In the next few lines of code, the transformation matrix of Aruco board pose is obtained by combining tvec and rvec.
rvec = np.reshape(rvec, (1, 3))
tvec = np.reshape(tvec, (1, 3))
R_rvec = R.from_rotvec(rvec)
R_rotmat = R_rvec.as_matrix()
R_euler = R_rvec.as_euler('xyz', degrees=True)
print("R_euler", R_euler)
RSTr = np.hstack([R_rotmat[0], tvec.transpose()])
RSTr = np.vstack([RSTr, [0, 0, 0, 1]])
image=grayL,
board=CHARUCO_BOARD)
responseR, charuco_cornersR, charuco_idsR = aruco.interpolateCornersCharuco(
markerCorners=cornersR,
markerIds=idsR,
image=grayR,
board=CHARUCO_BOARD)
# If a Charuco board was found, let's collect image/corner points
# Requiring all squares
if responseL == 24 and responseR == 24:
# Add these corners and ids to our calibration arrays
objectPoints.append(objp)
imgL = aruco.drawDetectedCornersCharuco(
image=imgL,
charucoCorners=charuco_cornersL,
charucoIds=charuco_idsL)
imgR = aruco.drawDetectedCornersCharuco(
image=imgR,
charucoCorners=charuco_cornersR,
charucoIds=charuco_idsR)
#cv2.imshow('Charuco board Left', imgL)
#cv2.waitKey(0)
#cv2.imshow('Charuco board Right', imgR)
#cv2.waitKey(0)
rt = cv2.cornerSubPix(grayL, charuco_cornersL, (11, 11), (-1, -1),
criteria)
imagePointsLeft.append(charuco_cornersL)
rt = cv2.cornerSubPix(grayR, charuco_cornersR, (11, 11), (-1, -1),
