class AntiInstagramNode(): def __init__(self): self.node_name = rospy.get_name() self.active = True self.locked = False self.image_pub_switch = rospy.get_param("~publish_corrected_image", False) # Initialize publishers and subscribers self.pub_image = rospy.Publisher("~corrected_image", Image, queue_size=1) self.pub_health = rospy.Publisher("~health", AntiInstagramHealth, queue_size=1, latch=True) self.pub_transform = rospy.Publisher("~transform", AntiInstagramTransform, queue_size=1, latch=True) #self.sub_switch = rospy.Subscriber("~switch",BoolStamped, self.cbSwitch, queue_size=1) #self.sub_image = rospy.Subscriber("~uncorrected_image",Image,self.cbNewImage,queue_size=1) self.sub_image = rospy.Subscriber("~uncorrected_image", CompressedImage, self.cbNewImage, queue_size=1) self.sub_click = rospy.Subscriber("~click", BoolStamped, self.cbClick, queue_size=1) self.trans_timer = rospy.Timer(rospy.Duration.from_sec(20), self.cbPubTrans, True) # Verbose option self.verbose = rospy.get_param('line_detector_node/verbose', True) # Initialize health message self.health = AntiInstagramHealth() # Initialize transform message self.transform = AntiInstagramTransform() # FIXME: read default from configuration and publish it self.ai_scale = np.array( [2.2728408473337893, 2.2728273205024614, 2.272844346401005]) self.ai_shift = np.array( [21.47181119272393, 37.14653160247276, 4.089311860796786]) self.ai = AntiInstagram() self.corrected_image = Image() self.bridge = CvBridge() self.image_msg = None self.click_on = False def cbPubTrans(self, _): self.transform.s[0], self.transform.s[1], self.transform.s[ 2] = self.ai_shift self.transform.s[3], self.transform.s[4], self.transform.s[ 5] = self.ai_scale self.pub_transform.publish(self.transform) rospy.loginfo('ai: Color transform published.') def cbNewImage(self, image_msg): # memorize image self.image_msg = image_msg if self.image_pub_switch: tk = TimeKeeper(image_msg) cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") corrected_image_cv2 = self.ai.applyTransform(cv_image) tk.completed('applyTransform') corrected_image_cv2 = np.clip(corrected_image_cv2, 0, 255).astype(np.uint8) self.corrected_image = self.bridge.cv2_to_imgmsg( corrected_image_cv2, "bgr8") tk.completed('encode') self.pub_image.publish(self.corrected_image) tk.completed('published') if self.verbose: rospy.loginfo('ai:\n' + tk.getall()) def cbClick(self, _): # if we have seen an image: if self.image_msg is not None: self.click_on = not self.click_on if self.click_on: self.processImage(self.image_msg) else: self.transform.s = [0, 0, 0, 1, 1, 1] self.pub_transform.publish(self.transform) rospy.loginfo('ai: Color transform is turned OFF!') def processImage(self, msg): ''' Inputs: msg - CompressedImage - uncorrected image from raspberry pi camera Uses anti_instagram library to adjust msg so that it looks like the same color temperature as a duckietown reference image. Calculates health of the node and publishes the corrected image and the health state. Health somehow corresponds to how good of a transformation it is. ''' rospy.loginfo('ai: Computing color transform...') tk = TimeKeeper(msg) #cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8") try: cv_image = image_cv_from_jpg(msg.data) except ValueError as e: rospy.loginfo('Anti_instagram cannot decode image: %s' % e) return tk.completed('converted') self.ai.calculateTransform(cv_image) tk.completed('calculateTransform') # if health is much below the threshold value, do not update the color correction and log it. if self.ai.health <= 0.001: # health is not good rospy.loginfo("Health is not good") else: self.health.J1 = self.ai.health self.transform.s[0], self.transform.s[1], self.transform.s[ 2] = self.ai.shift self.transform.s[3], self.transform.s[4], self.transform.s[ 5] = self.ai.scale rospy.set_param('antiins_shift', self.ai.shift.tolist()) rospy.set_param('antiins_scale', self.ai.scale.tolist()) self.pub_health.publish(self.health) self.pub_transform.publish(self.transform) rospy.loginfo('ai: Color transform published.')
def main(): # check number of arguments if len(sys.argv) != 3: print( 'This program expects exactly two arguments: an image filename and an output directory.' ) sys.exit() # store inputs file = sys.argv[1] outdir = sys.argv[2] # check if file exists if not os.path.isfile(file): print('file not found') sys.exit(2) # check if dir exists, create if not if not os.path.exists(outdir): os.makedirs(outdir) # read the image input_img = cv2.imread(file, cv2.IMREAD_UNCHANGED) # lets do the masking! surf = identifyLaneSurface(input_img, use_hsv=False, grad_thresh=20) input_img = np.expand_dims(surf, -1) * input_img print('masked image!') cv2.imshow('mask', input_img) cv2.waitKey(0) # create instance of AntiInstagram ai = AntiInstagram() ai.calculateTransform(input_img) print('Transform Calculation completed!') corrected_img = ai.applyTransform(input_img) print('Transform applied!') cv2.imshow('mask', corrected_img) cv2.waitKey(0) # polygons for pic1_smaller.jpg and pic2_smaller.jpg (hardcoded for now) polygon_black = [(280, 570), (220, 760), (870, 750), (810, 580)] polygon_white = [(781, 431), (975, 660), (1040, 633), (827, 418)] polygon_yellow = [(131, 523), (67, 597), (99, 609), (161, 530)] polygon_red = [(432, 282), (418, 337), (577, 338), (568, 283)] """ # polygons for pic3_smaller.jpg and pic2_smaller.jpg (hardcoded for now) polygon_black = [(280, 570), (220, 760), (870, 750), (810, 580)] polygon_white = [(900, 520), (1000, 640), (1060, 620), (970, 515)] polygon_yellow = [(234, 430), (190, 485), (230, 490), (270, 430)] polygon_red = [(285, 435), (250, 490), (830, 480), (800, 437)] # polygons for pic4_smaller.jpg (hardcoded for now) polygon_black = [(316, 414), (215, 623), (783, 605), (673, 422)] polygon_white = [(710, 388), (947, 656), (1018, 620), (788, 400)] polygon_yellow = [(148, 474), (94, 537), (133, 542), (184, 475)] polygon_red = [(285, 435), (250, 490), (830, 480), (800, 437)] # polygons for pic5_smaller.jpg (hardcoded for now) polygon_black = [(354, 418), (291, 612), (804, 590), (677, 396)] polygon_white = [(783, 424), (949, 602), (1002, 564), (840, 420)] polygon_yellow = [(344, 307), (331, 319), (354, 319), (366, 306)] polygon_red = [(135, 325), (119, 332), (325, 316), (332, 309)] """ # create dictionary containing colors polygons = { 'black': polygon_black, 'white': polygon_white, 'yellow': polygon_yellow, 'red': polygon_red } # initialize estimateError class E = estimateError(corrected_img) # set polygons E.createPolygon(polygons) # estimate error E.GetErrorEstimation() # write the corrected image date = datetime.datetime.now().strftime("%H-%M-%S") path = outdir + '/' + str(date) + '_polygon.jpg' cv2.imwrite(path, E.out_image)
class AntiInstagramNode(): def __init__(self): self.node_name = rospy.get_name() self.active = True self.image_pub_switch = rospy.get_param("~publish_corrected_image",False) # Initialize publishers and subscribers self.pub_image = rospy.Publisher("~corrected_image",Image,queue_size=1) self.pub_health = rospy.Publisher("~health",AntiInstagramHealth,queue_size=1,latch=True) self.pub_transform = rospy.Publisher("~transform",AntiInstagramTransform,queue_size=1,latch=True) #self.sub_switch = rospy.Subscriber("~switch",BoolStamped, self.cbSwitch, queue_size=1) #self.sub_image = rospy.Subscriber("~uncorrected_image",Image,self.cbNewImage,queue_size=1) self.sub_image = rospy.Subscriber("~uncorrected_image", CompressedImage, self.cbNewImage,queue_size=1) self.sub_click = rospy.Subscriber("~click", BoolStamped, self.cbClick, queue_size=1) # Verbose option self.verbose = rospy.get_param('~verbose',True) # Initialize health message self.health = AntiInstagramHealth() # Initialize transform message self.transform = AntiInstagramTransform() # FIXME: read default from configuration and publish it self.ai = AntiInstagram() self.corrected_image = Image() self.bridge = CvBridge() self.image_msg = None self.click_on = False def cbNewImage(self,image_msg): # memorize image self.image_msg = image_msg if self.image_pub_switch: tk = TimeKeeper(image_msg) cv_image = self.bridge.imgmsg_to_cv2(image_msg, "bgr8") corrected_image_cv2 = self.ai.applyTransform(cv_image) tk.completed('applyTransform') corrected_image_cv2 = np.clip(corrected_image_cv2, 0, 255).astype(np.uint8) self.corrected_image = self.bridge.cv2_to_imgmsg(corrected_image_cv2, "bgr8") tk.completed('encode') self.pub_image.publish(self.corrected_image) tk.completed('published') if self.verbose: rospy.loginfo('ai:\n' + tk.getall()) def cbClick(self, _): # if we have seen an image: if self.image_msg is not None: self.click_on = not self.click_on if self.click_on: self.processImage(self.image_msg) else: self.transform.s = [0,0,0,1,1,1] self.pub_transform.publish(self.transform) rospy.loginfo('ai: Color transform is turned OFF!') else: rospy.loginfo('No image seen yet') def processImage(self,msg): ''' Inputs: msg - CompressedImage - uncorrected image from raspberry pi camera Uses anti_instagram library to adjust msg so that it looks like the same color temperature as a duckietown reference image. Calculates health of the node and publishes the corrected image and the health state. Health somehow corresponds to how good of a transformation it is. ''' rospy.loginfo('ai: Computing color transform...') tk = TimeKeeper(msg) #cv_image = self.bridge.imgmsg_to_cv2(msg,"bgr8") try: cv_image = image_cv_from_jpg(msg.data) except ValueError as e: rospy.loginfo('Anti_instagram cannot decode image: %s' % e) return tk.completed('converted') self.ai.calculateTransform(cv_image) tk.completed('calculateTransform') # FIXME !!! if False: # health is not good rospy.loginfo("Health is not good") else: self.health.J1 = self.ai.health self.transform.s[0], self.transform.s[1], self.transform.s[2] = self.ai.shift self.transform.s[3], self.transform.s[4], self.transform.s[5] = self.ai.scale self.pub_health.publish(self.health) self.pub_transform.publish(self.transform) rospy.loginfo('ai: Color transform published!!!')
os.makedirs(outdir) # read the image input_img = cv2.imread(file, cv2.IMREAD_UNCHANGED) #lets do the masking! #surf = identifyLaneSurface(input_img, use_hsv=False, grad_thresh=20) #input_img = np.expand_dims(surf, -1) * input_img #cv2.imshow('mask', input_img) #cv2.waitKey(0) # create instance of AntiInstagram ai = AntiInstagram() ai.calculateTransform(input_img) print('Transform Calculation completed!') corrected_img = ai.applyTransform(input_img) print('Transform applied!') # write the corrected image date = datetime.datetime.now().strftime("%H-%M-%S") path = outdir + '/' + str(date) + '_corrected.jpg' cv2.imwrite(path, corrected_img) print('calculated scale: ' + str(ai.scale)) print('calculated shift: ' + str(ai.shift))