def detect_and_draw(imgmsg):
global skp, sd
global im_ref
#print 'number of KeyPoint objects skp', len(skp)
br = CvBridge()
temp = br.imgmsg_to_cv(imgmsg, "bgr8")
im_height = temp.height
im_length = temp.width
cv.ShowImage("view", temp)
cv.WaitKey(10)
template = np.asarray(temp)
tkp = detector.detect(template)
tkp, td = descriptor.compute(template, tkp)
#print 'number of KeyPoint objects tkp', len(tkp)
#print 'number of KeyPoint objects skp', len(skp)
flann_params = dict(algorithm=1, trees=4)
flann = cv2.flann_Index(sd, flann_params)
idx, dist = flann.knnSearch(td, 1, params={})
del flann
dist = dist[:, 0] / 2500.0
dist = dist.reshape(-1, ).tolist()
idx = idx.reshape(-1).tolist()
indices = range(len(dist))
indices.sort(key=lambda i: dist[i])
dist = [dist[i] for i in indices]
idx = [idx[i] for i in indices]
skp_final = []
for i, dis in itertools.izip(idx, dist):
if dis < threshold:
skp_final.append(skp[i])
else:
break
tkp_final = []
for i, dis in itertools.izip(range(len(idx)), dist):
if dis < threshold:
tkp_final.append(tkp[indices[i]])
else:
break
h1, w1 = im_ref.shape[:2]
h2, w2 = template.shape[:2]
nWidth = w1 + w2
nHeight = max(h1, h2)
hdif = (h1 - h2) / 2
newimg = np.zeros((nHeight, nWidth, 3), np.uint8)
newimg[hdif:hdif + h2, :w2] = template
newimg[:h1, w2:w1 + w2] = im_ref
tkp_final
skp_final
#print 'number of KeyPoint objects in skp_final', len(skp_final)
#print 'number of KeyPoint objects in tkp_final', len(tkp_final)
for i in range(min(len(tkp), len(skp_final))):
pt_a = (int(tkp_final[i].pt[0]), int(tkp_final[i].pt[1] + hdif))
pt_b = (int(skp_final[i].pt[0] + w2), int(skp_final[i].pt[1]))
cv2.circle(newimg, pt_a, int(tkp_final[i].size), (160, 32, 240), 1)
cv2.circle(newimg, pt_b, int(skp_final[i].size), (160, 32, 240), 1)
cv2.line(newimg, pt_a, pt_b, (255, 0, 0))
cv.ShowImage("sift", cv.fromarray(newimg))
kp_array = sift_keypoints_array()
kp_array.skp = [sift_keypoint(*k.pt) for k in skp_final]
kp_array.tkp = [sift_keypoint(*k.pt) for k in tkp_final]
pk_pub.publish(kp_array)
key = cv.WaitKey(10) & 0xFF
if key == ord('d'):
im_ref = template
skp = tkp
sd = td
def detect_and_draw(self, imgmsg):
self.computationalTime = rospy.Time.now()
#print 'number of KeyPoint objects skp', len(self.skp)
br = CvBridge()
temp = br.imgmsg_to_cv(imgmsg, "bgr8")
im_height = temp.height
im_length = temp.width
cv.ShowImage("view",temp)
cv.WaitKey(10)
template = np.asarray(temp)
tkp = self.detector.detect(template)
tkp,td = self.descriptor.compute(template, tkp)
if (self.init == 1):
self.im_ref = template
self.skp = tkp
self.sd = td
self.init = 0
command_cam = Axis()
command_cam.pan = 0
command_cam.tilt = 0
command_cam.zoom = 1
self.command_cam_pub.publish(command_cam);
#print 'number of KeyPoint objects tkp', len(tkp)
#print 'number of KeyPoint objects skp', len(self.skp)
flann_params = dict(algorithm=1, trees=4)
flann = cv2.flann_Index(self.sd, flann_params)
idx, dist = flann.knnSearch(td, 1, params={})
del flann
dist = dist[:,0]/2500.0
dist = dist.reshape(-1,).tolist()
idx = idx.reshape(-1).tolist()
indices = range(len(dist))
indices.sort(key=lambda i: dist[i])
dist = [dist[i] for i in indices]
idx = [idx[i] for i in indices]
h1, w1 = self.im_ref.shape[:2]
h2, w2 = template.shape[:2]
skp_final = []
for i, dis in itertools.izip(idx, dist):
if dis < self.threshold and self.skp[i].pt[1]*1.0 < 5*h1/6.0:
skp_final.append(self.skp[i])
else:
break
tkp_final = []
for i, dis in itertools.izip(range(len(idx)), dist):
if dis < self.threshold and self.skp[idx[i]].pt[1]*1.0 < 4*h1/6.0:
tkp_final.append(tkp[indices[i]])
else:
break
nWidth = w1+w2
nHeight = max(h1, h2)
hdif = (h1-h2)/2
newimg = np.zeros((nHeight, nWidth, 3), np.uint8)
newimg[hdif:hdif+h2, :w2] = template
newimg[:h1, w2:w1+w2] = self.im_ref
tkp_final
skp_final
#print 'number of KeyPoint objects in skp_final', len(skp_final)
#print 'number of KeyPoint objects in tkp_final', len(tkp_final)
for i in range(min(len(tkp_final), len(skp_final))):
pt_a = (int(tkp_final[i].pt[0]), int(tkp_final[i].pt[1]+hdif))
pt_b = (int(skp_final[i].pt[0]+w2), int(skp_final[i].pt[1]))
cv2.circle(newimg, pt_a, int(tkp_final[i].size),(160,32,240),1)
cv2.circle(newimg, pt_b, int(skp_final[i].size),(160,32,240),1)
cv2.line(newimg, pt_a, pt_b, (255, 0, 0))
cv.ShowImage("sift",cv.fromarray(newimg))
kp_array = sift_keypoints_array()
#kp_array.header = imgmsg.header
kp_array.header.frame_id = imgmsg.header.frame_id
kp_array.header.stamp = rospy.Time(0) #Time().now()
kp_array.skp = [sift_keypoint(*k.pt) for k in skp_final]
kp_array.tkp = [sift_keypoint(*k.pt) for k in tkp_final]
self.kp_pub.publish(kp_array)
self.computationalTime = rospy.Time.now() - self.computationalTime
self.Time_pub.publish(self.computationalTime)
key=cv.WaitKey(10) & 0xFF
if key == ord('d'):
self.im_ref = template
self.skp = tkp
self.sd = td
def detect_and_draw(imgmsg):
global skp, sd
global im_ref
#print 'number of KeyPoint objects skp', len(skp)
br = CvBridge()
temp = br.imgmsg_to_cv(imgmsg, "bgr8")
im_height = temp.height
im_length = temp.width
cv.ShowImage("view",temp)
cv.WaitKey(10)
template = np.asarray(temp)
tkp = detector.detect(template)
tkp,td = descriptor.compute(template, tkp)
#print 'number of KeyPoint objects tkp', len(tkp)
#print 'number of KeyPoint objects skp', len(skp)
flann_params = dict(algorithm=1, trees=4)
flann = cv2.flann_Index(sd, flann_params)
idx, dist = flann.knnSearch(td, 1, params={})
del flann
dist = dist[:,0]/2500.0
dist = dist.reshape(-1,).tolist()
idx = idx.reshape(-1).tolist()
indices = range(len(dist))
indices.sort(key=lambda i: dist[i])
dist = [dist[i] for i in indices]
idx = [idx[i] for i in indices]
skp_final = []
for i, dis in itertools.izip(idx, dist):
if dis < threshold:
skp_final.append(skp[i])
else:
break
tkp_final = []
for i, dis in itertools.izip(range(len(idx)), dist):
if dis < threshold:
tkp_final.append(tkp[indices[i]])
else:
break
h1, w1 = im_ref.shape[:2]
h2, w2 = template.shape[:2]
nWidth = w1+w2
nHeight = max(h1, h2)
hdif = (h1-h2)/2
newimg = np.zeros((nHeight, nWidth, 3), np.uint8)
newimg[hdif:hdif+h2, :w2] = template
newimg[:h1, w2:w1+w2] = im_ref
tkp_final
skp_final
#print 'number of KeyPoint objects in skp_final', len(skp_final)
#print 'number of KeyPoint objects in tkp_final', len(tkp_final)
for i in range(min(len(tkp), len(skp_final))):
pt_a = (int(tkp_final[i].pt[0]), int(tkp_final[i].pt[1]+hdif))
pt_b = (int(skp_final[i].pt[0]+w2), int(skp_final[i].pt[1]))
cv2.circle(newimg, pt_a, int(tkp_final[i].size),(160,32,240),1)
cv2.circle(newimg, pt_b, int(skp_final[i].size),(160,32,240),1)
cv2.line(newimg, pt_a, pt_b, (255, 0, 0))
cv.ShowImage("sift",cv.fromarray(newimg))
kp_array = sift_keypoints_array()
kp_array.skp = [sift_keypoint(*k.pt) for k in skp_final]
kp_array.tkp = [sift_keypoint(*k.pt) for k in tkp_final]
pk_pub.publish(kp_array)
key=cv.WaitKey(10) & 0xFF
if key == ord('d'):
im_ref = template
skp = tkp
sd = td
