왜 동차좌표가 필요한가
회전은 행렬 곱이고 이동은 벡터 덧셈이다. 3 × 3 행렬 하나로는 이 둘을 묶을 수 없다.
회전: p' = Rp
이동: p' = p + t
---------------
p' = Rp + t
곱셈과 덧셈이 섞여 있어서 변환 두 개를 이어 붙일 때 행렬 곱 하나로 합성이 안 된다. 링크가 여러 개인 로봇이면 관절마다 이걸 반복해야 하는데, 매번 곱하고 더하는 식을 손으로 전개할 수는 없다. 그래서 차원을 하나 늘려서 곱셈 하나로 만든다.
또 하나 붙잡아야 할 게 있다. 좌표값은 물체가 들고 있는 고유한 값이 아니다. 어떤 좌표계에서 봤는가에 따라 달라지는 상대적인 값이다. 그래서 좌표를 말할 때는 항상 기준 좌표계가 같이 따라다녀야 한다.
동차좌표
마지막 성분을 하나 붙여서 점과 방향을 구분한다.
| 구분 | 표현 | 의미 |
|---|---|---|
| 점 (point) | [x y z 1]ᵀ | 위치까지 표현 |
| 방향 (vector) | [x y z 0]ᵀ | 방향만 표현 |
동차변환행렬 T
[ R t ]
T = [ ] (4 × 4)
[ 0ᵀ 1 ]
[ p ] [ Rp + t ] [ v ] [ Rv ]
T [ ] = [ ] T [ ] = [ ]
[ 1 ] [ 1 ] [ 0 ] [ 0 ]
마지막 성분이 0이면 t가 곱해질 자리가 없어진다. 그래서 방향 벡터는 이동의 영향을 받지 않는다. 이게 점과 방향을 나눠 쓰는 이유 전부다. 0과 1 하나가 “이동을 적용할까 말까"를 결정하는 스위치인 셈이다.
R 오른쪽에 t를 붙이고 아래에 [0 0 0 1] 한 줄을 얹으면 끝이다. 코드도 식 모양 그대로다.
def make_T(R, t) -> np.ndarray:
"""회전 R(3x3)과 병진 t(3,)로 4x4 동차변환을 만든다."""
R = np.array(R, dtype=np.float64)
if R.shape != (3, 3):
raise ValueError("R이 3x3이 아닙니다.")
t = np.array(t, dtype=np.float64)
if t.shape != (3,):
raise ValueError("t가 3, 이 아닙니다.")
return np.vstack((np.hstack((R, t.reshape(-1, 1))),
np.array([0., 0., 0., 1.], dtype=np.float64)))
직접 계산해본 것
R = I, t = (3, 0, 0) 일 때
[ 1 0 0 3 ] [ 1 ] [ 4 ]
[ 0 1 0 0 ] [ 2 ] [ 2 ]
[ 0 0 1 0 ] [ 0 ] = [ 0 ] → p' = (4, 2, 0)
[ 0 0 0 1 ] [ 1 ] [ 1 ]
같은 변환을 방향 v = (1, 0, 0)에 적용하면 v' = (1, 0, 0)으로 그대로다. 좌표계를 3만큼 옮겨도 “x축 방향"은 여전히 x축 방향이니까 당연한 결과인데, 식에서 이게 자동으로 나온다는 게 핵심이다.
R = I, t = (2, 3, 0), p = (1, 1, 0) 인 경우도 마찬가지다.
p' = Rp + t = (1, 1, 0) + (2, 3, 0) = (3, 4, 0)
T · [1 0 0 0]ᵀ = [1 0 0 0]ᵀ 방향은 그대로
점과 방향을 코드로 가르기
점과 방향은 붙이는 마지막 성분만 다르다. w를 인자로 빼두면 함수 하나로 둘 다 처리된다. 위 표의 1 / 0이 그대로 인자가 되는 셈이다.
def to_homogeneous(P, w: float = 1.0) -> np.ndarray:
"""(3,) 또는 (N,3) 좌표에 마지막 성분 w를 붙인다. w=1이면 점, w=0이면 방향."""
P = np.array(P, dtype=np.float64)
if not ((P.ndim == 1 and P.shape == (3,)) or (P.ndim == 2 and P.shape[1] == 3)):
raise ValueError("점 또는 점들이 3차원이 아닙니다.")
return np.hstack((P, np.full((P.shape[0], 1), w) if P.ndim == 2 else np.full((1,), w)))
def transform_point(T, p) -> np.ndarray:
"""점 변환 (w = 1): 회전과 병진이 모두 적용된다."""
return (np.array(T, dtype=np.float64) @ np.append(np.array(p, dtype=np.float64), 1.))[:3]
def transform_direction(T, v) -> np.ndarray:
"""방향 변환 (w = 0): 회전만 적용되고 병진은 무시된다."""
return (np.array(T, dtype=np.float64) @ np.append(np.array(v, dtype=np.float64), 0.))[:3]
점군은 반복문으로 돌리지 않는다. (T @ P_h.T).T 대신 P_h @ T.T로 쓰면 전치가 한 번으로 끝나고, 메모리 접근도 행 방향이라 캐시에 유리하다.
def transform_points(T, P, w: float = 1.0) -> np.ndarray:
"""(N,3) 점군을 반복문 없이 한 번에 변환한다."""
P_h = to_homogeneous(P, w)
return (P_h @ np.array(T, dtype=np.float64).T)[:3] if P_h.ndim == 1 \
else (P_h @ np.array(T, dtype=np.float64).T)[:, :3]
좌표계 연결
카메라 좌표계에서 본 점 P_cam을 베이스 좌표계로 옮기려면
P_base = T(cam → base) · P_cam
윗첨자와 아랫첨자를 이렇게 붙여 쓰면 안쪽 인덱스끼리 상쇄되는 게 눈으로 보인다. 링크가 여러 개면 관절마다 변환을 이어 붙인다.
T(cam → base) = T(link₁ → base) · T(link₂ → link₁) ··· T(cam → linkₙ)
표기 규칙이 곧 검산 도구다. 이어 붙였는데 인덱스가 안 맞으면 그 자리에서 틀린 걸 안다.
순서가 곧 의미다
| 순서 | 결과 |
|---|---|
| 1m 이동 → 90° 회전 | (1, 0) |
| 90° 회전 → 1m 이동 | (0, 1) |
행렬 곱은 교환법칙이 성립하지 않으니까 AB ≠ BA다. 변환을 적는 순서 자체가 정보라서, 순서를 바꾸면 다른 로봇이 된다.
역변환
A → B로 갔다면 B → A로 돌아올 때는 적용한 순서의 역순으로 되돌려야 한다.
현재 위치 → 회전 → 이동 → 최종 위치
최종 위치 → 이동 원상복구 → 회전 원상복구 → 처음 위치
회전행렬은 직교행렬이라 R⁻¹ = Rᵀ다. 이걸 넣고 T · T⁻¹ = I를 풀면
[ Rᵀ -Rᵀt ]
T⁻¹ = [ ]
[ 0ᵀ 1 ]
-Rᵀt를 그냥 외운 공식으로 보면 안 된다. **이동을 먼저 빼고(-t), 그 결과를 회전으로 되돌린다(Rᵀ)**는 순서가 식 안에 그대로 적혀 있는 것이다. -tRᵀ가 아니라 -Rᵀt인 이유가 여기 있다.
그래서 4×4 역행렬을 일반 역행렬 함수로 구할 이유가 없다. 전치 한 번과 행렬-벡터 곱 한 번이면 끝난다.
def inv_T(T) -> np.ndarray:
"""일반 역행렬 함수를 쓰지 않고 T^-1 = [[R^T, -R^T t], [0, 1]] 공식으로 구한다."""
T = np.array(T, dtype=np.float64)
if T.shape != (4, 4):
raise ValueError("T가 4x4가 아닙니다.")
R, t = T[:3, :3], T[:3, -1]
return np.vstack((np.hstack((R.T, -R.T @ t.reshape(-1, 1))),
np.array([0., 0., 0., 1.], dtype=np.float64)))
(N, 4, 4) 묶음도 같은 공식을 배치 축으로 늘리면 된다. 전치는 swapaxes(1, 2), 배치 행렬-벡터 곱은 einsum으로 쓴다. 단건 호출은 파이썬/NumPy 호출 오버헤드가 지배해서 연산량 차이가 안 드러나기 때문에, 속도 비교는 이 배치 쪽으로 해야 의미가 있다.
def inv_T_batch(Ts) -> np.ndarray:
Ts = np.array(Ts, dtype=np.float64)
if Ts.ndim != 3 or Ts.shape[1] != 4 or Ts.shape[2] != 4:
raise ValueError("동차변환의 묶음이 아닙니다.")
T_inv = np.zeros_like(Ts)
Rs = np.swapaxes(Ts[:, :3, :3], 1, 2)
T_inv[:, :3, :3] = Rs
np.einsum("nij,nj->ni", Rs, Ts[:, :3, 3], out=T_inv[:, :3, 3])
T_inv[:, :3, 3] *= -1
T_inv[:, 3, 3] = 1.
return T_inv
좌표계 정렬과 최소자승법
두 좌표계 사이의 R, t를 모를 때는 대응점 쌍으로 추정한다.
[ r₁₁ r₁₂ r₁₃ t₁ ]
M = [ r₂₁ r₂₂ r₂₃ t₂ ]
[ r₃₁ r₃₂ r₃₃ t₃ ]
3 × 4 = 12, 즉 미지수가 12개다. 대응점 한 쌍 p = (x,y,z) → p' = (x',y',z')이 주는 방정식은 3개이므로 점이 3개면 식이 9개뿐이라 부족하고, 최소 4점이 필요하다.
실제로는 훨씬 많이 모은다. 카메라 쪽 P₁ … P₃₀과 베이스 쪽 Q₁ … Q₃₀을 짝지으면 식이 미지수보다 훨씬 많아진다(과결정). 측정 오차 때문에 전부 정확히 만족하는 해는 아예 없다. 그래서 “푸는” 게 아니라 오차를 최소로 만드는 해를 고르는 문제로 바뀐다. 이때 측정값과 예측값의 차이가 **잔차(residual)**이다.
r = b − Ax
왜 하필 제곱합인가
측정값이 10, 11, 12로 나왔을 때 대표값 x를 무엇으로 잡아야 할까.
x | 제곱합 | 값 |
|---|---|---|
| 10 | (10−10)² + (11−10)² + (12−10)² | 5 |
| 11 | (10−11)² + (11−11)² + (12−11)² | 2 |
| 12 | (10−12)² + (11−12)² + (12−12)² | 5 |
x = 11일 때 제곱합이 가장 작다. 즉 제곱합을 최소화하는 값이 곧 평균이고, 최소자승법은 이 1차원 이야기를 고차원으로 확장한 것뿐이다.
정규방정식
잔차를 r = Ax − b로 두고 ‖r‖² = rᵀr을 최소화한다.
‖Ax − b‖² = (Ax − b)ᵀ(Ax − b)
= xᵀAᵀAx − 2xᵀAᵀb + bᵀb
x에 대해 미분해서 0으로 두면
0 = 2AᵀAx − 2Aᵀb → AᵀAx = Aᵀb
이 식을 그대로 코드로 옮긴다. (AᵀA)⁻¹은 어제 만든 inverse_gauss_jordan을 가져다 쓰고, np.linalg.lstsq는 비교 대상으로만 둔다.
def least_squares_normal_equation(A, b):
"""정규방정식 (A^T A) x = A^T b 를 직접 세워 최소자승해를 구한다."""
A = np.array(A, dtype=np.float64)
b = np.array(b, dtype=np.float64)
A_inv = inverse_gauss_jordan(A.T @ A)
x = A_inv @ (A.T @ b)
return x, b - A @ x
def rmse(residual) -> float:
"""잔차의 RMSE = sqrt(mean(r^2))."""
return float(np.sqrt(np.mean(residual ** 2)))
기하로 보면 미분이 필요 없다
잔차 r이 A의 열공간에 직교할 때 오차가 최소가 된다.
Aᵀr = 0 → Aᵀ(Ax − b) = 0 → AᵀAx = Aᵀb
미분해서 얻은 식과 글자 하나까지 같다. 최소자승해는 계산 트릭이 아니라 b를 A의 열공간에 정사영한 점이다. Ax로 도달할 수 있는 곳은 A의 열공간이 전부고, 그 밖에 있는 b에 가장 가까운 점은 수선의 발이니까.
정리
- 회전은 곱, 이동은 덧셈이라
3 × 3로는 합성이 안 된다. 차원을 하나 늘리는 건 둘을 행렬 곱 하나로 묶으려는 장치다. - 좌표값은 물체의 고유값이 아니라 어떤 좌표계에서 봤는가에 따라 달라지는 상대값이다.
- 마지막 성분
1/0하나가 점과 방향을 가른다.0이면t가 곱해질 자리가 없어서 방향은 이동의 영향을 받지 않는다. 코드에서도w인자 하나로 갈린다. - 좌표계 연결은
T(cam → base) = T(link₁ → base) ··· T(cam → linkₙ)처럼 안쪽 인덱스가 상쇄되게 적는다. 표기 규칙 자체가 검산 도구다. AB ≠ BA라서 변환을 적는 순서가 곧 정보다. 이동 후 회전과 회전 후 이동은 다른 결과다.T⁻¹ = [[Rᵀ, −Rᵀt], [0, 1]]은 외울 공식이 아니라 이동을 먼저 빼고 회전으로 되돌리는 순서가 적힌 식이다. 그래서 일반 역행렬 함수가 필요 없다.R, t추정은 미지수 12개라 최소 4점이 필요하고, 실제로는 과결정으로 모아서 오차 최소해를 고른다.- 제곱합을 최소화하는 값은 1차원에서 평균이다. 최소자승법은 이걸 고차원으로 늘린 것뿐이다.
- 최소자승해는
b를A의 열공간에 정사영한 점이다. 미분으로 얻은AᵀAx = Aᵀb와 직교 조건Aᵀr = 0이 같은 식으로 떨어진다 — 어제 조건수를 볼 때 쓴 것과 같은 열공간 관점이다.