def get_pose(self, transform):
if transform == []:
pose = []
else:
R = transform[:3, :3]
euler = np.rad2deg(U.R_to_euler(R))
pose = euler[0]
return pose # block angle
def get_poses(self, transform):
pose = []
for n, T in enumerate(transform):
if T == []:
pose.append([n, 0.0, []])
else:
R = T[:3, :3]
euler = np.rad2deg(U.R_to_euler(R))
pose.append([n, euler[2], T])
return pose # [nb_block, euler angle, T]
def plot_trans_rot_vectors(Rs, Ts):
Eulers = np.array([U.R_to_euler(R) for R in Rs])
Ts = np.array(Ts)
fig = plt.figure()
ax = fig.add_subplot(211, projection='3d') # translation
ax.scatter(Ts[:, 0], Ts[:, 1], Ts[:, 2])
ax.set_xlabel('X Label (m)')
ax.set_ylabel('Y Label (m)')
ax.set_zlabel('Z Label (m)')
ax2 = fig.add_subplot(212, projection='3d') # rotation
Eulers_deg = np.rad2deg(Eulers) # (deg)
ax2.scatter(Eulers_deg[:, 0], Eulers_deg[:, 1], Eulers_deg[:, 2])
ax2.set_xlabel('X Label (deg)')
ax2.set_ylabel('Y Label (deg)')
ax2.set_zlabel('Z Label (deg)')
plt.show()
def remove_outliers(Rs, Ts):
Eulers = np.array([U.R_to_euler(R) for R in Rs])
Ts = np.array(Ts)
# for translation
clustering = DBSCAN(eps=0.3, min_samples=2).fit(Ts)
index = clustering.labels_
# for rotation
clustering2 = DBSCAN(eps=0.7, min_samples=2).fit(Eulers)
index2 = clustering2.labels_
# cross sectional index
index_cross = index + index2
Ts = Ts[index_cross == 0]
T = np.average(Ts, axis=0)
Euler = Eulers[index_cross == 0]
Euler = np.average(Euler, axis=0)
R = U.euler_to_R(Euler)
return R, T
#######################################
## Rectification
#######################################
img_shape = (1280, 960)
R1, R2, P1, P2, Q, roi1, roi2 = cv2.stereoRectify(K[0], D[0], K[1], D[1],
img_shape, R, T)
mapx1, mapy1 = cv2.initUndistortRectifyMap(K[0], D[0], R1, P1, img_shape,
cv2.CV_32F)
mapx2, mapy2 = cv2.initUndistortRectifyMap(K[1], D[1], R2, P2, img_shape,
cv2.CV_32F)
# print results
print("Stereo calibration results")
print("R=", R)
Euler = U.R_to_euler(R)
print("Euler=", np.rad2deg(Euler), "(deg)")
print("T=", np.array(T), "(m)")
print("E=", E)
print("F=", F)
print("")
print("Rectify matrices")
print("R1=", R1)
print("R2=", R2)
print("P1=", P1)
print("P2=", P2)
print("Q=", Q)
print("mapx1=", mapx1)
print("mapy1=", mapy1)
print("mapx2=", mapx2)
print("mapy2=", mapy2)