제미나이 구현

이 코드는 제공된 특허 US 9,118,816 B2의 Detailed Description 섹션, 특히 FIG. 3, 5, 6에 설명된 "Road Vertical Contour Detection" 알고리즘을 Python과 OpenCV로 구현한 것입니다.

사용자의 요청대로 Top-view (IPM) 변환을 사용하지 않고, 특허에서 설명한 대로 이미지 평면(Image Space)에서의 Warping과 Homography를 통해 도로 평면을 정렬하고 잔차(Residual)를 계산하는 방식을 따랐습니다.

핵심 알고리즘 구현 (Python)

이 구현은 다음 단계들을 포함합니다:

1. Grid-based Tracking: 도로 영역의 고정 격자 포인트 트래킹 (Fig. 8a).
2. Planar Homography Estimation: RANSAC을 이용해 도로 평면 정렬 (Step 511).
3. Refined Warping: 두 번째 프레임을 첫 번째 프레임 시점으로 워핑 (Step 513).
4. Residual Optical Flow: 평면 정렬 후 남은 잔차 흐름 계산 (Dense Tracking).
5. Vertical Deviation Estimation: 잔차 흐름을 실제 높이($\delta H$)로 변환 (Eq. 5).

Python

 

import cv2
import numpy as np
import matplotlib.pyplot as plt

class RoadContourDetector:
    def __init__(self, focal_length, cam_height, horizon_y, grid_step=(20, 10)):
        """
        초기화 파라미터 설정 (특허 내 수치 참조)
        :param focal_length: 카메라 초점 거리 (픽셀 단위) [cite: 321]
        :param cam_height: 카메라 설치 높이 (미터 단위, 예: 1.25m) [cite: 321]
        :param horizon_y: 소실점 y좌표 (y0) [cite: 323]
        :param grid_step: 트래킹을 위한 그리드 간격 (dx, dy) [cite: 337]
        """
        self.f = focal_length
        self.H = cam_height
        self.y0 = horizon_y
        self.grid_dx, self.grid_dy = grid_step
        
        # RANSAC 파라미터
        self.ransac_thresh = 1.0  # 픽셀 단위

    def process_frames(self, img1, img2, vehicle_speed, dt):
        """
        두 연속 프레임을 처리하여 수직 윤곽(Vertical Contour) 감지
        FIG. 3의 Step 305 상세 구현
        """
        # 1. 전처리 (Gray scale 변환 등)
        gray1 = cv2.cvtColor(img1, cv2.COLOR_BGR2GRAY)
        gray2 = cv2.cvtColor(img2, cv2.COLOR_BGR2GRAY)

        # 2. 특징점(Grid) 선택 (FIG. 8a, Step 503)
        # 특허에서는 "Lane marks"나 "Shadow"에 치우치지 않기 위해 고정 그리드를 사용함 
        pts1 = self._get_grid_points(gray1)

        # 3. 포인트 트래킹 (Step 507)
        # 특허는 Correlation Patch 매칭을 언급하나(Step 341), 현대적 구현을 위해 LK Optical Flow 사용
        pts2, status, _ = cv2.calcOpticalFlowPyrLK(gray1, gray2, pts1, None, winSize=(17, 17))
        
        # 유효한 포인트만 선택
        valid_idx = np.where(status == 1)[0]
        pts1_valid = pts1[valid_idx]
        pts2_valid = pts2[valid_idx]

        # 4. 호모그래피(Homography) 추정 (Step 511)
        # 도로 평면 모델을 찾기 위해 RANSAC 사용 [cite: 352]
        H_mat, mask = cv2.findHomography(pts2_valid, pts1_valid, cv2.RANSAC, self.ransac_thresh)

        # 5. Refined Warp 수행 (Step 513)
        # img2를 img1의 시점으로 워핑. 평면이 완벽하다면 도로 영역은 일치해야 함.
        h, w = gray1.shape
        img2_warped = cv2.warpPerspective(gray2, H_mat, (w, h))

        # 6. Residual Optical Flow 계산 (Dense Tracking)
        # 평면 정렬 후에도 남은 움직임(잔차)을 계산 [cite: 357, 362]
        # Farneback 알고리즘을 사용하여 Dense Flow 계산
        flow = cv2.calcOpticalFlowFarneback(gray1, img2_warped, None, 
                                            0.5, 3, 15, 3, 5, 1.2, 0)
        
        # flow_y: 수직 방향 잔차 흐름 (delta_v)
        residual_flow_y = flow[..., 1] 

        # 7. 높이 편차(Deviation) 계산 (Eq. 5)
        # delta_H = (delta_v / v_planar) * H
        height_map = self._compute_height_deviation(residual_flow_y, vehicle_speed, dt, h, w)

        return img2_warped, residual_flow_y, height_map

    def _get_grid_points(self, img):
        """
        도로 영역(이미지 하단)에 고정 그리드 생성 (Step 503, [cite: 336])
        """
        h, w = img.shape
        # 도로 영역 ROI 설정 (horizon_y 아래부터)
        roi_y_start = int(self.y0 + 50) 
        roi_y_end = h - 50
        
        # 그리드 포인트 생성
        y_grid = np.arange(roi_y_start, roi_y_end, self.grid_dy)
        x_grid = np.arange(50, w - 50, self.grid_dx)
        
        mesh_x, mesh_y = np.meshgrid(x_grid, y_grid)
        pts = np.float32(np.vstack((mesh_x.flatten(), mesh_y.flatten())).T)
        return pts.reshape(-1, 1, 2)

    def _compute_height_deviation(self, residual_flow_y, speed, dt, h, w):
        """
        특허 식(4), (5)를 사용하여 잔차 흐름을 실제 높이 편차로 변환
        [cite: 401, 403]
        """
        height_map = np.zeros_like(residual_flow_y)
        
        # 각 픽셀의 y 좌표에 대해
        y_coords, x_coords = np.mgrid[0:h, 0:w]
        
        # 수평선(Horizon) 아래의 유효한 도로 영역만 계산
        valid_mask = y_coords > (self.y0 + 10)
        
        # 이론적 평면 흐름 (v_planar) 계산
        # planar flow v = dy/dt. 
        # 기하학적으로 Z = fH / (y - y0) (Eq. 4) 
        # 도로 평면상에서의 예상 pixel motion v는 Z의 변화에 따름.
        # 근사적으로 v_planar pixel motion = (y - y0)^2 * (speed * dt) / (f * H)
        
        y_diff = (y_coords - self.y0)
        Z = np.divide(self.f * self.H, y_diff, where=valid_mask)
        
        # 분모가 0이 되는 것을 방지
        Z[Z < 0.1] = 0.1 
        
        # 예상 평면 흐름 (v)
        # forward motion에 의한 y축 흐름 근사식
        expected_v = (y_diff ** 2) * (speed * dt) / (self.f * self.H)
        
        # 너무 작은 예상 흐름은 노이즈를 유발하므로 필터링
        mask_v = (expected_v > 0.5) & valid_mask
        
        # Eq (5): delta_H = - (delta_v / v) * H
        # 특허 식 (5)  변형
        # residual_flow_y가 delta_v에 해당
        
        numerator = residual_flow_y * self.H
        height_map[mask_v] = - np.divide(numerator[mask_v], expected_v[mask_v])
        
        # 노이즈 제거 (Median Filtering) 
        height_map = cv2.medianBlur(height_map.astype(np.float32), 3)
        
        return height_map

# ==========================================
# 실행 예시 (가상 데이터)
# ==========================================

# 1. 설정
# 가정: FHD 이미지, 초점거리 약 1000픽셀, 카메라 높이 1.25m, 소실점 y=400
detector = RoadContourDetector(focal_length=1000, cam_height=1.25, horizon_y=400)

# 2. 이미지 로드 (사용자가 가지고 있는 실제 이미지 경로로 대체 필요)
# 여기서는 테스트를 위해 동일한 이미지를 약간 이동시켜 가상 생성
img1 = cv2.imread('road_frame_1.jpg') # 실제 이미지 필요
img2 = cv2.imread('road_frame_2.jpg') # 실제 이미지 필요

if img1 is not None and img2 is not None:
    # 차량 속도 15 m/s, 프레임 간격 0.033초 (30fps) 가정
    warped_img, residual, height_map = detector.process_frames(img1, img2, vehicle_speed=15.0, dt=0.033)

    # 3. 결과 시각화
    plt.figure(figsize=(12, 6))
    
    plt.subplot(131)
    plt.title("Warped Image (Planar Aligned)")
    plt.imshow(warped_img, cmap='gray')
    
    plt.subplot(132)
    plt.title("Residual Flow (Vertical Deviation)")
    plt.imshow(residual, cmap='jet', vmin=-2, vmax=2) # [cite: 372] +/- 0.5 threshold 언급
    plt.colorbar()
    
    plt.subplot(133)
    plt.title("Estimated Height Map (m)")
    plt.imshow(height_map, cmap='RdBu', vmin=-0.1, vmax=0.1) # +/- 10cm 범위
    plt.colorbar()
    
    plt.tight_layout()
    plt.show()
else:
    print("이미지 파일을 로드할 수 없습니다. 경로를 확인해주세요.")

구현 상세 설명 및 특허 매핑

이 코드는 특허의 Detailed Description을 충실히 따르고 있습니다:

1. 초기 워핑 vs 호모그래피 정렬:
   • 특허는 차량의 속도/Yaw 정보를 이용한 **Initial Warp (Step 501)**와 이미지 포인트를 이용한 **Refined Warp (Step 513)**를 설명합니다.
   • 이 구현에서는 가장 핵심적이고 정확한 방법인 Refined Warp를 중점으로 구현했습니다. 특허에서도 "rotation and the road model warp can be combined into a single warp"라고 언급하며 호모그래피 매트릭스 하나로 합칠 수 있음을 시사합니다1111.
      
   •  
   • findHomography with RANSAC을 사용하여 **도로 평면(Road Plane)**을 모델링합니다. 이 단계가 성공하면 평평한 도로는 완벽하게 겹쳐지게 됩니다.
2. 그리드 기반 추적 (Fixed Grid Tracking):
   • _get_grid_points 함수는 특허의 Fig. 8a와 **2**에서 설명한 대로, 차선이나 그림자 같은 강한 특징점에 편향되지 않도록 도로 영역에 균일한 격자를 생성하여 추적합니다.
   •  
3. 잔차 흐름 (Residual Flow) 계산:
   • 특허의 Fig. 10a~10d 및 설명 **3**에 따르면, 호모그래피로 정렬된 이미지(Warped Image)와 원본 이미지 사이의 Optical Flow를 계산합니다.
   •  
   • 평면 위에 있는 물체(과속방지턱, 맨홀 등)는 정렬이 맞지 않아 흐름(Flow)이 발생합니다.
   • 코드에서는 calcOpticalFlowFarneback을 사용하여 Dense Flow를 계산했습니다.
4. 높이 추정 (Height Estimation):
   • _compute_height_deviation 함수는 특허의 **Equation (4)**와 **Equation (5)**를 구현합니다.
   • 
     
      
   • Equation (4): $Z = \frac{fH}{y - y_0}$ 4를 이용해 화면상의 y좌표를 실제 거리 Z로 변환합니다.
   • 
     
      
   • Equation (5): $\delta H = \frac{\delta v}{v} H$ 5를 이용해 잔차 흐름($\delta v$)을 실제 높이 변화($\delta H$)로 변환합니다. 여기서 $v$는 해당 지점에서의 예상 평면 흐름(Expected Planar Flow)입니다.
5. 필터링:
   • 특허 **6**에서 언급된 대로 결과의 노이즈를 줄이기 위해 Median Filter를 적용했습니다.
   •  

사용 시 주의사항

• Camera Parameters: focal_length, cam_height, horizon_y 값은 실제 사용하는 카메라 및 차량 세팅에 맞춰 정확히 입력해야 정확한 높이($\delta H$)가 계산됩니다.
• Masking: 코드는 horizon_y 아래쪽 전체를 도로로 가정하지만, 실제로는 보닛(Bonnet)이나 주변 차량을 제외하는 ROI 설정이 추가로 필요할 수 있습니다.

 

 

Multiframe Analysis 적용

 

Multi-Frame Analysis는 이 특허(US 9,118,816 B2)에서 노이즈를 제거하고 실제 도로의 굴곡(Bump)과 단순한 영상 아티팩트(Artifact)를 구분하는 데 매우 중요한 역할을 합니다111.

 

 

이전 답변에서는 알고리즘의 핵심 원리인 **"단일 프레임에서의 평면 정렬 및 잔차 계산(Single Frame System)"**을 명확히 보여드리기 위해, 코드가 복잡해지는 시계열(Temporal) 처리 부분을 생략했습니다. 특허에서도 Single Frame System을 먼저 설명한 후, 이를 개선하기 위해 Multi-Frame 개념을 도입하고 있습니다2222.

 

 

사용자님의 요청대로, 특허의 Columns 13-14 및 Sheet 13-14에 설명된 Multi-Frame Analysis 아이디어를 구현하는 방법을 설명해 드립니다.

Multi-Frame Analysis 구현의 핵심 아이디어

특허에 따르면 멀티 프레임 분석의 핵심은 다음과 같습니다:

1. 좌표계 변환 (Transformation): 과거 프레임($n-m$)에서 계산된 도로 높이(Profile)를 현재 프레임($n$)의 시점(Coordinate system)으로 변환해야 합니다. 이때 호모그래피($H$)의 역행렬을 사용합니다3.
2.  
3. 누적 및 가중 평균 (Weighted Averaging): 현재 프레임에서 계산된 높이 값과, 과거에서 변환되어 온 높이 값을 가중 평균하여 노이즈를 줄입니다. 실제 굴곡은 차량 이동과 함께 일관되게 나타나지만, 노이즈는 무작위로 나타나기 때문입니다44.
    
4.  

Multi-Frame 기능이 추가된 Python 코드

기존 클래스를 상속받아 시계열 처리 기능을 추가한 코드입니다.

Python

 

import cv2
import numpy as np

# 이전 답변의 RoadContourDetector 클래스가 정의되어 있다고 가정합니다.
# (이전 코드의 설정을 그대로 상속받습니다)

class MultiFrameRoadDetector(RoadContourDetector):
    def __init__(self, focal_length, cam_height, horizon_y, alpha=0.3):
        """
        :param alpha: 최신 프레임의 반영 비율 (0.0 ~ 1.0). 
                      특허의 가중치 'a'에 해당.
                      작을수록 이전 데이터를 더 많이 신뢰(부드러워짐).
        """
        super().__init__(focal_length, cam_height, horizon_y)
        self.alpha = alpha
        
        # 이전 프레임의 정보를 저장할 변수들
        self.prev_height_map = None  # 이전 프레임 시점의 높이 맵 (Accumulated)
        self.prev_gray = None        # 이전 프레임 이미지 (트래킹용)

    def process_multi_frame(self, img_curr, vehicle_speed, dt):
        """
        연속된 프레임을 받아 Multi-frame 분석을 수행
        """
        gray_curr = cv2.cvtColor(img_curr, cv2.COLOR_BGR2GRAY)
        
        # 1. 첫 프레임인 경우 초기화만 하고 종료
        if self.prev_gray is None:
            self.prev_gray = gray_curr
            # 초기 높이 맵은 0으로 가정하거나 Single frame 로직을 돌릴 수 있음
            h, w = gray_curr.shape
            self.prev_height_map = np.zeros((h, w), dtype=np.float32)
            return None, np.zeros((h, w), dtype=np.float32)

        # 2. Single Frame 처리 (이전 프레임 vs 현재 프레임)
        # 부모 클래스의 메서드 로직을 재사용하되, 입력을 맞춰줌
        # 주의: 부모 클래스의 process_frames는 img1, img2를 받도록 설계됨
        # 여기서는 self.prev_gray(img1 역할)와 gray_curr(img2 역할)를 사용
        
        # (2-1) Grid Points & Optical Flow (Step 503, 507)
        pts_prev = self._get_grid_points(self.prev_gray)
        pts_curr, status, _ = cv2.calcOpticalFlowPyrLK(self.prev_gray, gray_curr, pts_prev, None, winSize=(17, 17))
        
        valid_idx = np.where(status == 1)[0]
        pts_prev_v = pts_prev[valid_idx]
        pts_curr_v = pts_curr[valid_idx]

        # (2-2) Homography (H) 계산 (Step 511)
        # H는 pts_curr(현재) -> pts_prev(과거)로 가는 변환 행렬
        H_mat, _ = cv2.findHomography(pts_curr_v, pts_prev_v, cv2.RANSAC, self.ransac_thresh)
        
        if H_mat is None:
            # 트래킹 실패 시 초기화
            self.prev_gray = gray_curr
            return None, None

        # (2-3) Single Frame Height Estimation
        # 현재 프레임 기준의 순수 측정값 (Raw Measurement)
        # 부모 클래스의 로직 일부를 차용 (재구현)
        h, w = gray_curr.shape
        
        # Warp Current to Previous (평면 정렬)
        # H_mat은 Curr -> Prev 좌표 변환이므로, warpPerspective에 바로 사용 가능 (Inverse Warp)
        # 하지만 특허의 flow는 Prev -> Curr 워핑 후 잔차 계산이므로,
        # 여기서는 편의상 "현재 프레임 시점"에서의 높이 맵을 구하기 위해
        # 부모 클래스와 동일한 방식(잔차 계산)을 수행합니다.
        
        # 현재 이미지를 과거 시점으로 워핑하여 비교 (혹은 반대)
        # 여기서는 [현재] 시점의 결과가 필요하므로, [과거] 이미지를 [현재]로 가져옵니다.
        # H_inv: Prev -> Curr
        H_inv = np.linalg.inv(H_mat)
        prev_warped_to_curr = cv2.warpPerspective(self.prev_gray, H_inv, (w, h))
        
        # Residual Flow 계산 (현재 시점 기준)
        flow = cv2.calcOpticalFlowFarneback(prev_warped_to_curr, gray_curr, None, 
                                            0.5, 3, 15, 3, 5, 1.2, 0)
        residual_flow_y = flow[..., 1]
        
        # 현재 프레임의 단일 측정 높이 (Single Frame Result)
        curr_height_raw = self._compute_height_deviation(residual_flow_y, vehicle_speed, dt, h, w)

        # ---------------------------------------------------------
        # 3. Multi-Frame Fusion (특허의 핵심 부분) [cite: 464, 472-474]
        # ---------------------------------------------------------
        
        # (3-1) 과거의 누적된 높이 맵(self.prev_height_map)을 현재 시점으로 가져옴
        # 특허 Step 5: "Use the inverse of H to transform the path coordinates... to frame n"
        # 2D 맵 전체를 현재 시점으로 워핑합니다.
        # H_inv는 Prev 좌표계 -> Curr 좌표계 변환 행렬입니다.
        prev_height_map_warped = cv2.warpPerspective(self.prev_height_map, H_inv, (w, h))

        # (3-2) 가중 평균 (Weighted Update) 
        # 식: New = alpha * Current_Raw + (1 - alpha) * Previous_Warped
        # 특허에서는 신뢰도(Confidence score)도 가중치에 포함하지만, 여기서는 alpha로 단순화
        
        # 유효 영역 마스킹 (워핑으로 인해 검은색이 된 영역 제외)
        mask = (prev_height_map_warped != 0).astype(np.float32)
        
        # 융합된 높이 맵 계산
        fused_height_map = (self.alpha * curr_height_raw) + \
                           ((1 - self.alpha) * prev_height_map_warped)
        
        # 이전 데이터가 없던 영역(새로 들어온 도로 영역)은 현재 측정값만 사용
        fused_height_map = np.where(mask > 0, fused_height_map, curr_height_raw)

        # 4. 상태 업데이트
        self.prev_gray = gray_curr
        self.prev_height_map = fused_height_map

        return prev_warped_to_curr, fused_height_map

코드 설명 및 특허 매핑

1. 
   
    
   • 특허에서는 p2 = p1 * inv(Hnm) 수식을 통해 과거 프레임($n-m$)의 좌표를 현재 프레임($n$)으로 변환한다고 명시합니다.
   • 코드에서도 np.linalg.inv(H_mat)을 사용하여 이전에 누적된 prev_height_map을 현재 시점으로 워핑(warpPerspective)하여 가져옵니다. 이렇게 하면 차량이 앞으로 전진해도 이전 프레임에서 감지한 방지턱의 위치가 현재 화면상의 방지턱 위치와 일치하게 됩니다.
2. H_inv (역행렬) 사용5:
3. 
   
    
   • 특허의 수식 Prof_L_1 = (a * VS * ProfL + (1-a) * VS_1Interp * ProfL_1Interp) / ...을 구현한 것입니다.
   • self.alpha가 특허의 a에 해당합니다. curr_height_raw(현재 측정값)와 prev_height_map_warped(과거 예측값)를 섞어서 노이즈를 필터링합니다.
4. Weighted Average (가중 평균)6:
5. 효과:
   • 이 로직을 추가하면, 순간적인 진동이나 매칭 오류로 인해 높이 값이 튀는 현상(Spurious bumps)이 사라지고, 여러 프레임에 걸쳐 지속적으로 관측되는 실제 방지턱만 선명하게 남게 됩니다7.