optical_flow_matcher.py 文件源码

def publish_interframe_motion(self, last_features, new_features, status, err):
        self.good_old = last_features[(status == 1) & (err < 12.0)]
        self.good_new = new_features[(status == 1) & (err < 12.0)]

        # TODO: clean up these features before publishing

        self.pub_keypoint_motion.publish(
            header=Header(
                stamp=rospy.Time.now(),  # TODO: use camera image time
                frame_id='tango_camera_2d'
            ),
            prev_x=self.good_old[:, 0],
            prev_y=self.good_old[:, 1],
            cur_x=self.good_new[:, 0],
            cur_y=self.good_new[:, 1]
        )