Devin.KR
통합 검색 Ctrl K

거리 스캔과 좌표 변환

80분 안팎

학습 목표

거리·각도 배열을 점으로 변환하며 누락과 범위 밖 값을 제외합니다.

개념

거리 배열을 지도 점으로 바꾸는 이유

경로 계획기는 map의 장애물을 사용하지만 거리 센서가 준 숫자는 sensor 원점에서 특정 방향으로 잰 길이입니다. 두 데이터를 바로 비교하면 로봇이 회전할 때 장애물이 따라 움직이는 것처럼 보입니다. 이 레슨의 목표는 유효한 빔을 골라 sensor→base_link→map 순서로 변환하고 원래 빔 번호를 남기는 것입니다. 점이 만들어졌다는 사실만으로 그 주변 셀의 점유 확률을 결정하지는 않습니다.

각도와 거리를 같은 인덱스로 묶습니다

입력 angles_deg와 ranges_m은 같은 길이의 배열입니다. 각도 0은 센서 정면, 양수 각도는 반시계 방향입니다. +90도 빔은 센서 왼쪽을 가리킵니다. x=r*cos(a), y=r*sin(a)에서 a는 라디안이므로 math.radians를 사용합니다. 90이라는 도 값을 그대로 sin에 넣으면 함수는 90rad로 해석하여 왼쪽 점이 다른 방향으로 나옵니다. 0도·90도·-90도의 알려진 결과로 먼저 검사합니다.

각도 목록이 명시되어 있으므로 빔 간격이 항상 일정하다고 가정하지 않습니다. 한 빔을 제외하더라도 남은 각도를 새 인덱스로 재계산하면 안 됩니다. 출력은 [원래 인덱스,map_x,map_y]의 목록입니다. 두 번째 빔이 null이고 세 번째가 유효하면 세 번째의 인덱스는 2입니다. 디버깅 시 화면 점에서 원시 기록으로 돌아갈 수 있도록 번호를 보존합니다.

범위 검사는 좌표 계산보다 먼저 합니다

min_m과 max_m은 이 실습의 정상 거리 범위이며 양 끝값을 포함합니다. 기본 예시의 0.05m와 4.0m는 모델 설정이고 모든 센서의 공통 사양이 아닙니다. null은 누락으로 제외하고 숫자지만 범위 밖이거나 유한하지 않은 거리도 제외합니다. 문자열 거리나 true는 정상 센서 값이 아니므로 전체 입력을 ERROR로 처리합니다. 누락과 형식 오류는 서로 다른 문제라는 점을 유지합니다.

무효 빔을 0m로 바꾸면 센서 원점에 장애물이 있다고 보고하게 됩니다. max_m로 바꾸면 최대 범위에 반사점이 있다는 근거 없는 점을 만들게 됩니다. 이번 변환은 무효 빔을 점 목록에 넣지 않습니다. 유효 점이 하나도 없으면 []를 반환합니다. []는 장애물 없음의 증명이 아니라 유효한 관측점이 없음이라는 결과입니다. 주행 안전 정책은 이 사실을 받아 별도로 정해야 합니다.

각도는 빔 방향의 계약이므로 비정상 각도 하나는 전체 ERROR입니다. 거리처럼 조용히 제외하지 않습니다. 배열 길이가 다르면 zip이 짧은 쪽에서 끝나며 오류를 숨길 수 있으므로 먼저 길이를 검사합니다. 이 검사가 없으면 스캔의 뒤쪽 장애물이 아무 경고 없이 사라집니다. 최소·최대 범위가 뒤집힌 입력도 데이터가 모두 무효라는 정상 결과와 구분해 오류를 반환합니다.

센서 장착 위치와 회전도 변환입니다

mount는 [차체 기준 sensor_x,sensor_y,sensor_yaw]이며 위치는 m, 방향은 rad입니다. 센서 점을 mount 방향으로 회전한 다음 차체 기준 장착 위치를 더합니다. 센서가 전방 0.10m에 달렸다면 정면 1m 빔은 차체에서 (1.10,0)입니다. 장착 오프셋을 센서 거리값에 더하는 방법은 센서 방향이 정면이고 한 축만 볼 때 우연히 맞을 뿐 일반적인 2차원 변환이 아닙니다.

pose는 [map_x,map_y,map_yaw]입니다. 앞 단계에서 만든 차체 점을 pose 방향으로 회전하고 map 위치를 더합니다. 차체가 map에서 (1,2)에 있고 +90도 향하면 차체 전방 점 (1.10,0)은 map의 (1,3.10)이 됩니다. 장착 위치도 차체와 함께 회전합니다. map x에 0.10을 마지막에 더하는 구현은 센서가 늘 지도 동쪽에 달린 것처럼 처리하는 오류입니다.

회전과 평행이동의 순서는 교환되지 않습니다. sensor 점에 map 위치를 먼저 더하고 전체를 회전시키면 로봇 자체의 위치까지 원점을 중심으로 돌아갑니다. 코드에서는 sx/sy, bx/by, x/y처럼 단계마다 이름을 달리합니다. 중간 좌표를 따로 출력하면 극좌표 계산 오류인지 장착 오류인지 map 변환 오류인지 좁혀 볼 수 있습니다. 최종 출력만 보고 부호를 바꾸며 맞추지 않습니다.

방향과 경계 사례를 검산합니다

pose와 mount가 모두 0이면 변환 결과는 sensor 좌표와 같습니다. 반경 2m·90도는 (0,2)이며 -90도는 (0,-2)입니다. cos(pi/2)가 정확한 0이 아니라 아주 작은 값이 될 수 있어 출력에서만 여섯 자리 반올림을 사용합니다. 계산의 중간 단계에서 작은 값을 제거하면 여러 변환을 합성할 때 오차가 바뀔 수 있습니다. 내부와 표현 단계의 역할을 구분합니다.

유효 범위 경계의 빔, null, 0m, 최대를 넘는 빔을 한 입력에 섞어 봅니다. 예상 유효 인덱스를 손으로 적고 출력과 비교하면 필터의 포함 조건을 확인할 수 있습니다. 입력 순서를 유지하므로 결과를 거리순으로 정렬하지 않습니다. 최근접 점을 찾는 기능은 별도 단계에서 수행할 수 있고 이번 출력은 방향별 대응을 검증하는 데 쓰입니다.

센서의 origin을 바꾼 뒤에도 거리만 보면 같은 값이지만 map 점은 달라집니다. 로봇 pose만 바꿔 봤을 때 모든 점에 같은 map 평행이동이 생기는지 확인합니다. 센서 mount 회전만 바꾸면 차체 안에서 빔 방향이 변하고, 차체 pose 회전은 장착 위치까지 회전시킵니다. 두 실험을 분리하면 같은 yaw라는 이름을 써도 기준 프레임이 다름을 이해할 수 있습니다.

점 변환의 한계를 기록합니다

이 레슨은 스캔 전체가 하나의 pose에서 얻어졌다고 가정합니다. 실제로 회전하는 스캐너가 빔들을 다른 시각에 얻는다면 스캔 내부 움직임도 점 형태를 왜곡할 수 있습니다. 여기서는 빔별 시간 보정을 구현하지 않고 한 스캔의 대표 측정 시각을 다음 시각 정렬 레슨에서 다룹니다. 정적인 좌표 식이 맞았다고 이동 중의 시간 문제까지 해결한 것은 아닙니다.

map 점 하나가 계획용 장애물 셀과 맞지 않을 때는 단위, 빔 인덱스, 장착 방향, pose 기준 순서로 확인합니다. 로봇 위치를 센서 기준으로 표현한 역변환을 잘못 쓰면 이동 방향의 부호가 뒤집힙니다. 센서 원점 (0,0)을 변환해 map에서 예상 장착 위치가 나오는지 검사하는 것도 좋습니다. 원점 테스트는 거리 측정 없이 변환의 방향만 확인하는 간단한 방법입니다.

LENGTH는 배열 개수 불일치, RANGE는 거리 범위 설정 오류, POSE는 장착 또는 로봇 자세 형식 오류, ANGLE은 빔 방향 오류입니다. 브라우저 wrapper는 ERROR 한 줄을 내지만 함수 단독 실행에서는 ValueError의 이유를 볼 수 있습니다. 오류를 []로 바꾸면 관측 누락과 설정 오류를 구분할 수 없으므로 제공한 wrapper의 실패 정책을 유지합니다. 주행 코드는 관측이 없을 때 어떻게 멈출지도 명시해야 합니다.

완성 후 최소 네 조건을 비교하고 왜 일부 빔이 빠졌는지 원시 번호로 설명합니다. 점 목록은 원래 스캔과 함께 보관해야 추후 누락 원인을 재현할 수 있습니다. 더 읽기의 라이다 장은 최근접 빔을 찾는 과정과 센서 맥락을 확장합니다. 여기서는 서재의 예제를 옮기지 않고 프로젝트의 장착 오프셋과 map pose를 가진 입력 계약을 직접 구현합니다.

따라하기

센서의 좌우 방향을 확인합니다

좌표축만 확인하는 항등 변환입니다. 출력의 인덱스와 y 부호를 읽습니다.

import json,sys,math

def points(obj):
    angles=obj['angles_deg'];ranges=obj['ranges_m'];lo=obj['min_m'];hi=obj['max_m']
    pose=obj['pose'];mount=obj['mount']
    numeric=lambda v: type(v) in (int,float) and math.isfinite(v)
    if not isinstance(angles,list) or not isinstance(ranges,list) or len(angles)!=len(ranges): raise ValueError('LENGTH')
    if not numeric(lo) or not numeric(hi) or not 0<lo<=hi: raise ValueError('RANGE')
    if not isinstance(pose,list) or len(pose)!=3 or not isinstance(mount,list) or len(mount)!=3 or not all(numeric(v) for v in pose+mount): raise ValueError('POSE')
    if not all(numeric(a) for a in angles): raise ValueError('ANGLE')
    out=[]
    for i,(angle,r) in enumerate(zip(angles,ranges)):
        if r is None: continue
        if type(r) not in (int,float): raise ValueError('TYPE')
        if not math.isfinite(r) or not lo<=r<=hi: continue
        a=math.radians(angle);sx,sy=r*math.cos(a),r*math.sin(a)
        mx,my,myaw=mount;px,py,yaw=pose
        bx=mx+math.cos(myaw)*sx-math.sin(myaw)*sy
        by=my+math.sin(myaw)*sx+math.cos(myaw)*sy
        x=px+math.cos(yaw)*bx-math.sin(yaw)*by
        y=py+math.sin(yaw)*bx+math.cos(yaw)*by
        clean=lambda v: 0.0 if abs(v)<0.0000005 else round(v,6)
        out.append([i,clean(x),clean(y)])
    return out

print(points({'angles_deg': [0, 90, -90], 'ranges_m': [1, 2, 1], 'min_m': 0.05, 'max_m': 4, 'pose': [0, 0, 0], 'mount': [0, 0, 0]}))

실행 결과

[[0, 1.0, 0.0], [1, 0.0, 2.0], [2, 0.0, -1.0]]

장착 위치도 함께 회전합니다

전방 0.10m 오프셋이 map의 y 방향으로 이동합니다.

import json,sys,math

def points(obj):
    angles=obj['angles_deg'];ranges=obj['ranges_m'];lo=obj['min_m'];hi=obj['max_m']
    pose=obj['pose'];mount=obj['mount']
    numeric=lambda v: type(v) in (int,float) and math.isfinite(v)
    if not isinstance(angles,list) or not isinstance(ranges,list) or len(angles)!=len(ranges): raise ValueError('LENGTH')
    if not numeric(lo) or not numeric(hi) or not 0<lo<=hi: raise ValueError('RANGE')
    if not isinstance(pose,list) or len(pose)!=3 or not isinstance(mount,list) or len(mount)!=3 or not all(numeric(v) for v in pose+mount): raise ValueError('POSE')
    if not all(numeric(a) for a in angles): raise ValueError('ANGLE')
    out=[]
    for i,(angle,r) in enumerate(zip(angles,ranges)):
        if r is None: continue
        if type(r) not in (int,float): raise ValueError('TYPE')
        if not math.isfinite(r) or not lo<=r<=hi: continue
        a=math.radians(angle);sx,sy=r*math.cos(a),r*math.sin(a)
        mx,my,myaw=mount;px,py,yaw=pose
        bx=mx+math.cos(myaw)*sx-math.sin(myaw)*sy
        by=my+math.sin(myaw)*sx+math.cos(myaw)*sy
        x=px+math.cos(yaw)*bx-math.sin(yaw)*by
        y=py+math.sin(yaw)*bx+math.cos(yaw)*by
        clean=lambda v: 0.0 if abs(v)<0.0000005 else round(v,6)
        out.append([i,clean(x),clean(y)])
    return out

print(points({'angles_deg': [0], 'ranges_m': [1], 'min_m': 0.05, 'max_m': 4, 'pose': [1, 2, 1.5707963267948966], 'mount': [0.1, 0, 0]}))

실행 결과

[[0, 1.0, 3.1]]

경계와 누락을 비교합니다

유효한 경계의 원래 빔 번호가 유지되는지 확인합니다.

import json,sys,math

def points(obj):
    angles=obj['angles_deg'];ranges=obj['ranges_m'];lo=obj['min_m'];hi=obj['max_m']
    pose=obj['pose'];mount=obj['mount']
    numeric=lambda v: type(v) in (int,float) and math.isfinite(v)
    if not isinstance(angles,list) or not isinstance(ranges,list) or len(angles)!=len(ranges): raise ValueError('LENGTH')
    if not numeric(lo) or not numeric(hi) or not 0<lo<=hi: raise ValueError('RANGE')
    if not isinstance(pose,list) or len(pose)!=3 or not isinstance(mount,list) or len(mount)!=3 or not all(numeric(v) for v in pose+mount): raise ValueError('POSE')
    if not all(numeric(a) for a in angles): raise ValueError('ANGLE')
    out=[]
    for i,(angle,r) in enumerate(zip(angles,ranges)):
        if r is None: continue
        if type(r) not in (int,float): raise ValueError('TYPE')
        if not math.isfinite(r) or not lo<=r<=hi: continue
        a=math.radians(angle);sx,sy=r*math.cos(a),r*math.sin(a)
        mx,my,myaw=mount;px,py,yaw=pose
        bx=mx+math.cos(myaw)*sx-math.sin(myaw)*sy
        by=my+math.sin(myaw)*sx+math.cos(myaw)*sy
        x=px+math.cos(yaw)*bx-math.sin(yaw)*by
        y=py+math.sin(yaw)*bx+math.cos(yaw)*by
        clean=lambda v: 0.0 if abs(v)<0.0000005 else round(v,6)
        out.append([i,clean(x),clean(y)])
    return out

print(points({'angles_deg': [0, 0, 0, 0, 0], 'ranges_m': [0.05, None, 4, 0, 4.01], 'min_m': 0.05, 'max_m': 4, 'pose': [0, 0, 0], 'mount': [0, 0, 0]}))

실행 결과

[[0, 0.05, 0.0], [2, 4.0, 0.0]]

확인 문제

실습

angles_degとranges_mは同じ長さです。min_m・max_mは含む境界でm単位、poseはmapの[x,y,yaw]、mountはbase_linkのsensor[x,y,yaw]です。yawはradです。sensorからbase_linkへの変換は提供済みなのでmapへのx・yのTODOを完成します。null・範囲外・非有限の距離は除外し、原インデックスを保った[index,map_x,map_y]をJSONで出力します。空の点群は[]、形の誤り・文字列や真偽値の距離はERRORです。

모범 답안
import json,sys,math

def points(obj):
    angles=obj['angles_deg'];ranges=obj['ranges_m'];lo=obj['min_m'];hi=obj['max_m']
    pose=obj['pose'];mount=obj['mount']
    numeric=lambda v: type(v) in (int,float) and math.isfinite(v)
    if not isinstance(angles,list) or not isinstance(ranges,list) or len(angles)!=len(ranges): raise ValueError('LENGTH')
    if not numeric(lo) or not numeric(hi) or not 0<lo<=hi: raise ValueError('RANGE')
    if not isinstance(pose,list) or len(pose)!=3 or not isinstance(mount,list) or len(mount)!=3 or not all(numeric(v) for v in pose+mount): raise ValueError('POSE')
    if not all(numeric(a) for a in angles): raise ValueError('ANGLE')
    out=[]
    for i,(angle,r) in enumerate(zip(angles,ranges)):
        if r is None: continue
        if type(r) not in (int,float): raise ValueError('TYPE')
        if not math.isfinite(r) or not lo<=r<=hi: continue
        a=math.radians(angle);sx,sy=r*math.cos(a),r*math.sin(a)
        mx,my,myaw=mount;px,py,yaw=pose
        bx=mx+math.cos(myaw)*sx-math.sin(myaw)*sy
        by=my+math.sin(myaw)*sx+math.cos(myaw)*sy
        x=px+math.cos(yaw)*bx-math.sin(yaw)*by
        y=py+math.sin(yaw)*bx+math.cos(yaw)*by
        clean=lambda v: 0.0 if abs(v)<0.0000005 else round(v,6)
        out.append([i,clean(x),clean(y)])
    return out
try: print(json.dumps(points(json.load(sys.stdin))))
except (ValueError,TypeError,KeyError,OverflowError): print('ERROR')

더 읽기
면접 질문
  • 서로 다른 좌표계의 위치를 변환하는 과정을 설명해 주시면 됩니다.