def main_calculateCameraPose(WD, pose_amount, polar_ang, CAMERA_POSE_PATH):
# ax = frame3D.make_3D_fig(axSize=200)
World = frame3D.Frame(np.matlib.identity(4))
Camera = frame3D.Frame(World.pose)
orientation = frame3D.Orientation()
thetas = np.linspace(0, 360, pose_amount,
endpoint=False) # azimuthal angle
X0 = (np.radians(45), np.radians(45), np.radians(45)) # initial guess
camOrientations = np.zeros((pose_amount, 6))
if polar_ang == 0:
for ind, theta in enumerate(thetas):
t = theta + 60
camOrientations[ind, :] = (0.0, 0.0, WD, np.pi, 0.0, np.radians(t))
else:
for ind, theta in enumerate(thetas):
orientation_2 = get_photo_orientation_by_given_shoot_angle(X0,\
World.pose, WD, np.radians(polar_ang), np.radians(theta))
X0 = orientation_2[3:6]
cmd = orientation.asItself(orientation_2).cmd()
Camera.transform(cmd, refFrame=World.pose)
camOrientations[ind, :] = Camera.get_orientation_from_pose(
Camera.pose, refFrame=World.pose)
return camOrientations
def main(DEBUG):
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
BASE = path['pegInHole'] if DEBUG else path['ROOT']
XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']
HS_BASE = path['holeSearching'] if DEBUG else path['ROOT']
TCP_BASE = path['TCPCalibration'] if DEBUG else path['ROOT']
PIH_goal_PATH = BASE + 'goal/pih_goal.yaml'
Y_PATH = TCP_BASE + 'goal/Y.yaml'
X_PATH = XZ_BASE + 'goal/HX.yaml'
W_PATH = HS_BASE + 'goal/HW.yaml'
with open(W_PATH) as f:
W = np.matrix(yaml.load(f))
with open(Y_PATH) as f:
Y = np.matrix(yaml.load(f))
with open(X_PATH) as f:
X = np.matrix(yaml.load(f))
# holeID from most precision 1 to 9
Whole = convertToHole(W, holeID=6)
WDs = [0.04, 0.02, 0.01, 0.0, -0.01, -0.015, -0.02]
wayposes = np.zeros((len(WDs), 7))
for i, WD in enumerate(WDs):
C = np.matrix([[0.0, 1.0, 0.0, 0.0], [1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, -1.0, WD], [0.0, 0.0, 0.0, 1.0]])
B = Whole * np.linalg.inv(C) * np.linalg.inv(Y)
pih_goal = as_ROSgoal(B)
wayposes[i] = pih_goal
with open(PIH_goal_PATH, 'w') as f:
yaml.dump(wayposes.tolist(), f, default_flow_style=False)
# Data visualization
World = frame3D.Frame(np.matlib.identity(4))
Base = frame3D.Frame(World.pose)
Workpiece = frame3D.Frame(Base.pose)
Flange = frame3D.Frame(Base.pose)
Tool = frame3D.Frame(Base.pose)
Camera = frame3D.Frame(Base.pose)
orientation = frame3D.Orientation()
ax = frame3D.make_3D_fig(axSize=0.45)
Flange.transform_by_rotation_mat(B, refFrame=Base.pose)
Tool.transform_by_rotation_mat(Y, refFrame=Flange.pose)
Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
Workpiece.transform_by_rotation_mat(Whole, refFrame=Base.pose)
Base.plot_frame(ax, 'Base')
Flange.plot_frame(ax, 'Flange')
Tool.plot_frame(ax, 'Tool')
Camera.plot_frame(ax, 'Camera')
Workpiece.plot_frame(ax, 'Workpiece ')
def main(DEBUG):
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
BASE = path['TCPCalibration'] if DEBUG else path['ROOT']
HS_BASE = path['holeSearching'] if DEBUG else path['ROOT']
XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']
Y_PATH = BASE + 'goal/Y.yaml'
Y = TCPCalibration()
with open(Y_PATH, 'w') as f:
yaml.dump(Y.tolist(), f, default_flow_style=False)
# Data visualization
X_PATH = XZ_BASE + 'goal/HX.yaml'
Bc_PATH = HS_BASE + 'goal/Bc.yaml'
WHole_PATH = HS_BASE + 'goal/Whole.yaml'
with open(Bc_PATH) as f:
Bc = np.array(yaml.load(f))
with open(WHole_PATH) as f:
Wh = np.array(yaml.load(f))
with open(X_PATH) as f:
X = np.array(yaml.load(f))
World = frame3D.Frame(np.matlib.identity(4))
Base = frame3D.Frame(World.pose)
Workpiece = frame3D.Frame(Base.pose)
Flange = frame3D.Frame(Base.pose)
Tool = frame3D.Frame(Base.pose)
Camera = frame3D.Frame(Base.pose)
orientation = frame3D.Orientation()
ax = frame3D.make_3D_fig(axSize=0.45)
Flange.transform_by_rotation_mat(Bc, refFrame=Base.pose)
Tool.transform_by_rotation_mat(Y, refFrame=Flange.pose)
Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
Workpiece.transform_by_rotation_mat(Wh, refFrame=Base.pose)
Base.plot_frame(ax, 'Base')
Flange.plot_frame(ax, 'Flange')
Tool.plot_frame(ax, 'Tool')
Camera.plot_frame(ax, 'Camera')
Workpiece.plot_frame(ax, 'Workpiece ')
plt.show()
def main_denso(DEBUG):
# PATH SETTING
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
BASE = path['APose'] if DEBUG else path['ROOT']
AF_BASE = path['AFocus'] if DEBUG else path['ROOT']
BF_GOAL = AF_BASE + 'goal/bf_goal.yaml'
CAMERA_POSE_PATH = BASE + 'goal/camera_pose.yaml'
ROS_GOAL_PATH = BASE + 'goal/ap_goal.yaml'
Bs_PATH = BASE + 'goal/Bs.yaml'
X_PATH = BASE + 'goal/X.yaml'
Z_PATH = BASE + 'goal/Z.yaml'
Z2_PATH = BASE + 'goal/Z2.yaml'
with open(BF_GOAL) as f:
bf_goal = np.array(yaml.load(f))
cam_pose = [-0.040, 0, 0.040, 0, 0,
-90] # pose wrt Flange in meter: (x,y,z,Rx,Ry,Rz)
WD = bf_goal[2] - cam_pose[2] # work distence from camera to pattern(m)
# First layer of poses
phi = 0 # polarangle 6
camOrien1 = main_calculateCameraPose(WD, 6, phi, CAMERA_POSE_PATH)
# Second layer of poses
phi = 10 # polarangle
camOrien2 = main_calculateCameraPose(WD, 12, phi, CAMERA_POSE_PATH)
# Third layer of poses
phi = 15 # polarangle
camOrien3 = main_calculateCameraPose(WD, 6, phi, CAMERA_POSE_PATH)
camOrientations = np.concatenate((camOrien1, camOrien2, camOrien3), axis=0)
# data visualization
World = frame3D.Frame(np.matlib.identity(4))
Pattern = frame3D.Frame(World.pose)
Image = frame3D.Frame(Pattern.pose)
Camera = frame3D.Frame(Pattern.pose)
Flange = frame3D.Frame(Pattern.pose)
Base = frame3D.Frame(Pattern.pose)
Flange_test = frame3D.Frame(Pattern.pose)
orientation = frame3D.Orientation()
ax = frame3D.make_3D_fig(axSize=0.45)
##################
# Determine Z
##################
# Base as World coordinate
Base.plot_frame(ax, 'Base')
# Base to Flange
B0 = as_MatrixGoal(bf_goal)
Flange.transform_by_rotation_mat(B0, refFrame=Base.pose)
Flange.plot_frame(ax, 'initFlange')
# Flange to Camera
cmd_X = orientation.asRad(cam_pose).cmd()
X = Camera.transform(cmd_X, refFrame=World.pose)
Camera.transform(cmd_X, refFrame=Flange.pose)
Camera.plot_frame(ax, 'initCamera')
# Camera to Pattern
cmd_A0 = orientation.asRad((0, 0, WD, 0, 180, 0)).cmd()
A0 = Pattern.transform(cmd_A0, refFrame=World.pose)
Pattern.transform(cmd_A0, refFrame=Camera.pose)
Pattern.plot_frame(ax, 'Pattern')
# Base to Pattern
Z = B0 * X * A0
##################
# Pose calculation
##################
pose_amount = len(camOrientations)
iAs = np.zeros((pose_amount, 4, 4))
Bs = np.zeros((pose_amount, 4, 4))
iX = np.linalg.inv(X)
for i, camOrientation in enumerate(camOrientations):
cmd_iA = orientation.asItself(camOrientation).cmd()
iAs[i] = Camera.transform(cmd_iA, refFrame=World.pose)
Camera.transform(cmd_iA, refFrame=Pattern.pose)
Camera.plot_frame(ax, '')
Flange.transform_by_rotation_mat(iX, refFrame=Camera.pose)
# Flange.plot_frame(ax, '')
# {Test Flange calculate from Z*A*X}
Bs[i] = Z * iAs[i] * iX
plt.show()
# plt.show(block=False)
# plt.pause(0.5)
# plt.close()
with open(Bs_PATH, 'w') as f:
yaml.dump(Bs.tolist(), f, default_flow_style=False)
print('Save the Bs to yaml data file.')
with open(X_PATH, 'w') as f:
yaml.dump(X.tolist(), f, default_flow_style=False)
print('Save the X to yaml data file.')
with open(Z_PATH, 'w') as f:
yaml.dump(Z.tolist(), f, default_flow_style=False)
print('Save the Z to yaml data file.')
#################
# Save as moveit command
#################
goals = as_ROSgoal(Bs)
with open(ROS_GOAL_PATH, 'w') as f:
yaml.dump(goals.tolist(), f, default_flow_style=False)
print('Save the ROS goal to yaml data file.')
def main(DEBUG):
# # PATH SETTING
# CONFIG = 'config.yaml'
# with open(CONFIG) as f:
# path = yaml.load(f)
# BASE = path['APose'] if DEBUG else path['ROOT']
# CAMERA_POSE_PATH = BASE + 'goal/camera_pose.yaml'
# X_PATH = BASE + 'goal/X.yaml'
# Z_PATH = BASE + 'goal/Z.yaml'
# Z2_PATH = BASE + 'goal/Z2.yaml'
# PE_BASE = path['PoseEstimation'] if DEBUG else path['ROOT']
# IMAGE_PATH = BASE + 'img/hs*.bmp'
# CAMERA_MAT = PE_BASE + 'goal/camera_mat.yaml'
# D_MAT = BASE + 'goal/D.yaml'
# Bc_PATH = BASE + 'goal/Bc.yaml'
# cam_pose = [-0.040, 0, 0.040, 0, 0, -90] # pose wrt Flange in meter: (x,y,z,Rx,Ry,Rz)
# ----------------------------
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
BASE = path['holeSearching'] if DEBUG else path['ROOT']
AF_BASE = path['AFocus'] if DEBUG else path['ROOT']
XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']
BF_GOAL = AF_BASE + 'goal/bf_goal.yaml'
HX_PATH = XZ_BASE + 'goal/HX.yaml'
Init_Hole_GOAL = BASE + 'goal/init_hole.yaml'
HW_PATH = BASE + 'goal/HW.yaml'
# ROS_GOAL_PATH = BASE + 'goal/hs_goal.yaml'
with open(BF_GOAL) as f:
bf_goal = np.array(yaml.load(f))
with open(HX_PATH) as f:
X = np.matrix(yaml.load(f))
with open(HW_PATH) as f:
W = np.matrix(yaml.load(f))
with open(Init_Hole_GOAL) as f:
ihs = np.array(yaml.load(f)) # ROS goal: (x, y, z, qx, qy,qz, qw)
Bs = np.zeros((len(ihs), 4, 4))
for i, ih in enumerate(ihs):
q = np.quaternion(ih[6], ih[3], ih[4], ih[5])
B = np.identity(4)
B[0:3, 0:3] = quaternion.as_rotation_matrix(q)
B[0:3, 3] = ih[0:3]
Bs[i] = B
WD = bf_goal[2] - X[2, 3]# work distence from camera to pattern(m)
phi = 5 # polarangle
camOrientations = main_calculateCameraPose(WD, 3, phi)
# data visualization
World = frame3D.Frame(np.matlib.identity(4))
Workpiece = frame3D.Frame(World.pose)
Camera = frame3D.Frame(Workpiece.pose)
Flange = frame3D.Frame(Workpiece.pose)
Base = frame3D.Frame(Workpiece.pose)
orientation = frame3D.Orientation()
ax = frame3D.make_3D_fig(axSize=0.45)
##################
# Pose calculation
##################
pose_amount = len(camOrientations)
iDs = np.zeros((pose_amount,4,4))
Bs = np.zeros((pose_amount,4,4))
iX = np.linalg.inv(X)
Workpiece.transform_by_rotation_mat(W, refFrame=Base.pose)
Workpiece.plot_frame(ax, 'Workpiece')
Base.plot_frame(ax, 'Base')
for i, camOrientation in enumerate(camOrientations):
cmd_iA = orientation.asItself(camOrientation).cmd()
iDs[i] = Camera.transform(cmd_iA, refFrame=World.pose)
Camera.transform(cmd_iA, refFrame=Workpiece.pose)
Camera.plot_frame(ax, '')
Flange.transform_by_rotation_mat(iX, refFrame=Camera.pose)
# Flange.plot_frame(ax, '')
# {Test Flange calculate from Z*A*X}
Bs[i] = W*iDs[i]*iX
plt.show()
# plt.show(block=False)
# plt.pause(0.5)
# plt.close()
#################
# Save as moveit command
#################
goals = as_ROSgoal(Bs)
with open(Init_Hole_GOAL, 'w') as f:
yaml.dump(goals.tolist(), f, default_flow_style=False)
def main(DEBUG):
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
BASE = path['holeSearching'] if DEBUG else path['ROOT']
PE_BASE = path['PoseEstimation'] if DEBUG else path['ROOT']
XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']
Init_Hole_GOAL = BASE + 'goal/init_hole.yaml'
IMAGE_PATH = BASE + 'img/hs0*.bmp'
CAMERA_MAT = PE_BASE + 'goal/camera_mat.yaml'
HX_PATH = XZ_BASE + 'goal/HX.yaml'
D_MAT = BASE + 'goal/D.yaml'
HW_MAT = BASE + 'goal/HW.yaml'
Bc_PATH = BASE + 'goal/Bc.yaml'
HS_GOAL_PATH = BASE + 'goal/hs_goal.yaml'
HS_RESULT = BASE + 'goal/hs_result.yaml'
squareLength = 0.01 # Here, our measurement unit is m.
markerLength = 0.25/40.0 # Here, our measurement unit is m.
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250 )
arucoParams = cv2.aruco.DetectorParameters_create()
with open(CAMERA_MAT) as f:
cameraMatrix = np.array(yaml.load(f))
distCoeffs = np.array([[0.0, 0.0, 0.0, 0.0, 0.0]]) # Assume to zero
rvecs = []
tvecs = []
images = sorted(glob.glob(IMAGE_PATH))
for ind, fname in enumerate(images):
frame = cv2.imread(fname)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(gray, aruco_dict, parameters=arucoParams)
if len(corners)>0:
diamondCorners, diamondIds = cv2.aruco.detectCharucoDiamond(gray, corners, ids, squareLength/markerLength)
if len(diamondCorners) >= 1:
img_with_diamond = cv2.aruco.drawDetectedDiamonds(frame, diamondCorners, diamondIds, (0,255,0))
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(diamondCorners, squareLength, cameraMatrix, distCoeffs)
img_with_diamond = cv2.aruco.drawAxis(img_with_diamond, cameraMatrix, distCoeffs, rvec, tvec, 1) # axis length 100 can be changed according to your requirement
rvecs.append(rvec)
tvecs.append(tvec)
else:
img_with_diamond = gray
# cv2.namedWindow('diamond', cv2.WINDOW_NORMAL)
# cv2.resizeWindow('diamond', 1200, 800)
# cv2.imshow("diamond", img_with_diamond) # display
# cv2.waitKey(30)
# cv2.destroyAllWindows()
Ds = as_homogeneous_mats(rvecs, tvecs) # from Camera to Object
with open(D_MAT, 'w') as f:
yaml.dump(Ds.tolist(), f, default_flow_style=False)
#################
# Calculate workpiece
#################
with open(HX_PATH) as f:
X = np.matrix(yaml.load(f))
with open(Init_Hole_GOAL) as f:
ihs = np.array(yaml.load(f)) # ROS goal: (x, y, z, qx, qy,qz, qw)
Bs = np.zeros((len(ihs), 4, 4))
for i, ih in enumerate(ihs):
q = np.quaternion(ih[6], ih[3], ih[4], ih[5])
B = np.identity(4)
B[0:3, 0:3] = quaternion.as_rotation_matrix(q)
B[0:3, 3] = ih[0:3]
Bs[i] = B
Ws = getWbyMatrixProduct(Bs, X, Ds)
rf, W = solveWbylstsq(Bs, X, Ds)
(x, y) = (-0.015, 0.015)
# (x, y) = (0, 0)
# (x, y) = (-0.0149+0.0725, 0.0153-0.050)
Worigin = convertToOrigin(W, x, y)
with open(HW_MAT, 'w') as f:
yaml.dump(Worigin.tolist(), f, default_flow_style=False)
World = frame3D.Frame(np.matlib.identity(4))
Base = frame3D.Frame(World.pose)
Flange = frame3D.Frame(Base.pose)
Camera = frame3D.Frame(Base.pose)
Workpiece = frame3D.Frame(Base.pose)
orientation = frame3D.Orientation()
ax = frame3D.make_3D_fig(axSize=0.45)
# {Base as World}
poseID = 0
Base.plot_frame(ax, 'Base')
Flange.transform_by_rotation_mat(Bs[poseID], refFrame=Base.pose)
Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
Workpiece.transform_by_rotation_mat(Ds[poseID], refFrame=Camera.pose)
Base.plot_frame(ax, 'Base')
Camera.plot_frame(ax, 'Camera')
Flange.plot_frame(ax, 'Flange')
Workpiece.plot_frame(ax, 'Workpiece')
SIM = False
if SIM:
W_ideal = np.array([[-1.0, 0.0, 0.0, -0.06],
[0.0, -1.0, 0.0, -0.30],
[0.0, 0.0, 1.0, 0.01],
[0.0, 0.0, 0.0, 1.0]])
pError, oError = calPositionAndOrientationError(W, W_ideal)
print(pError*1000, oError)
for w in Ws:
pError, oError = calPositionAndOrientationError(w, W_ideal)
print(pError*1000, oError)
else:
W_assumed = np.identity(4)
_, oError = calPositionAndOrientationError(W, W_assumed)
print(oError)
for w in Ws:
_, oError = calPositionAndOrientationError(w, W_assumed)
print(oError)
hs_res = np.array([np.copy(W)])
hs_res[0,3,3] = rf
with open(HS_RESULT, 'a') as f:
yaml.safe_dump(hs_res.tolist(), f, default_flow_style=False)
# yaml.safe_dump()
# Centering command validation
WD = 0.32692938211698336
cmd_Dc = orientation.asRad((0.0, 0.0, WD, 0.0, 180, 0.0)).cmd()
Dc = Workpiece.transform(cmd_Dc, refFrame=World.pose)
Bc = Worigin*np.linalg.inv(Dc)*np.linalg.inv(X)
goal = as_ROSgoal(Bc)
with open(HS_GOAL_PATH, 'w') as f:
yaml.dump(goal.tolist(), f, default_flow_style=False)
def main(DEBUG):
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
BASE = path['holeSearching'] if DEBUG else path['ROOT']
PE_BASE = path['PoseEstimation'] if DEBUG else path['ROOT']
XZ_BASE = path['solveXZ'] if DEBUG else path['ROOT']
Init_Hole_GOAL = BASE + 'goal/init_hole.yaml'
IMAGE_PATH = BASE + 'img/hs*.bmp'
CAMERA_MAT = PE_BASE + 'goal/camera_mat.yaml'
HX_PATH = XZ_BASE + 'goal/HX.yaml'
D_MAT = BASE + 'goal/D.yaml'
HW_MAT = BASE + 'goal/HW.yaml'
Bc_PATH = BASE + 'goal/Bc.yaml'
squareLength = 0.01 # Here, our measurement unit is m.
markerLength = 0.25 / 40.0 # Here, our measurement unit is m.
aruco_dict = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_6X6_250)
arucoParams = cv2.aruco.DetectorParameters_create()
with open(CAMERA_MAT) as f:
cameraMatrix = np.array(yaml.load(f))
distCoeffs = np.array([[0.0, 0.0, 0.0, 0.0, 0.0]]) # Assume to zero
rvecs = []
tvecs = []
images = sorted(glob.glob(IMAGE_PATH))
for ind, fname in enumerate(images):
frame = cv2.imread(fname)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
corners, ids, rejectedImgPoints = cv2.aruco.detectMarkers(
gray, aruco_dict, parameters=arucoParams)
if len(corners) > 0:
diamondCorners, diamondIds = cv2.aruco.detectCharucoDiamond(
gray, corners, ids, squareLength / markerLength)
if len(diamondCorners) >= 1:
img_with_diamond = cv2.aruco.drawDetectedDiamonds(
frame, diamondCorners, diamondIds, (0, 255, 0))
rvec, tvec, _ = cv2.aruco.estimatePoseSingleMarkers(
diamondCorners, squareLength, cameraMatrix, distCoeffs)
img_with_diamond = cv2.aruco.drawAxis(
img_with_diamond, cameraMatrix, distCoeffs, rvec, tvec, 1
) # axis length 100 can be changed according to your requirement
rvecs.append(rvec)
tvecs.append(tvec)
else:
img_with_diamond = gray
cv2.namedWindow('diamond', cv2.WINDOW_NORMAL)
cv2.resizeWindow('diamond', 1200, 800)
cv2.imshow("diamond", img_with_diamond) # display
cv2.waitKey(10)
cv2.destroyAllWindows()
Ds = as_homogeneous_mats(rvecs, tvecs) # from Camera to Object
with open(D_MAT, 'w') as f:
yaml.dump(Ds.tolist(), f, default_flow_style=False)
#################
# Calculate workpiece
#################
with open(HX_PATH) as f:
X = np.matrix(yaml.load(f))
with open(Init_Hole_GOAL) as f:
ihs = np.array(yaml.load(f)) # ROS goal: (x, y, z, qx, qy,qz, qw)
Bs = np.zeros((len(ihs), 4, 4))
for i, ih in enumerate(ihs):
q = np.quaternion(ih[6], ih[3], ih[4], ih[5])
B = np.identity(4)
B[0:3, 0:3] = quaternion.as_rotation_matrix(q)
B[0:3, 3] = ih[0:3]
Bs[i] = B
Ws = getWbyMatrixProduct(Bs, X, Ds)
W = solveWbylstsq(Bs, X, Ds)
(x, y) = (-0.0149, 0.0153)
# (x, y) = (0, 0)
Worigin = convertToOrigin(W, x, y)
print('Worigin')
print(Worigin)
with open(HW_MAT, 'w') as f:
yaml.dump(Worigin.tolist(), f, default_flow_style=False)
World = frame3D.Frame(np.matlib.identity(4))
Base = frame3D.Frame(World.pose)
Flange = frame3D.Frame(Base.pose)
Camera = frame3D.Frame(Base.pose)
Workpiece = frame3D.Frame(Base.pose)
Workorigin = frame3D.Frame(Base.pose)
orientation = frame3D.Orientation()
ax = frame3D.make_3D_fig(axSize=0.45)
def main(DEBUG, output_pattern_img=True):
# PATH SETTING
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
BASE = path['PoseEstimation'] if DEBUG else path['ROOT']
AC_BASE = path['ACenter'] if DEBUG else path['ROOT']
AP_BASE = path['APose'] if DEBUG else path['ROOT']
IMAGE_PATH = AP_BASE + 'img/ap*.bmp'
INTMAT_GUESS = AC_BASE + 'goal/camera_mat.yaml'
INTMAT = BASE + 'goal/camera_mat.yaml'
EXTMAT = BASE + 'goal/As.yaml'
IDEAL_A = AP_BASE + 'goal/A_desired.yaml'
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_100)
board = cv2.aruco.CharucoBoard_create(11, 9, 0.010, 0.005, dictionary)
#Dump the calibration board to a file
if output_pattern_img:
img = board.draw((100 * 11, 100 * 9))
cv2.imwrite(BASE + 'img/charuco.png', img)
with open(INTMAT_GUESS) as f:
cameraMatrix_guess = np.array(yaml.load(f))
distCoeffs = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
allCHCors = []
allCHIds = []
images = sorted(glob.glob(IMAGE_PATH))
for ind, fname in enumerate(images):
frame = cv2.imread(fname)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ARcors, ARids, _ = cv2.aruco.detectMarkers(gray, dictionary)
if len(ARcors) > 0:
ret, CHcors, CHids = cv2.aruco.interpolateCornersCharuco(
ARcors, ARids, gray, board)
if CHcors is not None and CHids is not None and len(CHcors) > 3:
allCHCors.append(CHcors)
allCHIds.append(CHids)
cv2.aruco.drawDetectedMarkers(frame, ARcors, ARids)
# cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
# cv2.resizeWindow('frame', 1200, 800)
# cv2.imshow('frame', frame)
# cv2.waitKey(30)
cv2.destroyAllWindows()
imsize = gray.shape
retval, cameraMatrix, distCoeffs, rvecs, tvecs = cv2.aruco.calibrateCameraCharuco(
allCHCors, allCHIds, board, imsize, None, None)
tot_error = 0
tot_points = 0
for i, iCHIds in enumerate(allCHIds):
objpoints = board.chessboardCorners[iCHIds.flatten(), :]
imgpoints2, _ = cv2.projectPoints(objpoints, rvecs[i], tvecs[i],
cameraMatrix, distCoeffs)
# tot_error += cv2.norm(allCHCors[i],imgpoints2, cv2.NORM_L2)/len(imgpoints2)
tot_error += np.sum(np.abs(allCHCors[i] - imgpoints2)**2)
tot_points += len(allCHCors[i])
if i > 18:
plt.scatter(allCHCors[i][:, 0, 0],
allCHCors[i][:, 0, 1],
color='red')
plt.scatter(imgpoints2[:, 0, 0], imgpoints2[:, 0, 1], color='blue')
# plt.draw()
# plt.pause(0.3)
# plt.clf()
# plt.show()
mean_error = np.sqrt(tot_error / tot_points)
print("Mean reprojection error: {0}".format(mean_error))
# print("Final reprojection error opencv: {0}".format(retval))
# ext_mat: Extrinsic matrix from Pattern to Camera
As = as_homogeneous_mat(rvecs, tvecs)
with open(EXTMAT, 'w') as f:
yaml.dump(As.tolist(), f, default_flow_style=False)
with open(INTMAT, 'w') as f:
yaml.dump(cameraMatrix.tolist(), f, default_flow_style=False)
# SIM = True
# if SIM:
# with open(IDEAL_A) as f:
# As_ideal = np.array(yaml.load(f))
# for (A, A_ideal) in zip(As, As_ideal):
# pError, oError = calPositionAndOrientationError(A, A_ideal)
# print(pError)
# Data visualization
X_PATH = AP_BASE + 'goal/X.yaml'
B_PATH = AP_BASE + 'goal/Bs.yaml'
Z_PATH = AP_BASE + 'goal/Z.yaml'
with open(X_PATH) as f:
X = np.array(yaml.load(f))
with open(B_PATH) as f:
Bs = np.array(yaml.load(f))
with open(Z_PATH) as f:
Z = np.array(yaml.load(f))
World = frame3D.Frame(np.matlib.identity(4))
Pattern = frame3D.Frame(World.pose)
Image = frame3D.Frame(Pattern.pose)
Camera = frame3D.Frame(Pattern.pose)
Flange = frame3D.Frame(Pattern.pose)
Base = frame3D.Frame(Pattern.pose)
Flange_test = frame3D.Frame(Pattern.pose)
orientation = frame3D.Orientation()
ax = frame3D.make_3D_fig(axSize=0.45)
##################
# Determine Z
##################
Base.plot_frame(ax, 'Base')
# cmd_Z1 = orientation.asRad((0.045, 0.055, 0, 0, 0, 0)).cmd()
# cmd_Z2 = orientation.asRad((0.31, 0.05, 0.001, 0.0, 0.0, -60)).cmd()
# cmd_iZ1 = orientation.asRad((-0.045, -0.055, 0, 0, 0, 0)).cmd()
# cmd_Z3 = orientation.asRad((0.2398686, 0.06147114, 0.001, 0.0, 0.0, -60)).cmd()
cmd_Z2 = orientation.asRad((0.3, 0.0, 0.001, 0.0, 0.0, -90.0)).cmd()
cmd_Z3 = orientation.asRad((0.255, 0.055, 0.002, 0.0, 0.0, -90.0)).cmd()
Pattern.transform(cmd_Z2, refFrame=Base.pose)
# Image.transform(cmd_iZ1, refFrame=Pattern.pose)
Image.transform(cmd_Z3, refFrame=Base.pose)
for i, (A, B) in enumerate(zip(As, Bs)):
Flange.transform_by_rotation_mat(B, refFrame=Base.pose)
Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
# Image.transform_by_rotation_mat(A, refFrame=Camera.pose)
# Pattern.transform(cmd_Z1, refFrame=Image.pose)
Flange.plot_frame(ax, '')
Camera.plot_frame(ax, '')
Image.plot_frame(ax, 'Image')
Pattern.plot_frame(ax, 'Pattern')
def main(DEBUG, output_pattern_img=True):
# PATH SETTING
CONFIG = 'config.yaml'
with open(CONFIG) as f:
path = yaml.load(f)
BASE = path['PoseEstimation'] if DEBUG else path['ROOT']
AC_BASE = path['ACenter'] if DEBUG else path['ROOT']
AP_BASE = path['APose'] if DEBUG else path['ROOT']
IMAGE_PATH = AP_BASE + 'img/ap*.bmp'
INTMAT = AC_BASE + 'goal/camera_mat.yaml'
INTMAT_CP = BASE + 'goal/camera_mat.yaml'
EXTMAT = BASE + 'goal/As.yaml'
dictionary = cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_100)
board = cv2.aruco.CharucoBoard_create(11, 9, 0.010, 0.005, dictionary)
#Dump the calibration board to a file
if output_pattern_img:
img = board.draw((100 * 11, 100 * 9))
cv2.imwrite(BASE + 'charuco.png', img)
# Camera parameter
with open(INTMAT) as f:
cameraMatrix = np.array(yaml.load(f))
distCoeffs = np.array([0.0, 0.0, 0.0, 0.0, 0.0])
rvecs = []
tvecs = []
allCHCors = []
allCHIds = []
# CHimgpoints = [] # 2d points in image plane.
# ARimgpoints = [] # 2d points in image plane.
images = sorted(glob.glob(IMAGE_PATH))
for ind, fname in enumerate(images):
frame = cv2.imread(fname)
gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)
ARcors, ARids, _ = cv2.aruco.detectMarkers(gray, dictionary)
if len(ARcors) > 0:
ret, CHcors, CHids = cv2.aruco.interpolateCornersCharuco(
ARcors, ARids, gray, board)
retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(
CHcors, CHids, board, cameraMatrix, distCoeffs)
if retval and CHcors is not None and CHids is not None and len(
CHcors) > 3:
rvecs.append(rvec)
tvecs.append(tvec)
allCHCors.append(CHcors)
allCHIds.append(CHids)
cv2.aruco.drawDetectedMarkers(frame, ARcors, ARids)
cv2.namedWindow('frame', cv2.WINDOW_NORMAL)
cv2.resizeWindow('frame', 1200, 800)
cv2.imshow('frame', frame)
cv2.waitKey(10)
tot_error = 0
tot_points = 0
for i, iCHIds in enumerate(allCHIds):
objpoints = board.chessboardCorners[iCHIds.flatten(), :]
imgpoints2, _ = cv2.projectPoints(objpoints, rvecs[i], tvecs[i],
cameraMatrix, distCoeffs)
tot_error += np.sum(np.abs(allCHCors[i] - imgpoints2)**2)
tot_points += len(allCHCors[i])
# plt.scatter(allCHCors[i][:, 0, 0], allCHCors[i][:, 0, 1], color='red')
# plt.scatter(imgpoints2[:, 0, 0], imgpoints2[:, 0, 1], color='blue')
# plt.show()
# plt.show(block=False)
# plt.pause(0.5)
# plt.close()
mean_error = np.sqrt(tot_error / tot_points)
print("Mean reprojection error: {0}".format(mean_error))
cv2.destroyAllWindows()
imsize = gray.shape
# ext_mat: Extrinsic matrix from Pattern to Camera
As = as_homogeneous_mat(rvecs, tvecs)
with open(EXTMAT, 'w') as f:
yaml.dump(As.tolist(), f, default_flow_style=False)
print('Save the Extrinsic matrix to yaml data file.')
with open(INTMAT_CP, 'w') as f:
yaml.dump(cameraMatrix.tolist(), f, default_flow_style=False)
print('Save the Intrinsic matrix to yaml data file.')
# Data visualization
X_PATH = AP_BASE + 'goal/X.yaml'
B_PATH = AP_BASE + 'goal/Bs.yaml'
Z_PATH = AP_BASE + 'goal/Z.yaml'
with open(X_PATH) as f:
X = np.array(yaml.load(f))
with open(B_PATH) as f:
Bs = np.array(yaml.load(f))
with open(Z_PATH) as f:
Z = np.array(yaml.load(f))
World = frame3D.Frame(np.matlib.identity(4))
Pattern = frame3D.Frame(World.pose)
Image = frame3D.Frame(Pattern.pose)
Camera = frame3D.Frame(Pattern.pose)
Flange = frame3D.Frame(Pattern.pose)
Base = frame3D.Frame(Pattern.pose)
Flange_test = frame3D.Frame(Pattern.pose)
orientation = frame3D.Orientation()
ax = frame3D.make_3D_fig(axSize=0.45)
##################
# Determine Z
##################
Base.plot_frame(ax, 'Base')
cmd_Z1 = orientation.asRad((0.045, 0.055, 0, 0, 0, 0)).cmd()
for i, (A, B) in enumerate(zip(As, Bs)):
Flange.transform_by_rotation_mat(B, refFrame=Base.pose)
Camera.transform_by_rotation_mat(X, refFrame=Flange.pose)
Image.transform_by_rotation_mat(A, refFrame=Camera.pose)
Pattern.transform(cmd_Z1, refFrame=Image.pose)
if i < 12:
# Flange.plot_frame(ax, '')
Camera.plot_frame(ax, '')
Image.plot_frame(ax, 'Image')
Pattern.plot_frame(ax, 'Pattern')