def generate_poses(n: int, noise_list: list):
# groud truth hand eye calibration matrix and chessboard pose
hec = HT.from_matrix(
np.array([[1, 0, 0, 50], [0, 1, 0, 0], [0, 0, 1, 100], [0, 0, 0, 1]]))
cb_pose = HT.from_matrix(
np.array([[0, -1, 0, 200], [1, 0, 0, 70], [0, 0, 1, 0], [0, 0, 0, 1]]))
Ai = []
Bi = []
nx = 20
ny = 13
obj_pts = np.zeros((nx * ny, 4))
obj_pts[:, 3] = 1
# obj_pts[:, :2] = np.mgrid[0:nx, 0:ny].T.reshape(-1, 2)*20.0
obj_pts[:, :2] = np.mgrid[0:nx, 0:ny].T.reshape(-1, 2) * 20.0
obj_pts[:, :3] = obj_pts[:, :3] - np.mean(obj_pts[:, :3], axis=0)
Ry_180 = HT(np.pi, np.array([0, 1, 0]))
# chessboard_pts = cb_pose @ obj_pts.T
for i in range(n):
a_rvec = np.pi * np.random.randn(3)
a_tvec = 300 * np.sqrt(3) * np.random.randn(3)
a_tvec[2] = np.abs(a_tvec[2])
A_i = HT.from_vecs(a_rvec, a_tvec)
if A_i.matrix[2, 2] > 0:
A_i = A_i @ Ry_180
Ai.append(A_i)
# temp_Bi = (Ai[i] @ hec).inv() @ cb_pose
# b_rvec = temp_Bi.rvec()
# b_tvec = temp_Bi.t# + noise * np.random.randn(3)
# Bi.append(HT.from_vecs(b_rvec, b_tvec))
Bi.append((Ai[i] @ hec).inv() @ cb_pose)
bpts = []
bplanes = []
for noise in noise_list:
board_points = []
board_poses_plane = []
for i in range(n):
point_noise = np.zeros((4, nx * ny))
point_noise[:3, :] = noise * np.random.randn(3, nx * ny)
chessboard_pts = cb_pose @ (obj_pts + point_noise.T).T
board_points.append(((
(Ai[i] @ hec).inv() @ chessboard_pts).T)) # + point_noise).T)
board_poses_plane.append(plane_fit(board_points[i], nx, ny)[0])
bpts.append(board_points)
bplanes.append(board_poses_plane)
return Ai, Bi, bpts, bplanes
def calculate_chessboard_poses_2D(self):
'''
Calculates the poses of the calibration objects in self.point_clouds using
opencv camera calibration, and strores them in the list self.chessboard_poses.
If the intrinsic parameters of the camera are known,
and set with the method self.setIntrinsics(...) cv2.solvePnP(...) is used, else
cv2.CalibrateCamera(...) is used to calibrate the intrinsic and extrinsic camera
parameters.
'''
assert (len(self.point_clouds) > 0), 'No calibration images loaded'
print('Calculating chessboard poses')
self.method = '2D'
img_pts, obj_pts, self.XYZs, self.corners, self.rgbs, img_size = calibration_pts(
self.nx, self.ny, self.square_size, self.point_clouds)
if self.camera_matrix is not None and self.dist_coeffs is not None:
print('solvepnp')
for i in range(len(self.point_clouds)):
_, rvec, tvec = \
cv2.solvePnP(obj_pts[i].astype('float32'), img_pts[i].astype(
'float32'), self.camera_matrix, self.dist_coeffs)
self.rvecs.append(rvec)
self.tvecs.append(tvec)
else:
_, self.camera_matrix, self.dist_coeffs, self.rvecs, self.tvecs = \
cv2.calibrateCamera(obj_pts.astype(
'float32'), img_pts.astype('float32'), img_size, None, None)
self.chessboard_poses = [
HT.from_vecs(r, t) for (r, t) in zip(self.rvecs, self.tvecs)
]
def load_quat_poses(path: str):
files = sorted(glob.glob(f'{path}/*.txt'))
poses = [
HT.from_quat_pose(
np.fromfile(pose_file, dtype=float, count=-1, sep=' '))
for pose_file in files
]
return files, poses
def load_t4s(path: str):
files = sorted(glob.glob(f'{path}/*.t4'))
poses = [
HT.from_matrix(
np.fromfile(pose_file, dtype=float, count=-1,
sep=' ').reshape(4, 4)) for pose_file in files
]
return files, poses
def pnt_cld_transf(pnt_cld1: np.ndarray, pnt_cld2: np.ndarray):
c1 = np.mean(pnt_cld1, axis=0)
c2 = np.mean(pnt_cld2, axis=0)
U, Z, V = np.linalg.svd((pnt_cld2 - c2).T @ (pnt_cld1 - c1))
S = np.diag([1, 1, np.linalg.det(V.T @ U.T)])
R = V.T @ S @ U.T
t = c1.T - R @ c2.T
T = np.zeros((4, 4))
T[:3, :3] = R
T[:3, 3] = t
return HT.from_matrix(T)
def calib_err_2D(hec, rob_poses: list, board_poses: list, eye_in_hand=True):
'''Uses the variation in the constant transformation between
- robot base frame and object points for eye-in-hand calibration
- end-effector frame and object points for eye-in-base calibration
to calculate an estimate for the accuracy of the calibration.
'''
if eye_in_hand:
T_bo = np.array([
(rob_pose @ hec @ board_pose).matrix
for rob_pose, board_pose in zip(rob_poses, board_poses)
])
else:
T_bo = np.array([
(rob_pose.inv() @ hec @ board_pose).matrix
for rob_pose, board_pose in zip(rob_poses, board_poses)
])
mean_r = np.mean(np.array([HT.from_matrix(T).rvec() for T in T_bo]),
axis=0)
mean_theta = np.mean(
np.array([HT.from_matrix(T).angle_axis()[0] for T in T_bo]))
mean_t = np.mean(T_bo[:, :3, 3], axis=0)
mean_board_pose = HT.from_vecs(mean_r, mean_t)
trans_err = np.zeros((len(rob_poses), 3))
rot_err = np.zeros((len(rob_poses)))
theta_err = np.zeros((len(rob_poses)))
for i in range(len(rob_poses)):
Terr = HT.from_matrix(mean_board_pose.inv() @ T_bo[i])
trans_err[i, :3] = Terr.t
rot_err[i] = np.rad2deg(Terr.angle_axis()[0])
theta_err[i] = np.rad2deg(
np.abs(HT.from_matrix(T_bo[i]).angle_axis()[0] - mean_theta))
return (np.mean(np.linalg.norm(trans_err,
axis=1)), np.mean(np.abs(theta_err)))
def plane_fit(pnt_cld: np.ndarray, nx: int, ny: int):
U, Z, V = np.linalg.svd(pnt_cld)
P = V.T[:, 3]
if P[3] < 0:
P = -P
x_ctrl = (pnt_cld[nx-2]-pnt_cld[0]) / \
np.linalg.norm(pnt_cld[nx-2]-pnt_cld[0])
# best fit x:
X = pnt_cld # np.array(pnt_cld[:nx-1])
_, _, v = np.linalg.svd(X[:, :3] - np.mean(X[:, :3], axis=0))
x = v[0] / np.linalg.norm(v[0])
# ensure x is pointing in the correct direction
if (np.isclose(x, -x_ctrl[0:3], rtol=0.1).any()):
x = -x
# z-axis normal to chessboard plane, and unit vector
z = P[0:3] / np.linalg.norm(P[0:3])
# z = -v[2]/np.linalg.norm(v[2])
# if z[2] > 0:
# z = -z
# ensure x is orthogonal to z
x = x - ((x @ z) / np.linalg.norm(z)**2) * z
x = x / np.linalg.norm(x)
# y is orthogonal to x and z, xyz is now an orthogonal basis in R3
y = np.cross(z, x)
# centroid selected as origin
t = np.mean(pnt_cld, axis=0)
# if(t[2]<0):
# x=-x
# z=-z
T = np.zeros((4, 4))
T[:3, 0] = x[0:3]
T[:3, 1] = y[0:3]
T[:3, 2] = z[0:3]
T[:, 3] = t
T = HT.from_matrix(T)
return T, P
def park_martin(A: list, B: list):
'''Calculates the transformation between the robot end-effector frame and the camera frame.
Using lists containing pairs of relative motions of camera and robot.
Method: AX = XB in the style of Park and Martin.
Parameters: A, B lists of HTransf objects.
Returns: HTransf object representing the transformation
between robot end-effector frame and the camera frame
'''
K_a = np.zeros((3, len(A)))
K_b = np.zeros((3, len(B)))
for i in range(len(A)):
angle_a, axis_a = A[i].angle_axis()
angle_b, axis_b = B[i].angle_axis()
K_a[:, i] = (angle_a * axis_a).flatten()
K_b[:, i] = (angle_b * axis_b).flatten()
# calcualte the optimal least squares solution for the
# rotation matrix between camrea frame and end-effector frame
H = K_b @ K_a.T
U, Z, V = np.linalg.svd(H)
# umeyama correction, to correct for reflection matrices
S = np.diag([1, 1, np.linalg.det(V.T @ U.T)])
R_optimal = HT.from_matrix(V.T @ S @ U.T)
# calculate the best fit translation (dependent on the rotation)
C = np.zeros((3 * len(A), 3))
d = np.zeros((3 * len(B), 1))
for i in range(0, len(A) * 3, 3):
C[i:i + 3, :] = A[int(i / 3)].rot_matrix - np.identity(3)
d[i:i+3, 0] = R_optimal @ B[int(i/3)].get_translation() - \
A[int(i/3)].get_translation()
C[i:i + 3, :] = np.identity(3) - A[int(i / 3)].rot_matrix
d[i:i+3, 0] = A[int(i/3)].get_translation() - \
R_optimal @ B[int(i/3)].get_translation()
t_optimal = np.linalg.lstsq(C, d, rcond=None)[0]
a, k = R_optimal.angle_axis()
return HT(a, k, t_optimal)
f1 = plt.figure('Translation')
plt.plot(range(3, self.num_imgs + 1), trans_errs)
f2 = plt.figure('Rotation')
plt.plot(range(3, self.num_imgs + 1), rot_errs)
plt.show(block=False)
print(hec)
print(trans_err, rot_err)
return trans_errs, rot_errs
if __name__ == '__main__':
'''example taken from You Cheung Shiu et al.
"calibration of wrist-mounted robotic sensors by solving
homogeneous transformation equations of the form AX=XB"
'''
X_act = HT(a=0.2, k=np.array([1, 0, 0]), t=np.array([10, 50, 100]))
A1 = HT(a=3.0, k=np.array([0, 0, 1]), t=np.array([0, 0, 0]))
A2 = HT(a=1.5, k=np.array([0, 1, 0]), t=np.array([-400, 0, 400]))
B1 = X_act.inv() @ A1 @ X_act
B2 = X_act.inv() @ A2 @ X_act
# print(A1, A2, B1, B2)
A11 = np.array([[-0.989992, -0.141120, 0.0, 0.0],
[0.141120, -0.989992, 0.0, 0.0], [0.0, 0.0, 1.0, 0.0],
[0.0, 0.0, 0.0, 1]])
B11 = np.array([[-0.989992, -0.138307, 0.028036, -26.9559],
[0.138307, -0.911449, 0.387470, -96.1332],
[-0.028036, 0.387470, 0.921456, 19.4872],
[0.0, 0.0, 0.0, 1]])
A22 = np.array([[0.070737, 0.0, 0.997495, -400.0], [0.0, 1.0, 0.0, 0.0],
[-0.997495, 0.0, 0.070737, 400.0], [0.0, 0.0, 0.0, 1]])
B22 = np.array([[0.070737, 0.198172, 0.997612, -309.543],