Grid Filter로 트랙 ROI 필터링하기
정적 맵(occupancy grid)을 이용해 “이 점이 트랙 안(ROI)에 있는가?” 를 빠르게 판별하는 유틸리티, Grid Filter를 알아봅니다.
Grid Filter가 제공하는 기능은 사실상 하나입니다 — is_point_inside(x, y): 주어진 월드 좌표가 주행 가능한 트랙 내부인지 True/False로 답합니다. 이 단순한 프리미티브를 두 곳에서 소비합니다.
- Detection ROI 제한 — LiDAR 인지를 트랙 내부 영역으로만 한정합니다. 트랙 밖(벽 너머 등)에서 돌아온 점은 클러스터링 후보에 넣지 않습니다. (→ 벽을 상대 차량으로 오인하는 오감지 감소, 처리 점 수 감소로 연산량 절감은 여기서 파생되는 부수효과입니다.)
- 경로 유효성 검사 — 플래너가 생성한 경로(스플라인·회피 경로)가 주행 가능한 트랙 안에 있는지 확인합니다.
즉 “맵으로 트랙 안/밖을 가린다”는 하나의 기능이 인지(Detection) 와 계획(Planner) 양쪽을 받칩니다.
핵심 아이디어
주행 전에 우리는 트랙의 정적 지도를 이미 가지고 있습니다 (SLAM 등으로 만든 occupancy grid). 흰색 = 주행 가능(트랙), 검은색 = 벽/트랙 밖 입니다.
아이디어는 두 줄입니다.
- 어떤 월드 좌표든 맵 픽셀로 바꿔 그 자리가 흰색인지 검은색인지 조회하면 “트랙 안인가?”를 즉시 알 수 있다.
- 벽 바로 옆은 localization·맵 오차로 애매하므로, 트랙 영역을 벽에서 살짝 안쪽으로 깎아(erosion) 안전 마진을 둔다.
동작 원리
1) 좌표 변환과 flip
월드 좌표 $(x, y)$ 를 맵 픽셀 $(u, v)$ 로 매핑합니다.
\[u = \left\lfloor \frac{x - x_{origin}}{resolution} \right\rfloor, \qquad v = \left\lfloor \frac{y - y_{origin}}{resolution} \right\rfloor\]- $(x, y)$: 월드 좌표 (단위: $m$)
- $(x_{origin}, y_{origin})$: 맵
origin의 월드 좌표 — ROS 맵 규약상 이미지의 좌하단 픽셀에 대응 (단위: $m$) - $resolution$: 픽셀 한 칸의 물리 크기 (단위: $m/pixel$)
- $(u, v)$: 픽셀 인덱스 ($Int$)
여기서 한 가지 주의할 점이 있습니다. 위 $v$ 식은 “행 인덱스가 위로 갈수록 커진다(원점 행이 맨 아래)” 를 전제합니다. 그런데 맵을 어디서 받느냐에 따라 이미지의 행 방향이 다릅니다.
PNG/PGM 이미지 파일로 불러오는 경우: 이미지 파일은 좌상단이 row 0 이라 월드 $y$ 와 위아래가 반대입니다. 그대로 쓰면 상하가 뒤집혀 조회되므로, 로드 직후 상하 flip을 한 번 해서 원점을 좌하단에 맞춰야 합니다.
1 2
image = cv::imread(path, cv::IMREAD_GRAYSCALE); cv::flip(image, image, 0); // flipCode 0 = 상하 반전(행 뒤집기)
OccupancyGrid토픽을 받는 경우: ROS 규약상data는 좌하단 (0,0)에서 시작하는 row-major(먼저 +x, 다음 +y)로 저장됩니다. 즉reshape(H, W)하면 row 0이 이미 맨 아래(원점) 라, flip 없이 그대로[v, u]로 인덱싱하면 됩니다. (단imshow로 띄우면 위아래가 뒤집혀 보이는데, 이는 표시상의 문제일 뿐 인덱싱 계산은 정확합니다.)
정리하면 flip은 딱 한 번, 그리고 입력이 PNG면 직접 flip / OccupancyGrid면 불필요입니다. (두 번 뒤집으면 원위치라 오히려 틀립니다.)
2) Erosion으로 안전 마진 만들기
트랙 경계 바로 옆 점은 위치 오차 때문에 벽인지 트랙인지 애매합니다. 그래서 주행 가능 영역(흰색)을 벽 쪽에서 안쪽으로 조금 깎아 마진을 둡니다. OpenCV의 erosion이 이 역할을 합니다 — 흰색(free)을 침식하면 결과적으로 검은색(벽)이 그만큼 확장됩니다. (침식과 팽창은 전경/배경을 뒤집으면 서로 바뀌는 쌍대 관계입니다: erode(free) = 벽을 dilate한 것과 동일.)
중요한 점: 커널 크기 $k$ 가 안쪽으로 깎는 폭은 $k$ 가 아니라 $(k-1)/2$ 픽셀입니다. 사각 커널의 anchor가 중앙이라, 벽에서 “반지름”만큼만 물러납니다.
\[d_{margin} = \frac{k - 1}{2} \times resolution\]| kernel_size | 침식 폭 $(k{-}1)/2$ | 물리 마진 ($res=0.05\,m$) |
|---|---|---|
| 1 | 0 px | 0.00 m (침식 없음, 트랙 폭 최대) |
| 5 | 2 px | 0.10 m |
| 11 | 5 px | 0.25 m |
예: 5 cm 해상도에서 커널 11 → 5 px → 벽에서 0.25 m 안쪽으로 마진. (커널 5는 0.25 m가 아니라 0.10 m입니다 — 커널 크기와 실제 침식 폭을 혼동하지 않도록 주의.)
3) 필터링 로직
전체 파이프라인은 다음과 같습니다.
- 맵 로드 시 (필요하면 flip 후) erosion을 한 번 미리 적용해
eroded_image를 만들어 둡니다. (매 프레임 재계산 X) - 입력 점 $(x, y)$ → 픽셀 $(u, v)$ 변환.
eroded_image[v, u]조회: 255(흰색)이면 트랙 안 → True, 0이면 밖 → False.- Detection에서는 False인 점을 클러스터링 후보에서 즉시 제외.
미리 만들어 둔 이미지를 좌표로 조회하는 방식이라, 사실상 2D 룩업 테이블(precompute-once, O(1) 조회)입니다.
아래 데모에서 Erosion kernel 크기를 직접 조절해 보세요. 커널을 키울수록 벽이 안쪽으로 더 부풀려지고, 그 경계 안으로 들어온 LiDAR 점들이 어떻게 걸러지는지(회색=제거) 바로 확인할 수 있습니다.
파라미터 & 튜닝
- Resolution — 맵 해상도 (예:
0.05 m/px). 맵에 종속됩니다. - kernel_size — 유일한 튜닝 손잡이. 마진 = $(k{-}1)/2 \times res$.
- 작게 → 트랙 폭 최대 확보, 벽 근처 점까지 살림 (localization이 흔들리면 벽 노이즈 유입 위험).
- 크게 → 벽에서 넉넉히 이격, 오감지 억제 (단 과하면 벽 옆 실제 장애물까지 지울 수 있음).
- 커널 모양 — 지금은 사각(
cv::MORPH_RECT). 사각이라 코너에서는 대각선 방향으로 더($\sqrt{2}$배 거리까지) 깎입니다. 벽에서 균일한 물리 거리(등방성) 마진을 원하면cv::MORPH_ELLIPSE로 바꾸면 됩니다.
결과
Case 1 — kernel size 1
침식 0 px → 트랙 폭을 최대한 확보합니다. 다만 급격한 움직임 시 벽 노이즈가 유입될 수 있습니다.
Case 2 — kernel size 11
침식 5 px(0.25 m) → 벽으로부터 충분한 이격 거리를 확보하여 오감지 억제력이 우수합니다.
- 클러스터 생성 제한: Grid Filter가
false를 반환하는 영역(침식으로 확장된 벽)의 포인트는 클러스터링 입력에서 제외됩니다. - 결론: 확장된 벽 영역 안쪽으로 들어오는 점은 클러스터 생성 자체가 무시되므로 인지 신뢰도가 향상됩니다.
전제조건 & 한계
Grid Filter는 몇 가지 전제 위에서만 제대로 동작합니다.
- Localization 성능에 크게 의존합니다 — 맵 위 어디에 있는지(pose)를 알아야 점의 월드 좌표를 맵 픽셀로 정확히 변환할 수 있습니다. 위치 추정이 정확할수록 필터도 정확합니다.
- 급격한 움직임에 취약합니다 — pose가 흔들리면 트랙 안의 실제 장애물을 벽으로 오인해 지워 버릴 수 있습니다. 그래서 급격한 움직임에도 강건한 localization이 필요하며, 커널 마진으로 어느 정도 흡수하되 과하면 실제 장애물까지 지웁니다.
- 맵 origin 회전(yaw)을 무시합니다 — 현재 구현은
origin의 yaw를 고려하지 않으므로, 맵 origin이 회전돼 있으면 좌표 변환에 별도 처리가 필요합니다. - 정적 맵을 전제합니다 — 실제 트랙 형상이 맵과 같아야 합니다. 맵에 없는 변경(옮겨진 벽 등)은 반영되지 않습니다.
마무리
Grid Filter는 정적 지도를 이용해 “이 점이 트랙 안인가?”를 O(1)로 답하는 단순하지만 강력한 유틸입니다.
- 하나의 프리미티브, 두 소비자:
is_point_inside로 Detection ROI 제한 + 경로 유효성 검사. - 좌표 변환 + flip: 월드→픽셀 변환. PNG면 로드 시 상하 flip 한 번, OccupancyGrid면 이미 좌하단 row-major라 그대로.
- Erosion 마진: 트랙을 벽에서 $(k{-}1)/2$ 픽셀 깎아 안전 마진 확보, 커널로 튜닝.
- 연산 절감: 벽 위 점을 미리 제외해 클러스터링 부하 감소. 미리 만든 이미지 조회라 사실상 LUT.
단, 이 모든 것은 localization이 받쳐줘야 성립하므로, 강건한 위치 추정과 함께 사용해야 합니다.