TF2 좌표변환 — lookup 한 줄 밑의 수학

카메라 기준 3D 점을 팔 기준 좌표로 바꿔야 팔이 움직일 수 있어요. 그 “바꾸는 한 줄”이 TF lookup이고, 이 글은 그 한 줄의 철자와 밑에서 일어나는 수학을 다뤄요.

저장소와 구독자를 멤버로 잡아요

등장인물은 둘이에요. 버퍼는 변환들을 시간별로 쌓아 두는 저장소이고, 리스너는 토픽을 구독해 받은 변환을 그 버퍼에 넣어 주는 구독자예요.

class PickNode : public rclcpp::Node {
    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;               // 변환 저장소
    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;  // 구독 → 버퍼 적재
public:
    PickNode() : Node("pick_node") {
        tf_buffer_ = std::make_unique<tf2_ros::Buffer>(get_clock());
        tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
    }
};

멤버로 잡는 이유는 C++ rclcpp — 콜백 수명과 스레딩이 만드는 사고의 구독과 같은 생존 원칙이에요. 리스너가 지역 변수면 생성자를 나가는 순간 구독이 해제되고 버퍼는 영원히 비어 있어요. 에러 없이 조용히요.

버퍼가 시계를 인자로 받는 것도 이유가 있어요. 어느 시계 기준으로 스탬프를 해석할지를 버퍼가 알아야 하거든요. 그리고 노드가 켜진 직후엔 버퍼가 비어 있어서, 생성자에서 바로 조회하면 아직 아무것도 없어 실패해요. 첫 조회가 실패하는 단골 원인이에요.

target이 먼저, source가 나중

인자 순서가 가장 많이 헷갈리는 지점이라 먼저 못 박을게요.

auto t = tf_buffer_->lookupTransform("base_link",     // target — 이 프레임의 말로
                                     "camera_link",   // source — 이 프레임의 데이터를
                                     tf2::TimePointZero);

판별법은 데이터 기준으로 묻는 거예요. 내 데이터가 지금 어느 프레임 말로 적혀 있나가 source, 어느 말로 바꾸고 싶나가 target이에요.

순서를 반대로 넣어도 예외가 안 나요. 반대 방향 변환이 멀쩡히 나오고, 그걸 점에 적용하면 좌표가 엉뚱한 곳으로 가요. “숫자는 오는데 로봇이 엉뚱한 곳을 집는다”의 유력 용의자예요.

반환값은 이렇게 읽어요.

header.frame_id:  "base_link"       # target — 이 좌표계의 말로 적혔다
child_frame_id:   "camera_link"     # source — 이 좌표계에서 왔다
transform:
  translation: {x: 0.2, y: 0.0, z: 0.1}     # 카메라 원점은 몸통 앞 0.2m, 위 0.1m
  rotation:    {…}                          # source 축이 target 기준으로 돌아간 정도

병진을 source 원점의 target 좌표로 읽는 감각이 중요해요. 자주 하는 오독이 방향을 반대로 읽는 거예요.

프레임 이름에는 표준이 있어서 이름만 봐도 성격이 읽혀요. 지도 프레임은 전역 기준이라 점프할 수 있지만 누적 오차가 없고, 오도메트리 프레임은 연속적이지만 시간이 지나면 밀려요. 그래서 순간적인 제어는 오도메트리 기준으로, 목표 위치는 지도 기준으로 갈라 써요.

카메라만 예외라 한 번 짚어야 해요. 로봇 표준은 x가 앞·y가 왼쪽·z가 위인데, 비전 관례를 따르는 광학 프레임은 z가 앞·x가 오른쪽·y가 아래예요. 축이 완전히 달라요. 그래서 보통 로봇 표준 축 프레임과 광학 축 프레임 둘이 다 있고 사이에 고정 회전이 끼어 있어요. 역투영 결과는 광학 프레임 좌표라서 source를 잘못 적으면 축이 뒤바뀐 채 변환돼요. “z를 앞으로 넣었는데 로봇이 위를 집는다”의 원인이에요.

보간은 해 주고 외삽은 거부해요

세 번째 인자인 시간이 이 주제의 핵심이에요. 실전에서 가장 자주 만나는 예외가 여기서 나거든요.

버퍼는 간선마다 스탬프 찍힌 변환을 시간순으로 쌓아 둔 시계열이에요.

버퍼:   5.0s ──── 5.1s ──── 5.2s
                     ▲              ▲
요청 5.15s ──────────┘ 보간 OK      │
요청 5.3s  ─────────────────────────┘ 범위 밖 → 예외
요청 0     ──────────── "최신 것" = 5.2s 반환

5.15초를 요청하면 버퍼가 두 관측 사이를 보간해서 줘요. 병진은 시간 비율로 선형 보간하고 회전은 구면 보간으로요. 두 관측 사이를 메우는 건 근거 있는 추정이거든요.

문제는 범위 밖이에요. 5.3초를 요청하면 그건 관측 범위 밖을 지어내는 것이라 tf2는 절대 하지 않아요. 로봇이 다음 순간 가속할지 멈출지 알 수 없는데 지어내면 조용히 틀린 좌표가 나가니까요. 그래서 예외를 던지고, 메시지에 미래인지 과거인지 방향까지 적혀 있어요.

실전에서 나는 상황은 둘이에요. 미래 방향은 방금 도착한 센서 메시지의 스탬프로 조회했는데 그 시각의 변환이 아직 안 들어왔을 때예요. 센서와 TF 발행자는 다른 노드라 도착 순서가 보장되지 않아요. 과거 방향은 처리가 밀려 오래된 메시지를 이제야 변환하려는데 그 시각이 보관 범위 밖으로 밀려났을 때고요.

대처는 셋이고 상황에 따라 골라요.

try {
    auto t = tf_buffer_->lookupTransform(
        "base_link", "camera_link",
        msg->header.stamp,               // 센서가 그 물체를 본 순간
        tf2::durationFromSec(0.1));      // 그 시각 데이터를 최대 0.1초 대기
} catch (const tf2::TransformException& ex) {
    RCLCPP_WARN(get_logger(), "TF 실패: %s", ex.what());
    return;   // 이번 프레임은 버리고 다음을 기다린다
}

첫째는 타임아웃을 줘서 기다리는 것으로 미래 방향의 표준 해법이에요. 주의할 점은 이 대기가 스레드를 실제로 막는다는 거예요. 단일 스레드 실행기의 콜백 안에서 0.5초짜리를 걸면 그동안 다른 콜백이 하나도 안 돌아요. 그래서 수십 밀리초 수준으로 짧게 잡는 게 관례예요.

둘째는 정확한 시각 대신 “버퍼에 있는 것 중 최신”을 뜻하는 특수값을 쓰는 거예요. 예외는 거의 안 나지만 정확도를 타협해요. 센서 시점과 최신 변환이 0.1초 어긋난 채 로봇이 0.5m/s로 움직이면 5cm 틀린 위치를 집어요. 고정 카메라나 정지 상태처럼 그 오차가 무해할 때만 쓰는 타협이에요.

셋째는 미리 물어보고 안 되면 그 주기를 건너뛰는 거예요. 제어 루프처럼 기다리면 안 되는 자리의 방식이고, 예외 대신 참·거짓을 돌려주는 함수를 써요.

시뮬레이션이나 기록 재생에서는 이 예외가 한 층 더 자주 나요. 시뮬 시간을 쓰는 노드와 벽시계를 쓰는 노드가 섞이면 한쪽 스탬프는 유닉스 시각이고 다른 쪽은 시작 기준 몇 초라, 요청 시각이 아예 다른 세계에 있어요. 이때는 어떤 타임아웃을 줘도 항상 실패해요. 모든 노드가 같은 시계를 쓰는지부터 확인하는 게 순서예요.

회전 곱하고 병진 더하기

변환을 꺼냈으면 점에 적용할 차례예요. 3D는 표기가 무거우니 구조가 똑같은 2D로 볼게요.

p_target = R(θ) · p_source + t

카메라가 몸통 앞 0.2m에 90도 돌아 붙어 있고, 카메라가 자기 기준 (0.5, 0)에서 물체를 봤다고 하면요.

단계	계산	결과
회전	R(90°)·(0.5, 0)	(0, 0.5)
병진	(0, 0.5) + (0.2, 0)	(0.2, 0.5)

순서가 회전 먼저, 병진 나중이라는 게 요점이에요. 이 둘을 행렬 하나로 합친 게 동차변환이고, 점에 1을 덧붙이면 회전과 병진이 곱 한 번이 돼요.

합쳐 두는 이유는 합성 때문이에요. 변환 두 개를 이어 적용하는 게 행렬 곱 하나로 줄어요. 인접한 프레임 이름이 맞물리며 지워지는 게 옳은 곱의 확인법이에요.

T_a←c.θ = T_a←b.θ + T_b←c.θ
T_a←c.t = R(T_a←b.θ) · T_b←c.t + T_a←b.t

병진 항에 회전이 끼는 이유가 이 식의 전부예요. 뒤 변환의 병진은 중간 프레임의 말로 적힌 거라, 앞 프레임의 말로 옮기려면 회전을 먼저 먹여야 해요.

숫자로 보면 분명해져요. 로봇이 지도에서 (1.0, 2.0)에 90도 돌아 서 있고 카메라가 몸통 앞 0.2m에 붙어 있으면, 병진은 R(90°)·(0.2, 0) + (1.0, 2.0) = (1.0, 2.2) 예요. 로봇이 돌아 있으니 카메라는 지도에서 보면 앞이 아니라 위쪽으로 나가 있어요. 회전을 안 먹이고 병진끼리 그냥 더하면 (1.2, 2.0)이 나오는데, 이게 로봇이 제자리에서 돌 때만 좌표가 틀리는 버그의 전형이에요.

역변환도 성분으로 알아 두면 좋아요. 회전을 반대로 돌리고, 병진을 반대 회전시켜 부호를 뒤집은 거예요. 맞는지 확인하는 가장 빠른 방법은 원래 변환과 합성해 보는 것으로, 항등변환이 나와야 해요.

트리 간선만 저장하면 충분한 이유

이 셋이면 왜 트리로 충분한지가 설명돼요. tf2는 모든 프레임 쌍의 변환을 저장하지 않아요. 부모-자식 간선만 저장해요.

        map
         │
        odom
         │
      base_link
       ╱     ╲
 camera_link  gripper_link

그리퍼 기준으로 카메라를 물으면, 저장된 간선은 몸통에서 각각으로 가는 둘뿐이에요. 공통 조상이 몸통이니 카메라 쪽은 그대로 올라가고 그리퍼 쪽은 뒤집어 내려가서 합성돼요. N개 프레임의 모든 쌍을 간선 N−1개로 대신하는 구조이고, 조회 한 번이 내부에서 하는 일이 경로 찾기와 시간 보간과 곱 누적이에요.

트리라서 생기는 제약도 여기서 나와요. 모든 프레임은 부모가 정확히 하나여야 해요. 두 노드가 같은 자식을 서로 다른 부모 밑에 발행하면 트리가 아니라 그래프가 되고, tf2는 나중에 온 것으로 계속 덮어써서 좌표가 두 값 사이를 왕복해요. 로봇이 시각화에서 부르르 떠는 증상의 원인이에요.

3D라고 다른 건 회전이 쿼터니언이 되고 행렬이 4×4가 되는 것뿐, 구조는 전부 같아요.

좌표가 틀리면 트리부터 봐요

코드를 뜯기 전에 세 명령이면 대부분 자리가 잡혀요.

명령	보는 것
`tf2_echo <target> <source>`	값이 실제로 무엇인지, 지금 나오긴 하는지
`view_frames`	전체 트리 구조 — 끊긴 곳·부모 둘인 곳이 그림으로
`topic hz /tf`	변환이 계속 갱신되는지

첫 명령의 인자 순서가 코드와 같아서, 같은 순서로 확인해 보고 값이 코드 안 값과 같은지 대조하면 인자 순서 실수인지 아닌지가 바로 갈려요.

발행 쪽도 한 줄 짚어 둘게요. 움직이는 관계는 계속 발행되고 고정된 관계는 한 번만 발행돼요. 대부분 내 코드가 아니라 로봇 모델 발행자가 URDF를 읽어 내보내니, 카메라 위치를 옮기면 코드가 아니라 URDF를 고치는 게 맞는 자리예요. 그 흐름은 joint_states와 robot_description, 그리고 서보 두 점 캘리브레이션에 있어요.

조회는 결국 세 인자 질문이에요. 어느 말로, 무엇을, 언제 기준으로. 인자 순서와 시간 처리 둘만 잡으면 좌표 사고의 대부분이 막혀요.

hello, robot