def __init__(self):
camera_id = str(rospy.get_param('~camera_id'))
color_width = rospy.get_param('~color_width')
color_high = rospy.get_param('~color_high')
charuco_row = rospy.get_param('~charuco_row')
charuco_col = rospy.get_param('~charuco_col')
square_length = rospy.get_param('~square_length')
marker_length = rospy.get_param('~marker_length')
self.cnd = 0
self.frameId = 0
self.pipeline = rs.pipeline()
rs_config = rs.config()
rs_config.enable_stream(rs.stream.color, color_width, color_high,
rs.format.bgr8, 30)
self.pipeline.start(rs_config)
# Constant parameters used in Aruco methods
self.aruco_param = aruco.DetectorParameters_create()
self.aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_50)
# Create grid board object we're using in our stream
self.charuco_board = aruco.CharucoBoard_create(
squaresX=charuco_col,
squaresY=charuco_row,
squareLength=square_length,
markerLength=marker_length,
dictionary=self.aruco_dict)
# Check for camera calibration data
config = ConfigParser.ConfigParser()
config.optionxform = str
rospack = rospkg.RosPack()
self.curr_path = rospack.get_path('hand_eye')
config.read(self.curr_path + '/config/camera_' + str(camera_id) +
'_internal.ini')
internal_name = 'Internal_' + str(color_width) + '_' + str(color_high)
b00 = float(config.get(internal_name, "Key_1_1"))
b01 = float(config.get(internal_name, "Key_1_2"))
b02 = float(config.get(internal_name, "Key_1_3"))
b10 = float(config.get(internal_name, "Key_2_1"))
b11 = float(config.get(internal_name, "Key_2_2"))
b12 = float(config.get(internal_name, "Key_2_3"))
b20 = float(config.get(internal_name, "Key_3_1"))
b21 = float(config.get(internal_name, "Key_3_2"))
b22 = float(config.get(internal_name, "Key_3_3"))
self.cameraMatrix = np.mat([[b00, b01, b02], [b10, b11, b12],
[b20, b21, b22]])
# c_x = 643.47548083
# c_y = 363.67742746
# f_x = 906.60886808
# f_y = 909.34831447
# k_1 = 0.16962942
# k_2 = -0.5560001
# p_1 = 0.00116353
# p_2 = -0.00122694
# k_3 = 0.52491878
# c_x = 649.007507324219
# c_y = 356.122222900391
# f_x = 922.76806640625
# f_y = 923.262023925781
# self.cameraMatrix = np.array([[f_x, 0, c_x],
# [0, f_y, c_y],
# [0, 0, 1]])
# self.distCoeffs = np.array([k_1, k_2, p_1, p_2, k_3])
self.distCoeffs = np.array([0.0, 0, 0, 0, 0])
# Create vectors we'll be using for rotations and translations for postures
self.rvecs = None
self.tvecs = None
self.rvecs_arr = np.zeros((3, NUMBER))
self.tvecs_arr = np.zeros((3, NUMBER))
self.cam = None
self.QueryImg = None
self.init_server()
frames = self.pipeline.wait_for_frames()
color_frame = frames.get_color_frame()
self.QueryImg = np.asanyarray(color_frame.get_data())
if cameraMatrix is None or distCoeffs is None:
print(
"Calibration issue. Remove ./calibration/ProCamCalibration.pckl and recalibrate your camera with CalibrateCamera.py."
)
exit()
# Constant parameters used in Aruco methods
ARUCO_PARAMETERS = aruco.DetectorParameters_create()
ARUCO_DICT = aruco.Dictionary_get(aruco.DICT_5X5_50)
CHARUCOBOARD_ROWCOUNT = 8
CHARUCOBOARD_COLCOUNT = 6
# Create grid board object we're using in our stream
CHARUCO_BOARD = aruco.CharucoBoard_create(squaresX=CHARUCOBOARD_COLCOUNT,
squaresY=CHARUCOBOARD_ROWCOUNT,
squareLength=0.04,
markerLength=0.02,
dictionary=ARUCO_DICT)
# Create vectors we'll be using for rotations and translations for postures
rvecs, tvecs = None, None
# cam = cv2.VideoCapture('charucoboard-test-at-kyles-desk.mp4')
cam = cv2.VideoCapture('ChAruco_Circles.webm')
while (cam.isOpened()):
# Capturing each frame of our video stream
ret, QueryImg = cam.read()
if ret == True:
# grayscale image
gray = cv2.cvtColor(QueryImg, cv2.COLOR_BGR2GRAY)
def camera_calibration(dataDir):
# This file can be used to generate camera calibration parameters
# to improve the default values
fileNameK = "{}intrinsic.txt".format(dataDir)
fileNameD = "{}distCoeffs.txt".format(dataDir)
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
board = aruco.CharucoBoard_create(6, 4, 0.056, 0.042, aruco_dict)
allCorners = []
allIds = []
decimator = 0
images = np.array(
[dataDir + f for f in os.listdir(dataDir) if f.endswith(".png")])
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100,
0.00001)
for im in images:
print("=> Processing image {0}".format(im))
frame = cv2.imread(im)
gray = cv2.cvtColor(frame, 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=(3, 3),
zeroZone=(-1, -1),
criteria=criteria)
res2 = cv2.aruco.interpolateCornersCharuco(corners, ids, gray,
board)
if res2[1] is not None and res2[2] is not None and len(
res2[1]) > 3 and decimator % 1 == 0:
allCorners.append(res2[1])
allIds.append(res2[2])
print("Image: {}/{}".format(decimator + 1, len(images)))
print("Corners found: {}".format(len(corners)))
decimator += 1
imsize = gray.shape
print("\n")
print("Checkerboard detected in: {}/{} images".format(
len(allCorners), decimator))
cameraMatrixInit = np.array([[1000., 0., imsize[0] / 2.],
[0., 1000., imsize[1] / 2.], [0., 0., 1.]])
distCoeffsInit = np.zeros((5, 1))
flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL +
cv2.CALIB_FIX_ASPECT_RATIO)
(ret, camera_matrix, distortion_coefficients0, rotation_vectors,
translation_vectors, _, _, _) = cv2.aruco.calibrateCameraCharucoExtended(
charucoCorners=allCorners,
charucoIds=allIds,
board=board,
imageSize=imsize,
cameraMatrix=cameraMatrixInit,
distCoeffs=distCoeffsInit,
flags=flags,
criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000,
1e-9))
np.savetxt(fileNameK, camera_matrix, delimiter=',')
np.savetxt(fileNameD, distortion_coefficients0, delimiter=',')
i = 5 # select image id
plt.figure()
frame = cv2.imread(images[i])
img_undist = cv2.undistort(frame, camera_matrix, distortion_coefficients0,
None)
plt.subplot(1, 2, 1)
plt.imshow(frame)
plt.title("Raw image")
plt.axis("off")
plt.subplot(1, 2, 2)
plt.imshow(img_undist)
plt.title("Corrected image")
plt.axis("off")
plt.show()
def startTracking(marker_type, MLRSTr, n_sq_y, n_sq_x, squareLen, markerLen,
aruco_dict, socket_connect, c):
RSTr = None
#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)
# 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]])
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()
import time
import cv2.aruco as A
import cv2
import numpy as np
dictionary = A.getPredefinedDictionary(cv2.aruco.DICT_4X4_50)
board = A.CharucoBoard_create(5, 7, 1, 0.75, dictionary)
#img = board.draw((200*3,200*3))
#Dump the calibration board to a file
#cv2.imwrite('charuco.png',img)
#Start capturing images for calibration
cap = cv2.VideoCapture(4)
allCorners = []
allIds = []
decimator = 0
for i in range(300):
ret, frame = cap.read()
if not ret:
break
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
res = cv2.aruco.detectMarkers(gray, dictionary)
if len(res[0]) > 0:
res2 = cv2.aruco.interpolateCornersCharuco(res[0], res[1], gray, board)
if res2[1] is not None and res2[2] is not None and len(
res2[1]) > 3 and decimator % 3 == 0:
allCorners.append(res2[1])
def createCharucoBoard():
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_50)
board = aruco.CharucoBoard_create(6, 8, 0.025, 0.02, aruco_dict)
img = board.draw((200 * 3, 200 * 3))
cv2.imwrite("CharucoBoard.jpg", img)
def generateCharucoBoard():
board = aruco.CharucoBoard_create(7, 5, 0.04, 0.02, aruco_dict)
imboard = board.draw((2000, 2000))
cv.imwrite('charuco_board.tiff', imboard)
return board
# Amaan Vally
# Written using Python 3.8 and OpenCV 4.4.0
# Tested on Windows 10
import cv2
import cv2.aruco as aruco
# Create ChArUco board
# Adjust squaresX, squaresY, squareLength and markerLength as desired
gridboard = aruco.CharucoBoard_create(squaresX=5,
squaresY=7,
squareLength=0.045,
markerLength=0.0275,
dictionary=aruco.Dictionary_get(
aruco.DICT_5X5_1000))
# Create an image from the gridboard and store to "ChArUco Board.jpg"
# 96 DPI jpg, so image size max 1123x1587 for A3
img = gridboard.draw(outSize=(1123, 1587))
cv2.imwrite("ChArUco Board.jpg", img)
# Show the ChArUco board on screen
cv2.imshow('ChArUco Board', img)
# Exit when a key is pressed
cv2.waitKey(0)
cv2.destroyAllWindows()
def calibrateCamera(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)
# Sub pixel corner detection criteria
subPixCriteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100, 0.00001)
# Storage variables
cornerList = []
idList = []
# Get image paths from calibration folder
paths = glob.glob(self.calibrationDir + '*' + self.imgExtension)
# Empty imageSize variable to be determined at runtime
imageSize = None
# 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)
# Sub pixel detection
for corner in corners:
cv2.cornerSubPix(
image=gray,
corners=corner,
winSize = (3,3),
zeroZone = (-1,-1),
criteria = subPixCriteria)
# 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:
# Add the corners and ids to calibration list
cornerList.append(charucoCorners)
idList.append(charucoIDs)
# If image size is still None, set it to the image size
if not imageSize:
imageSize = gray.shape[::-1]
else:
# Error message
print('Error in: ' + str(filePath))
# Make sure at least one image was found
if len(paths) < 1:
print('No images of charucoboards were found.')
return
# Make sure at least one charucoboard was found
if not imageSize:
print('Images supplied were not regonized by calibration')
return
# Start a timer
startTime = time.time()
# Run calibration
_, self.mtx, self.dist, _, _ = aruco.calibrateCameraCharuco(
charucoCorners=cornerList,
charucoIds=idList,
board=board,
imageSize=imageSize,
cameraMatrix=None,
distCoeffs=None)
# Print how long it took
print("Calibration took: ", round(time.time()-startTime),' s')
# Display matrix and distortion coefficients
print('Image size: ', imageSize)
print(self.mtx)
print(self.dist)
# Pickle the results
f = open('resources/calibration.pckl', 'wb')
pickle.dump((self.mtx, self.dist), f)
f.close()
def main():
# First we set up the cameras to start streaming
# Define some constants
resolution_width = 1280 # pixels
resolution_height = 720 # pixels
frame_rate = 30 # fps
dispose_frames_for_stablisation = 30 # frames
# Enable the streams from all the intel realsense devices
rs_config = rs.config()
rs_config.enable_stream(rs.stream.depth, resolution_width,
resolution_height, rs.format.z16, frame_rate)
rs_config.enable_stream(rs.stream.infrared, 1, resolution_width,
resolution_height, rs.format.y8, frame_rate)
rs_config.enable_stream(rs.stream.color, resolution_width,
resolution_height, rs.format.bgr8, frame_rate)
# Use the device manager class to enable the devices and get the frames
device_manager = DeviceManager(rs.context(), rs_config)
device_manager.enable_all_devices()
device_manager._available_devices.sort()
device_list = device_manager._available_devices
# Allow some frames for the auto-exposure controller to stablise
for frame in range(dispose_frames_for_stablisation):
frames = device_manager.poll_frames_keep()
assert (len(device_manager._available_devices) > 0)
#Then we calibrate the images
# Get the intrinsics of the realsense device
intrinsics_devices = device_manager.get_device_intrinsics(frames)
# Set the charuco board parameters for calibration
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
charuco_width = 8
charuco_height = 5
square_length = 0.03425
marker_length = .026
coordinate_dimentions = 3
charuco_board = aruco.CharucoBoard_create(charuco_width, charuco_height,
square_length, marker_length,
aruco_dict)
# Set the charuco board parameters for robot
aruco_dict_r = aruco.Dictionary_get(aruco.DICT_4X4_250)
charuco_width_r = 3
charuco_height_r = 3
square_length_r = 0.0311
marker_length_r = .0247
charuco_board_robot = aruco.CharucoBoard_create(charuco_width_r,
charuco_height_r,
square_length_r,
marker_length_r,
aruco_dict_r)
# Set the charuco board parameters for robot
aruco_dict_ro = aruco.Dictionary_get(aruco.DICT_5X5_100)
charuco_width_ro = 3
charuco_height_ro = 3
square_length_ro = 0.0311
marker_length_ro = .0247
charuco_board_robot_2 = aruco.CharucoBoard_create(charuco_width_ro,
charuco_height_ro,
square_length_ro,
marker_length_ro,
aruco_dict_ro)
# Decide if you want to use previous calibration or make a new one
calibration_decision = input(
'To use previous calibration, press "y". For new calibration press any key \n'
)
if calibration_decision != 'y':
# Choose amount of frames to average
correct_input = False
while not correct_input:
the_input = input(
"Write amount of frames you want to use to calibrate \n")
try:
amount_frames = int(the_input)
if amount_frames > 0:
correct_input = True
else:
print("Frame count has to be positive")
except ValueError:
print('Input has to be int')
# Make a dict to store all images for calibration
frame_dict = {}
transform_dict = {}
rms_dict = {}
for from_device in device_list:
transform_dict[from_device] = {}
rms_dict[from_device] = {}
for to_device in device_list:
transform_dict[from_device][to_device] = {}
rms_dict[from_device][to_device] = np.inf
print("Starting to take images in 5 seconds")
time.sleep(5)
devices_stitched = False
while not devices_stitched:
print("taking new set of images")
for frame_count in range(amount_frames):
print("taking image")
print(amount_frames - frame_count, "images left")
frames = device_manager.poll_frames_keep()
time.sleep(0.5)
frame_dict[frame_count] = {}
for device in device_list:
ir_frame = np.asanyarray(
frames[device][(rs.stream.infrared, 1)].get_data())
depth_frame = np.asanyarray(
post_process_depth_frame(
frames[device][rs.stream.depth]).get_data())
frame_dict[frame_count][device] = {
'ir_frame': ir_frame,
'depth_frame': depth_frame
}
del frames
# Make transfer matrices between all possible cameras
for idx, from_device in enumerate(device_list[:-1]):
for to_device in device_list[idx + 1:]:
if to_device != from_device:
temp_transform, temp_rms = get_transformation_matrix_wout_rsobject(
frame_dict, [from_device, to_device],
intrinsics_devices, charuco_board)
if temp_rms < rms_dict[from_device][to_device]:
rms_dict[from_device][to_device] = temp_rms
rms_dict[to_device][from_device] = temp_rms
transform_dict[from_device][
to_device] = temp_transform
transform_dict[to_device][
from_device] = temp_transform.inverse()
#Use Djikstra's to fin shortest path and check if all cameras are connected
transformation_matrices = least_error_transfroms(
transform_dict, rms_dict)
if transformation_matrices != 0:
devices_stitched = True
# Prints rms error between camera transforms
test = matrix_viewer(rms_dict)
print(test)
# Turns transformation matrices into Transfomation objects
transformation_devices = {}
for matrix in transformation_matrices:
the_matirx = transformation_matrices[matrix]
transformation_devices[matrix] = Transformation(
the_matirx[:3, :3], the_matirx[:3, 3])
# Saves calibration transforms if the user wants to
save_calibration = input(
'Press "y" if you want to save calibration \n')
if save_calibration == "y":
calibration_name = input
saved = False
while not saved:
name = input(
"Type in name of file to save. remember to end name with '.npy' \n"
)
try:
np.save(name, transformation_matrices)
saved = True
except:
print(
"could not save, try another name and remember '.npy' in the end"
)
frame_dict.clear()
else:
correct_filename = False
while not correct_filename:
file_name = input('Type in calibration file name \n')
try:
transfer_matirices_save = np.load(file_name, allow_pickle=True)
transfer_matrices = transfer_matirices_save[()]
correct_filename = True
except:
print('No such file in directory: "', file_name, '"')
transformation_devices = {}
for matrix in transfer_matrices:
the_matrix = transfer_matrices[matrix]
transformation_devices[matrix] = Transformation(
the_matrix[:3, :3], the_matrix[:3, 3])
print("Calibration completed...")
# Enable the emitter of the devices and extract serial numbers to identify cameras
device_manager.enable_emitter(True)
key_list = device_manager.poll_frames().keys()
while True:
visualisation = input(
'Presss "1" for RMS error, "2" for chessboard visualisation and "3" for 3d pointcloud and "4" for robot to camea calibration\n'
)
if visualisation == '1':
calculate_avg_rms_error(device_list, device_manager,
transformation_devices, charuco_board,
intrinsics_devices)
elif visualisation == '2':
visualise_chessboard(device_manager, device_list,
intrinsics_devices, charuco_board,
transformation_devices)
elif visualisation == '3':
visualise_point_cloud(key_list, resolution_height,
resolution_width, device_manager,
coordinate_dimentions,
transformation_devices)
elif visualisation == '4':
robot_calibration = CameraRobotCalibration(transformation_devices,
device_manager,
charuco_board_robot_2,
charuco_board_robot)
input("Set target in position for calibration\n")
test = robot_calibration.test_functions()
print(test)
else:
print("Input not recognised")
def createCheckerBoard(self):
board = aruco.CharucoBoard_create(3, 3, 1, 0.8, self.aruco_dict)
imboard = board.draw((4000, 4000))
return board
import numpy as np
import cv2, os
from cv2 import aruco
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import matplotlib as mpl
import pandas as pd
arucoParams = aruco.DetectorParameters_create()
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
markerLength = 1
markerSeparation = 0.5
targetwidth = 4
targetheight = 4
board = aruco.CharucoBoard_create(targetwidth, targetheight, markerLength,
markerSeparation, aruco_dict)
imboard = board.draw((1000, 1000))
handle = plt.imshow(imboard, cmap='gray')
plt.axis('off')
plt.savefig(basefolder + "/charuco_markers.png")
camera = cv2.VideoCapture(0)
while True:
status, img_charuco = camera.read()
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)
import csv
cwd = os.getcwd()
fk_ur5 = UR5Kin()
################### Initialisation ################################
camera = 2 # number of cameras
rootDir = "/home/zheyu/ur5_calib_toolkit/ZED/"
robotYAML = "{}robotCalibration.yaml".format(rootDir)
cameraYAML = "cameraCalibration.yaml"
extrinsicDataDir = "{}calibDataset/".format(rootDir)
tableDataDir = "{}tableCalibDataset/".format(rootDir)
fileName = "jointAngles.csv"
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000) # 4X4 = 4x4 bit markers
charuco_board = aruco.CharucoBoard_create(
4, 6, 0.063, 0.048,
aruco_dict) # width, height (full board), square length, marker length
##################################################################
class chArucoCalib:
def __init__(self):
images = np.array([
extrinsicDataDir + f for f in os.listdir(extrinsicDataDir)
if f.endswith(".png")
])
order = np.argsort(
[int(p.split(".")[-2].split("_")[-1]) for p in images])
images = images[order]
q = []
import cv2
from cv2 import aruco
dictionary = aruco.getPredefinedDictionary(aruco.DICT_4X4_50)
nx = int(input('Define board size, # of squares in x [int]: '))
ny = int(input('Define board size, # of squares in y [int]: '))
wc = float(
input('Define marker size, chess square size in m [float] (e.g. 0.025): '))
wa = float(
input(
'Define marker size, aruco square size in m [float] (e.g. 0.0175): '))
board = aruco.CharucoBoard_create(nx, ny, wc, wa, dictionary)
img = board.draw((9 * 500, 6 * 500))
cv2.imwrite('charuco_board.png', img)
import numpy as np
import cv2, PIL, os
from cv2 import aruco
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import matplotlib as mpl
import pandas as pd
import imutils
#matplotlib nbagg
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
imagesFolder = "images/"
########## create marker board for calibration ##########
# settings
board = aruco.CharucoBoard_create(4, 4, 0.04, 0.03, aruco_dict)
imboard = board.draw((4000, 4000))
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
plt.imshow(imboard, cmap=mpl.cm.gray, interpolation="nearest")
ax.axis("off")
cv2.imwrite(imagesFolder + "chessboard.tiff", imboard)
#plt.savefig(imagesFolder + "camCal.pdf")
print("-> print it out for calibration!")
plt.grid()
plt.show()
# ######### convert video to images ##########
# # Settings
# videoFile = "videos/camCal.mp4"
# aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
parser.add_argument('--dataset',
type=str,
help='directory for saving the data')
#
parser.add_argument('--aruco_bit',
type=int,
default=4,
help='format of aruco dictionary')
parser.add_argument('--board_dim',
type=int,
nargs="+",
default=[4, 6],
help='width, height of checkerboard (unit: squares)')
parser.add_argument('--square_len',
type=float,
default=0.062,
help='measured in metre')
parser.add_argument('--marker_len',
type=float,
default=0.046,
help='measured in metre')
parser.add_argument('--camera_topic',
type=str,
default='/camera/color/image_raw')
args = parser.parse_args()
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_1000) # 4X4 = 4x4 bit markers
charuco_board = aruco.CharucoBoard_create(args.board_dim[0], args.board_dim[1],
args.square_len, args.marker_len,
aruco_dict)
import os
import numpy as np
import cv2
from cv2 import aruco
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
board = aruco.CharucoBoard_create(8, 6, 1, 0.8, aruco_dict)
imboard = board.draw((8000, 6000))
imboard_show = cv2.resize(imboard, (800, 600))
cv2.imshow("Charuco", imboard_show)
cv2.waitKey(0)
chessboard_file = os.path.join('data', 'charuco_board.png')
cv2.imwrite(chessboard_file, imboard)
def makechessandcharucoboard(nrowchess=3,
ncolumnchess=5,
squaresize=25,
nrowcharuco=3,
ncolumncharuco=5,
markerdict=aruco.DICT_6X6_250,
squaresizearuco=25,
savepath='./',
paperwidth=210,
paperheight=297,
dpi=600,
framesize=None):
"""
create half-chess and half-charuco board
the paper is in portrait orientation, nrow means the number of markers in the vertical direction
:param nrow:
:param ncolumn:
:param squaresize: mm
:param markerdict:
:param savepath:
:param paperwidth: mm
:param paperheight: mm
:param dpi:
:param framesize: (width, height) the 1pt frame for easy cut, nothing is drawn by default
:return:
author: weiwei
date: 20190420
"""
aruco_dict = aruco.Dictionary_get(markerdict)
# 1mm = 0.0393701inch
a4npxrow = int(paperheight * 0.0393701 * dpi)
a4npxcolumn = int(paperwidth * 0.0393701 * dpi)
bgimg = np.ones((a4npxrow, a4npxcolumn), dtype='uint8') * 255
if framesize is not None:
framesize[0] = int(framesize[0] * 0.0393701 * dpi)
framesize[1] = int(framesize[1] * 0.0393701 * dpi)
if a4npxcolumn < framesize[0] + 2:
print("Frame width must be smaller than the #pt in each row.")
if a4npxrow < framesize[1] + 2:
print("Frame height must be smaller than the #pt in each column.")
framelft = int((a4npxcolumn - framesize[0]) / 2 - 1)
framergt = int(framelft + 1 + framesize[0])
frametop = int((a4npxrow - framesize[1]) / 2 - 1)
framedown = int(frametop + 1 + framesize[1])
bgimg[frametop:framedown + 1, framelft:framelft + 1] = 0
bgimg[frametop:framedown + 1, framergt:framergt + 1] = 0
bgimg[frametop:frametop + 1, framelft:framergt + 1] = 0
bgimg[framedown:framedown + 1, framelft:framergt + 1] = 0
# upper half, charuco
squaresizepx = int(squaresizearuco * 0.0393701 * dpi)
squareareanpxrow = squaresizepx * nrowchess
uppermargin = int((a4npxrow / 2 - squareareanpxrow) / 2)
squareareanpxcolumn = squaresizepx * ncolumnchess
leftmargin = int((a4npxcolumn - squareareanpxcolumn) / 2)
if (uppermargin <= 10) or (leftmargin <= 10):
print("Too many markers! Reduce nrow and ncolumn.")
return
board = aruco.CharucoBoard_create(ncolumnchess, nrowchess, squaresizearuco,
.57 * squaresizearuco, aruco_dict)
imboard = board.draw((squareareanpxcolumn, squareareanpxrow))
print(imboard.shape)
startrow = uppermargin
endrow = uppermargin + squareareanpxrow
startcolumn = leftmargin
endcolumn = leftmargin + squareareanpxcolumn
bgimg[startrow:endrow, startcolumn:endcolumn] = imboard
# lower half, chess
squaresizepx = int(squaresize * 0.0393701 * dpi)
squareareanpxrow = squaresizepx * nrowcharuco
uppermargin = int((a4npxrow / 2 - squareareanpxrow) / 2)
squareareanpxcolumn = squaresizepx * ncolumncharuco
leftmargin = int((a4npxcolumn - squareareanpxcolumn) / 2)
if (uppermargin <= 10) or (leftmargin <= 10):
print("Too many markers! Reduce nrow and ncolumn.")
return
for idnr in range(nrowcharuco):
for idnc in range(ncolumncharuco):
startrow = int(a4npxrow / 2) + uppermargin + idnr * squaresizepx
endrow = startrow + squaresizepx
startcolumn = leftmargin + idnc * squaresizepx
endcolumn = squaresizepx + startcolumn
if idnr % 2 != 0 and idnc % 2 != 0:
bgimg[startrow:endrow, startcolumn:endcolumn] = 0
if idnr % 2 == 0 and idnc % 2 == 0:
bgimg[startrow:endrow, startcolumn:endcolumn] = 0
im = Image.fromarray(bgimg).convert("L")
im.save(savepath + "test.pdf", "PDF", resolution=dpi)
import numpy as np
import cv2
import cv2.aruco as aruco
ID = 1 #ID du marqueur !
'''
drawMarker(...)
drawMarker(dictionary, id, sidePixels[, img[, borderBits]]) -> img
'''
files = ""
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
squareLength = 40 # Here, our measurement unit is centimetre.
markerLength = 30 # Here, our measurement unit is centimetre.
board = aruco.CharucoBoard_create(5, 7, squareLength, markerLength, aruco_dict)
for i in range(12):
# second parameter is id number
# last parameter is total image size
img = aruco.drawMarker(aruco_dict, i, 125)
# BBB = board.draw((200*3,200*3))
files = "ID" + str(i) + ".jpg"
cv2.imwrite(files, img)
# cv2.imwrite("test.jpg",board)
cv2.imshow('frame', img)
cv2.waitKey(0)
cv2.destroyAllWindows()
def charuco_detect(path, video_resolution):
aruco_dict = aruco.Dictionary_get(aruco.DICT_4X4_250)
board = aruco.CharucoBoard_create(7, 5, 1, .8, aruco_dict)
images = glob.glob(path)
def read_chessboards(images):
"""
Charuco base pose estimation.
"""
print("POSE ESTIMATION STARTS:")
allCorners = []
allIds = []
decimator = 0
# SUB PIXEL CORNER DETECTION CRITERION
criteria = (cv2.TERM_CRITERIA_EPS + cv2.TERM_CRITERIA_MAX_ITER, 100,
0.00001)
for im in images:
print("=> Processing image {0}".format(im))
frame = cv2.imread(im)
gray = cv2.cvtColor(frame, 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=(5, 5),
zeroZone=(-1, -1),
criteria=criteria)
res2 = cv2.aruco.interpolateCornersCharuco(
corners, ids, gray, board)
if res2[1] is not None and res2[2] is not None and len(
res2[1]) > 3 and decimator % 1 == 0:
allCorners.append(res2[1])
allIds.append(res2[2])
decimator += 1
imsize = gray.shape
return allCorners, allIds, imsize
def calibrate_camera(allCorners, allIds, imsize):
"""
Calibrates the camera using the dected corners.
"""
print("CAMERA CALIBRATION")
cameraMatrixInit = np.array([[1000., 0., imsize[0] / 2.],
[0., 1000., imsize[1] / 2.], [0., 0.,
1.]])
distCoeffsInit = np.zeros((5, 1))
flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL +
cv2.CALIB_FIX_ASPECT_RATIO)
#flags = (cv2.CALIB_RATIONAL_MODEL)
(ret, camera_matrix, distortion_coefficients0, rotation_vectors,
translation_vectors, stdDeviationsIntrinsics, stdDeviationsExtrinsics,
perViewErrors) = cv2.aruco.calibrateCameraCharucoExtended(
charucoCorners=allCorners,
charucoIds=allIds,
board=board,
imageSize=imsize,
cameraMatrix=cameraMatrixInit,
distCoeffs=distCoeffsInit,
flags=flags,
criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000,
1e-9))
return ret, camera_matrix, distortion_coefficients0, rotation_vectors, translation_vectors
allCorners, allIds, imsize = read_chessboards(images)
ret, mtx, dist, rvecs, tvecs = calibrate_camera(allCorners, allIds, imsize)
tvecs = np.array(tvecs)
mtx = np.array(mtx)
rvecs = np.array(rvecs)
return mtx, dist, rvecs[0], tvecs[0], allCorners[0], allIds[0]
}
# For validating results, show aruco board to camera.
board_params["aruco_dict"] = aruco.getPredefinedDictionary(board_params["dict_type"])
"""
# create arUco board
if board_params["board_type"] == "aruco":
board = aruco.GridBoard_create(board_params["markersX"],
board_params["markersY"],
board_params["markerLength"],
board_params["markerSeparation"],
board_params["aruco_dict"])
elif board_params["board_type"] == "charuco":
board = aruco.CharucoBoard_create(board_params["squaresX"],
board_params["squaresY"],
board_params["squareLength"],
board_params["markerLength"],
board_params["aruco_dict"])
else:
raise ValueError("Couldn't find correct board type (aruco or charuco)")
"""uncomment following block to draw and show the board"""
# img = board.draw((864, 1080))
# cv2.imshow("aruco", img)
# cv2.imwrite("aruco.png", img)
arucoParams = aruco.DetectorParameters_create()
# Set second camera outside laptop
# cv2.CAP_DSHOW removes delay on logitech C920 webcam
camera = cv2.VideoCapture(1, cv2.CAP_DSHOW)
camera.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
def run(window_placement_x, window_placement_y, max_window_size_x,
max_window_size_y):
# board properties
# aruco example
board_params = {
"board_type": "aruco",
"dict_type": aruco.DICT_6X6_1000,
"markersX": 4,
"markersY": 5,
"markerLength": 3.75, # Provide length of the marker's side [cm]
"markerSeparation": 0.5, # Provide separation between markers [cm]
}
# For validating results, show aruco board to camera.
board_params["aruco_dict"] = aruco.getPredefinedDictionary(
board_params["dict_type"])
"""# charuco example
board_params_params = {
"board_type": "charuco",
"dict_type": aruco.DICT_4X4_1000,
"squaresX": 4,
"squaresY": 4,
"squareLength": 3, # Provide length of the square's side [cm]
"markerLength": 3 * 7 / 9, # Provide length of the marker's side [cm]
}
# For validating results, show aruco board to camera.
board_params["aruco_dict"] = aruco.getPredefinedDictionary(board_params["dict_type"])
"""
# create arUco board
if board_params["board_type"] == "aruco":
board = aruco.GridBoard_create(board_params["markersX"],
board_params["markersY"],
board_params["markerLength"],
board_params["markerSeparation"],
board_params["aruco_dict"])
elif board_params["board_type"] == "charuco":
board = aruco.CharucoBoard_create(board_params["squaresX"],
board_params["squaresY"],
board_params["squareLength"],
board_params["markerLength"],
board_params["aruco_dict"])
else:
raise ValueError("Couldn't find correct board type (aruco or charuco)")
"""uncomment following block to draw and show the board"""
# img = board.draw((864, 1080))
# cv2.imshow("aruco", img)
# cv2.imwrite("aruco.png", img)
arucoParams = aruco.DetectorParameters_create()
# Set second camera outside laptop
# cv2.CAP_DSHOW removes delay on logitech C920 webcam
camera = cv2.VideoCapture(1, cv2.CAP_DSHOW)
camera.set(cv2.CAP_PROP_FOURCC, cv2.VideoWriter_fourcc('M', 'J', 'P', 'G'))
camera.set(cv2.CAP_PROP_FRAME_WIDTH, 960)
camera.set(cv2.CAP_PROP_FRAME_HEIGHT, 540)
# Turn the autofocus off
camera.set(cv2.CAP_PROP_AUTOFOCUS, 0)
# Open calibration file created with data_generation.py
with open('calibration.yaml') as f:
loadeddict = yaml.safe_load(f)
mtx = loadeddict.get('camera_matrix')
dist = loadeddict.get('dist_coeff')
mtx = np.array(mtx)
dist = np.array(dist)
ret, img = camera.read()
img_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
h, w = img_gray.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dist, (w, h), 1,
(w, h))
pose_r, pose_t = [], []
while True:
ret, img = camera.read()
img_aruco = img
im_gray = cv2.cvtColor(img, cv2.COLOR_RGB2GRAY)
h, w = im_gray.shape[:2]
dst = cv2.undistort(im_gray, mtx, dist, None, newcameramtx)
corners, ids, rejectedImgPoints = aruco.detectMarkers(
dst, board_params["aruco_dict"], parameters=arucoParams)
# cv2.imshow("original", img_gray)
if corners == None:
raise Exception("No aruco corners found!")
else:
rvec = None
tvec = None
ret, rvec, tvec = aruco.estimatePoseBoard(corners, ids, board,
newcameramtx, dist, rvec,
tvec) # For a board
print("Rotation ", rvec, "\nTranslation", tvec, "\n=======")
if ret != 0:
img_aruco = aruco.drawDetectedMarkers(img, corners, ids,
(0, 255, 0))
img_aruco = aruco.drawAxis(
img_aruco, newcameramtx, dist, rvec, tvec, 10
) # axis length 100 can be changed according to your requirement
cv2.imshow("World co-ordinate frame axes", img_aruco)
if cv2.waitKey(0) & 0xFF == ord('q'):
break
cv2.destroyAllWindows()
# output from aruco
theta = Symbol('theta')
# aruco marker values [cm]
aruco_rvec = [rvec[0][0], rvec[1][0], rvec[2][0]]
aruco_tvec = [tvec[0][0], tvec[1][0], tvec[2][0]]
# x-value is off by 84 mm
# y-value is off by 4 mm
# z-value is off by 25 mm
aruco_x = aruco_tvec[0] / 100
aruco_y = aruco_tvec[
2] / 100 # y and z-axis are switched in the robot coordinates!!!
aruco_z = aruco_tvec[
1] / -100 # y and z-axis are switched in the robot coordinates!!! the minus is to compensate for axis orientation
aruco_angle = 0
theta = aruco_angle
x, y, z = 0.485, -0.295, 0.456 # coordinates of camera in respect to robot coord [m]
a, b, c = aruco_x, aruco_y, aruco_z # coordinates of marker in respect to the camera [m]
# translate 3d coordinate systems
Robot = CoordSys3D('Robot') # base coordinate system
Cam = Robot.locate_new('Cam', x * Robot.i + y * Robot.j + z * Robot.k)
Cam2 = Cam.orient_new_axis('Cam2', theta, Cam.i)
Marker = Cam2.locate_new('Marker', a * Cam2.i + b * Cam2.j + c * Cam2.k)
# Calculate marker position with respect to the robots base
mat = Marker.position_wrt(Robot).to_matrix(Robot)
mat = dense.matrix2numpy(mat, float)
fig = pyplot.figure()
ax = Axes3D(fig)
ax.scatter(0, 0, 0, label='Robot position')
ax.scatter(x, y, z, label='Camera position')
ax.scatter(mat[0][0], mat[1][0], mat[2][0], label='Label position')
ax.legend()
ax.set_xlabel("x-axis")
ax.set_ylabel("y-axis")
ax.set_zlabel("z-axis")
# Plot positions of robot_base, camera and position of the marker in 3D
# todo turn on plot for info (turned off for fast testing)
# pyplot.show()
# Robot movements
# todo: Moving x, y and z-plane robot
# todo: Line up with aruco Marker (0.2m y-offset from board)
rob = van_robot.Robot("192.168.0.1", True) # Set IP address
orig_tcp = (0, 0, 0.150, 0, 0, 0)
rob.set_tcp(orig_tcp) # Set Tool Center Point
rob.set_payload(0.5, (0, 0, 0)) # Set payload for joints
time.sleep(0.2) # leave some time to robot to process the setup commands
a, v = (0.05, 0.5) # acceleration and movementspeed of robot
# Start from the pose the robot is in
pos0 = rob.getl()
# Tool orientation [rx, ry, rz]
tool_orient = [0, -2.221441469, -2.221441469]
# Put the X, Y, Z of start pose in (last 3 coordinates are tool head positioning)
pos_start = [
pos0[0], pos0[1], pos0[2], tool_orient[0], tool_orient[1],
tool_orient[2]
]
# Take the Y-distance 0.10[m] out of the van
safe_dist = mat[1] - 0.12
# Move to the aruco marker point
# todo turn on move command
rob.movel((mat[0], safe_dist, mat[2], tool_orient[0], tool_orient[1],
tool_orient[2]), a, v)
# Calibration Y-plane and X,Z-Axis
# Declare the port and baudrate for the Arduino and connect to the Arduino
# todo CHECK COM!
arduino = arduino_serial.Arduino("COM3")
# start position of the robot (from aruco marker)
# assumed is a robot start position which is close to the centre of the measured plane
pos_start = (mat[0], safe_dist, mat[2], tool_orient[0], tool_orient[1],
tool_orient[2])
# Take safe offset from Y-plan after global aruco Y-value
measurement_distance = -0.03
board_distance = -0.1
rob.centre_for_triangle(arduino, measurement_distance, board_distance)
# Define Y-plane
rob.measure_triangle(arduino, 0.160, a, v)
# Define X,Z-axis
points1, points2 = rob.repetitive_def_axis(arduino, -0.02)
# Create origin
# coordinates in [X,Z]
# SIDE Coordinates [z-formula]
coor1 = [points1[0][2][0], points1[0][2][2]] # [X,Z]
coor2 = [points2[0][2][0], points2[0][2][2]]
# TOP Coordinates [x-formula]
coor3 = [points1[1][2][0], points1[1][2][2]]
coor4 = [points2[1][2][0], points2[1][2][2]]
# create_origin.formula_line('x',coor1,coor2)
# create_origin.formula_line('z',coor3,coor4)
line_x = create_origin.formula_line('x', coor1, coor2)
line_z = create_origin.formula_line('z', coor3, coor4)
origin = create_origin.intersect_lines(line_x, line_z)
print(origin) # [X,Z]
# Calculate translation to origin
cur_pos = rob.getl()
cur_pos[0] = origin[0]
cur_pos[2] = origin[1]
# Move to origin (first moves out, then in x,z-plane)
print("X,Z-Axis origin has been set.")
print("Moving to origin.")
rob.move_y_plane(-0.15, cur_pos)
#test stuff for rectangle
delta_x = window_placement_x # distance in meters
delta_z = window_placement_y # distance in meters
radius = 0.06
x_axis_offset = max_window_size_x # [m]
z_axis_offset = -max_window_size_y # [m]
rob.draw_square(delta_x, delta_z, radius, -0.012, x_axis_offset,
z_axis_offset)
import cv2
from cv2 import aruco
# Create aruco chessboard then we load it into Unreal
aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
board = aruco.CharucoBoard_create(3, 3, .025, .0125, aruco_dict)
image_board = board.draw((200 * 3, 200 * 3))
cv2.imwrite('.\src\calibration\charuco.png', image_board)
def calibcharucoboard(nrow,
ncolumn,
arucomarkerdict=aruco.DICT_6X6_250,
squaremarkersize=25,
imgspath='./',
savename='mycam_charuco_data.yaml'):
"""
:param nrow:
:param ncolumn:
:param markerdict:
:param imgspath:
:param savename:
:return:
author: weiwei
date: 20190420
"""
# read images and detect cornders
aruco_dict = aruco.Dictionary_get(arucomarkerdict)
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, squaremarkersize,
.57 * squaremarkersize, aruco_dict)
images = glob.glob(imgspath + '*.png')
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])
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(savename, "w") as f:
yaml.dump([mtx, dist, rvecs, tvecs, candfiles], f)
def make_charuco_board(nrow,
ncolumn,
marker_dict=aruco.DICT_4X4_250,
square_size=25,
save_path='./',
name='test',
frame_size=None,
paper_width=210,
paper_height=297,
dpi=600):
"""
create charuco board
the paper is in portrait orientation, nrow means the number of markers in the vertical direction
:param nrow:
:param ncolumn:
:param marker_dict:
:param save_path:
:param name
:param frame_size: (width, height) the 1pt frame for easy cut, nothing is drawn by default
:param paper_width: mm
:param paper_height: mm
:param dpi:
:return:
author: weiwei
date: 20190420
"""
aruco_dict = aruco.Dictionary_get(marker_dict)
# 1mm = _MM_TO_INCHinch
a4npxrow = int(paper_height * _MM_TO_INCH * dpi)
a4npxcolumn = int(paper_width * _MM_TO_INCH * dpi)
bgimg = np.ones((a4npxrow, a4npxcolumn), dtype='uint8') * 255
if frame_size is not None:
frame_size[0] = int(frame_size[0] * _MM_TO_INCH * dpi)
frame_size[1] = int(frame_size[1] * _MM_TO_INCH * dpi)
if a4npxcolumn < frame_size[0] + 2:
print("Frame width must be smaller than the #pt in each row.")
if a4npxrow < frame_size[1] + 2:
print("Frame height must be smaller than the #pt in each column.")
framelft = int((a4npxcolumn - frame_size[0]) / 2 - 1)
framergt = int(framelft + 1 + frame_size[0])
frametop = int((a4npxrow - frame_size[1]) / 2 - 1)
framedown = int(frametop + 1 + frame_size[1])
bgimg[frametop:framedown + 1, framelft:framelft + 1] = 0
bgimg[frametop:framedown + 1, framergt:framergt + 1] = 0
bgimg[frametop:frametop + 1, framelft:framergt + 1] = 0
bgimg[framedown:framedown + 1, framelft:framergt + 1] = 0
squaresizepx = int(square_size * _MM_TO_INCH * dpi)
squareareanpxrow = squaresizepx * nrow
uppermargin = int((a4npxrow - squareareanpxrow) / 2)
squareareanpxcolumn = squaresizepx * ncolumn
leftmargin = int((a4npxcolumn - squareareanpxcolumn) / 2)
if (uppermargin <= 10) or (leftmargin <= 10):
print("Too many markers! Reduce nrow and ncolumn.")
return
board = aruco.CharucoBoard_create(ncolumn, nrow, square_size,
.57 * square_size, aruco_dict)
imboard = board.draw((squareareanpxcolumn, squareareanpxrow))
print(imboard.shape)
startrow = uppermargin
endrow = uppermargin + squareareanpxrow
startcolumn = leftmargin
endcolumn = leftmargin + squareareanpxcolumn
bgimg[startrow:endrow, startcolumn:endcolumn] = imboard
im = Image.fromarray(bgimg).convert("L")
im.save(save_path + name + ".pdf", "PDF", resolution=dpi)
def calibrate_camera_charucoBoard(self):
"""
Method to obtain the camera intrinsic parameters to perform the aruco pose estimation
Args:
Returns:
mtx: cameraMatrix Output 3x3 floating-point camera matrix
dist: Output vector of distortion coefficient
rvecs: Output vector of rotation vectors (see Rodrigues ) estimated for each board view
tvecs: Output vector of translation vectors estimated for each pattern view.
"""
aruco_dict = aruco.Dictionary_get(self.aruco_dict)
board = aruco.CharucoBoard_create(7, 5, 1, .8, aruco_dict)
images = []
logger.info('Start moving randomly the aruco board')
n = 400 # number of frames
for i in range(n):
frame = self.kinect.get_color()
images.append(frame)
logger.info("Stop moving the board")
img_frame = numpy.array(images)[0::5]
logger.info("Calculating Aruco location of %s images" %
str(img_frame.shape[0]))
allCorners = []
allIds = []
decimator = 0
for im in img_frame:
# print("=> Processing image {0}".format(im))
# frame = cv2.imread(im)
gray = cv2.cvtColor(im, cv2.COLOR_BGR2GRAY)
res = cv2.aruco.detectMarkers(gray, aruco_dict)
if len(res[0]) > 0:
res2 = cv2.aruco.interpolateCornersCharuco(
res[0], res[1], gray, board)
if res2[1] is not None and res2[2] is not None and len(
res2[1]) > 3 and decimator % 1 == 0:
allCorners.append(res2[1])
allIds.append(res2[2])
decimator += 1
imsize = gray.shape
logger.info("Finish")
logger.info("Calculating camera parameters")
cameraMatrixInit = numpy.array([[2000., 0., imsize[0] / 2.],
[0., 2000., imsize[1] / 2.],
[0., 0., 1.]])
distCoeffsInit = numpy.zeros((5, 1))
flags = (cv2.CALIB_USE_INTRINSIC_GUESS + cv2.CALIB_RATIONAL_MODEL)
ret, mtx, dist, rvecs, tvecs, stdDeviationsIntrinsics, stdDeviationsExtrinsics, perViewErrors = \
cv2.aruco.calibrateCameraCharucoExtended(
charucoCorners=allCorners,
charucoIds=allIds,
board=board,
imageSize=imsize,
cameraMatrix=cameraMatrixInit,
distCoeffs=distCoeffsInit,
flags=flags,
criteria=(cv2.TERM_CRITERIA_EPS & cv2.TERM_CRITERIA_COUNT, 10000, 1e-9)
)
logger.info("Finish")
self.calib.camera_mtx = mtx.tolist()
self.calib.camera_dist = dist.tolist()
return mtx, dist, rvecs, tvecs
# Define the type of ArUco markers to make the board from
# aruco_dict = aruco.Dictionary_get(aruco.DICT_ARUCO_ORIGINAL)
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
# Define the board's setup and create it
NUM_SQUARES_X = 7
NUM_SQUARES_Y = 5
# We're using inches to match opencv dpi saving
SQUARE_LENGTH = 1.5 # absolute length of a square (inch)
MARKER_LENGTH = 0.8 * SQUARE_LENGTH # absolute length of a marker (inch)
# CharucoBoard_create(squaresX, squaresY, squareLength, markerLength, dictionary)
board = aruco.CharucoBoard_create(NUM_SQUARES_X,
NUM_SQUARES_Y,
SQUARE_LENGTH,
MARKER_LENGTH,
aruco_dict)
# Then draw it, specifyin the output dimensions in pixels. We're using 72
# pixels per inch here, to match openCV saving for png images. This will create
# markers whose physicaly size, when printed at 100%, matches that which we
# specified above.
imboard = board.draw((int(NUM_SQUARES_X * SQUARE_LENGTH * 72),
int(NUM_SQUARES_Y * SQUARE_LENGTH * 72)))
# Now, save the board. Be sure to change the filename to reflect the aruco_dict
# that you used to generate it. We also include the square length and marker
# length since those parameters are important for both calibration and pose
# estimation. We'll first save it as a .png
cv2.imwrite(f'ChArUco_orig_sq{SQUARE_LENGTH}_mark{MARKER_LENGTH:.2f}.png',
from __future__ import print_function
import cv2
import cv2.aruco as aruco
import numpy as np
from PIL import Image
import sys
import rosbag
from cv_bridge import CvBridge, CvBridgeError
#arucoType = aruco.DICT_6X6_100
#arucoType = aruco.DICT_6X6_1000 # The single marker
#arucoType = aruco.DICT_5X5_250
arucoType = aruco.DICT_4X4_250
arucoBoard = aruco.CharucoBoard_create(5, 5, 0.7, 0.5,
aruco.Dictionary_get(arucoType))
dictionary = cv2.aruco.getPredefinedDictionary(arucoType)
board = arucoBoard.draw((7014, 4962))
# The following is the calibration matrix for the ZED camera
cal = (2.114370012156985,
np.array([[1.20642153e+04, 0.00000000e+00, 3.87673066e+02],
[0.00000000e+00, 2.40412556e+04, 6.09900945e+02],
[0.00000000e+00, 0.00000000e+00, 1.00000000e+00]]),
np.array([[
2.97019230e+01, -1.81529858e+02, 4.60817372e-01, -4.19902342e-01,
-1.34497073e+04
]]), [
np.array([[1.70704508], [-1.40506077], [0.78495047]]),
np.array([[1.70206233], [-1.40638324], [0.78495157]]),
import numpy as np
import cv2, PIL, os
from cv2 import aruco
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import matplotlib as mpl
aruco_dict = aruco.Dictionary_get(aruco.DICT_6X6_250)
board = aruco.CharucoBoard_create(7, 5, 1, .8, aruco_dict)
imboard = board.draw((2000, 2000))
cv2.imwrite("./chessboard.tiff", imboard)
fig = plt.figure()
ax = fig.add_subplot(1, 1, 1)
plt.imshow(imboard, cmap=mpl.cm.gray, interpolation="nearest")
ax.axis("off")
#plt.show()
plt.savefig("generated_charuco_board.png")
plt.show()
import numpy as np
import cv2, os
from cv2 import aruco
from mpl_toolkits.mplot3d import Axes3D
import matplotlib.pyplot as plt
import matplotlib as mpl
workdir = "./workdir/"
aruco_dict = aruco.Dictionary_get(aruco.DICT_5X5_100)
board = aruco.CharucoBoard_create(8, 5, 5, 4, aruco_dict)
imboard = board.draw((1750, 1250))
cv2.imwrite("chessboard.png", imboard)
fig = plt.figure()
ax = fig.add_subplot(1,1,1)
plt.imshow(imboard, cmap = mpl.cm.gray, interpolation = "nearest")
ax.axis("off")
plt.show()