vo.py 文件源码

def feature_tracking(image_ref, image_cur, px_ref):
    """Feature Tracking

    Parameters
    ----------
    image_ref : np.array
        Reference image
    image_cur : np.array
        Current image
    px_ref :
        Reference pixels

    Returns
    -------
    (kp1, kp2) : (list of Keypoints, list of Keypoints)

    """
    # Setup
    win_size = (21, 21)
    max_level = 3
    criteria = (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 30, 0.01)

    # Perform LK-tracking
    lk_params = {"winSize": win_size,
                 "maxLevel": max_level,
                 "criteria": criteria}
    kp2, st, err = cv2.calcOpticalFlowPyrLK(image_ref,
                                            image_cur,
                                            px_ref,
                                            None,
                                            **lk_params)

    # Post-process
    st = st.reshape(st.shape[0])
    kp1 = px_ref[st == 1]
    kp2 = kp2[st == 1]

    return kp1, kp2