SELECTING_NEW_IMG = 0

SELECTING_REGION_POINT_1 = 1

SELECTING_REGION_POINT_2 = 2

def __init__(self,descriptor):

self.detector = cv2.FeatureDetector_create(descriptor)

self.extractor = cv2.DescriptorExtractor_create(descriptor)

self.matcher = cv2.BFMatcher()

self.new_img = None

self.new_region = None

self.hist_last_detection = None

self.kp_last_detection = None

self.new_descriptors = None

rospy.Subscriber("scan", LaserScan, self.scan_received, queue_size=1)

self.pub = rospy.Publisher('cmd_vel',Twist,queue_size=1)

self.new_keypoints = None

self.magnitude = None

self.xpos = None

self.corner_threshold = 0.0

self.ratio_threshold = 1.0

self.state = ShoeStalker.SELECTING_NEW_IMG

try:

#for image capture

self.camera_listener = rospy.Subscriber("camera/image_raw", Image, self.capture)

self.bridge = CvBridge()

#make image something useful

except AttributeError:

print "ERROR!"

pass

def histcompare(self,im1,im2):

"""

Compares two images and rates their similarity by color

Author: Jasper

Args:

im1 (object) : cropped image selected by user

im2 (object) : real-time image from robot

Returns:

float: a color similarity metric between two histograms of images

"""

hist1 = cv2.calcHist([im1],[0],mask=None,histSize=[256],ranges=[0,255])

hist2 = cv2.calcHist([im2],[0],mask=None,histSize=[256],ranges=[0,255])

cv2.normalize(hist1,hist1,0,255,cv2.NORM_MINMAX)

cv2.normalize(hist2,hist2,0,255,cv2.NORM_MINMAX)

return cv2.compareHist(hist1,hist2,cv.CV_COMP_CORREL)

def track(self,im):

im_hsv = cv2.cvtColor(im,cv2.COLOR_BGR2HSV)

track_im = cv2.calcBackProject([im_hsv],[0],self.new_hist,[0,255],1)

track_im_visualize = track_im.copy()

# convert to (x,y,w,h)

track_roi = (self.hist_last_detection[0],self.hist_last_detection[1],self.hist_last_detection[2]-self.hist_last_detection[0],self.hist_last_detection[3]-self.hist_last_detection[1])

# Setup the termination criteria, either 10 iteration or move by atleast 1 pt

# this is done to plot intermediate results of mean shift

for max_iter in range(1,10):

term_crit = ( cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, max_iter, 1 )

(ret, intermediate_roi) = cv2.meanShift(track_im,track_roi,term_crit)

cv2.rectangle(track_im_visualize,(intermediate_roi[0],intermediate_roi[1]),(intermediate_roi[0]+intermediate_roi[2],intermediate_roi[1]+intermediate_roi[3]),max_iter/10.0,2)

self.hist_last_detection = [intermediate_roi[0],intermediate_roi[1],intermediate_roi[0]+intermediate_roi[2],intermediate_roi[1]+intermediate_roi[3]]

cv2.imshow("histogram",track_im_visualize)

def capture(self,msg):

# IMAGE FROM NEATO

#useful link for image types http://wiki.ros.org/cv_bridge/Tutorials/ConvertingBetweenROSImagesAndOpenCVImagesPython

cv_Shoeimage = self.bridge.imgmsg_to_cv2(msg, "bgr8")

#Shoeimage = np.asanyarray(cv_Shoeimage)

self.new_shoeimg = cv_Shoeimage

if self.new_shoeimg.shape[0] == 480:

self.image_stream = True

else:

self.image_stream = False

#cv2.imshow("ShoeImage", cv_Shoeimage)

#print "image"

# # set up the ROI for tracking

# region = self.new_img[self.new_region[1]:self.new_region[3],self.new_region[0]:self.new_region[2],:]

# hsv_region = cv2.cvtColor(region, cv2.COLOR_BGR2HSV)

def get_new_histogram(self):

# set up the ROI for tracking

region = self.new_img[self.new_region[1]:self.new_region[3],self.new_region[0]:self.new_region[2],:]

hsv_region = cv2.cvtColor(region, cv2.COLOR_BGR2HSV)

# play with the number of histogram bins by changing histSize

self.new_hist = cv2.calcHist([hsv_region],[0],mask=None,histSize=[256],ranges=[0,255])

cv2.normalize(self.new_hist,self.new_hist,0,255,cv2.NORM_MINMAX)

def set_ratio_threshold(self,thresh):

self.ratio_threshold = thresh

def set_corner_threshold(self,thresh):

self.corner_threshold = thresh

def get_new_keypoints(self):

# #makes new image black and white

new_img_bw = cv2.cvtColor(self.new_img,cv2.COLOR_BGR2GRAY)

#detect keypoints

keyp = self.detector.detect(new_img_bw)

#compare keypoints

keyp = [pt

for pt in keyp if (pt.response > self.corner_threshold and

self.new_region[0] <= pt.pt[0] < self.new_region[2] and

self.new_region[1] <= pt.pt[1] < self.new_region[3])]

dc, describe = self.extractor.compute(new_img_bw,keyp)

#remap keypoints so relative to new region

for pt in keyp:

pt.pt = (pt.pt[0] - self.new_region[0],pt.pt[1] - self.new_region[1])

#reassign keypoints and descriptors

self.new_keypoints = keyp

self.new_descriptors = describe

def detecting(self,im):

#print 'detecting'

#Pauls Code - went through it and changed it to fit ours. will probably need further alterations

img_bw = cv2.cvtColor(im,cv2.COLOR_BGR2GRAY)

training_keypoints = self.detector.detect(img_bw)

desc, training_descriptors = self.extractor.compute(img_bw,training_keypoints)

#finds the k best matches for each descriptor from a query set. (http://docs.opencv.org/modules/features2d/doc/common_interfaces_of_descriptor_matchers.html)

matches = self.matcher.knnMatch(self.new_descriptors, training_descriptors, k=2)

good_matches = []

for m,n in matches:

#makes sure distance to closest match is sufficiently better than to 2nd closest

if (m.distance < self.ratio_threshold*n.distance and

training_keypoints[m.trainIdx].response > self.corner_threshold):

#print 'finding matches'

good_matches.append((m.queryIdx, m.trainIdx))

self.matching_new_pts = np.zeros((len(good_matches),2))

self.matching_training_pts = np.zeros((len(good_matches),2))

track_im = np.zeros(img_bw.shape)

for idx in range(len(good_matches)):

match = good_matches[idx]

self.matching_new_pts[idx,:] = self.new_keypoints[match[0]].pt

self.matching_training_pts[idx,:] = training_keypoints[match[1]].pt

track_im[training_keypoints[match[1]].pt[1], training_keypoints[match[1]].pt[0]] = 1.0

track_im_visualize = track_im.copy()

#converting to (x,y,z,h)\

track_region = (self.kp_last_detection[0],self.kp_last_detection[1],self.kp_last_detection[2]-self.kp_last_detection[0],self.kp_last_detection[3]-self.kp_last_detection[1])

#setup criterial for termination, either 10 iteritation or move at least 1 pt

#done to plot intermediate results of mean shift

for max_iter in range(1,10):

term_crit = ( cv2.TERM_CRITERIA_EPS | cv2.TERM_CRITERIA_COUNT, max_iter, 1 )

(ret, intermediate_region) = cv2.meanShift(track_im,track_region,term_crit)

cv2.rectangle(track_im_visualize,(intermediate_region[0],intermediate_region[1]),(intermediate_region[0]+intermediate_region[2],intermediate_region[1]+intermediate_region[3]),max_iter/10.0,2)

self.kp_last_detection = [intermediate_region[0],intermediate_region[1],intermediate_region[0]+intermediate_region[2],intermediate_region[1]+intermediate_region[3]]

cv2.imshow("track_win", track_im_visualize)

#compare image of the shoe to shoe database (color histogram/SIFT technique) (this may be very time-consuming)

#pick shoe by image of shoe with the most keypoints

#return location of shoes (I think it might be easier to use one location of a shoe)

#xpos = 0

#distance = 0

def approach_shoe(self,msg):

# making the robot stop if it gets within a meter of the shoe (the thing directly in front of it)

#print "approach_shoe"

liner = 0

angular = 0

for degree in range():

if msg.ranges[degree] > 340.0 and msg.ranges[degree] < 20.0:

#get position of laser points

data_x = self.odom_pose[0] + msg.ranges[degree]*math.cos(degree*math.pi/180.0 + self.odom_pose[2])

data_y = self.odom_pose[1] + msg.ranges[degree]*math.sin(degree*math.pi/180 + self.odom_pose[2])

self.magnitude[degree] = math.sqrt(data_x**2 + data_y**2)

pub.publish(Twist(linear=Vector3(x=linear),angular=Vector3(z=angular)))

def stalk(self,hist,kp):

"""

The arbitrater of the robot's movements

Move robots so shoe is in center of image

Author: Jasper

Args:

hist (float): a similarity metric between two color histograms of an original image and real-time image

kp (float): the ratio between keypoints in bounding box over keypoints in real-time image

Functions:

Send movement commands based on keypoint and histogram values

"""

if kp > .5:

self.xpos = (((self.kp_last_detection[2]- self.kp_last_detection[0])/2)+self.kp_last_detection[0])

elif hist > .9:

self.xpos = (((self.hist_last_detection[2]- self.hist_last_detection[0])/2)+self.hist_last_detection[0])

else:

self.lostshoe()

if self.xpos == None:

linear = 0

angular = 0

elif self.xpos > 155:

linear = .05

angular = self.xpos * -.002

elif self.xpos < 145:

linear = .05

angular = (140-self.xpos) * .002

elif self.xpos <= 155 and self.xpos >= 145:

linear = .1

angular = 0

self.pub.publish(Twist(linear=Vector3(x=linear),angular=Vector3(z=angular)))

def lostshoe(self):

"""

Refinds a lost shoe, turn towards location of last shoe.

written by Jasper"""

linear = 0

angular = .1

self.pub.publish(Twist(linear=Vector3(x=linear),angular=Vector3(z=angular)))

# def run(self): #perhaps should move to if __name__ == '__main__', but this is how it is in the fixed image code

# print 'run'

# capture = cv2.VideoCapture(0)

# ret, frame = capture.read()

# cv2.namedWindow("ShoeImage")

# cv2.imshow("ShoeImage",frame)

# cv2.setMouseCallback("ShoeImage", mouse_event)

# def publisher(self):

# rospy.init_node('ShoeStalker', anonymous = True )

# pub=rospy.Publisher('cmd_vel',Twist,queue_size=10)

# pub.publish('a')

# rospy.spin()

def mouse_event(self,event,x,y,flag,im):

"""Logic for creating bounding box around image"""

if event == cv2.EVENT_FLAG_LBUTTON:

if self.state == self.SELECTING_NEW_IMG:

#first mouse click

self.new_img_visualize = frame.copy()

self.new_img = frame

self.new_region = None

self.state = self.SELECTING_REGION_POINT_1

elif self.state == self.SELECTING_REGION_POINT_1:

#second mouse click to create bounding box

self.new_region = [x,y,-1,-1]

cv2.circle(self.new_img_visualize,(x,y),5,(255,0,0),5)

self.state = self.SELECTING_REGION_POINT_2

else:

#get new keypoints

self.new_region[2:] = [x,y]

self.kp_last_detection = self.new_region

self.hist_last_detection = self.new_region

cv2.circle(self.new_img_visualize,(x,y),5,(255,0,0),5)

self.state = self.SELECTING_NEW_IMG

self.get_new_keypoints()

self.get_new_histogram()

def set_corner_threshold_callback(self, thresh):

""" Sets the threshold to consider an interest point a corner. The higher the value

the more the point must look like a corner to be considered """

self.set_corner_threshold(thresh/1000.0)

def set_ratio_threshold_callback(self, ratio):

""" Sets the ratio of the nearest to the second nearest neighbor to consider the match a good one """

self.set_ratio_threshold(ratio/100.0)

def set_corner_threshold_callback(self, thresh):

""" Sets the threshold to consider an interest point a corner. The higher the value

the more the point must look like a corner to be considered """

self.set_corner_threshold(thresh/1000.0)

def set_ratio_threshold_callback(self, ratio):

""" Sets the ratio of the nearest to the second nearest neighbor to consider the match a good one """

self.set_ratio_threshold(ratio/100.0)

def is_in_bounding_box(self, x,y,w,h,kp):

if kp[0] > x and kp[0] < w and kp[1] > y and kp[1] < h:

return True

else: