def visual_servo(self,kPoints):
#kPoints.skp=[sift_keypoint(0,0), sift_keypoint(0,10), sift_keypoint(10,0)]
#kPoints.tkp=[sift_keypoint(0,0), sift_keypoint(0,10), sift_keypoint(5,0)]
self.computationalTime = rospy.Time.now()
nKP = len(kPoints.tkp)
#print("************ len nkp = "+str(nKP))
Np = 3;
if(nKP > Np):
self.ok_model = 0
nTimes = 0
while self.ok_model == 0 and nTimes < nKP:
nInliers = Np
data_ind = range(nKP)
###############INIT#####################
#if(Np==0):
# Np=1
#print("number of points for init = ", Np)
kp_array = sift_keypoints_array()
kp_array.header = kPoints.header
for p in range(Np):
ind = choice(data_ind)
kp_array.skp.append(kPoints.skp[ind])
kp_array.tkp.append(kPoints.tkp[ind])
data_ind.remove(ind)
#print("data_ind",data_ind)
vel_init = self.cumputeCommand(kp_array)
################END_INIT##########################
while len(data_ind) >= Np:
kp_array_loc = sift_keypoints_array()
kp_array_loc.header = kPoints.header
for p in range(Np):
ind = choice(data_ind)
kp_array_loc.skp.append(kPoints.skp[ind])
kp_array_loc.tkp.append(kPoints.tkp[ind])
data_ind.remove(ind)
#print("data_ind",data_ind)
vel_to_compare = self.cumputeCommand(kp_array_loc)
ransac = Float64()
ransac.data = abs(norm_angle(atan2(vel_to_compare[1,0],vel_to_compare[0,0]) - atan2(vel_init[1,0],vel_init[0,0])))
self.vel_pk.publish(ransac)
if(abs(norm_angle(atan2(vel_to_compare[1,0],vel_to_compare[0,0]) - atan2(vel_init[1,0],vel_init[0,0]))) < 1):
for p in range(Np):
kp_array.skp.append(kp_array_loc.skp[p])
kp_array.tkp.append(kp_array_loc.tkp[p])
nInliers += Np
if nInliers*1.0/len(kPoints.tkp) > 0.7:
self.ok_model = 1
#print("--model ok--")
self.vel = self.cumputeCommand(kp_array)
pourcent = Float64()
pourcent.data = nInliers*1.0/len(kPoints.tkp)
self.vel_pourcent.publish(pourcent)
nTimes += 1
################END_REESTIM##########################
if(self.ok_model == 0):
print("!!!! no model found !!!!")
vel = self.vel
t = Twist()
#vc = [vx vz wy] = [ros.vy ros.vx ros.wz]
# transform the linear velocities from the camera frame to the robot frame
v = PointStamped()
v.header = kPoints.header
v.point.x = vel[1,0]
v.point.y = vel[0,0]
v.point.z = 0
#v.point.x = vel[0,0]
#v.point.y = 0
#v.point.z = vel[1,0]
((x,y,_),rot) = self.listener.lookupTransform(self.robotFrame,self.cameraFrame, rospy.Time(0))
self.listener.waitForTransform(self.cameraFrame, self.robotFrame, kPoints.header.stamp, rospy.Duration(1.0))
v = self.listener.transformPoint(self.robotFrame,v)
v.point.x = v.point.x-x
v.point.y = v.point.y-y
#the rotation is also brought back to the z axis
t.linear.x = v.point.x
t.linear.y = v.point.y
t.linear.z = 0
t.angular.x = 0
t.angular.y = 0
t.angular.z = vel[2,0]
vx = Float64()
vx.data = v.point.x
vy = Float64()
vy.data = v.point.y
wz = Float64()
wz.data = vel[2,0]
self.vx_pub.publish(vx)
self.vy_pub.publish(vy)
self.wz_pub.publish(wz)
self.twist_pub.publish(t)
self.computationalTime = rospy.Time.now() - self.computationalTime
self.Time_pub.publish(self.computationalTime)
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
