동차 변환과 역변환
150분 안팎
학습 목표
3×3 행렬로 센서 좌표를 로봇 좌표로 옮깁니다.
개념
장착 위치를 빠뜨리지 않습니다
센서가 로봇 중심에서 전방 0.2m에 장착되어 있다면 센서의 거리 1m는 로봇 중심 기준 전방 1.2m의 점입니다. 회전만 적용하면 장착 위치가 사라집니다. 이번 레슨은 회전과 이동을 3×3 동차 행렬 하나로 묶고, 반대 방향 변환도 구현합니다. 미션에서는 이 변환을 앞 모듈의 frame_id와 연결하여 같은 센서 기록을 map 기준 점으로 읽을 수 있게 만듭니다.
자식 자세를 부모 기준으로 씁니다
parent_from_child는 자식 좌표를 부모 좌표로 바꾸는 행렬입니다. 이동 (tx,ty)는 자식 원점이 부모에서 어디에 있는지 나타내고 yaw는 자식 축이 부모 축에서 얼마나 돌아 있는지 나타냅니다. 점 계산은 p_parent=R*p_child+t입니다. 오른쪽의 자식 좌표를 먼저 회전시켜 부모 축에 맞추고, 부모 기준으로 표현한 원점 위치를 더합니다. 이동값을 자식 축의 값처럼 읽으면 장착 방향이 달라집니다.
예를 들어 부모에서 본 자식 원점이 (1,2)m이고 yaw가 +90도라면 자식의 (1,0)m 점은 부모의 (1,3)m 점입니다. 회전으로 (0,1)을 얻은 뒤 (1,2)를 더합니다. 회전과 이동의 순서를 바꿔 자식 점에 (1,2)를 먼저 더하고 전체를 돌리면 다른 점이 나옵니다. 코드가 맞는지 보기 전에 원점과 축을 그려 알려진 기대점을 정합니다. 이 예시는 미션의 수치 수용 기준에도 사용합니다.
동차 좌표의 마지막 성분
점 (x,y)에 1을 붙여 열벡터 [x,y,1]^T로 나타내면 이동도 행렬 곱 안에 들어갑니다. 행렬의 세 행은 (c,-s,tx), (s,c,ty), (0,0,1)이며 c=cosθ, s=sinθ입니다. 첫 두 행을 점과 내적하면 회전 결과에 이동이 더해집니다. 마지막 행은 점의 마지막 성분 1을 유지합니다. 이동을 마지막 행에 넣는 자료는 행벡터 약속일 수 있으므로 열벡터 코드와 그대로 섞지 않습니다.
방향이나 두 점의 차이를 나타내는 벡터에는 동차 성분 0을 붙입니다. 두 원점의 이동 차이를 더하지 않기 위해서입니다. 이번 apply 함수와 브라우저 과제는 위치를 나타내는 점만 입력받아 마지막 성분을 1로 처리합니다. 속도와 방향 벡터에도 같은 함수를 쓰면 장착 이동이 잘못 더해집니다. 이름이 point인 입력을 유지하고 다른 물리량의 변환은 별도 함수로 확장하는 것이 검토하기 쉽습니다.
일반 행렬 곱을 작성합니다
3×3 행렬 a와 b를 합성할 때 결과의 i,j 성분은 a의 i번째 행과 b의 j번째 열을 곱해 더한 값입니다. Python에서는 sum(a[i][k]*b[k][j] for k in range(3))로 표현합니다. 각 행과 열에 대해 새 값을 만들고 원본 행렬을 수정하지 않습니다. 단순히 같은 위치 원소끼리 곱하는 것은 행렬 곱이 아니므로 두 단계 이동과 회전을 합성할 수 없습니다.
map_from_base와 base_from_sensor가 있으면 map_from_sensor는 앞 행렬에 뒤 행렬을 곱한 결과입니다. 점에는 오른쪽의 base_from_sensor부터 적용됩니다. 변수 이름의 가운데 프레임 base가 맞물리는지 확인합니다. 순서를 뒤집으면 수학적으로 곱할 수는 있어도 출발지와 목적지의 의미가 맞지 않습니다. 행렬 크기가 모두 3×3이라는 사실만으로 프레임 연결의 적합성을 보장하지 않습니다.
센서 장착 이동이 (0.2,0)이고 로봇의 map 자세가 (1,2,+90도)일 때 센서 앞 1m 점은 map에서 (1,3.2)m입니다. 먼저 로봇 전방 1.2m 점으로 만들고 회전한 뒤 원점 위치를 더합니다. 반대로 장착 행렬을 map 변환 왼쪽에 놓으면 0.2m가 map x축 쪽으로 더해집니다. 미션은 두 결과를 비교하여 순서 오류를 발견하는 테스트를 제공합니다.
역변환을 수식에서 구합니다
원래 식 p_parent=R*p_child+t에서 t를 빼면 부모 축으로 표현된 상대 벡터가 남습니다. 여기에 역회전 R^T를 적용해야 자식 성분을 얻습니다. 따라서 p_child=R^T*(p_parent-t)입니다. 역행렬의 회전은 R^T이고 이동은 -R^T*t입니다. 단순히 마지막 열의 부호만 바꾸는 구현은 회전이 없을 때만 우연히 맞을 수 있어 +90도와 임의 각도에서 시험합니다.
평면 강체 변환의 역은 첫 행 (c,s,-c*tx-s*ty), 둘째 행 (-s,c,s*tx-c*ty), 마지막 행 (0,0,1)입니다. 이 공식은 회전과 이동만 있는 행렬을 전제로 합니다. 축소나 전단이 섞인 임의의 3×3 행렬에는 적용하지 않습니다. 미션 API는 transform과 multiply가 만든 강체 행렬을 받는 것으로 계약을 제한하며 일반 역행렬 계산기를 만드는 과제로 확장하지 않습니다.
입출력과 오차를 따로 다룹니다
브라우저 실습은 tx ty angle_deg px py 다섯 숫자를 받습니다. 이동과 점의 단위는 m, 각도 입력은 도입니다. 내부에서는 각도를 라디안으로 바꾸고 동차 행렬을 만들어 점에 적용합니다. 출력은 부모 점 x,y 다음 역변환한 자식 점 x,y 네 값입니다. 모두 소수점 아래 세 자리이며 0에 가까운 표시값은 양의 0.000으로 정리합니다. 행렬 내부 값과 복원 검증값은 반올림하지 않습니다.
원점만 시험하면 회전 부분이 잘못되어도 드러나지 않을 수 있습니다. 회전 0의 이동 시험, 이동 0의 회전 시험, 둘이 함께 있는 알려진 점 시험을 따로 준비합니다. 그다음 임의 각도의 왕복 시험을 추가합니다. 복원 오차는 각 좌표의 절대 차이 1e-9 이하로 검사합니다. 화면에서 소수점 세 자리만 같다는 사실은 내부 값이 1e-9 이내라는 증거가 아니므로 수치 비교를 별도로 실행합니다.
오류 메시지와 조용한 오류
IndexError: tuple index out of range가 나면 행렬에 세 성분을 넣었는지, 점의 길이를 잘못 기대했는지 확인합니다. 미션 apply는 길이 두 점을 받고 마지막 1을 내부에서 처리하므로 호출할 때 세 성분 점을 넣지 않습니다. too many values to unpack도 이 경계를 혼동했을 때 나올 수 있습니다. 모양 오류와 변환 방향 오류를 분리하여 먼저 함수 입력 형태를 맞춥니다.
숫자는 나오지만 왕복이 실패하면 inverse의 이동항을 먼저 봅니다. 알려진 점이 틀리고 왕복만 통과하면 회전 부호나 같은 오류를 공유하는 짝 함수가 의심됩니다. 합성 결과만 틀리면 행렬 순서와 장착 이동의 기준을 확인합니다. 출력 소수만 고쳐 채점을 맞추지 않고, 실패한 계약에 해당하는 함수부터 수정합니다. 수치값을 로그에 남길 때 행렬의 출발 프레임과 목적 프레임도 함께 적습니다.
이 레슨을 마치면 3×3 행렬을 만들고 센서 점을 로봇 점으로 옮기며 역변환으로 복원할 수 있어야 합니다. 미션 시작 코드의 apply와 inverse가 이 목표에 해당하는 TODO입니다. 회전·이동·합성의 다른 예제와 3차원 동차 표현은 더 읽기로 보냅니다. 다음 레슨에서는 같은 행렬을 어디에서 찾아야 하는지, 프레임 이름과 트리 연결의 실패 조건을 문서로 정리합니다.
따라하기
3×3 변환으로 점 이동
자식 원점 (1,2)m와 +90도 자세를 행렬에 넣습니다.
import math
def transform(tx, ty, angle):
c, s = math.cos(angle), math.sin(angle)
return ((c,-s,tx),(s,c,ty),(0,0,1))
def apply(t, p):
return tuple(t[i][0]*p[0]+t[i][1]*p[1]+t[i][2] for i in range(2))
def inverse(t):
c, ms, tx = t[0]
s, c2, ty = t[1]
return ((c,s,-c*tx-s*ty),(ms,c2,-ms*tx-c2*ty),(0,0,1))
def show(value):
return f"{0.0 if abs(value) < 0.0005 else value:.3f}"
t=transform(1,2,math.pi/2)
q=apply(t,(1,0))
print(" ".join(show(v) for v in q))
실행 결과
1.000 3.000
역변환으로 복원
이동항에 전치 회전을 적용하여 자식 점을 되찾습니다.
import math
def transform(tx, ty, angle):
c, s = math.cos(angle), math.sin(angle)
return ((c,-s,tx),(s,c,ty),(0,0,1))
def apply(t, p):
return tuple(t[i][0]*p[0]+t[i][1]*p[1]+t[i][2] for i in range(2))
def inverse(t):
c, ms, tx = t[0]
s, c2, ty = t[1]
return ((c,s,-c*tx-s*ty),(ms,c2,-ms*tx-c2*ty),(0,0,1))
def show(value):
return f"{0.0 if abs(value) < 0.0005 else value:.3f}"
t=transform(1,2,math.pi/2)
q=apply(t,(1,0))
r=apply(inverse(t),q)
print(" ".join(show(v) for v in r))
print("error_ok", max(abs(a-b) for a,b in zip(r,(1,0))) <= 1e-9)
실행 결과
1.000 0.000 error_ok True
장착 변환을 먼저 적용
센서 점을 본체로 옮긴 후 지도 자세를 적용합니다.
import math
def transform(tx, ty, angle):
c, s = math.cos(angle), math.sin(angle)
return ((c,-s,tx),(s,c,ty),(0,0,1))
def apply(t, p):
return tuple(t[i][0]*p[0]+t[i][1]*p[1]+t[i][2] for i in range(2))
def inverse(t):
c, ms, tx = t[0]
s, c2, ty = t[1]
return ((c,s,-c*tx-s*ty),(ms,c2,-ms*tx-c2*ty),(0,0,1))
def show(value):
return f"{0.0 if abs(value) < 0.0005 else value:.3f}"
map_from_base=transform(1,2,math.pi/2)
base_from_sensor=transform(.2,0,0)
def multiply(a,b):
return tuple(tuple(sum(a[i][k]*b[k][j] for k in range(3)) for j in range(3)) for i in range(3))
for matrix in (multiply(map_from_base,base_from_sensor),multiply(base_from_sensor,map_from_base)):
print(" ".join(show(v) for v in apply(matrix,(1,0))))
실행 결과
1.000 3.200 1.200 3.000
확인 문제
실습
tx ty angle_deg px py 순서의 유한한 숫자 다섯 개를 읽습니다. 부모에서 본 자식 이동 m와 각도 도, 자식 점 m입니다. 3×3 열벡터 동차 변환을 만들고 부모 점 x y, 역변환으로 복원한 자식 점 x y를 소수점 아래 세 자리로 출력합니다. 좌표 절댓값 10000 이하, 각도 절댓값 360 이하이며 show가 작은 표시값을 정리합니다. apply와 inverse의 TODO를 완성합니다.
모범 답안
import sys, math
def transform(tx, ty, angle):
c, s = math.cos(angle), math.sin(angle)
return ((c,-s,tx),(s,c,ty),(0,0,1))
def apply(t, p):
return tuple(t[i][0]*p[0]+t[i][1]*p[1]+t[i][2] for i in range(2))
def inverse(t):
c, ms, tx = t[0]
s, c2, ty = t[1]
return ((c,s,-c*tx-s*ty),(ms,c2,-ms*tx-c2*ty),(0,0,1))
def show(value):
return f"{0.0 if abs(value) < 0.0005 else value:.3f}"
tx,ty,deg,px,py=map(float,sys.stdin.read().split())
t=transform(tx,ty,math.radians(deg))
q=apply(t,(px,py))
r=apply(inverse(t),q)
print(" ".join(show(v) for v in (*q,*r)))
더 읽기
면접 질문
- 서로 다른 좌표계의 위치를 변환하는 과정을 설명해 주시면 됩니다.