diff --git a/config/compound_eye.png b/config/compound_eye.png index e324d1a..20941cf 100644 Binary files a/config/compound_eye.png and b/config/compound_eye.png differ diff --git a/config/stereo.json b/config/stereo.json new file mode 100644 index 0000000..95d2fe9 --- /dev/null +++ b/config/stereo.json @@ -0,0 +1,35 @@ +{ + "name": "prototype", + "version": "1.0", + "camera": { + "resolution_raw": [1920, 1080], + "fps": 60, + "units": 15, + "units_used": 9, + "resolution_stitch": [400, 400] + }, + "stitcher": { + "min_area_frac": 0.01, + "max_area_frac": 0.05, + "max_aspect_ratio": 100, + "index": [1, 2, 3, 6, 7, 8, 11, 12, 13], + "margin": 10, + "nominal_ov_pos": 100, + "n": [3, 3] + }, + "tracker": { + "adaptive": true, + "lower_threshold": 12.0, + "upper_threshold": 18.0, + "loop_closure_interval": 1, + "dis_preset": "medium" + }, + "contact": { + "min_contour_area": 3000, + "flow_threshold": 1, + "div_threshold": 0.02, + "expansion_threshold": 0.01, + "kernel_size": [5, 5], + "denoise_sigma": 15 + } +} diff --git a/src/orisys/Stitcher.py b/src/orisys/Stitcher.py index 9aa6fa1..88a2c47 100644 --- a/src/orisys/Stitcher.py +++ b/src/orisys/Stitcher.py @@ -111,7 +111,7 @@ class Stitcher: img_corr = self.__correct_edge_bloom(img_src) self.__detect_nonblack_regions(img_corr,show = show) - return len(self.boxes) + return len(self.boxes), self.boxes # return self.boxes diff --git a/src/orisys/sensor.py b/src/orisys/sensor.py index 828445c..7ebf9ce 100644 --- a/src/orisys/sensor.py +++ b/src/orisys/sensor.py @@ -131,7 +131,7 @@ class Sensor: # Initialize components - self.__stitcher = Stitcher(isstitch=isstitch, config_stitcher=self.__config_stitcher, verbose=self.verbose) + self.stitcher = Stitcher(isstitch=isstitch, config_stitcher=self.__config_stitcher, verbose=self.verbose) self.__tracker = Tracker(adaptive=True, cuda=cuda, verbose=self.verbose, dis_preset=self._config_dis_preset) self.__contact3d = Contact3D(config_contact=self.__config_contact, verbose=self.verbose) self.__hhd = None @@ -168,17 +168,18 @@ class Sensor: # Convert to grayscale if log_timing: t0 = time.perf_counter() - gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + # gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY) + gray = frame if log_timing: timings['cvt_color'] = (time.perf_counter() - t0) * 1000 # Extract and stitch tiles if log_timing: t0 = time.perf_counter() - self.__stitcher.get_stitch_unit(gray) + self.stitcher.get_stitch_unit(gray) self._extract_tiles(gray) if log_timing: timings['get_tiles'] = (time.perf_counter() - t0) * 1000 if log_timing: t0 = time.perf_counter() - img = np.ascontiguousarray(self.__stitcher.stitch_rt_optimized()) + img = np.ascontiguousarray(self.stitcher.stitch_rt_optimized()) if log_timing: timings['stitch'] = (time.perf_counter() - t0) * 1000 self.__img_size_raw = img.shape @@ -379,12 +380,12 @@ class Sensor: if self.verbose: print("Loading calibration from", self.cal_path) self.stitch_calibration = np.load(self.cal_path, allow_pickle=True) - self.__stitcher.stitch_calibration = self.stitch_calibration - return self.__stitcher.stitch_rt_optimized() + self.stitcher.stitch_calibration = self.stitch_calibration + return self.stitcher.stitch_rt_optimized() else: if self.verbose: print("Creating new calibration...") - img, self.stitch_calibration = self.__stitcher.stitch_cal() + img, self.stitch_calibration = self.stitcher.stitch_cal() os.makedirs("./config", exist_ok=True) np.save(self.cal_path, self.stitch_calibration) if self.verbose: @@ -448,8 +449,8 @@ class Sensor: if override: if box is not None: - self.__stitcher.isstitch = self.isstitch = False - self.__stitcher.boxes = box + self.stitcher.isstitch = self.isstitch = False + self.stitcher.boxes = box else: print("Override=True but no box provided.") else: @@ -467,15 +468,16 @@ class Sensor: v_mean = np.sum(cv2.mean(frame)) if abs(v_mean - v_mean0) < 5: - n_boxes = self.__stitcher.find_stitch_boxes(frame, show=show) + n_boxes, boxes = self.stitcher.find_stitch_boxes(frame, show=show) if (cv2.getTickCount() - start_time) / cv2.getTickFrequency() > timeout: print("Timeout finding vision units.") - self.__stitcher.isstitch = self.isstitch = False + self.stitcher.isstitch = self.isstitch = False break v_mean0 = v_mean self.__n_boxes = n_boxes + self.boxes = boxes self.stitch_valid = self.isstitch and n_boxes == self._config_units status = "valid for stitching" if self.stitch_valid else "non-stitch mode" @@ -484,8 +486,8 @@ class Sensor: def _extract_tiles(self, img_src): """Extract tiles from source image.""" - self.__stitcher.get_stitch_unit(img_src) - self.__stitcher.extract_tiles() + self.stitcher.get_stitch_unit(img_src) + self.stitcher.extract_tiles() return True diff --git a/stereo_1.jpg b/stereo_1.jpg new file mode 100644 index 0000000..e794021 Binary files /dev/null and b/stereo_1.jpg differ diff --git a/stereo_1.mp4 b/stereo_1.mp4 new file mode 100644 index 0000000..e3611d0 Binary files /dev/null and b/stereo_1.mp4 differ diff --git a/streo.py b/streo.py new file mode 100644 index 0000000..974a3db --- /dev/null +++ b/streo.py @@ -0,0 +1,163 @@ +import numpy as np +import cv2 +from matplotlib import pyplot as plt +import orisys + +sensor = orisys.Sensor(vid_src='stereo_1.mp4',config_name='./config/stereo.json',mirror=False) +print(sensor.boxes) +sensor.get_img() +tiles = sensor.stitcher.tile_list +print(tiles) + +imgL = tiles[0] +imgR = tiles[1] + + +# imgL = cv.imread('tsukuba_l.png', cv.IMREAD_GRAYSCALE) +# imgR = cv.imread('tsukuba_r.png', cv.IMREAD_GRAYSCALE) + +plt.subplot(1,2,1) +plt.imshow(imgL,'gray') +plt.subplot(1,2,2) +plt.imshow(imgR,'gray') +plt.show() + +# Feature matching for stereo computation +def match_features(img1, img2, method='SIFT', max_matches=50): + """ + Detect and match features between two images. + + Args: + img1: Left image (grayscale) + img2: Right image (grayscale) + method: 'SIFT' or 'ORB' + max_matches: Maximum number of matches to show + + Returns: + img_matches: Image with matches drawn + matches: List of matches + kp1, kp2: Keypoints in img1 and img2 + """ + # Convert to grayscale if needed + if len(img1.shape) == 3: + img1_gray = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY) + else: + img1_gray = img1.copy() + + if len(img2.shape) == 3: + img2_gray = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY) + else: + img2_gray = img2.copy() + + # Initialize detector + if method == 'SIFT': + detector = cv2.SIFT_create() + elif method == 'ORB': + detector = cv2.ORB_create() + else: + raise ValueError("method must be 'SIFT' or 'ORB'") + + # Detect keypoints and descriptors + kp1, des1 = detector.detectAndCompute(img1_gray, None) + kp2, des2 = detector.detectAndCompute(img2_gray, None) + + if des1 is None or des2 is None or len(des1) == 0 or len(des2) == 0: + print(f"Warning: No features detected with {method}") + return None, [], kp1, kp2 + + # Match features + if method == 'SIFT': + # Check if we have enough descriptors for knnMatch (need at least k=2) + min_descriptors = min(len(des1), len(des2)) + if min_descriptors < 2: + print(f"Warning: Not enough descriptors ({min_descriptors}) for knnMatch, using regular matching") + # Fallback to BFMatcher + matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=False) + matches = matcher.match(des1, des2) + matches = sorted(matches, key=lambda x: x.distance)[:max_matches] + else: + # Use FLANN matcher for SIFT + try: + FLANN_INDEX_KDTREE = 1 + index_params = dict(algorithm=FLANN_INDEX_KDTREE, trees=5) + search_params = dict(checks=50) + matcher = cv2.FlannBasedMatcher(index_params, search_params) + matches = matcher.knnMatch(des1, des2, k=2) + # Apply Lowe's ratio test + good_matches = [] + for match_pair in matches: + if len(match_pair) == 2: + m, n = match_pair + if m.distance < 0.7 * n.distance: + good_matches.append(m) + matches = sorted(good_matches, key=lambda x: x.distance)[:max_matches] + except cv2.error: + # Fallback to BFMatcher if FLANN fails + print("Warning: FLANN matcher failed, using BFMatcher instead") + matcher = cv2.BFMatcher(cv2.NORM_L2, crossCheck=False) + matches = matcher.match(des1, des2) + matches = sorted(matches, key=lambda x: x.distance)[:max_matches] + else: # ORB + # Use BF matcher for ORB + matcher = cv2.BFMatcher(cv2.NORM_HAMMING, crossCheck=True) + matches = matcher.match(des1, des2) + matches = sorted(matches, key=lambda x: x.distance)[:max_matches] + + # Draw matches + img_matches = cv2.drawMatches( + img1_gray, kp1, img2_gray, kp2, matches, None, + flags=cv2.DrawMatchesFlags_NOT_DRAW_SINGLE_POINTS + ) + + print(f"Found {len(matches)} good matches using {method}") + return img_matches, matches, kp1, kp2 + +# Perform feature matching +print("\n=== Feature Matching ===") +img_matches, matches, kp1, kp2 = match_features(imgL, imgR, method='SIFT', max_matches=50) + +if img_matches is not None: + plt.figure(figsize=(15, 6)) + plt.imshow(img_matches) + plt.title(f'Feature Matches (SIFT) - {len(matches)} matches') + plt.axis('off') + plt.tight_layout() + plt.show() + + # Also create a side-by-side visualization with lines connecting matches + h1, w1 = imgL.shape[:2] + h2, w2 = imgR.shape[:2] + h_max = max(h1, h2) + combined = np.zeros((h_max, w1 + w2), dtype=np.uint8) + combined[:h1, :w1] = imgL + combined[:h2, w1:w1+w2] = imgR + combined_color = combined + + # Draw lines connecting matched features + for match in matches[:30]: # Show top 30 matches + pt1 = tuple(map(int, kp1[match.queryIdx].pt)) + pt2 = tuple(map(int, kp2[match.trainIdx].pt)) + pt2 = (pt2[0] + w1, pt2[1]) + cv2.line(combined_color, pt1, pt2, (0, 255, 0), 1) + cv2.circle(combined_color, pt1, 3, (0, 0, 255), -1) + cv2.circle(combined_color, pt2, 3, (255, 0, 0), -1) + + plt.figure(figsize=(15, 6)) + plt.imshow(cv2.cvtColor(combined_color, cv2.COLOR_BGR2RGB)) + plt.title(f'Stereo Feature Matching - {len(matches[:30])} matches shown') + plt.axis('off') + plt.tight_layout() + plt.show() + +# Stereo disparity computation +print("\n=== Stereo Disparity Computation ===") +stereo = cv2.StereoBM.create(numDisparities=32, blockSize=15) +disparity = stereo.compute(imgL, imgR) +plt.figure(figsize=(12, 5)) +plt.imshow(disparity, cmap='jet') +plt.colorbar(label='Disparity') +plt.title('Stereo Disparity Map') +plt.axis('off') +plt.tight_layout() +plt.show() +