image_overlay.py
Go to the documentation of this file.
1 #!/usr/bin/env python
2 # Authors: Marcey and Daniel
3 #
4 import os
5 import sys
6 import cv2
7 import rospy
8 import click
9 import numpy as np
10 
11 from std_msgs.msg import Float32
12 from sensor_msgs.msg import CompressedImage
13 from cv_bridge import CvBridge, CvBridgeError
14 
15 
17  return 'unnamed' not in rospy.get_name()
18 
19 # Save the original rospy.log* functions for when ROS is enabled. This script also supports alternative use by command line and without ROS. For convienence I am overriding rospy.loginfo to support both use cases
20 rospy_loginfo = rospy.loginfo
21 def loginfo(message):
22  if is_ros_node():
23  rospy_loginfo(message)
24  else:
25  click.echo(click.style(str(message), fg='green'))
26 rospy.loginfo = loginfo
27 
28 rospy_logerr = rospy.logerr
29 def logerr(message):
30  if is_ros_node():
31  rospy_logerr(message)
32  else:
33  click.echo(click.style(str(message), fg='red'))
34 rospy.logerr = logerr
35 
36 def load_image(filename):
37  img = cv2.imread(filename, -1)
38  if img is None:
39  rospy.logerr("ERROR image file didn't load or doesn't exist: %s" % filename)
40  exit(1)
41  return img
42 
43 def resize_image(img, width_px):
44  """Args:
45  img: The image to be resized.
46  width_px: The new width of the image in pixels relative to itself. Aspect ratio is not changed.
47  """
48  # we need to keep in mind aspect ratio so the image does
49  # not look skewed or distorted -- therefore, we calculate
50  # the ratio of the new image to the old image
51  r = float(width_px) / img.shape[1]
52  dimensions = (int(width_px), int(img.shape[0] * r))
53  img = cv2.resize(img, dimensions, interpolation = cv2.INTER_CUBIC)
54  return img
55 
56 def rotate_image(img, rotation_angle):
57  num_rows, num_cols = img.shape[:2]
58  rotation_matrix = cv2.getRotationMatrix2D((int(num_rows/2), int(num_cols/2)),rotation_angle,1)
59  img = cv2.warpAffine(img, rotation_matrix, (num_cols, num_rows))
60  return img
61 
62 def warp_image(img):
63  """Warp image in 3D so it looks like its pointing into the image"""
64  compass_width = int(img.shape[1])
65  compass_height = int(img.shape[0])
66  square_corners = np.float32([[0,0], [compass_height,0], [compass_height,compass_width],[0,compass_width]])
67  quad_corners = np.float32([[153,0], [429,0], [600,286], [0,286]])
68  h, mask = cv2.findHomography(square_corners, quad_corners)
69  warped_img = cv2.warpPerspective(img, h, (600,286))
70  return warped_img
71 
72 def overlay_image(fg_img, bg_img, x, y):
73  """Overlay a background image onto a foreground image (with transparency support)"""
74  for c in range(0,3):
75  # pseudo code explaination: bg_img[where the overlay is going to be placed] = (overlay_image[all pixcels] * transparency) + (original_image * transparency)
76  bg_img[y:y+fg_img.shape[0], x:x+fg_img.shape[1], c] = fg_img[:,:,c] * (fg_img[:,:,3]/255.0) + bg_img[y:y+fg_img.shape[0], x:x+fg_img.shape[1], c] * (1.0 - fg_img[:,:,3]/255.0)
77  return bg_img
78 
79 
80 class ImageOverlay():
81  def __init__(self):
82  self.scale_text = 0
83  self.heading = 0
84  self.img = np.ones((700,1000,3),dtype=np.uint8)*128 # default black background
85 
86  current_dir = os.path.dirname(os.path.abspath(__file__))
87  compass_filename = '%s/compass.png' % current_dir
88  scale_filename = '%s/scale.png' % current_dir
89 
90  # Load scale image
91  self.scale_img = load_image(scale_filename)
92 
93  # Load compass image
94  self.compass_img = load_image(compass_filename)
95 
96  def image_callback(self, data):
97  self.img = CvBridge().compressed_imgmsg_to_cv2(data)
98 
99  def heading_callback(self, data):
100  self.heading = data.data
101 
102  def scale_callback(self, data):
103  self.scale_text = data.data
104 
105  def overlay_telemetry(self):
106  im_out = np.uint8(self.img.copy())
107  min_image_dimension = np.min([self.img.shape[1],self.img.shape[0]])
108 
109  compass_img = self.compass_img
110  compass_img = rotate_image(compass_img, self.heading)
111  compass_img = warp_image(compass_img)
112 
113  # Resize compass image
114  width_percentage = 0.6
115  width_px = width_percentage*min_image_dimension
116  compass_img = resize_image(compass_img, width_px)
117 
118  # Resize scale image
119  scale_img = resize_image(self.scale_img, width_px)
120 
121  # Overlay compass image
122  x = im_out.shape[1] / 2 - compass_img.shape[1] / 2 # center width position
123  y = im_out.shape[0] - int(compass_img.shape[0]) - 1 # near bottom
124  im_out = overlay_image(compass_img, im_out, x, y)
125 
126  # Overlay scale image
127  x = im_out.shape[1] / 2 - scale_img.shape[1] / 2 # center width position
128  y = im_out.shape[0] / 2 # # center height position
129  im_out = overlay_image(scale_img, im_out, x, y)
130 
131  # Set text overlay near scale_img indicating the scale value
132  scale_factor = (min_image_dimension / 350.0) # scale font relative to img
133  font = 2
134  scale = 0.5 * scale_factor
135  thick = 1 * scale_factor
136  line_type = 4
137 
138  color = tuple(np.ones(im_out.shape[2])*255) # white text, ie. same number of 255s and number of image channels
139  # Set text at the small end of the scale bar
140  loc = (int(x-13*scale_factor),int(y+7*scale_factor))
141  cv2.putText(im_out, "0", loc, font, scale, color, int(thick), line_type)
142  # Set text at the large end of the scale bar
143  scale_text_pretty = "%.1f cm" % self.scale_text
144  loc = (x+scale_img.shape[1]+5,int(y+7*scale_factor))
145  cv2.putText(im_out, scale_text_pretty, loc, font, scale, color, int(thick), line_type)
146 
147  ## Preview result
148  # cv2.imshow("Image", im_out)
149  # cv2.waitKey(0)
150  # cv2.destroyAllWindo()
151 
152  return im_out
153 
154  def ros_loop(self):
155  publisher = rospy.Publisher('overlay/compressed', CompressedImage, queue_size=1)
156  rospy.Subscriber('camera/compressed', CompressedImage, self.image_callback, queue_size=1)
157 
158  if not self.heading:
159  rospy.Subscriber('heading', Float32, self.heading_callback, queue_size=1)
160  if not self.scale_text:
161  rospy.Subscriber('scale', Float32, self.scale_callback, queue_size=1)
162 
163  rospy.loginfo('Setup complete. Image overlay process has started.')
164 
165  framerate = rospy.Rate(rospy.get_param('~framerate',3)) # default 3 Hz
166  while not rospy.is_shutdown():
167  img = self.overlay_telemetry()
168  compressed_image = CvBridge().cv2_to_compressed_imgmsg(img)
169  publisher.publish(compressed_image)
170  framerate.sleep()
171 
172 def ros_main():
173  # The ROS entry point to this code
174  rospy.init_node('overlay_scale_and_compass')
175  imgOverlay = ImageOverlay()
176  imgOverlay.ros_loop()
177 
178 # Command line options
179 @click.command()
180 @click.option('--input-image', required=True, help='Path to input image file')
181 @click.option('--heading', required=True, type=float, help='Current heading relative to north in degrees')
182 @click.option('--scale-text', required=True, type=float, help='The value to be displayed on the right of the scale bar in centimeters')
183 @click.option('--output-file', default='output.png', help='Output filename to save result to [default=\'output.png\']')
184 def cli_main(input_image, heading, scale_text, output_file):
185  # The CLI entry point to this code
186  imgOverlay = ImageOverlay()
187  imgOverlay.heading = heading
188  imgOverlay.scale_text = scale_text
189  imgOverlay.img = load_image(input_image)
190  img = imgOverlay.overlay_telemetry()
191  cv2.imwrite(output_file,img)
192 
193 if __name__ == '__main__':
194  cli_main()
def rotate_image(img, rotation_angle)
def cli_main(input_image, heading, scale_text, output_file)
def overlay_image(fg_img, bg_img, x, y)
def resize_image(img, width_px)


image_overlay_scale_and_compass
Author(s): Daniel Snider
autogenerated on Mon Jun 10 2019 13:35:22