roslaunch openni_launch openni.launch

roslaunch rbx1_vision good_features.launch

#!/usr/bin/env python""" lk_tracker.py - Version 1.1 2013-12-20
    Based on the OpenCV lk_track.py demo codeCreated for the Pi Robot Project: http://www.pirobot.orgCopyright (c) 2011 Patrick Goebel.  All rights reserved.This program is free software; you can redistribute it and/or modifyit under the terms of the GNU General Public License as published bythe Free Software Foundation; either version 2 of the License, or(at your option) any later version.This program is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without even the implied warranty ofMERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See theGNU General Public License for more details at:http://www.gnu.org/licenses/gpl.html
"""

import rospy
import cv2
import cv2.cv as cv
import numpy as np
from rbx1_vision.good_features import GoodFeaturesclass LKTracker(GoodFeatures):def __init__(self, node_name):super(LKTracker, self).__init__(node_name)self.show_text = rospy.get_param("~show_text", True)self.feature_size = rospy.get_param("~feature_size", 1)# LK parametersself.lk_winSize = rospy.get_param("~lk_winSize", (10, 10))self.lk_maxLevel = rospy.get_param("~lk_maxLevel", 2)self.lk_criteria = rospy.get_param("~lk_criteria", (cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, 20, 0.01))self.lk_params = dict( winSize  = self.lk_winSize, maxLevel = self.lk_maxLevel, criteria = self.lk_criteria)    self.detect_interval = 1self.keypoints = Noneself.detect_box = Noneself.track_box = Noneself.mask = Noneself.grey = Noneself.prev_grey = Nonedef process_image(self, cv_image):try:# If we don't yet have a detection box (drawn by the user
            # with the mouse), keep waitingif self.detect_box is None:return cv_image# Create a greyscale version of the imageself.grey = cv2.cvtColor(cv_image, cv2.COLOR_BGR2GRAY)# Equalize the grey histogram to minimize lighting effectsself.grey = cv2.equalizeHist(self.grey)# If we haven't yet started tracking, set the track box to the
            # detect box and extract the keypoints within itif self.track_box is None or not self.is_rect_nonzero(self.track_box):self.track_box = self.detect_boxself.keypoints = self.get_keypoints(self.grey, self.track_box)else:if self.prev_grey is None:self.prev_grey = self.grey# Now that have keypoints, track them to the next frame# using optical flowself.track_box = self.track_keypoints(self.grey, self.prev_grey)# Process any special keyboard commands for this moduleif self.keystroke != -1:try:cc = chr(self.keystroke & 255).lower()if cc == 'c':# Clear the current keypointsself.keypoints = Noneself.track_box = Noneself.detect_box = Noneexcept:passself.prev_grey = self.greyexcept:passreturn cv_image               def track_keypoints(self, grey, prev_grey):# We are tracking points between the previous frame and the# current frameimg0, img1 = prev_grey, grey# Reshape the current keypoints into a numpy array required# by calcOpticalFlowPyrLK()p0 = np.float32([p for p in self.keypoints]).reshape(-1, 1, 2)# Calculate the optical flow from the previous frame to the current framep1, st, err = cv2.calcOpticalFlowPyrLK(img0, img1, p0, None, **self.lk_params)# Do the reverse calculation: from the current frame to the previous frametry:p0r, st, err = cv2.calcOpticalFlowPyrLK(img1, img0, p1, None, **self.lk_params)# Compute the distance between corresponding points in the two flowsd = abs(p0-p0r).reshape(-1, 2).max(-1)# If the distance between pairs of points is < 1 pixel, set# a value in the "good" array to True, otherwise Falsegood = d < 1# Initialize a list to hold new keypointsnew_keypoints = list()# Cycle through all current and new keypoints and only keep# those that satisfy the "good" condition abovefor (x, y), good_flag in zip(p1.reshape(-1, 2), good):if not good_flag:continuenew_keypoints.append((x, y))# Draw the keypoint on the imagecv2.circle(self.marker_image, (x, y), self.feature_size, (0, 255, 0, 0), cv.CV_FILLED, 8, 0)# Set the global keypoint list to the new list    self.keypoints = new_keypoints# Convert the keypoints list to a numpy arraykeypoints_array = np.float32([p for p in self.keypoints]).reshape(-1, 1, 2)  # If we have enough points, find the best fit ellipse around themif len(self.keypoints) > 6:track_box = cv2.fitEllipse(keypoints_array)else:# Otherwise, find the best fitting rectangletrack_box = cv2.boundingRect(keypoints_array)except:track_box = Nonereturn track_boxif __name__ == '__main__':try:node_name = "lk_tracker"LKTracker(node_name)rospy.spin()except KeyboardInterrupt:print "Shutting down LK Tracking node."cv.DestroyAllWindows()