track_features_lk.py
Go to the documentation of this file.
00001 #! /usr/bin/env python
00002 
00003 import roslib
00004 roslib.load_manifest('image_algos')
00005 import rospy
00006 
00007 print "OpenCV Python version of lkdemo"
00008 
00009 import sys, math
00010 
00011 # import the necessary things for OpenCV
00012 import cv
00013 
00014 #############################################################################
00015 # some "constants"
00016 
00017 win_size = 10
00018 MAX_COUNT = 500
00019 
00020 #############################################################################
00021 # some "global" variables
00022 
00023 image = None
00024 pt = None
00025 add_remove_pt = False
00026 flags = 0
00027 night_mode = False
00028 need_to_init = False
00029 
00030 #############################################################################
00031 # the mouse callback
00032 
00033 # the callback on the trackbar
00034 def on_mouse (event, x, y, flags, param):
00035     
00036     # we will use the global pt and add_remove_pt
00037     global pt
00038     global add_remove_pt
00039     
00040     if image is None:
00041         # not initialized, so skip
00042         return
00043     
00044     if image.origin != 0:
00045         # different origin
00046         y = image.height - y
00047         
00048     if event == cv.CV_EVENT_LBUTTONDOWN:
00049         # user has click, so memorize it
00050         pt = (x, y)
00051         add_remove_pt = True
00052 
00053 class FeatureDists:
00054     feature = (0.0, 0.0)
00055     dists = []
00056         
00057 #############################################################################
00058 # so, here is the main part of the program
00059         
00060 if __name__ == '__main__':
00061     
00062     frames = sys.argv[1:]
00063     if frames == []:
00064         print "usage lkdemo.py <image files>"
00065         sys.exit(1)
00066         
00067     # display a small howto use it
00068     print "Hot keys: \n" \
00069         "\tESC - quit the program\n" \
00070         "\tr - auto-initialize tracking\n" \
00071         "\tc - delete all the points\n" \
00072         "\tn - switch the \"night\" mode on/off\n" \
00073         "\tSPACE - next frame\n" \
00074         "To add/remove a feature point click it\n"
00075     
00076     # first, create the necessary windows
00077     cv.NamedWindow ('LkDemo', cv.CV_WINDOW_AUTOSIZE)
00078     # cv.NamedWindow ('grey', cv.CV_WINDOW_AUTOSIZE)
00079     # cv.NamedWindow ('prev_grey', cv.CV_WINDOW_AUTOSIZE)
00080 
00081     # register the mouse callback
00082     cv.SetMouseCallback ('LkDemo', on_mouse, None)
00083     
00084     fc = 0
00085     while 1:
00086         # do forever
00087         
00088         frame = cv.LoadImage(frames[fc])
00089         
00090         if image is None:
00091             # create the images we need
00092             image = cv.CreateImage (cv.GetSize (frame), 8, 3)
00093             image.origin = frame.origin
00094             grey = cv.CreateImage (cv.GetSize (frame), 8, 1)
00095             prev_grey = cv.CreateImage (cv.GetSize (frame), 8, 1)
00096             pyramid = cv.CreateImage (cv.GetSize (frame), 8, 1)
00097             prev_pyramid = cv.CreateImage (cv.GetSize (frame), 8, 1)
00098             features = []
00099             
00100         # copy the frame, so we can draw on it
00101         cv.Copy (frame, image)
00102         
00103         # create a grey version of the image
00104         cv.CvtColor (image, grey, cv.CV_BGR2GRAY)
00105         
00106         if night_mode:
00107             # night mode: only display the points
00108             cv.SetZero (image)
00109             
00110         if need_to_init:
00111             # we want to search all the good points
00112             
00113             # create the wanted images
00114             eig = cv.CreateImage (cv.GetSize (grey), 32, 1)
00115             temp = cv.CreateImage (cv.GetSize (grey), 32, 1)
00116             
00117             # the default parameters
00118             quality = 0.01
00119             min_distance = 10
00120             
00121             # search the good points
00122             features = cv.GoodFeaturesToTrack (
00123                 grey, eig, temp,
00124                 MAX_COUNT,
00125                 quality, min_distance, None, 3, 0, 0.04)
00126             
00127             # refine the corner locations
00128             features = cv.FindCornerSubPix (
00129                 grey,
00130                 features,
00131                 (win_size, win_size),  (-1, -1),
00132                 (cv.CV_TERMCRIT_ITER | cv.CV_TERMCRIT_EPS, 20, 0.03))
00133             
00134         elif features != []:
00135             # we have points, so display them
00136             # cv.ShowImage ('prev_grey', prev_grey)
00137             # cv.ShowImage ('grey', grey)
00138 
00139             # calculate the optical flow
00140             features, status, track_error = cv.CalcOpticalFlowPyrLK (
00141                 prev_grey, grey, prev_pyramid, pyramid,
00142                 features,
00143                 (win_size, win_size), 3,
00144                 (cv.CV_TERMCRIT_ITER|cv.CV_TERMCRIT_EPS, 20, 0.03),
00145                 flags)
00146 #            print "num features: ", sum(status)
00147 #            print "status: ", status
00148 
00149             # set back the points we keep
00150             features = [ p for (st,p) in zip(status, features) if st]
00151             
00152             # count = 0;
00153             # for feat in features:
00154             #     print "feature coordinates: ", feat, count
00155             #     count = count + 1
00156 
00157             if add_remove_pt:
00158                 # we have a point to add, so see if it is close to
00159                 # another one. If yes, don't use it
00160                 def ptptdist(p0, p1):
00161                     dx = p0[0] - p1[0]
00162                     dy = p0[1] - p1[1]
00163                     return dx**2 + dy**2
00164                 if min([ ptptdist(pt, p) for p in features ]) < 25:
00165                     # too close
00166                     add_remove_pt = 0
00167                     
00168             # draw the points as green circles
00169             for the_point in features:
00170                 print  "feature coordinates: ", the_point[0], the_point[1] 
00171                 cv.Circle (image, (int(the_point[0]), int(the_point[1])), 3, (0, 255, 0, 0), -1, 8, 0)
00172 
00173 
00174             count = 0
00175             dist_list = []
00176             for the_point in features:
00177                 for p in features:
00178                     if the_point != p:
00179                         dist = math.sqrt(math.pow(the_point[0] - p[0], 2) + math.pow(the_point[1] - p[1], 2))
00180                         dist_list.append(dist)
00181                 print "new list: ", dist_list
00182                 dist_list = []
00183             # calculate distance to other features
00184             #for the_point in features:
00185                 
00186 
00187         if add_remove_pt:
00188             # we want to add a point
00189             # refine this corner location and append it to 'features'
00190             
00191             features += cv.FindCornerSubPix (
00192                 grey,
00193                 [pt],
00194                 (win_size, win_size),  (-1, -1),
00195                 (cv.CV_TERMCRIT_ITER | cv.CV_TERMCRIT_EPS,
00196                  20, 0.03))
00197             # we are no longer in "add_remove_pt" mode
00198             add_remove_pt = False
00199             
00200         # swapping
00201         prev_grey, grey = grey, prev_grey
00202         prev_pyramid, pyramid = pyramid, prev_pyramid
00203         need_to_init = False
00204         
00205         # we can now display the image
00206         cv.ShowImage ('LkDemo', image)
00207         
00208         # handle events
00209         c = cv.WaitKey (0)
00210         
00211         if c == 27:
00212             # user has press the ESC key, so exit
00213             break
00214         
00215         #print fc, len(frames)
00216         if fc == len(frames)-1:
00217             break
00218 
00219         # processing depending on the character
00220         if 32 <= c and c < 128:
00221             cc = chr(c).lower()
00222             if cc == 'r':
00223                 need_to_init = True
00224             elif cc == 'c':
00225                 features = []
00226             elif cc == 'n':
00227                 night_mode = not night_mode
00228             elif cc == ' ':
00229                 fc = (fc + 1) % len(frames)
00230                 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Properties Friends Defines


image_algos
Author(s): Dejan Pangercic
autogenerated on Thu May 23 2013 18:41:31