| 12 | |
| 13 | |
| 14 | class histogram_equalize: |
| 15 | def __init__(self, approximate_sync=True): |
| 16 | |
| 17 | self.left_subscriber = message_filters.Subscriber("/left/image_rect", |
| 18 | Image, |
| 19 | queue_size=1000) |
| 20 | self.right_subscriber = message_filters.Subscriber("/right/image_rect", |
| 21 | Image, |
| 22 | queue_size=1000) |
| 23 | |
| 24 | self.left_image_pub = rospy.Publisher("/left_hist/image_rect", |
| 25 | Image, |
| 26 | queue_size=1000) |
| 27 | self.right_image_pub = rospy.Publisher("/right_hist/image_rect", |
| 28 | Image, |
| 29 | queue_size=1000) |
| 30 | |
| 31 | # for speedo1 Lake Jocassee: clipLimit=1.5, titleGridSize=(2,2) |
| 32 | # for stereo rig2 Florida cave: don't use this node |
| 33 | # for stereo Rig Bus: clipLimit=2.0, tileGridSize=(5, 5) |
| 34 | # these parameters can be changed to obtain best results for a particluar package |
| 35 | |
| 36 | # For more documentation see: https://docs.opencv.org/3.1.0/d5/daf/tutorial_py_histogram_equalization.html |
| 37 | |
| 38 | self.histenhance = cv2.createCLAHE(clipLimit=2.0, tileGridSize=(5, 5)) |
| 39 | |
| 40 | Synchronizer = message_filters.ApproximateTimeSynchronizer if approximate_sync else message_filters.TimeSynchronizer |
| 41 | slop = {1} if approximate_sync else {} |
| 42 | self.time_sync = Synchronizer( |
| 43 | [self.left_subscriber, self.right_subscriber], 100, *slop) |
| 44 | |
| 45 | self.time_sync.registerCallback(self.img_callback) |
| 46 | |
| 47 | def img_callback(self, left_msg, right_msg): |
| 48 | |
| 49 | #rospy.loginfo(" Received Image message") |
| 50 | ## image msg to CV image## |
| 51 | |
| 52 | left_image_cv = bridge.imgmsg_to_cv2(left_msg, "mono8") |
| 53 | right_image_cv = bridge.imgmsg_to_cv2(right_msg, "mono8") |
| 54 | |
| 55 | ##Contrast Enhancement ## |
| 56 | left_enhanced_img = self.histenhance.apply(left_image_cv) |
| 57 | right_enhanced_img = self.histenhance.apply(right_image_cv) |
| 58 | |
| 59 | #print (type(left_enhanced_img)) |
| 60 | |
| 61 | #### Create Image Msg #### |
| 62 | enhanced_left_msg = bridge.cv2_to_imgmsg(left_enhanced_img, "mono8") |
| 63 | enhanced_left_msg.header.stamp = left_msg.header.stamp |
| 64 | enhanced_left_msg.height = left_msg.height |
| 65 | enhanced_left_msg.width = left_msg.width |
| 66 | enhanced_right_msg = bridge.cv2_to_imgmsg(right_enhanced_img, "mono8") |
| 67 | enhanced_right_msg.header.stamp = right_msg.header.stamp |
| 68 | enhanced_right_msg.height = right_msg.height |
| 69 | enhanced_right_msg.width = right_msg.width |
| 70 | |
| 71 | # Publish new image |