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)