From a24ff0097c1dc953d72634eeb48c6ce8413372a4 Mon Sep 17 00:00:00 2001 From: YassinEzzat2004 Date: Fri, 15 May 2026 18:33:37 +0300 Subject: [PATCH 01/11] Add vision module: lane detection pipeline, homography, and test data --- .gitignore | 1 + rover_ws/vision/.gitignore | 2 + rover_ws/vision/homography.py | 278 ++++++++++++++++++++++++++++++ rover_ws/vision/lane_detection.py | 123 +++++++++++++ rover_ws/vision/pipeline.py | 190 ++++++++++++++++++++ rover_ws/vision/requirements.txt | 2 + 6 files changed, 596 insertions(+) create mode 100644 .gitignore create mode 100644 rover_ws/vision/.gitignore create mode 100644 rover_ws/vision/homography.py create mode 100644 rover_ws/vision/lane_detection.py create mode 100644 rover_ws/vision/pipeline.py create mode 100644 rover_ws/vision/requirements.txt diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..ae8ee57 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +.vscode* \ No newline at end of file diff --git a/rover_ws/vision/.gitignore b/rover_ws/vision/.gitignore new file mode 100644 index 0000000..29e437e --- /dev/null +++ b/rover_ws/vision/.gitignore @@ -0,0 +1,2 @@ +*.pcd +__pycache__ \ No newline at end of file diff --git a/rover_ws/vision/homography.py b/rover_ws/vision/homography.py new file mode 100644 index 0000000..e7cb1e9 --- /dev/null +++ b/rover_ws/vision/homography.py @@ -0,0 +1,278 @@ +import cv2 +import numpy as np + + +class HomographyBEV: + + def __init__( + self, + K, + camera_height, + pitch_deg, + image_size, + dist_coeffs=None + ): + + self.K = K.astype(np.float64) + + self.dist_coeffs = ( + np.zeros(5, dtype=np.float64) + if dist_coeffs is None + else dist_coeffs.astype(np.float64) + ) + + self.camera_height = camera_height + self.pitch_deg = pitch_deg + self.pitch = np.deg2rad(pitch_deg) + + self.img_w = image_size[0] + self.img_h = image_size[1] + + self._build_extrinsics() + self._build_homography() + self._build_bev_scaling() + + # ========================================================= + # BUILD EXTRINSICS + # ========================================================= + + def _build_extrinsics(self): + + self.R = np.array([ + [1, 0, 0], + [0, np.cos(self.pitch), -np.sin(self.pitch)], + [0, np.sin(self.pitch), np.cos(self.pitch)] + ], dtype=np.float64) + + self.t = np.array([ + [0], + [0], + [self.camera_height] + ], dtype=np.float64) + + # ========================================================= + # BUILD HOMOGRAPHY + # ========================================================= + + def _build_homography(self): + + H = np.column_stack((self.R[:, 0], self.R[:, 1], self.t)) + + self.H = self.K @ H + self.H_inv = np.linalg.inv(self.H) + + # ========================================================= + # BUILD BEV SCALE + # ========================================================= + + def _build_bev_scaling(self): + + corners_px = np.array([ + [0, self.img_h // 2], + [self.img_w - 1, self.img_h // 2], + [0, self.img_h - 1], + [self.img_w - 1, self.img_h - 1], + ], dtype=np.float64) + + world_pts = [] + + for (u, v) in corners_px: + + ground = self.H_inv @ np.array([u, v, 1.0]) + ground /= ground[2] + + world_pts.append((ground[0], ground[1])) + + world_pts = np.array(world_pts) + + x_min = world_pts[:, 0].min() + x_max = world_pts[:, 0].max() + + y_min = world_pts[:, 1].min() + y_max = world_pts[:, 1].max() + + self.out_w = self.img_w + self.out_h = self.img_h // 2 + + scale = min( + self.out_w / (x_max - x_min), + self.out_h / (y_max - y_min) + ) + + self.S = np.array([ + [scale, 0, -scale * x_min], + [0, -scale, scale * y_max], + [0, 0, 1] + ], dtype=np.float64) + + self.S_inv = np.linalg.inv(self.S) + + self.H_bev = self.S @ self.H_inv + + # ========================================================= + # PIXEL -> GROUND + # ========================================================= + + def pixel_to_ground(self, u, v): + + pixel = np.array([u, v, 1.0], dtype=np.float64) + + ground = self.H_inv @ pixel + ground /= ground[2] + + return ground[0], ground[1] + + # ========================================================= + # MULTIPLE PIXELS -> GROUND + # ========================================================= + + def pixels_to_ground(self, pixels): + + pixels = np.asarray(pixels, dtype=np.float64) + + px = np.stack([ + pixels[:, 0], + pixels[:, 1], + np.ones(len(pixels)) + ], axis=0) + + ground = self.H_inv @ px + ground /= ground[2] + + X = ground[0] + Y = ground[1] + + return np.stack([X, Y], axis=1) + + # ========================================================= + # MASK -> POINT CLOUD + # ========================================================= + + def mask_to_pointcloud(self, mask): + + ys, xs = np.where(mask > 0) + + px = np.stack([ + xs.astype(np.float64), + ys.astype(np.float64), + np.ones_like(xs, dtype=np.float64) + ], axis=0) + + ground = self.H_inv @ px + ground /= ground[2] + + X = ground[0] + Y = ground[1] + Z = np.zeros_like(X) + + points = np.stack([X, Y, Z], axis=1) + + return points + + # ========================================================= + # WARP IMAGE TO BEV + # ========================================================= + + def warp_to_bev(self, image): + + bev = cv2.warpPerspective( + image, + self.H_bev, + (self.out_w, self.out_h), + flags=cv2.INTER_LINEAR, + borderMode=cv2.BORDER_CONSTANT, + borderValue=(0, 0, 0) + ) + + bev = cv2.flip(bev, 0) + + return bev + + # ========================================================= + # SAVE PCD + # ========================================================= + + def save_pcd(self, points, filename): + + n = len(points) + + header = ( + f"# .PCD v0.7\n" + f"FIELDS x y z\n" + f"SIZE 4 4 4\n" + f"TYPE F F F\n" + f"COUNT 1 1 1\n" + f"WIDTH {n}\n" + f"HEIGHT 1\n" + f"VIEWPOINT 0 0 0 1 0 0 0\n" + f"POINTS {n}\n" + f"DATA ascii\n" + ) + + with open(filename, 'w') as f: + + f.write(header) + np.savetxt(f, points, fmt="%.4f") + + print(f"Saved: {filename}") + + +# ============================================================= +# EXAMPLE USAGE +# ============================================================= +if __name__ == "__main__": + image = cv2.imread("../data/raw/ground.jpeg") + + h, w = image.shape[:2] + + K = np.array([ + [1000, 0, 960], + [0, 1000, 540], + [0, 0, 1] + ], dtype=np.float64) + + bev = HomographyBEV( + K=K, + camera_height=1.43, + pitch_deg=-50, + image_size=(w, h) + ) + + # ============================================================= + # SINGLE PIXEL + # ============================================================= + + X, Y = bev.pixel_to_ground(700, 700) + + print(f"Ground point: X={X:.3f}, Y={Y:.3f}") + + # ============================================================= + # WARP FULL IMAGE + # ============================================================= + + bird_eye = bev.warp_to_bev(image) + + # ============================================================= + # MASK -> POINT CLOUD + # ============================================================= + + gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) + + _, mask = cv2.threshold(gray, 120, 255, cv2.THRESH_BINARY) + + points = bev.mask_to_pointcloud(mask) + + print("Point cloud shape:", points.shape) + + bev.save_pcd(points, "ground_plane.pcd") + + # ============================================================= + # DISPLAY + # ============================================================= + + cv2.imshow("Original", image) + cv2.imshow("Mask", mask) + cv2.imshow("Bird Eye", bird_eye) + + cv2.waitKey(0) + cv2.destroyAllWindows() \ No newline at end of file diff --git a/rover_ws/vision/lane_detection.py b/rover_ws/vision/lane_detection.py new file mode 100644 index 0000000..bbbbddf --- /dev/null +++ b/rover_ws/vision/lane_detection.py @@ -0,0 +1,123 @@ +import cv2 +import numpy as np +from cv2 import ximgproc + +class LaneDetector: + + def __init__(self): + self.lower_white = np.array([0, 0, 200]) + self.upper_white = np.array([180, 50, 255]) + self.kernel = np.ones((5, 5), np.uint8) + + # ====================================================== + # ROI (kept separate, NOT used in pipeline by default) + # ====================================================== + def region_of_interest(self, frame): + + height, width = frame.shape[:2] + + polygon = np.array([[ + (0, height), + (width, height), + (width // 2 + 150, height // 2), + (width // 2 - 150, height // 2) + ]], np.int32) + + mask = np.zeros_like(frame) + cv2.fillPoly(mask, polygon, 255) + + return cv2.bitwise_and(frame, mask) + + # ====================================================== + # EDGE DETECTION (MATCHES YOUR ORIGINAL PIPELINE) + # ====================================================== + def detect_edges(self, frame): + + blur = cv2.GaussianBlur(frame, (5, 5), 0) + hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV) + + mask = cv2.inRange(hsv, self.lower_white, self.upper_white) + + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, self.kernel) + mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel) + + thin = ximgproc.thinning(mask) + + return thin + + # ====================================================== + # HOUGH LINES + # ====================================================== + def detect_lines(self, edges): + + lines = cv2.HoughLinesP( + edges, + 1, + np.pi / 180, + 50, + minLineLength=50, + maxLineGap=30 + ) + + return lines + + # ====================================================== + # DRAW LINES + # ====================================================== + def draw_lines(self, frame, lines): + + if lines is None: + return frame + + for line in lines: + x1, y1, x2, y2 = line[0] + cv2.line(frame, (x1, y1), (x2, y2), (0, 255, 0), 5) + + return frame + + # ====================================================== + # FULL PIPELINE (NO ROI BY DEFAULT → matches your working code) + # ====================================================== + def process(self, frame): + + edges = self.detect_edges(frame) + lines = self.detect_lines(edges) + + output = self.draw_lines(frame.copy(), lines) + + return output, edges, lines + + +# ====================================================== +# MAIN LOOP +# ====================================================== +if __name__ == "__main__": + cap = cv2.VideoCapture("../data/raw/test_lane.mp4") + + if not cap.isOpened(): + print("Cannot open video file") + exit() + + detector = LaneDetector() + + while True: + + ret, frame = cap.read() + + if not ret: + cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + continue + + output, edges, lines = detector.process(frame) + + print("lines:", lines) + + cv2.imshow("frame", output) + cv2.imshow("edges", edges) + + key = cv2.waitKey(1) + if key == 27: + break + + cap.release() + cv2.destroyAllWindows() \ No newline at end of file diff --git a/rover_ws/vision/pipeline.py b/rover_ws/vision/pipeline.py new file mode 100644 index 0000000..185f311 --- /dev/null +++ b/rover_ws/vision/pipeline.py @@ -0,0 +1,190 @@ +import cv2 +import numpy as np + +from homography import HomographyBEV +from lane_detection import LaneDetector + + +# ============================================================= +# HELPERS +# ============================================================= + +def lines_to_mask(lines, shape): + """ + Rasterise HoughLinesP segments onto a blank mask so we get + only the *detected-line* pixels, not the full edge image. + """ + mask = np.zeros(shape[:2], dtype=np.uint8) + + if lines is None: + return mask + + for line in lines: + x1, y1, x2, y2 = line[0] + cv2.line(mask, (x1, y1), (x2, y2), 255, thickness=2) + + return mask + + +def mask_to_pixels(mask): + """ + Return (N, 2) array of [u, v] pixel coordinates where mask > 0. + """ + ys, xs = np.where(mask > 0) + return np.stack([xs, ys], axis=1).astype(np.float64) + + +# ============================================================= +# PIPELINE CLASS +# ============================================================= + +class LaneBEVPipeline: + + def __init__(self, K, camera_height, pitch_deg, image_size): + + self.detector = LaneDetector() + + self.bev = HomographyBEV( + K=K, + camera_height=camera_height, + pitch_deg=pitch_deg, + image_size=image_size, + ) + + # ---------------------------------------------------------- + # PROCESS A SINGLE FRAME + # ---------------------------------------------------------- + + def process_frame(self, frame): + """ + Run the full pipeline on one BGR frame. + + Returns + ------- + output : annotated BGR frame from lane detector + bev_image : bird's-eye-view warp of the *frame* + lane_mask : rasterised detected-line mask + points : (N, 3) float32 ground-plane point cloud [X, Y, 0] + """ + + # 1. Lane detection ───────────────────────────────────── + output, edges, lines = self.detector.process(frame) + + # 2. Rasterise only the *detected lines* onto a mask ──── + lane_mask = lines_to_mask(lines, frame.shape) + + # 3. Project mask pixels → ground plane ───────────────── + pixels = mask_to_pixels(lane_mask) + + if len(pixels) == 0: + points = np.zeros((0, 3), dtype=np.float64) + else: + xy = self.bev.pixels_to_ground(pixels) # (N, 2) + z = np.zeros((len(xy), 1), dtype=np.float64) + points = np.hstack([xy, z]) # (N, 3) [X, Y, 0] + + # 4. BEV warp (visual debug) ───────────────────────────── + bev_image = self.bev.warp_to_bev(frame) + + return output, bev_image, lane_mask, points + + # ---------------------------------------------------------- + # SAVE HELPERS + # ---------------------------------------------------------- + + def save_pcd(self, points, filename="lane_cloud.pcd"): + self.bev.save_pcd(points, filename) + + +# ============================================================= +# MAIN LOOP +# ============================================================= + +def main(): + + # ── Camera intrinsics (adjust to your camera) ─────────────── + K = np.array([ + [1000, 0, 960], + [ 0, 1000, 540], + [ 0, 0, 1], + ], dtype=np.float64) + + cap = cv2.VideoCapture("../data/raw/test_lane.mp4") + + if not cap.isOpened(): + print("Cannot open video file") + return + + # Read the first frame to get resolution + ret, first_frame = cap.read() + if not ret: + print("Cannot read video") + return + + h, w = first_frame.shape[:2] + + # Reset to start + cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + + # ── Build pipeline ─────────────────────────────────────────── + pipeline = LaneBEVPipeline( + K=K, + camera_height=1.43, # metres + pitch_deg=-50, + image_size=(w, h), + ) + + frame_idx = 0 + all_points = [] # accumulate across frames if desired + + while True: + + ret, frame = cap.read() + + if not ret: + cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + continue + + output, bev_image, lane_mask, points = pipeline.process_frame(frame) + + print(f"[frame {frame_idx:04d}] " + f"lane pixels: {len(points):6d} " + f"| sample XY: {points[:2, :2] if len(points) >= 2 else '—'}") + + # ── Optional: save PCD every N frames ─────────────────── + if frame_idx % 30 == 0 and len(points) > 0: + pipeline.save_pcd(points, f"lane_cloud_{frame_idx:04d}.pcd") + + # ── Accumulate (comment out if memory is a concern) ───── + all_points.append(points) + + # ── Visualise ─────────────────────────────────────────── + # Overlay lane mask in red on the BEV for a quick debug view + bev_debug = cv2.cvtColor(bev_image, cv2.COLOR_BGR2RGB) \ + if len(bev_image.shape) == 3 else bev_image.copy() + + cv2.imshow("Lane Detection", output) + cv2.imshow("Lane Mask", lane_mask) + cv2.imshow("BEV", bev_image) + + key = cv2.waitKey(1) + if key == 27: # ESC → quit + break + if key == ord('s') and len(points) > 0: # S → save current frame + pipeline.save_pcd(points, f"lane_cloud_manual_{frame_idx:04d}.pcd") + print(f" → saved lane_cloud_manual_{frame_idx:04d}.pcd") + + frame_idx += 1 + + # ── (Optional) save the full accumulated cloud ─────────────── + if all_points: + merged = np.vstack([p for p in all_points if len(p) > 0]) + pipeline.save_pcd(merged, "lane_cloud_full.pcd") + print(f"Saved merged cloud: {len(merged)} points") + + cap.release() + cv2.destroyAllWindows() + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/rover_ws/vision/requirements.txt b/rover_ws/vision/requirements.txt new file mode 100644 index 0000000..d385a52 --- /dev/null +++ b/rover_ws/vision/requirements.txt @@ -0,0 +1,2 @@ +numpy==2.4.4 +opencv-contrib-python==4.13.0.92 From a9699c7dfff600ee0f95ad93dc58c5f5268d64b4 Mon Sep 17 00:00:00 2001 From: YassinEzzat2004 Date: Sun, 17 May 2026 00:05:04 +0300 Subject: [PATCH 02/11] updated rotation matrix based on camera phone resolution[960,1280] error up to 4 cm --- rover_ws/vision/homography.py | 33 ++++++++++++++++++++------------- 1 file changed, 20 insertions(+), 13 deletions(-) diff --git a/rover_ws/vision/homography.py b/rover_ws/vision/homography.py index e7cb1e9..4f0ed38 100644 --- a/rover_ws/vision/homography.py +++ b/rover_ws/vision/homography.py @@ -38,18 +38,26 @@ def __init__( def _build_extrinsics(self): + cp = np.cos(self.pitch) + sp = np.sin(self.pitch) + + # World-to-camera rotation self.R = np.array([ - [1, 0, 0], - [0, np.cos(self.pitch), -np.sin(self.pitch)], - [0, np.sin(self.pitch), np.cos(self.pitch)] + [-1, 0, 0], + [0, cp, -sp], + [0, sp, cp] ], dtype=np.float64) - self.t = np.array([ + # Camera position in world frame + C = np.array([ [0], [0], [self.camera_height] ], dtype=np.float64) + # Proper translation + self.t = -self.R @ C + # ========================================================= # BUILD HOMOGRAPHY # ========================================================= @@ -68,8 +76,8 @@ def _build_homography(self): def _build_bev_scaling(self): corners_px = np.array([ - [0, self.img_h // 2], - [self.img_w - 1, self.img_h // 2], + [0, self.img_h *0.5], + [self.img_w - 1, self.img_h *0.5], [0, self.img_h - 1], [self.img_w - 1, self.img_h - 1], ], dtype=np.float64) @@ -184,7 +192,7 @@ def warp_to_bev(self, image): borderValue=(0, 0, 0) ) - bev = cv2.flip(bev, 0) + #bev = cv2.flip(bev, 0) return bev @@ -226,23 +234,22 @@ def save_pcd(self, points, filename): h, w = image.shape[:2] K = np.array([ - [1000, 0, 960], - [0, 1000, 540], + [1000, 0, w/2], + [0, 1000, h/2], [0, 0, 1] ], dtype=np.float64) bev = HomographyBEV( K=K, camera_height=1.43, - pitch_deg=-50, + pitch_deg=-45, image_size=(w, h) ) - + print(f"w={w}\nh={h}") # ============================================================= # SINGLE PIXEL # ============================================================= - - X, Y = bev.pixel_to_ground(700, 700) + X, Y = bev.pixel_to_ground(442,620) print(f"Ground point: X={X:.3f}, Y={Y:.3f}") From 8b86a7ef5204eb8b1914187e985459ff32c3efae Mon Sep 17 00:00:00 2001 From: YassinEzzat2004 Date: Tue, 19 May 2026 00:43:00 +0300 Subject: [PATCH 03/11] circle detector methods added --- rover_ws/vision/lane_detection.py | 123 ---------- rover_ws/vision/pipeline.py | 102 ++++---- rover_ws/vision/road_features_detector.py | 274 ++++++++++++++++++++++ 3 files changed, 316 insertions(+), 183 deletions(-) delete mode 100644 rover_ws/vision/lane_detection.py create mode 100644 rover_ws/vision/road_features_detector.py diff --git a/rover_ws/vision/lane_detection.py b/rover_ws/vision/lane_detection.py deleted file mode 100644 index bbbbddf..0000000 --- a/rover_ws/vision/lane_detection.py +++ /dev/null @@ -1,123 +0,0 @@ -import cv2 -import numpy as np -from cv2 import ximgproc - -class LaneDetector: - - def __init__(self): - self.lower_white = np.array([0, 0, 200]) - self.upper_white = np.array([180, 50, 255]) - self.kernel = np.ones((5, 5), np.uint8) - - # ====================================================== - # ROI (kept separate, NOT used in pipeline by default) - # ====================================================== - def region_of_interest(self, frame): - - height, width = frame.shape[:2] - - polygon = np.array([[ - (0, height), - (width, height), - (width // 2 + 150, height // 2), - (width // 2 - 150, height // 2) - ]], np.int32) - - mask = np.zeros_like(frame) - cv2.fillPoly(mask, polygon, 255) - - return cv2.bitwise_and(frame, mask) - - # ====================================================== - # EDGE DETECTION (MATCHES YOUR ORIGINAL PIPELINE) - # ====================================================== - def detect_edges(self, frame): - - blur = cv2.GaussianBlur(frame, (5, 5), 0) - hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV) - - mask = cv2.inRange(hsv, self.lower_white, self.upper_white) - - mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, self.kernel) - mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel) - - thin = ximgproc.thinning(mask) - - return thin - - # ====================================================== - # HOUGH LINES - # ====================================================== - def detect_lines(self, edges): - - lines = cv2.HoughLinesP( - edges, - 1, - np.pi / 180, - 50, - minLineLength=50, - maxLineGap=30 - ) - - return lines - - # ====================================================== - # DRAW LINES - # ====================================================== - def draw_lines(self, frame, lines): - - if lines is None: - return frame - - for line in lines: - x1, y1, x2, y2 = line[0] - cv2.line(frame, (x1, y1), (x2, y2), (0, 255, 0), 5) - - return frame - - # ====================================================== - # FULL PIPELINE (NO ROI BY DEFAULT → matches your working code) - # ====================================================== - def process(self, frame): - - edges = self.detect_edges(frame) - lines = self.detect_lines(edges) - - output = self.draw_lines(frame.copy(), lines) - - return output, edges, lines - - -# ====================================================== -# MAIN LOOP -# ====================================================== -if __name__ == "__main__": - cap = cv2.VideoCapture("../data/raw/test_lane.mp4") - - if not cap.isOpened(): - print("Cannot open video file") - exit() - - detector = LaneDetector() - - while True: - - ret, frame = cap.read() - - if not ret: - cap.set(cv2.CAP_PROP_POS_FRAMES, 0) - continue - - output, edges, lines = detector.process(frame) - - print("lines:", lines) - - cv2.imshow("frame", output) - cv2.imshow("edges", edges) - - key = cv2.waitKey(1) - if key == 27: - break - - cap.release() - cv2.destroyAllWindows() \ No newline at end of file diff --git a/rover_ws/vision/pipeline.py b/rover_ws/vision/pipeline.py index 185f311..6f5ca02 100644 --- a/rover_ws/vision/pipeline.py +++ b/rover_ws/vision/pipeline.py @@ -2,7 +2,7 @@ import numpy as np from homography import HomographyBEV -from lane_detection import LaneDetector +from road_features_detector import RoadFeatureDetector # ============================================================= @@ -10,26 +10,16 @@ # ============================================================= def lines_to_mask(lines, shape): - """ - Rasterise HoughLinesP segments onto a blank mask so we get - only the *detected-line* pixels, not the full edge image. - """ mask = np.zeros(shape[:2], dtype=np.uint8) - if lines is None: return mask - for line in lines: x1, y1, x2, y2 = line[0] cv2.line(mask, (x1, y1), (x2, y2), 255, thickness=2) - return mask def mask_to_pixels(mask): - """ - Return (N, 2) array of [u, v] pixel coordinates where mask > 0. - """ ys, xs = np.where(mask > 0) return np.stack([xs, ys], axis=1).astype(np.float64) @@ -38,55 +28,56 @@ def mask_to_pixels(mask): # PIPELINE CLASS # ============================================================= -class LaneBEVPipeline: +class RoadFeatureBEVPipeline: def __init__(self, K, camera_height, pitch_deg, image_size): - self.detector = LaneDetector() - - self.bev = HomographyBEV( + self.detector = RoadFeatureDetector( K=K, camera_height=camera_height, pitch_deg=pitch_deg, image_size=image_size, ) + self.bev = self.detector.bev # reuse same HomographyBEV instance + # ---------------------------------------------------------- # PROCESS A SINGLE FRAME # ---------------------------------------------------------- def process_frame(self, frame): """ - Run the full pipeline on one BGR frame. - Returns ------- - output : annotated BGR frame from lane detector - bev_image : bird's-eye-view warp of the *frame* - lane_mask : rasterised detected-line mask - points : (N, 3) float32 ground-plane point cloud [X, Y, 0] + output : annotated BGR frame (lanes + circles drawn) + bev_image : bird's-eye-view warp of the frame + lane_mask : rasterised detected-line mask + lane_points : (N, 3) float64 lane ground point cloud [X, Y, 0] + ground_circles : list of (X, Y, radius_m) center tuples + circle_clouds : list of (N, 3) arrays, one per detected circle """ - # 1. Lane detection ───────────────────────────────────── - output, edges, lines = self.detector.process(frame) + # 1. Run detector (lanes + circles) ───────────────────── + output, edges, lines, ground_circles, circle_clouds, _ = \ + self.detector.process(frame, draw_bev=False) - # 2. Rasterise only the *detected lines* onto a mask ──── + # 2. Rasterise detected lines → mask ───────────────────── lane_mask = lines_to_mask(lines, frame.shape) - # 3. Project mask pixels → ground plane ───────────────── + # 3. Project lane mask pixels → ground plane ───────────── pixels = mask_to_pixels(lane_mask) if len(pixels) == 0: - points = np.zeros((0, 3), dtype=np.float64) + lane_points = np.zeros((0, 3), dtype=np.float64) else: - xy = self.bev.pixels_to_ground(pixels) # (N, 2) - z = np.zeros((len(xy), 1), dtype=np.float64) - points = np.hstack([xy, z]) # (N, 3) [X, Y, 0] + xy = self.bev.pixels_to_ground(pixels) + z = np.zeros((len(xy), 1), dtype=np.float64) + lane_points = np.hstack([xy, z]) - # 4. BEV warp (visual debug) ───────────────────────────── + # 4. BEV warp ──────────────────────────────────────────── bev_image = self.bev.warp_to_bev(frame) - return output, bev_image, lane_mask, points + return output, bev_image, lane_mask, lane_points, ground_circles, circle_clouds # ---------------------------------------------------------- # SAVE HELPERS @@ -102,7 +93,6 @@ def save_pcd(self, points, filename="lane_cloud.pcd"): def main(): - # ── Camera intrinsics (adjust to your camera) ─────────────── K = np.array([ [1000, 0, 960], [ 0, 1000, 540], @@ -115,68 +105,60 @@ def main(): print("Cannot open video file") return - # Read the first frame to get resolution ret, first_frame = cap.read() if not ret: print("Cannot read video") return h, w = first_frame.shape[:2] - - # Reset to start cap.set(cv2.CAP_PROP_POS_FRAMES, 0) - # ── Build pipeline ─────────────────────────────────────────── - pipeline = LaneBEVPipeline( + pipeline = RoadFeatureBEVPipeline( K=K, - camera_height=1.43, # metres + camera_height=1.43, pitch_deg=-50, image_size=(w, h), ) - frame_idx = 0 - all_points = [] # accumulate across frames if desired + frame_idx = 0 + all_points = [] while True: ret, frame = cap.read() - if not ret: cap.set(cv2.CAP_PROP_POS_FRAMES, 0) continue - output, bev_image, lane_mask, points = pipeline.process_frame(frame) + output, bev_image, lane_mask, lane_points, ground_circles, circle_clouds = \ + pipeline.process_frame(frame) print(f"[frame {frame_idx:04d}] " - f"lane pixels: {len(points):6d} " - f"| sample XY: {points[:2, :2] if len(points) >= 2 else '—'}") + f"lane pixels: {len(lane_points):6d} " + f"circles: {len(ground_circles)}") - # ── Optional: save PCD every N frames ─────────────────── - if frame_idx % 30 == 0 and len(points) > 0: - pipeline.save_pcd(points, f"lane_cloud_{frame_idx:04d}.pcd") + for i, (cloud, (X, Y, rm)) in enumerate(zip(circle_clouds, ground_circles)): + print(f" circle {i+1}: center=({X:.2f}m, {Y:.2f}m) " + f"r={rm:.2f}m cloud_pts={len(cloud)}") - # ── Accumulate (comment out if memory is a concern) ───── - all_points.append(points) + #if frame_idx % 30 == 0 and len(lane_points) > 0: + # pipeline.save_pcd(lane_points, f"lane_cloud_{frame_idx:04d}.pcd") - # ── Visualise ─────────────────────────────────────────── - # Overlay lane mask in red on the BEV for a quick debug view - bev_debug = cv2.cvtColor(bev_image, cv2.COLOR_BGR2RGB) \ - if len(bev_image.shape) == 3 else bev_image.copy() + all_points.append(lane_points) - cv2.imshow("Lane Detection", output) - cv2.imshow("Lane Mask", lane_mask) - cv2.imshow("BEV", bev_image) + cv2.imshow("Road Features", output) + cv2.imshow("Lane Mask", lane_mask) + cv2.imshow("BEV", bev_image) key = cv2.waitKey(1) - if key == 27: # ESC → quit + if key == 27: break - if key == ord('s') and len(points) > 0: # S → save current frame - pipeline.save_pcd(points, f"lane_cloud_manual_{frame_idx:04d}.pcd") + if key == ord('s') and len(lane_points) > 0: + pipeline.save_pcd(lane_points, f"lane_cloud_manual_{frame_idx:04d}.pcd") print(f" → saved lane_cloud_manual_{frame_idx:04d}.pcd") frame_idx += 1 - # ── (Optional) save the full accumulated cloud ─────────────── if all_points: merged = np.vstack([p for p in all_points if len(p) > 0]) pipeline.save_pcd(merged, "lane_cloud_full.pcd") diff --git a/rover_ws/vision/road_features_detector.py b/rover_ws/vision/road_features_detector.py new file mode 100644 index 0000000..fc60fd8 --- /dev/null +++ b/rover_ws/vision/road_features_detector.py @@ -0,0 +1,274 @@ +import cv2 +import numpy as np +from cv2 import ximgproc +from homography import HomographyBEV + + +class RoadFeatureDetector: + + def __init__(self, K, camera_height, pitch_deg, image_size, dist_coeffs=None, + min_radius=10, max_radius=200): + + self.bev = HomographyBEV( + K=np.array(K, dtype=np.float64), + camera_height=float(camera_height), + pitch_deg=float(pitch_deg), + image_size=tuple(image_size), + dist_coeffs=None if dist_coeffs is None else np.array(dist_coeffs, dtype=np.float64) + ) + + self.min_radius = min_radius + self.max_radius = max_radius + + self.lower_white = np.array([0, 0, 200]) + self.upper_white = np.array([180, 50, 255]) + self.kernel = np.ones((5, 5), np.uint8) + + # ====================================================== + # ROI (kept separate, NOT used in pipeline by default) + # ====================================================== + def region_of_interest(self, frame): + + height, width = frame.shape[:2] + + polygon = np.array([[ + (0, height), + (width, height), + (width // 2 + 150, height // 2), + (width // 2 - 150, height // 2) + ]], np.int32) + + mask = np.zeros_like(frame) + cv2.fillPoly(mask, polygon, 255) + + return cv2.bitwise_and(frame, mask) + + # ====================================================== + # EDGE DETECTION + # ====================================================== + def detect_edges(self, frame): + + blur = cv2.GaussianBlur(frame, (5, 5), 0) + hsv = cv2.cvtColor(blur, cv2.COLOR_BGR2HSV) + + mask = cv2.inRange(hsv, self.lower_white, self.upper_white) + + mask = cv2.morphologyEx(mask, cv2.MORPH_OPEN, self.kernel) + mask = cv2.morphologyEx(mask, cv2.MORPH_CLOSE, self.kernel) + + thin = ximgproc.thinning(mask) + + return thin + + # ====================================================== + # HOUGH LINES + # ====================================================== + def detect_lines(self, edges): + + lines = cv2.HoughLinesP( + edges, + 1, + np.pi / 180, + 50, + minLineLength=50, + maxLineGap=30 + ) + + return lines + + # ====================================================== + # DRAW LINES + # ====================================================== + def draw_lines(self, frame, lines): + + if lines is None: + return frame + + for line in lines: + x1, y1, x2, y2 = line[0] + cv2.line(frame, (x1, y1), (x2, y2), (0, 255, 0), 5) + + return frame + + # ====================================================== + # CIRCLE DETECTION + # ====================================================== + def _detect_circles(self, image): + + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + lower_white = np.array([0, 0, 200]) + upper_white = np.array([180, 40, 255]) + + white_mask = cv2.inRange(hsv, lower_white, upper_white) + kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)) + white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_OPEN, kernel) + white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_CLOSE, kernel) + + blurred = cv2.GaussianBlur(white_mask, (9, 9), 2) + circles = cv2.HoughCircles( + blurred, cv2.HOUGH_GRADIENT, dp=1.2, minDist=30, + param1=50, param2=30, + minRadius=self.min_radius, maxRadius=self.max_radius + ) + + detected = [] + if circles is not None: + for x, y, r in np.round(circles[0]).astype(int): + y0, y1 = max(0, y - r), min(image.shape[0], y + r) + x0, x1 = max(0, x - r), min(image.shape[1], x + r) + roi = white_mask[y0:y1, x0:x1] + if roi.size == 0: + continue + white_ratio = np.sum(roi > 0) / roi.size + if white_ratio > 0.35: + detected.append((int(x), int(y), int(r))) + + # Contour fallback + contours, _ = cv2.findContours(white_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + for cnt in contours: + area = cv2.contourArea(cnt) + if area < np.pi * (self.min_radius ** 2) or area > np.pi * (self.max_radius ** 2): + continue + perimeter = cv2.arcLength(cnt, True) + if perimeter == 0: + continue + circularity = 4 * np.pi * area / (perimeter * perimeter) + if circularity < 0.6: + continue + (cx, cy), radius = cv2.minEnclosingCircle(cnt) + if self.min_radius <= radius <= self.max_radius: + detected.append((int(cx), int(cy), int(radius))) + + # Merge close duplicates + merged = [] + for x, y, r in detected: + dup = False + for mx, my, mr in merged: + if np.hypot(mx - x, my - y) < 20: + dup = True + break + if not dup: + merged.append((x, y, r)) + + return merged + + # ====================================================== + # MAP CIRCLE CENTER TO GROUND + # ====================================================== + def circle_to_ground(self, circle): + + x, y, r = circle + X, Y = self.bev.pixel_to_ground(x, y) + Xp, Yp = self.bev.pixel_to_ground(x + r, y) + radius_m = np.hypot(Xp - X, Yp - Y) + + return (X, Y, radius_m) + + # ====================================================== + # CIRCLE POINT CLOUD + # ====================================================== + def circle_to_ground_cloud(self, circle, num_points=36, filled=False): + + x, y, r = circle + X, Y = self.bev.pixel_to_ground(x, y) + Xp, Yp = self.bev.pixel_to_ground(x + r, y) + radius_m = np.hypot(Xp - X, Yp - Y) + + angles = np.linspace(0, 2 * np.pi, num_points, endpoint=False) + + if filled: + radii = np.linspace(0, radius_m, num=10) + points = [ + (X + rr * np.cos(a), Y + rr * np.sin(a), 0.0) + for rr in radii for a in angles + ] + else: + points = [ + (X + radius_m * np.cos(a), Y + radius_m * np.sin(a), 0.0) + for a in angles + ] + + return np.array(points, dtype=np.float64) # (N, 3) + + # ====================================================== + # FULL PIPELINE + # ====================================================== + def process(self, frame, draw_bev=False): + + # --- Lanes --- + edges = self.detect_edges(frame) + lines = self.detect_lines(edges) + output = self.draw_lines(frame.copy(), lines) + + # --- Circles --- + circles = self._detect_circles(frame) + ground_circles = [self.circle_to_ground(c) for c in circles] + circle_clouds = [self.circle_to_ground_cloud(c) for c in circles] + + for x, y, r in circles: + cv2.circle(output, (x, y), r, (0, 255, 0), 2) + + bev_image = None + if draw_bev: + bev_image = self.bev.warp_to_bev(output) + + return output, edges, lines, ground_circles, circle_clouds, bev_image + + +# ====================================================== +# MAIN LOOP +# ====================================================== +if __name__ == "__main__": + + K = np.array([ + [1000, 0, 960], + [0, 1000, 540], + [0, 0, 1] + ], dtype=np.float64) + + camera_height = 1.2 + pitch_deg = -30 + + cap = cv2.VideoCapture("../data/raw/test_lane.mp4") + if not cap.isOpened(): + print("Cannot open video file") + exit() + + ret, frame = cap.read() + if not ret: + print("Cannot read frame from camera") + exit() + + img_h, img_w = frame.shape[:2] + cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + + detector = RoadFeatureDetector( + K=K, + camera_height=camera_height, + pitch_deg=pitch_deg, + image_size=(img_w, img_h) + ) + + while True: + ret, frame = cap.read() + if not ret: + cap.set(cv2.CAP_PROP_POS_FRAMES, 0) + continue + + output, edges, lines, ground_circles, circle_clouds, bev = \ + detector.process(frame, draw_bev=True) + + print("Ground circles:", ground_circles) + for i, cloud in enumerate(circle_clouds): + print(f" circle {i+1} cloud points: {len(cloud)}") + + cv2.imshow("Road Features", output) + cv2.imshow("Edges", edges) + if bev is not None: + cv2.imshow("BEV", bev) + + if cv2.waitKey(1) == 27: + break + + cap.release() + cv2.destroyAllWindows() \ No newline at end of file From 0428ada6bd5a63810a79b4a96437a0b74320f00d Mon Sep 17 00:00:00 2001 From: unknown Date: Mon, 18 May 2026 23:19:05 +0300 Subject: [PATCH 04/11] add circles detection --- rover_ws/vision/circle_detection_bev.py | 165 ++++++++++++++++++++++++ 1 file changed, 165 insertions(+) create mode 100644 rover_ws/vision/circle_detection_bev.py diff --git a/rover_ws/vision/circle_detection_bev.py b/rover_ws/vision/circle_detection_bev.py new file mode 100644 index 0000000..a075085 --- /dev/null +++ b/rover_ws/vision/circle_detection_bev.py @@ -0,0 +1,165 @@ +import cv2 +import numpy as np +from homography import HomographyBEV + + +class CircleDetectorBEV: + + def __init__(self, K, camera_height, pitch_deg, image_size, dist_coeffs=None, + min_radius=10, max_radius=200): + self.bev = HomographyBEV( + K=np.array(K, dtype=np.float64), + camera_height=float(camera_height), + pitch_deg=float(pitch_deg), + image_size=tuple(image_size), + dist_coeffs=None if dist_coeffs is None else np.array(dist_coeffs, dtype=np.float64) + ) + + self.min_radius = min_radius + self.max_radius = max_radius + + # ----------------------- + # Circle detection + # ----------------------- + def _detect_circles(self, image): + hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) + lower_white = np.array([0, 0, 200]) + upper_white = np.array([180, 40, 255]) + + white_mask = cv2.inRange(hsv, lower_white, upper_white) + kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)) + white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_OPEN, kernel) + white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_CLOSE, kernel) + + # Hough on blurred mask + blurred = cv2.GaussianBlur(white_mask, (9, 9), 2) + circles = cv2.HoughCircles( + blurred, cv2.HOUGH_GRADIENT, dp=1.2, minDist=30, + param1=50, param2=30, + minRadius=self.min_radius, maxRadius=self.max_radius + ) + + detected = [] + if circles is not None: + for x, y, r in np.round(circles[0]).astype(int): + # validate white ratio inside circle + y0, y1 = max(0, y - r), min(image.shape[0], y + r) + x0, x1 = max(0, x - r), min(image.shape[1], x + r) + roi = white_mask[y0:y1, x0:x1] + if roi.size == 0: + continue + white_ratio = np.sum(roi > 0) / roi.size + if white_ratio > 0.35: + detected.append((int(x), int(y), int(r))) + + # Contour fallback + contours, _ = cv2.findContours(white_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) + for cnt in contours: + area = cv2.contourArea(cnt) + if area < np.pi * (self.min_radius ** 2) or area > np.pi * (self.max_radius ** 2): + continue + perimeter = cv2.arcLength(cnt, True) + if perimeter == 0: + continue + circularity = 4 * np.pi * area / (perimeter * perimeter) + if circularity < 0.6: + continue + (cx, cy), radius = cv2.minEnclosingCircle(cnt) + if self.min_radius <= radius <= self.max_radius: + detected.append((int(cx), int(cy), int(radius))) + + # merge close duplicates + merged = [] + for x, y, r in detected: + dup = False + for i, (mx, my, mr) in enumerate(merged): + if np.hypot(mx - x, my - y) < 20: + dup = True + break + if not dup: + merged.append((x, y, r)) + + return merged + + # ----------------------- + # Map circles to ground coordinates + # ----------------------- + def circle_to_ground(self, circle): + x, y, r = circle + X, Y = self.bev.pixel_to_ground(x, y) + # approximate radius in meters by mapping center and center+radius in image + xp, yp = x + r, y + Xp, Yp = self.bev.pixel_to_ground(xp, yp) + radius_m = np.hypot(Xp - X, Yp - Y) + return (X, Y, radius_m) + + # ----------------------- + # Full process + # ----------------------- + def process(self, image, draw_bev=False): + circles = self._detect_circles(image) + + ground_circles = [self.circle_to_ground(c) for c in circles] + + annotated = image.copy() + for i, (x, y, r) in enumerate(circles): + cv2.circle(annotated, (x, y), r, (0, 255, 0), 2) + cv2.circle(annotated, (x, y), 3, (0, 0, 255), -1) + X, Y, rm = ground_circles[i] + cv2.putText(annotated, f"{i+1}: ({X:.2f}m,{Y:.2f}m) r={rm:.2f}m", + (x - 30, y - r - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 0), 1) + + bev_image = None + if draw_bev: + bev_image = self.bev.warp_to_bev(annotated) + + return annotated, bev_image, ground_circles + + +# ----------------------- +# Main Loop +# ----------------------- +if __name__ == "__main__": + # Basic K example; replace with your calibrated intrinsics + K = np.array([ + [1000, 0, 960], + [0, 1000, 540], + [0, 0, 1] + ], dtype=np.float64) + + # Adjust these to match your camera + camera_height = 1.2 + pitch_deg = -30 + + cap = cv2.VideoCapture("D:\\Software work\\rover 26\\test_lane.mp4") + if not cap.isOpened(): + print("Cannot open camera, try changing source") + exit() + + ret, frame = cap.read() + if not ret: + print("Cannot read frame from camera") + exit() + + img_h, img_w = frame.shape[:2] + + detector = CircleDetectorBEV(K=K, camera_height=camera_height, pitch_deg=pitch_deg, + image_size=(img_w, img_h)) + + while True: + ret, frame = cap.read() + if not ret: + break + + annotated, bev, circles = detector.process(frame, draw_bev=True) + print("Detected circles (X, Y, radius_m):", circles) + cv2.imshow("Annotated", annotated) + if bev is not None: + cv2.imshow("BEV", bev) + + key = cv2.waitKey(1) + if key == 27: + break + + cap.release() + cv2.destroyAllWindows() From 9844528afdd302145bc49cfb08bda229e8a7b7fb Mon Sep 17 00:00:00 2001 From: YassinEzzat2004 Date: Tue, 19 May 2026 00:46:59 +0300 Subject: [PATCH 05/11] . --- rover_ws/vision/circle_detection_bev.py | 165 ------------------------ 1 file changed, 165 deletions(-) delete mode 100644 rover_ws/vision/circle_detection_bev.py diff --git a/rover_ws/vision/circle_detection_bev.py b/rover_ws/vision/circle_detection_bev.py deleted file mode 100644 index a075085..0000000 --- a/rover_ws/vision/circle_detection_bev.py +++ /dev/null @@ -1,165 +0,0 @@ -import cv2 -import numpy as np -from homography import HomographyBEV - - -class CircleDetectorBEV: - - def __init__(self, K, camera_height, pitch_deg, image_size, dist_coeffs=None, - min_radius=10, max_radius=200): - self.bev = HomographyBEV( - K=np.array(K, dtype=np.float64), - camera_height=float(camera_height), - pitch_deg=float(pitch_deg), - image_size=tuple(image_size), - dist_coeffs=None if dist_coeffs is None else np.array(dist_coeffs, dtype=np.float64) - ) - - self.min_radius = min_radius - self.max_radius = max_radius - - # ----------------------- - # Circle detection - # ----------------------- - def _detect_circles(self, image): - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - lower_white = np.array([0, 0, 200]) - upper_white = np.array([180, 40, 255]) - - white_mask = cv2.inRange(hsv, lower_white, upper_white) - kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)) - white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_OPEN, kernel) - white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_CLOSE, kernel) - - # Hough on blurred mask - blurred = cv2.GaussianBlur(white_mask, (9, 9), 2) - circles = cv2.HoughCircles( - blurred, cv2.HOUGH_GRADIENT, dp=1.2, minDist=30, - param1=50, param2=30, - minRadius=self.min_radius, maxRadius=self.max_radius - ) - - detected = [] - if circles is not None: - for x, y, r in np.round(circles[0]).astype(int): - # validate white ratio inside circle - y0, y1 = max(0, y - r), min(image.shape[0], y + r) - x0, x1 = max(0, x - r), min(image.shape[1], x + r) - roi = white_mask[y0:y1, x0:x1] - if roi.size == 0: - continue - white_ratio = np.sum(roi > 0) / roi.size - if white_ratio > 0.35: - detected.append((int(x), int(y), int(r))) - - # Contour fallback - contours, _ = cv2.findContours(white_mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE) - for cnt in contours: - area = cv2.contourArea(cnt) - if area < np.pi * (self.min_radius ** 2) or area > np.pi * (self.max_radius ** 2): - continue - perimeter = cv2.arcLength(cnt, True) - if perimeter == 0: - continue - circularity = 4 * np.pi * area / (perimeter * perimeter) - if circularity < 0.6: - continue - (cx, cy), radius = cv2.minEnclosingCircle(cnt) - if self.min_radius <= radius <= self.max_radius: - detected.append((int(cx), int(cy), int(radius))) - - # merge close duplicates - merged = [] - for x, y, r in detected: - dup = False - for i, (mx, my, mr) in enumerate(merged): - if np.hypot(mx - x, my - y) < 20: - dup = True - break - if not dup: - merged.append((x, y, r)) - - return merged - - # ----------------------- - # Map circles to ground coordinates - # ----------------------- - def circle_to_ground(self, circle): - x, y, r = circle - X, Y = self.bev.pixel_to_ground(x, y) - # approximate radius in meters by mapping center and center+radius in image - xp, yp = x + r, y - Xp, Yp = self.bev.pixel_to_ground(xp, yp) - radius_m = np.hypot(Xp - X, Yp - Y) - return (X, Y, radius_m) - - # ----------------------- - # Full process - # ----------------------- - def process(self, image, draw_bev=False): - circles = self._detect_circles(image) - - ground_circles = [self.circle_to_ground(c) for c in circles] - - annotated = image.copy() - for i, (x, y, r) in enumerate(circles): - cv2.circle(annotated, (x, y), r, (0, 255, 0), 2) - cv2.circle(annotated, (x, y), 3, (0, 0, 255), -1) - X, Y, rm = ground_circles[i] - cv2.putText(annotated, f"{i+1}: ({X:.2f}m,{Y:.2f}m) r={rm:.2f}m", - (x - 30, y - r - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.4, (255, 255, 0), 1) - - bev_image = None - if draw_bev: - bev_image = self.bev.warp_to_bev(annotated) - - return annotated, bev_image, ground_circles - - -# ----------------------- -# Main Loop -# ----------------------- -if __name__ == "__main__": - # Basic K example; replace with your calibrated intrinsics - K = np.array([ - [1000, 0, 960], - [0, 1000, 540], - [0, 0, 1] - ], dtype=np.float64) - - # Adjust these to match your camera - camera_height = 1.2 - pitch_deg = -30 - - cap = cv2.VideoCapture("D:\\Software work\\rover 26\\test_lane.mp4") - if not cap.isOpened(): - print("Cannot open camera, try changing source") - exit() - - ret, frame = cap.read() - if not ret: - print("Cannot read frame from camera") - exit() - - img_h, img_w = frame.shape[:2] - - detector = CircleDetectorBEV(K=K, camera_height=camera_height, pitch_deg=pitch_deg, - image_size=(img_w, img_h)) - - while True: - ret, frame = cap.read() - if not ret: - break - - annotated, bev, circles = detector.process(frame, draw_bev=True) - print("Detected circles (X, Y, radius_m):", circles) - cv2.imshow("Annotated", annotated) - if bev is not None: - cv2.imshow("BEV", bev) - - key = cv2.waitKey(1) - if key == 27: - break - - cap.release() - cv2.destroyAllWindows() From 0118132ee506d36e66ea15c26df7ad2e5dc3b27c Mon Sep 17 00:00:00 2001 From: YassinEzzat2004 Date: Tue, 19 May 2026 23:03:54 +0300 Subject: [PATCH 06/11] readme added --- rover_ws/vision/README.md | 205 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 205 insertions(+) create mode 100644 rover_ws/vision/README.md diff --git a/rover_ws/vision/README.md b/rover_ws/vision/README.md new file mode 100644 index 0000000..ed9f035 --- /dev/null +++ b/rover_ws/vision/README.md @@ -0,0 +1,205 @@ +# 🛰️ Rover Vision — Road Feature Detection & BEV Mapping + +A real-time computer vision pipeline for autonomous rover competition, performing **lane detection**, **road marker recognition**, and **bird's-eye-view (BEV) ground projection** from a single monocular camera. + +--- + +## Overview + +This system takes a forward-facing camera feed and produces: + +- Detected **lane lines** in pixel space and metric ground coordinates +- Detected **circular road markers** (e.g. roundabouts, stop circles) with real-world center and radius +- A **bird's-eye-view warp** of the scene for top-down situational awareness +- **3D point clouds** (`.pcd`) of lane lines and circle boundaries on the ground plane + +--- + +## Architecture + +``` +Camera Frame + │ + ▼ +RoadFeatureDetector + ├── detect_edges() → White-line mask (HSV threshold + morphology + thinning) + ├── detect_lines() → Hough line segments (lane markings) + └── _detect_circles() → Hough circles + contour fallback (road markers) + │ + ▼ +HomographyBEV + ├── pixel_to_ground() → Single pixel → (X, Y) metric ground coords + ├── pixels_to_ground() → Batch pixel projection + ├── mask_to_pointcloud()→ Full mask → (N, 3) XYZ point cloud + └── warp_to_bev() → Full image perspective warp to top-down view + │ + ▼ +RoadFeatureBEVPipeline + ├── Lane point cloud → (N, 3) float64 [X, Y, 0] in metres + ├── Circle list → [(X, Y, radius_m), ...] + ├── Circle clouds → [(N, 3), ...] ring/disc point clouds + └── BEV image → Warped top-down BGR frame +``` + +--- + +## Modules + +| File | Description | +|---|---| +| `homography.py` | Camera model, homography computation, ground projection, BEV warping, PCD export | +| `road_features_detector.py` | Lane and circle detection pipeline on raw frames | +| `pipeline.py` | End-to-end orchestration; outputs annotated frames, BEV, and point clouds | + +--- + +## Setup + +### Requirements + +```bash +pip install opencv-python opencv-contrib-python numpy +``` + +> `opencv-contrib-python` is required for `cv2.ximgproc.thinning` (skeletonisation of lane masks). + +### Folder Structure + +``` +project/ +├── data/ +│ └── raw/ +│ ├── test_lane.mp4 +│ └── ground.jpeg +├── homography.py +├── road_features_detector.py +└── pipeline.py +``` + +--- + +## Camera Calibration + +The pipeline requires intrinsic camera parameters. Provide the **3×3 intrinsic matrix K**: + +```python +K = np.array([ + [fx, 0, cx], + [ 0, fy, cy], + [ 0, 0, 1] +], dtype=np.float64) +``` + +And the camera **mounting parameters**: + +| Parameter | Description | +|---|---| +| `camera_height` | Height of camera above ground plane (metres) | +| `pitch_deg` | Camera pitch angle (degrees, negative = downward tilt) | + +Example values used in competition testing: + +```python +camera_height = 1.43 # metres +pitch_deg = -50 # degrees +``` + +--- + +## Usage + +### Run the full pipeline on a video + +```bash +python pipeline.py +``` + +This opens a video feed and displays three windows: + +- **Road Features** — annotated frame with detected lanes and circles +- **Lane Mask** — binary mask of detected lane lines +- **BEV** — bird's-eye-view warp of the current frame + +### Keyboard Controls + +| Key | Action | +|---|---| +| `ESC` | Quit | +| `s` | Save current frame's lane point cloud as `.pcd` | + +On exit, the full merged lane point cloud across all frames is saved to `lane_cloud_full.pcd`. + +### Run homography standalone (single image) + +```bash +python homography.py +``` + +Loads `data/raw/ground.jpeg`, projects a pixel to metric ground, warps to BEV, generates a point cloud from a thresholded mask, and saves `ground_plane.pcd`. + +--- + +## Outputs + +### Point Cloud (`.pcd`) + +Ground-plane point clouds are saved in **PCD ASCII format** compatible with CloudCompare, Open3D, and ROS: + +``` +FIELDS x y z +TYPE F F F +DATA ascii +``` + +Each point is a 3D position `(X, Y, 0)` in the rover's ground coordinate frame, where: + +- **X** — lateral axis (left/right) +- **Y** — forward axis (depth from rover) +- **Z** — always 0 (ground plane) + +### Circle Detection Output + +Each detected circle is reported as: + +``` +center = (X, Y) # metric ground position in metres +radius = r # estimated real-world radius in metres +``` + +--- + +## Coordinate Frame + +``` + ▲ Y (forward) + │ + │ + ────────┼────────▶ X (right) + │ + Camera/Rover origin +``` + +The ground plane is `Z = 0`. All projections assume a flat ground surface. + +--- + +## Tuning + +Key parameters to adjust for different environments: + +| Parameter | Location | Effect | +|---|---|---| +| `lower_white` / `upper_white` | `RoadFeatureDetector.__init__` | HSV range for white line detection | +| `min_radius` / `max_radius` | `RoadFeatureDetector.__init__` | Circle size filter (pixels) | +| Hough `threshold`, `minLineLength`, `maxLineGap` | `detect_lines()` | Lane line sensitivity | +| `circularity` threshold (0.6) | `_detect_circles()` | Roundness filter for contour fallback | +| `white_ratio` threshold (0.35) | `_detect_circles()` | Minimum white fill inside detected circle | + +--- + +## Competition Notes + +- The pipeline runs **frame-by-frame** with no temporal filtering — adding a Kalman filter or frame-to-frame tracking would improve stability. +- BEV warping assumes a **flat, level ground plane**. Uneven terrain will introduce projection errors. +- Point clouds accumulate across frames (`all_points` list in `pipeline.py`) and are merged on exit — useful for building a local map of the course. +- For real-time performance, consider downscaling input frames before processing. \ No newline at end of file From 987fa6c7a7e9658f4d65a8e648fd58a51eabf4ad Mon Sep 17 00:00:00 2001 From: YassinEzzat2004 Date: Wed, 20 May 2026 20:14:09 +0300 Subject: [PATCH 07/11] paw and roll added for error compensating --- rover_ws/vision/README.md | 42 ++++++++-- rover_ws/vision/homography.py | 151 ++++++++++++++++++++-------------- 2 files changed, 128 insertions(+), 65 deletions(-) diff --git a/rover_ws/vision/README.md b/rover_ws/vision/README.md index ed9f035..7438954 100644 --- a/rover_ws/vision/README.md +++ b/rover_ws/vision/README.md @@ -31,7 +31,7 @@ HomographyBEV ├── pixel_to_ground() → Single pixel → (X, Y) metric ground coords ├── pixels_to_ground() → Batch pixel projection ├── mask_to_pointcloud()→ Full mask → (N, 3) XYZ point cloud - └── warp_to_bev() → Full image perspective warp to top-down view + └── warp_to_bev() → Undistort + perspective warp to top-down view │ ▼ RoadFeatureBEVPipeline @@ -47,7 +47,7 @@ RoadFeatureBEVPipeline | File | Description | |---|---| -| `homography.py` | Camera model, homography computation, ground projection, BEV warping, PCD export | +| `homography.py` | Camera model, homography computation, lens undistortion, ground projection, BEV warping, PCD export | | `road_features_detector.py` | Lane and circle detection pipeline on raw frames | | `pipeline.py` | End-to-end orchestration; outputs annotated frames, BEV, and point clouds | @@ -90,19 +90,49 @@ K = np.array([ ], dtype=np.float64) ``` +Optionally provide a **distortion coefficient vector** (OpenCV 5-parameter model): + +```python +dist_coeffs = np.array([k1, k2, p1, p2, k3]) +``` + +If omitted, distortion is assumed to be zero. When provided, lens undistortion is applied automatically before every projection and BEV warp. + And the camera **mounting parameters**: | Parameter | Description | |---|---| | `camera_height` | Height of camera above ground plane (metres) | | `pitch_deg` | Camera pitch angle (degrees, negative = downward tilt) | +| `yaw_deg` | Camera yaw angle (degrees, positive = rotated right). Default `0.0` | +| `roll_deg` | Camera roll angle (degrees, positive = tilted right). Default `0.0` | Example values used in competition testing: ```python -camera_height = 1.43 # metres -pitch_deg = -50 # degrees +camera_height = 1.33 # metres +pitch_deg = -45 # degrees +yaw_deg = -2 # degrees — small left offset from mount +roll_deg = -7 # degrees — slight sideways tilt +``` + +### Rotation Convention + +The full world-to-camera rotation is composed as: + ``` +R = M · R_pitch · R_yaw · R_roll +``` + +where `M = diag(-1, 1, 1)` mirrors the X axis so that +X is rightward in the image. Each angle has an independent, physically meaningful effect: + +| Angle | Axis | Effect | +|---|---|---| +| `pitch_deg` | X | Tilts camera up/down — primary mounting angle | +| `yaw_deg` | Z | Rotates camera left/right relative to rover heading | +| `roll_deg` | Y | Tilts camera sideways — corrects lateral mounting error | + +For a perfectly aligned camera only `pitch_deg` is non-zero. In practice, small yaw/roll corrections (±2–7°) compensate for mounting tolerances that would otherwise produce a skewed BEV. --- @@ -194,6 +224,7 @@ Key parameters to adjust for different environments: | Hough `threshold`, `minLineLength`, `maxLineGap` | `detect_lines()` | Lane line sensitivity | | `circularity` threshold (0.6) | `_detect_circles()` | Roundness filter for contour fallback | | `white_ratio` threshold (0.35) | `_detect_circles()` | Minimum white fill inside detected circle | +| `yaw_deg` / `roll_deg` | `HomographyBEV.__init__` | Fine-tune mounting misalignment; adjust until BEV lanes appear straight and parallel | --- @@ -202,4 +233,5 @@ Key parameters to adjust for different environments: - The pipeline runs **frame-by-frame** with no temporal filtering — adding a Kalman filter or frame-to-frame tracking would improve stability. - BEV warping assumes a **flat, level ground plane**. Uneven terrain will introduce projection errors. - Point clouds accumulate across frames (`all_points` list in `pipeline.py`) and are merged on exit — useful for building a local map of the course. -- For real-time performance, consider downscaling input frames before processing. \ No newline at end of file +- For real-time performance, consider downscaling input frames before processing. +- Lens undistortion is applied on every frame and every pixel projection. If `dist_coeffs` are inaccurate, straight lines will appear curved in the BEV — re-run calibration with a checkerboard to obtain reliable coefficients. diff --git a/rover_ws/vision/homography.py b/rover_ws/vision/homography.py index 4f0ed38..193cfd4 100644 --- a/rover_ws/vision/homography.py +++ b/rover_ws/vision/homography.py @@ -9,7 +9,9 @@ def __init__( K, camera_height, pitch_deg, - image_size, + yaw_deg=0.0, + roll_deg=0.0, + image_size=None, dist_coeffs=None ): @@ -23,7 +25,12 @@ def __init__( self.camera_height = camera_height self.pitch_deg = pitch_deg + self.yaw_deg = yaw_deg + self.roll_deg = roll_deg + self.pitch = np.deg2rad(pitch_deg) + self.yaw = np.deg2rad(yaw_deg) + self.roll = np.deg2rad(roll_deg) self.img_w = image_size[0] self.img_h = image_size[1] @@ -38,24 +45,40 @@ def __init__( def _build_extrinsics(self): - cp = np.cos(self.pitch) - sp = np.sin(self.pitch) + cp, sp = np.cos(self.pitch), np.sin(self.pitch) + cy, sy = np.cos(self.yaw), np.sin(self.yaw) + cr, sr = np.cos(self.roll), np.sin(self.roll) - # World-to-camera rotation - self.R = np.array([ - [-1, 0, 0], - [0, cp, -sp], - [0, sp, cp] + # Rotation around X-axis (pitch: tilts camera up/down) + R_pitch = np.array([ + [ 1, 0, 0], + [ 0, cp, -sp], + [ 0, sp, cp] ], dtype=np.float64) - # Camera position in world frame - C = np.array([ - [0], - [0], - [self.camera_height] + # Rotation around Z-axis (yaw: rotates camera left/right) + R_yaw = np.array([ + [ cy, -sy, 0], + [ sy, cy, 0], + [ 0, 0, 1] ], dtype=np.float64) - # Proper translation + # Rotation around Y-axis (roll: tilts camera sideways) + R_roll = np.array([ + [ cr, 0, sr], + [ 0, 1, 0], + [-sr, 0, cr] + ], dtype=np.float64) + + # Full world-to-camera rotation + R_world = R_pitch @ R_yaw @ R_roll + + # Mirror X so +X is rightward in the image + M = np.diag([-1.0, 1.0, 1.0]) + self.R = M @ R_world + + # Camera position in world frame + C = np.array([[0], [0], [self.camera_height]], dtype=np.float64) self.t = -self.R @ C # ========================================================= @@ -76,26 +99,23 @@ def _build_homography(self): def _build_bev_scaling(self): corners_px = np.array([ - [0, self.img_h *0.5], - [self.img_w - 1, self.img_h *0.5], - [0, self.img_h - 1], + [0, self.img_h * 0.2], + [self.img_w - 1, self.img_h * 0.2], + [0, self.img_h - 1], [self.img_w - 1, self.img_h - 1], ], dtype=np.float64) world_pts = [] for (u, v) in corners_px: - ground = self.H_inv @ np.array([u, v, 1.0]) ground /= ground[2] - world_pts.append((ground[0], ground[1])) world_pts = np.array(world_pts) x_min = world_pts[:, 0].min() x_max = world_pts[:, 0].max() - y_min = world_pts[:, 1].min() y_max = world_pts[:, 1].max() @@ -108,13 +128,12 @@ def _build_bev_scaling(self): ) self.S = np.array([ - [scale, 0, -scale * x_min], - [0, -scale, scale * y_max], - [0, 0, 1] + [scale, 0, -scale * x_min], + [0, -scale, scale * y_max], + [0, 0, 1 ] ], dtype=np.float64) self.S_inv = np.linalg.inv(self.S) - self.H_bev = self.S @ self.H_inv # ========================================================= @@ -123,8 +142,11 @@ def _build_bev_scaling(self): def pixel_to_ground(self, u, v): - pixel = np.array([u, v, 1.0], dtype=np.float64) + pts = np.array([[[u, v]]], dtype=np.float32) + undist = cv2.undistortPoints(pts, self.K, self.dist_coeffs, P=self.K) + u2, v2 = undist[0, 0] + pixel = np.array([u2, v2, 1.0], dtype=np.float64) ground = self.H_inv @ pixel ground /= ground[2] @@ -136,21 +158,20 @@ def pixel_to_ground(self, u, v): def pixels_to_ground(self, pixels): - pixels = np.asarray(pixels, dtype=np.float64) + pixels = np.asarray(pixels, dtype=np.float32).reshape(-1, 1, 2) + undist = cv2.undistortPoints(pixels, self.K, self.dist_coeffs, P=self.K) + undist = undist.reshape(-1, 2).astype(np.float64) px = np.stack([ - pixels[:, 0], - pixels[:, 1], - np.ones(len(pixels)) + undist[:, 0], + undist[:, 1], + np.ones(len(undist)) ], axis=0) ground = self.H_inv @ px ground /= ground[2] - X = ground[0] - Y = ground[1] - - return np.stack([X, Y], axis=1) + return np.stack([ground[0], ground[1]], axis=1) # ========================================================= # MASK -> POINT CLOUD @@ -160,10 +181,14 @@ def mask_to_pointcloud(self, mask): ys, xs = np.where(mask > 0) + pts = np.stack([xs, ys], axis=1).astype(np.float32).reshape(-1, 1, 2) + undist = cv2.undistortPoints(pts, self.K, self.dist_coeffs, P=self.K) + undist = undist.reshape(-1, 2).astype(np.float64) + px = np.stack([ - xs.astype(np.float64), - ys.astype(np.float64), - np.ones_like(xs, dtype=np.float64) + undist[:, 0], + undist[:, 1], + np.ones(len(undist)) ], axis=0) ground = self.H_inv @ px @@ -173,9 +198,7 @@ def mask_to_pointcloud(self, mask): Y = ground[1] Z = np.zeros_like(X) - points = np.stack([X, Y, Z], axis=1) - - return points + return np.stack([X, Y, Z], axis=1) # ========================================================= # WARP IMAGE TO BEV @@ -183,8 +206,10 @@ def mask_to_pointcloud(self, mask): def warp_to_bev(self, image): + undistorted = cv2.undistort(image, self.K, self.dist_coeffs) + bev = cv2.warpPerspective( - image, + undistorted, self.H_bev, (self.out_w, self.out_h), flags=cv2.INTER_LINEAR, @@ -192,8 +217,6 @@ def warp_to_bev(self, image): borderValue=(0, 0, 0) ) - #bev = cv2.flip(bev, 0) - return bev # ========================================================= @@ -218,7 +241,6 @@ def save_pcd(self, points, filename): ) with open(filename, 'w') as f: - f.write(header) np.savetxt(f, points, fmt="%.4f") @@ -229,53 +251,62 @@ def save_pcd(self, points, filename): # EXAMPLE USAGE # ============================================================= if __name__ == "__main__": - image = cv2.imread("../data/raw/ground.jpeg") + image = cv2.imread("../data/raw/ground.jpeg") h, w = image.shape[:2] K = np.array([ - [1000, 0, w/2], - [0, 1000, h/2], - [0, 0, 1] + [793.79768697, 0, 290.78702859], + [ 0, 813.96117996, 241.57106901], + [ 0, 0, 1 ] ], dtype=np.float64) bev = HomographyBEV( K=K, - camera_height=1.43, + camera_height=1.33, pitch_deg=-45, - image_size=(w, h) + yaw_deg=-2, + roll_deg=-7, + image_size=(w, h), + dist_coeffs=np.array([ + -4.97661814e-01, + 8.05356640e+00, + 9.44660547e-03, + -2.64434172e-02, + -4.33974203e+01 + ]) ) + print(f"w={w}\nh={h}") - # ============================================================= + + # ========================================================= # SINGLE PIXEL - # ============================================================= - X, Y = bev.pixel_to_ground(442,620) + # ========================================================= + X, Y = bev.pixel_to_ground(585, 375) print(f"Ground point: X={X:.3f}, Y={Y:.3f}") - # ============================================================= + # ========================================================= # WARP FULL IMAGE - # ============================================================= + # ========================================================= bird_eye = bev.warp_to_bev(image) - # ============================================================= + # ========================================================= # MASK -> POINT CLOUD - # ============================================================= + # ========================================================= gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) - _, mask = cv2.threshold(gray, 120, 255, cv2.THRESH_BINARY) points = bev.mask_to_pointcloud(mask) - print("Point cloud shape:", points.shape) bev.save_pcd(points, "ground_plane.pcd") - # ============================================================= + # ========================================================= # DISPLAY - # ============================================================= + # ========================================================= cv2.imshow("Original", image) cv2.imshow("Mask", mask) From aa38a0d9561e88b5a2600f692eb73741ba5be67d Mon Sep 17 00:00:00 2001 From: YassinEzzat2004 Date: Thu, 21 May 2026 15:43:14 +0300 Subject: [PATCH 08/11] pipeline and lane detector updated with the new parameters --- rover_ws/vision/homography.py | 4 ++-- rover_ws/vision/pipeline.py | 26 ++++++++++++++++------- rover_ws/vision/road_features_detector.py | 8 +++++-- 3 files changed, 26 insertions(+), 12 deletions(-) diff --git a/rover_ws/vision/homography.py b/rover_ws/vision/homography.py index 193cfd4..ba35db1 100644 --- a/rover_ws/vision/homography.py +++ b/rover_ws/vision/homography.py @@ -99,8 +99,8 @@ def _build_homography(self): def _build_bev_scaling(self): corners_px = np.array([ - [0, self.img_h * 0.2], - [self.img_w - 1, self.img_h * 0.2], + [0, self.img_h * 0.5], + [self.img_w - 1, self.img_h * 0.5], [0, self.img_h - 1], [self.img_w - 1, self.img_h - 1], ], dtype=np.float64) diff --git a/rover_ws/vision/pipeline.py b/rover_ws/vision/pipeline.py index 6f5ca02..59b92dd 100644 --- a/rover_ws/vision/pipeline.py +++ b/rover_ws/vision/pipeline.py @@ -1,7 +1,6 @@ import cv2 import numpy as np -from homography import HomographyBEV from road_features_detector import RoadFeatureDetector @@ -30,13 +29,16 @@ def mask_to_pixels(mask): class RoadFeatureBEVPipeline: - def __init__(self, K, camera_height, pitch_deg, image_size): + def __init__(self, K, camera_height, pitch_deg,yaw_deg,roll_deg,dist_coeffs, image_size): self.detector = RoadFeatureDetector( K=K, camera_height=camera_height, pitch_deg=pitch_deg, - image_size=image_size, + yaw_deg=yaw_deg, + roll_deg=roll_deg, + dist_coeffs=dist_coeffs, + image_size=image_size ) self.bev = self.detector.bev # reuse same HomographyBEV instance @@ -94,8 +96,8 @@ def save_pcd(self, points, filename="lane_cloud.pcd"): def main(): K = np.array([ - [1000, 0, 960], - [ 0, 1000, 540], + [793.79768697, 0, 290.78702859], + [ 0, 813.96117996, 241.57106901], [ 0, 0, 1], ], dtype=np.float64) @@ -115,11 +117,19 @@ def main(): pipeline = RoadFeatureBEVPipeline( K=K, - camera_height=1.43, - pitch_deg=-50, + camera_height=1.33, + pitch_deg=-45, + yaw_deg=-2, + roll_deg=-7, image_size=(w, h), + dist_coeffs=np.array([ + -4.97661814e-01, + 8.05356640e+00, + 9.44660547e-03, + -2.64434172e-02, + -4.33974203e+01 + ]) ) - frame_idx = 0 all_points = [] diff --git a/rover_ws/vision/road_features_detector.py b/rover_ws/vision/road_features_detector.py index fc60fd8..79c970f 100644 --- a/rover_ws/vision/road_features_detector.py +++ b/rover_ws/vision/road_features_detector.py @@ -6,13 +6,15 @@ class RoadFeatureDetector: - def __init__(self, K, camera_height, pitch_deg, image_size, dist_coeffs=None, + def __init__(self, K, camera_height, pitch_deg,yaw_deg,roll_deg, image_size, dist_coeffs=None, min_radius=10, max_radius=200): self.bev = HomographyBEV( K=np.array(K, dtype=np.float64), camera_height=float(camera_height), pitch_deg=float(pitch_deg), + yaw_deg=float(yaw_deg), + roll_deg=float(roll_deg), image_size=tuple(image_size), dist_coeffs=None if dist_coeffs is None else np.array(dist_coeffs, dtype=np.float64) ) @@ -246,7 +248,9 @@ def process(self, frame, draw_bev=False): K=K, camera_height=camera_height, pitch_deg=pitch_deg, - image_size=(img_w, img_h) + image_size=(img_w, img_h), + yaw_deg=0, + roll_deg=0 ) while True: From d181e5b7aefd503633c381c77e58b33736e500ec Mon Sep 17 00:00:00 2001 From: unknown Date: Fri, 22 May 2026 18:08:21 +0300 Subject: [PATCH 09/11] edit in edge_detection and proccess and circle_detection --- rover_ws/vision/road_features_detector.py | 27 ++++++++--------------- 1 file changed, 9 insertions(+), 18 deletions(-) diff --git a/rover_ws/vision/road_features_detector.py b/rover_ws/vision/road_features_detector.py index 79c970f..ca18dfa 100644 --- a/rover_ws/vision/road_features_detector.py +++ b/rover_ws/vision/road_features_detector.py @@ -60,7 +60,7 @@ def detect_edges(self, frame): thin = ximgproc.thinning(mask) - return thin + return thin, mask # ====================================================== # HOUGH LINES @@ -95,23 +95,14 @@ def draw_lines(self, frame, lines): # ====================================================== # CIRCLE DETECTION # ====================================================== - def _detect_circles(self, image): - - hsv = cv2.cvtColor(image, cv2.COLOR_BGR2HSV) - lower_white = np.array([0, 0, 200]) - upper_white = np.array([180, 40, 255]) - - white_mask = cv2.inRange(hsv, lower_white, upper_white) - kernel = cv2.getStructuringElement(cv2.MORPH_ELLIPSE, (5, 5)) - white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_OPEN, kernel) - white_mask = cv2.morphologyEx(white_mask, cv2.MORPH_CLOSE, kernel) + def _detect_circles(self, image, white_mask): blurred = cv2.GaussianBlur(white_mask, (9, 9), 2) circles = cv2.HoughCircles( - blurred, cv2.HOUGH_GRADIENT, dp=1.2, minDist=30, - param1=50, param2=30, - minRadius=self.min_radius, maxRadius=self.max_radius - ) + blurred, cv2.HOUGH_GRADIENT, dp=1.2, minDist=30, + param1=50, param2=30, + minRadius=self.min_radius, maxRadius=self.max_radius + ) detected = [] if circles is not None: @@ -198,12 +189,12 @@ def circle_to_ground_cloud(self, circle, num_points=36, filled=False): def process(self, frame, draw_bev=False): # --- Lanes --- - edges = self.detect_edges(frame) + edges, white_mask = self.detect_edges(frame) lines = self.detect_lines(edges) output = self.draw_lines(frame.copy(), lines) # --- Circles --- - circles = self._detect_circles(frame) + circles = self._detect_circles(frame, white_mask) ground_circles = [self.circle_to_ground(c) for c in circles] circle_clouds = [self.circle_to_ground_cloud(c) for c in circles] @@ -231,7 +222,7 @@ def process(self, frame, draw_bev=False): camera_height = 1.2 pitch_deg = -30 - cap = cv2.VideoCapture("../data/raw/test_lane.mp4") + cap = cv2.VideoCapture("D:\\Software work\\rover 26\\test_lane.mp4") if not cap.isOpened(): print("Cannot open video file") exit() From ad4ffa082afdf21c001f078725e8843ca2a8a3bb Mon Sep 17 00:00:00 2001 From: YassinEzzat2004 Date: Fri, 22 May 2026 18:39:00 +0300 Subject: [PATCH 10/11] data path fixed --- rover_ws/vision/road_features_detector.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rover_ws/vision/road_features_detector.py b/rover_ws/vision/road_features_detector.py index ca18dfa..320fb45 100644 --- a/rover_ws/vision/road_features_detector.py +++ b/rover_ws/vision/road_features_detector.py @@ -222,7 +222,7 @@ def process(self, frame, draw_bev=False): camera_height = 1.2 pitch_deg = -30 - cap = cv2.VideoCapture("D:\\Software work\\rover 26\\test_lane.mp4") + cap = cv2.VideoCapture("../data/raw/test_lane.mp4") if not cap.isOpened(): print("Cannot open video file") exit() From 0ec76c2f2584355eb1adb91b7f4d2cc09c13d998 Mon Sep 17 00:00:00 2001 From: Abdelaziz Islam Galal Date: Sun, 24 May 2026 17:01:26 +0300 Subject: [PATCH 11/11] minor additions min radius and max radius input in pipeline seetters for camera height, pitch, yaw and roll --- rover_ws/vision/pipeline.py | 28 ++++++++++++++++++++++++++-- 1 file changed, 26 insertions(+), 2 deletions(-) diff --git a/rover_ws/vision/pipeline.py b/rover_ws/vision/pipeline.py index 59b92dd..b312b0e 100644 --- a/rover_ws/vision/pipeline.py +++ b/rover_ws/vision/pipeline.py @@ -29,7 +29,7 @@ def mask_to_pixels(mask): class RoadFeatureBEVPipeline: - def __init__(self, K, camera_height, pitch_deg,yaw_deg,roll_deg,dist_coeffs, image_size): + def __init__(self, K, camera_height, pitch_deg,yaw_deg,roll_deg,dist_coeffs, image_size, min_radius=10, max_radius=200): self.detector = RoadFeatureDetector( K=K, @@ -38,7 +38,9 @@ def __init__(self, K, camera_height, pitch_deg,yaw_deg,roll_deg,dist_coeffs, ima yaw_deg=yaw_deg, roll_deg=roll_deg, dist_coeffs=dist_coeffs, - image_size=image_size + image_size=image_size, + min_radius=min_radius, + max_radius=max_radius ) self.bev = self.detector.bev # reuse same HomographyBEV instance @@ -88,6 +90,28 @@ def process_frame(self, frame): def save_pcd(self, points, filename="lane_cloud.pcd"): self.bev.save_pcd(points, filename) + # setters to allow dynamic reconfigure + def set_camera_height(self, value): + self.bev.camera_height = value # same object shared with detector, so no need to update there + + def set_pitch_deg(self, value): + self.bev.pitch_deg = value + + def set_yaw_deg(self, value): + self.bev.yaw_deg = value + + def set_roll_deg(self, value): + self.bev.roll_deg = value + + def set_dist_coeffs(self, value): + self.bev.dist_coeffs = value + + def set_min_radius(self, value): + self.detector.min_radius = value + + def set_max_radius(self, value): + self.detector.max_radius = value + # ============================================================= # MAIN LOOP