vo.py 文件源码

def calc_pose(self, frame):
        """

        Parameters
        ----------
        frame :


        Returns
        -------

        """
        px_ref, px_cur = feature_tracking(self.last_frame, frame, self.px_ref)
        E, mask = cv2.findEssentialMat(px_cur,
                                       px_ref,
                                       focal=self.focal,
                                       pp=self.pp,
                                       method=cv2.RANSAC,
                                       prob=0.999,
                                       threshold=1.0)
        _, R, t, mask = cv2.recoverPose(E,
                                        px_cur,
                                        px_ref,
                                        focal=self.focal,
                                        pp=self.pp)
        return R, t, px_ref, px_cur