optical_flow_matcher.py 文件源码

def __init__(self, image_topic, feature_detector='FAST'):
        super(OpticalFlowMatcher, self).__init__()

        rospy.init_node('optical_flow_matcher')

        self.cv_bridge = CvBridge()

        self.rectified_image_topic = rospy.Subscriber(
            image_topic,
            Image,
            self.new_image_callback
        )

        self.pub_keypoint_motion = rospy.Publisher(
            'keypoint_motion',
            KeypointMotion,
            queue_size=10
        )

        self.feature_params = None

        if feature_detector == 'FAST':
            self.get_features = self.get_features_fast
            # Initiate FAST detector with default values
            self.fast = cv2.FastFeatureDetector_create()

            self.fast.setThreshold(20)

        elif feature_detector == 'GOOD':
            self.get_features = self.get_features_good
            # params for ShiTomasi 'GOOD' corner detection
            self.feature_params = dict(
                maxCorners=200,
                qualityLevel=0.3,
                minDistance=7,
                blockSize=7
            )
        else:
            raise Exception(
                '{} feature detector not implemented'.format(feature_detector)
            )

        # Parameters for lucas kanade optical flow
        self.lk_params = dict(
            winSize=(15, 15),
            maxLevel=2,
            criteria=(
                cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT,
                10,
                0.03
            )
        )

        self.last_frame_gray = None
        self.good_old = None
        self.good_new = None