def handle_req(self, req):
## Get Image out of req
cv_image = CvBridge().imgmsg_to_cv2(req.ima)
print '[Handle Request] cv_image.shape', cv_image.shape, '\ta=', req.a, '\tt=', req.ima.header.stamp
assert (cv_image.shape[0] == self.im_rows and
cv_image.shape[1] == self.im_cols and
cv_image.shape[2] == self.im_chnls),\
"\n[whole_image_descriptor_compute_server] Input shape of the image \
does not match with the allocated GPU memory. Expecting an input image of \
size %dx%dx%d, but received : %s" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )
# cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )
# cv2.waitKey(10)
# cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )
## Compute Descriptor
start_time = time.time()
u = self.model.predict(np.expand_dims(cv_image.astype('float32'), 0))
print 'Descriptor Computed in %4.4fms' % (1000. *
(time.time() - start_time)),
print '\tdesc.shape=', u.shape,
print '\tinput_image.shape=', cv_image.shape,
print '\tminmax=', np.min(u), np.max(u),
print '\tnorm=', np.linalg.norm(u[0]),
print '\tmodel_type=', self.model_type,
print '\tdtype=', cv_image.dtype
## Populate output message
result = WholeImageDescriptorComputeResponse()
# result.desc = [ cv_image.shape[0], cv_image.shape[1] ]
result.desc = u[0, :]
result.model_type = self.model_type
return result
def imageCallback(data):
global frame, WIDTH, HEIGHT
try:
frame = CvBridge().imgmsg_to_cv2(data, "bgr8")
frame = frame.astype(np.uint8)
#rospy.loginfo(frame.shape)
except CvBridgeError, e:
print e
def handle_req( self, req ):
""" The received image from CV bridge has to be [0,255]. In function makes it to
intensity range [-0.5 to 0.5]
"""
## Get Image out of req
cv_image = CvBridge().imgmsg_to_cv2( req.ima )
print '[HDF5ModelImageDescriptor Handle Request#%d] cv_image.shape' %(self.request_count), cv_image.shape, '\ta=', req.a, '\tt=', req.ima.header.stamp
if len(cv_image.shape)==2:
# print 'Input dimensions are NxM but I am expecting it to be NxMxC, so np.expand_dims'
cv_image = np.expand_dims( cv_image, -1 )
elif len( cv_image.shape )==3:
pass
else:
assert( False )
assert (cv_image.shape[0] == self.im_rows and
cv_image.shape[1] == self.im_cols and
cv_image.shape[2] == self.im_chnls),\
"\n[whole_image_descriptor_compute_server] Input shape of the image \
does not match with the allocated GPU memory. Expecting an input image of \
size %dx%dx%d, but received : %s" %(self.im_rows, self.im_cols, self.im_chnls, str(cv_image.shape) )
# cv2.imshow( 'whole_image_descriptor_compute_server:imshow', cv_image.astype('uint8') )
# cv2.waitKey(10)
# cv2.imwrite( '/app/tmp/%s.jpg' %( str(req.ima.header.stamp) ), cv_image )
## Compute Descriptor
start_time = time.time()
# i__image = np.expand_dims( cv_image.astype('float32'), 0 )
# i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)/255. [-0.5,0.5]
i__image = (np.expand_dims( cv_image.astype('float32'), 0 ) - 128.)*2.0/255. #[-1,1]
u = self.model.predict( i__image )
print tcol.HEADER, 'Descriptor Computed in %4.4fms' %( 1000. *(time.time() - start_time) ), tcol.ENDC
print '\tinput_image.shape=', cv_image.shape,
print '\tinput_image dtype=', cv_image.dtype
print tcol.OKBLUE, '\tinput image (to neuralnet) minmax=', np.min( i__image ), np.max( i__image ), tcol.ENDC
print '\tdesc.shape=', u.shape,
print '\tdesc minmax=', np.min( u ), np.max( u ),
print '\tnorm=', np.linalg.norm(u[0])
print '\tmodel_type=', self.model_type
self.request_count += 1
## Populate output message
result = WholeImageDescriptorComputeResponse()
# result.desc = [ cv_image.shape[0], cv_image.shape[1] ]
result.desc = u[0,:]
result.model_type = self.model_type
return result
class DiffImage(object):
def __init__(self):
self.image_sub = rospy.Subscriber("/image", Image, self.ImageCallback)
self.pub = rospy.Publisher('/move', Bool, queue_size=10)
self.image_pub_compress = rospy.Publisher('/diff_image/compressed',
CompressedImage)
self.move = Bool()
self.cv_image = np.zeros((1, 1, 3), np.uint8)
self.first_flag = True
self.count = 0
def ImageCallback(self, msg):
try:
self.cv_image = CvBridge().imgmsg_to_cv2(msg, "bgr8")
self.DiffImage()
except:
print(CvBridgeerror)
def DiffImage(self):
if self.first_flag:
self.back_img = np.zeros_like(self.cv_image, np.float32)
self.first_flag = False
f_img = self.cv_image.astype(np.float32)
diff_img = cv2.absdiff(f_img, self.back_img)
diff_img_sum = np.sum(diff_img, axis=2)
pixcel_diff = diff_img_sum > 50
rate = float(np.sum(pixcel_diff)) / (f_img.shape[0] * f_img.shape[1])
booling_img = cv2.cvtColor(diff_img, cv2.COLOR_RGB2GRAY) > 50
tmp_img = np.ones_like(booling_img, np.float32) * booling_img
moving_img = self.cv_image * cv2.cvtColor(tmp_img, cv2.COLOR_GRAY2RGB)
cv2.accumulateWeighted(f_img, self.back_img, 0.2)
if self.count < self.diff_count:
print("Count:{:>3} Calc :{:>4} Rate:{:>.4}".format(
self.count, np.sum(pixcel_diff), rate))
self.move.data = False
self.pub.publish(self.move)
elif self.diff_count < self.count and self.threshold < rate:
print("Count:{:>3} Move :{:>4} Rate:{:>.4}".format(
self.count, np.sum(pixcel_diff), rate))
self.move.data = True
self.pub.publish(self.move)
self.Reset()
else:
print("Count:{:>3} Stop :{:>4} Rate:{:>.4}".format(
self.count, np.sum(pixcel_diff), rate))
self.move.data = False
self.pub.publish(self.move)
compress = CompressedImage()
compress.header.stamp = rospy.Time.now()
compress.format = "jpeg"
compress.data = np.array(cv2.imencode('.jpg',
moving_img)[1]).tostring()
self.image_pub_compress.publish(compress)
self.count += 1
def Reset(self):
self.count = 0
self.back_img = np.zeros_like(self.cv_image, np.float32)
self.first_flag = True
self.move.data = False
def main(self):
print("start")
rospy.init_node("diff_image")
self.diff_count = rospy.get_param("~diff_count", 30)
self.threshold = rospy.get_param("`threshold", 0.05)
rospy.spin()
return 0
