|
| 1 | +#!/usr/bin/env python |
| 2 | +import os |
| 3 | + |
| 4 | +import cv2 as cv |
| 5 | +import matplotlib.pyplot as plt |
| 6 | +import numpy as np |
| 7 | +import rosbag |
| 8 | +from cv_bridge import CvBridge |
| 9 | + |
| 10 | + |
| 11 | +def calibPhotometric(img, photometric, is_rgb=True): |
| 12 | + if not is_rgb: |
| 13 | + ret = img.copy() |
| 14 | + if len(img.shape) == 3: |
| 15 | + ret = cv.cvtColor(ret, cv.COLOR_BGR2GRAY) |
| 16 | + ret = ret.astype(float)/photometric |
| 17 | + else: |
| 18 | + # Divide by photometric per channel |
| 19 | + ret = img.copy().astype(float) |
| 20 | + for i in range(img.shape[2]): |
| 21 | + ret[:, :, i] = ret[:, :, i]/photometric*0.7 |
| 22 | + ret = np.clip(ret, 0, 255).astype(np.uint8) |
| 23 | + return ret |
| 24 | + |
| 25 | + |
| 26 | +def calibPhotometricImgsIndividual(imgs, photometrics, is_rgb=True): |
| 27 | + photometric_calibed = [] |
| 28 | + if photometrics is not None: |
| 29 | + # Convert to grayscale |
| 30 | + for i in range(len(imgs)): |
| 31 | + calibed = calibPhotometric(imgs[i], photometrics[i], is_rgb=is_rgb) |
| 32 | + photometric_calibed.append(calibed) |
| 33 | + return photometric_calibed |
| 34 | + else: |
| 35 | + return imgs |
| 36 | + |
| 37 | + |
| 38 | +def findPhotometric(cul_img, verbose=False): |
| 39 | + # Average in polar coordinates |
| 40 | + w_2 = cul_img.shape[1] // 2 |
| 41 | + h_2 = cul_img.shape[0] // 2 |
| 42 | + w = cul_img.shape[1] |
| 43 | + h = cul_img.shape[0] |
| 44 | + cul_line = np.zeros((w_2, 1)) # Half the width of the image |
| 45 | + count_line = np.zeros((w_2, 1)) # Half the width of the image |
| 46 | + pixels = w_2 * 2 |
| 47 | + for theta in range(0, 360, 1): |
| 48 | + for r in np.linspace(0, w_2, pixels): |
| 49 | + x = int(r * np.cos(theta)) |
| 50 | + y = int(r * np.sin(theta)) |
| 51 | + # Ignore the top 20 pixels to get rid of the propeller |
| 52 | + if x + w_2 < 0 or x + w_2 >= w or y + h_2 >= h or y + h_2 < 20 or int(r) >= w_2: |
| 53 | + continue |
| 54 | + cul_line[int(r)] += cul_img[y + h_2, x + w_2] |
| 55 | + count_line[int(r)] += 1 |
| 56 | + cul_line = cul_line / count_line |
| 57 | + cul_line = (cul_line - np.min(cul_line)) / (np.max(cul_line) - np.min(cul_line)) |
| 58 | + if verbose: |
| 59 | + plt.plot(cul_line, label="Photometric") |
| 60 | + plt.legend() |
| 61 | + plt.show() |
| 62 | + mask = np.zeros(cul_img.shape, dtype=np.float) |
| 63 | + for x in range(0, w): |
| 64 | + for y in range(0, h): |
| 65 | + r = int(np.sqrt((x - w_2) ** 2 + (y - h_2) ** 2)) |
| 66 | + if r < w_2: |
| 67 | + mask[y, x] = cul_line[r] |
| 68 | + # Normalize the mask by min max to 0.0 - 1.0 |
| 69 | + return mask |
| 70 | + |
| 71 | + |
| 72 | +def calibPhotometricMask( |
| 73 | + bag_dir, out_dir, cam_topics, step=5, verbose=False, start_t=0.): |
| 74 | + # Read from preporcessed bag (expect msgs are image type) |
| 75 | + cul_imgs = [] |
| 76 | + for cam in cam_topics: |
| 77 | + bag = rosbag.Bag(os.path.join(bag_dir, "{}.bag".format(cam))) |
| 78 | + num_imgs = bag.get_message_count(cam) |
| 79 | + print("{}: {}".format(cam, num_imgs)) |
| 80 | + |
| 81 | + if (num_imgs == 0): |
| 82 | + raise ValueError("no {} imgs in bag".format(cam)) |
| 83 | + |
| 84 | + bridge = CvBridge() |
| 85 | + cam_idx = cam_topics.index(cam) |
| 86 | + count = 0 |
| 87 | + img_count = 1 |
| 88 | + t0 = None |
| 89 | + for topic, msg, t in bag.read_messages(): |
| 90 | + try: |
| 91 | + if t0 is None: |
| 92 | + t0 = t |
| 93 | + if (t - t0).to_sec() < start_t: |
| 94 | + continue |
| 95 | + |
| 96 | + if (topic == cam): |
| 97 | + |
| 98 | + if count % step != 0: |
| 99 | + count += 1 |
| 100 | + continue |
| 101 | + |
| 102 | + if msg._type == "sensor_msgs/Image": |
| 103 | + img = bridge.imgmsg_to_cv2(msg, desired_encoding='passthrough') |
| 104 | + else: |
| 105 | + img = bridge.compressed_imgmsg_to_cv2(msg, desired_encoding='passthrough') |
| 106 | + |
| 107 | + if cul_imgs == []: |
| 108 | + # Create float64 image from gray image |
| 109 | + gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY) |
| 110 | + cul_imgs = [np.zeros(gray.shape, dtype=np.float64) for _ in cam_topics] |
| 111 | + |
| 112 | + # Add gaussain |
| 113 | + cul_imgs[cam_idx] += cv.GaussianBlur(cv.cvtColor(img, cv.COLOR_BGR2GRAY), (5, 5), 0) / 255.0 |
| 114 | + |
| 115 | + # Show averaged image |
| 116 | + if verbose: |
| 117 | + avg = cul_imgs[cam_idx] / img_count |
| 118 | + cv.imshow("Raw {}".format(cam), img) |
| 119 | + cv.imshow("Avg {}".format(cam), avg) |
| 120 | + cv.waitKey(1) |
| 121 | + |
| 122 | + img_count += 1 |
| 123 | + if verbose: |
| 124 | + c = cv.waitKey(1) |
| 125 | + if c == ord('q'): |
| 126 | + break |
| 127 | + count += 1 |
| 128 | + |
| 129 | + except KeyboardInterrupt: |
| 130 | + break |
| 131 | + |
| 132 | + mask = findPhotometric(cul_imgs[cam_idx] / img_count) |
| 133 | + cv.imwrite("{}/cam_{}_vig_mask.png".format(out_dir, cam_idx), mask * 255) |
| 134 | + |
| 135 | + mask = findPhotometric((cul_imgs[2] + cul_imgs[3]) / 2.0 / img_count) |
| 136 | + if verbose: |
| 137 | + cv.imshow("Avg mask", mask) |
| 138 | + cv.imwrite("{}/avg23_mask.png".format(out_dir), mask * 255) |
0 commit comments