interframe_camera_estimator.py 文件源码

def manually_calculate_pose(self, f_mat):
        # get essential matrix from the fundamental
        # I am assuming that only one calibration matrix is fine here, because
        # only one type of camera is being used.
        e_mat = self.k_mat.T * f_mat * self.k_mat

        singular_values, u_mat, vt = cv2.SVDecomp(e_mat)
        # reconstruction from SVD:
        # np.dot(u_mat, np.dot(np.diag(singular_values.T[0]), vt))

        u_mat = np.matrix(u_mat)
        vt = np.matrix(vt)

        # from Epipolar Geometry and the Fundamental Matrix 9.13
        w_mat = np.matrix([
            [0, -1, 0],
            [1, 0, 0],
            [0, 0, 1]
        ], np.float32)

        R_mat = u_mat * w_mat * vt  # HZ 9.19
        t_mat = u_mat[:, 2]  # get third column of u

        # check rotation matrix for validity
        if np.linalg.det(R_mat) - 1.0 > 1e-07:
            print('{}\nDoes not appear to be a valid rotation matrix'.format(
                R_mat
            ))

        camera_matrix = np.column_stack((R_mat, t_mat))

        return camera_matrix, R_mat, t_mat