第2课: 相机与图像

import numpy as np import cv2 from typing import Tuple class CameraModel: """相机内参模型""" def __init__(self, fx=800, fy=800, cx=320, cy=240, k1=-0.1, k2=0.01, p1=0.001, p2=-0.001): self.fx, self.fy = fx, fy self.cx, self.cy = cx, cy self.K = np.array([[fx,0,cx],[0,fy,cy],[0,0,1]], dtype=np.float64) self.dist = np.array([k1,k2,p1,p2,0], dtype=np.float64) self.K_inv = np.linalg.inv(self.K) def project(self, pts3d): """3D点投影到图像""" n = pts3d.shape[0] homo = np.hstack([pts3d, np.ones((n,1))]) img = (self.K @ homo.T).T return img[:,:2] / img[:,2:3] def back_project(self, u, v, depth): """2D反投影到3D(已知深度)""" p = self.K_inv @ np.array([u, v, 1.0]) return p * depth class ImagePipeline: """自动驾驶视觉预处理管道""" @staticmethod def gaussian_blur(img, ksize=5): return cv2.GaussianBlur(img, (ksize,ksize), 0) @staticmethod def canny_edge(img, low=50, high=150): gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) if len(img.shape)==3 else img return cv2.Canny(gray, low, high) @staticmethod def color_threshold(img, lower=(20,100,100), upper=(40,255,255)): hsv = cv2.cvtColor(img, cv2.COLOR_BGR2HSV) return cv2.inRange(hsv, np.array(lower), np.array(upper)) @staticmethod def bird_eye_view(img, src_pts, dst_pts): M = cv2.getPerspectiveTransform(np.float32(src_pts), np.float32(dst_pts)) return cv2.warpPerspective(img, M, (img.shape[1], img.shape[0])) @staticmethod def hough_lines(img): edges = cv2.Canny( img if len(img.shape)==2 else cv2.cvtColor(img, cv2.COLOR_BGR2GRAY), 50, 150) lines = cv2.HoughLinesP(edges, 1, np.pi/180, 50, minLineLength=30, maxLineGap=10) return lines if lines is not None else [] # Test cam = CameraModel() print("Camera K:", cam.K.tolist()) pts3d = np.array([[10,0,20],[5,-3,15],[-5,2,25],[0,0,5],[15,-5,30]], dtype=np.float64) proj = cam.project(pts3d) total_err = 0 for i in range(len(pts3d)): bp = cam.back_project(proj[i,0], proj[i,1], pts3d[i,2]) err = np.linalg.norm(bp - pts3d[i]) total_err += err print(f" ({pts3d[i,0]:6.1f},{pts3d[i,1]:6.1f},{pts3d[i,2]:6.1f})" f" -> ({proj[i,0]:7.1f},{proj[i,1]:7.1f}) err={err:.10f}m") print(f"Avg back-projection error: {total_err/len(pts3d):.12f}m") img = np.zeros((480,640,3), dtype=np.uint8) img[:240] = (60,120,180); img[240:] = (80,80,80) for y in range(240,480): t = (y-240)/240.0 cv2.line(img, (int(320-120*t-50*t**2),y), (int(320-120*t-50*t**2)+2,y), (255,255,255), 2) cv2.line(img, (int(320+120*t+50*t**2),y), (int(320+120*t+50*t**2)+2,y), (255,255,255), 2) cv2.rectangle(img, (280,180), (360,240), (0,0,200), -1) pipe = ImagePipeline() edges = pipe.canny_edge(img) lines = pipe.hough_lines(img) print(f"Edges: {np.count_nonzero(edges)}, Lines: {len(lines)}")

import numpy as np import cv2 class LaneDetector: """完整车道检测器""" def __init__(self, canny_low=50, canny_high=150, alpha=0.3): self.canny_low = canny_low; self.canny_high = canny_high self.alpha = alpha self.left_coeffs = None; self.right_coeffs = None def region_of_interest(self, img): mask = np.zeros_like(img) h, w = img.shape[:2] verts = np.array([[[int(w*0.1),h],[int(w*0.4),int(h*0.6)], [int(w*0.6),int(h*0.6)],[int(w*0.9),h]]], dtype=np.int32) cv2.fillPoly(mask, verts, 255) return cv2.bitwise_and(img, mask) def detect(self, img): gray = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) if len(img.shape)==3 else img blur = cv2.GaussianBlur(gray, (5,5), 0) edges = cv2.Canny(blur, self.canny_low, self.canny_high) roi = self.region_of_interest(edges) lines = cv2.HoughLinesP(roi, 1, np.pi/180, 30, minLineLength=20, maxLineGap=15) left_pts, right_pts = [], [] if lines is not None: for ln in lines: x1,y1,x2,y2 = ln[0] if x2 == x1: continue slope = (y2-y1)/(x2-x1) if abs(slope) < 0.3: continue if slope < 0: left_pts.extend([(x1,y1),(x2,y2)]) else: right_pts.extend([(x1,y1),(x2,y2)]) result = {'left': None, 'right': None} if len(left_pts) >= 2: pts = np.array(left_pts) new_c = np.polyfit(pts[:,1], pts[:,0], 1) result['left'] = self.alpha*new_c + (1-self.alpha)*self.left_coeffs if self.left_coeffs else new_c self.left_coeffs = result['left'] if len(right_pts) >= 2: pts = np.array(right_pts) new_c = np.polyfit(pts[:,1], pts[:,0], 1) result['right'] = self.alpha*new_c + (1-self.alpha)*self.right_coeffs if self.right_coeffs else new_c self.right_coeffs = result['right'] return result # Test det = LaneDetector() road = np.zeros((480,640,3), dtype=np.uint8) road[:240] = (60,120,180); road[240:] = (80,80,80) for y in range(240,480): t = (y-240)/240.0 cv2.line(road, (int(320-120*t-30*t**2),y), (int(320-120*t-30*t**2)+3,y), (255,255,255), 3) cv2.line(road, (int(320+120*t+30*t**2),y), (int(320+120*t+30*t**2)+3,y), (255,255,255), 3) lanes = det.detect(road) if lanes['left']: print(f"Left: x={lanes['left'][0]:.4f}*y+{lanes['left'][1]:.1f}") if lanes['right']: print(f"Right: x={lanes['right'][0]:.4f}*y+{lanes['right'][1]:.1f}")