In a real robot we rarely have a single coordinate frame. We use a world frame, a body frame, multiple sensor frames, and often a tool frame. Each pair of frames is related by a homogeneous transform introduced in Lesson 4. The key questions are:

• How do we organize many frames so transforms are unambiguous and efficient to compute?
• When do given transforms “agree” with each other (consistency)?

Robotics software (e.g., ROS TF) typically stores frames as a rooted tree: every frame except the root has exactly one parent, so there is a unique path between any two frames. This guarantees that a relative transform is computed by composing transforms along that path.

flowchart TD
  W["World frame W (root)"] --> B["Body frame B"]
  B --> C["Camera frame C"]
  B --> L["LiDAR frame L"]
  C --> I["Image plane frame I"]
  L --> P["Point cloud frame P"]
        

The diagram shows a typical frame tree. Because there is only one path between any two nodes, a transform such as T_C_P is uniquely defined via the path C → B → L → P.

Let \( \mathcal{F}=\{F_0,F_1,\dots,F_n\} \) be the set of frames. We model their relations as a directed graph \( G=(\mathcal{F},\mathcal{E}) \), where an edge \( (F_i,F_j)\in\mathcal{E} \) means we know the transform from \(F_i\) to \(F_j\).

Each edge carries a homogeneous transform \( \mathbf{T}_{ij}\in SE(3) \) (conceptually introduced in Lesson 4):

\[ \mathbf{T}_{ij} = \begin{bmatrix} \mathbf{R}_{ij} & \mathbf{p}_{ij} \\ \mathbf{0}^\top & 1 \end{bmatrix}, \qquad \mathbf{R}_{ij}\in SO(3),\; \mathbf{p}_{ij}\in\mathbb{R}^3 . \]

If a point has coordinates \( \mathbf{x}_j=[x_j,y_j,z_j,1]^\top \) in frame \(F_j\), then in frame \(F_i\):

\[ \mathbf{x}_i = \mathbf{T}_{ij}\mathbf{x}_j . \]

The inverse transform always exists:

\[ \mathbf{T}_{ji} = \mathbf{T}_{ij}^{-1} = \begin{bmatrix} \mathbf{R}_{ij}^\top & -\mathbf{R}_{ij}^\top\mathbf{p}_{ij} \\ \mathbf{0}^\top & 1 \end{bmatrix}. \]

A frame tree is a directed, rooted tree over \(\mathcal{F}\). Formally, \(G\) is a tree if:

1. There is a distinguished root \(F_r\) (often “world”).
2. Every node \(F_i\neq F_r\) has exactly one parent.
3. There is a unique undirected path between any two nodes.

Let the unique path from \(F_a\) to \(F_b\) be \(F_a=F_{i_0}\to F_{i_1}\to\dots\to F_{i_k}=F_b\). The composed transform is

\[ \mathbf{T}_{ab} = \mathbf{T}_{i_0 i_1}\mathbf{T}_{i_1 i_2}\cdots\mathbf{T}_{i_{k-1}i_k}. \]

Proposition (Uniqueness). In a frame tree, \(\mathbf{T}_{ab}\) is uniquely determined for every pair of frames \(F_a,F_b\).

Proof.

By the tree property, there is exactly one path between \(F_a\) and \(F_b\). The transform along that path is defined as the ordered product above. Since no other path exists, no other product can define a different transform. Hence \(\mathbf{T}_{ab}\) is unique. \(\square\)

This is why trees are preferred: they prevent ambiguity. In a general graph with cycles, multiple paths might yield different transforms unless special consistency constraints hold.

Suppose the graph is not a tree and contains a cycle: \(F_{i_0}\to F_{i_1}\to\dots\to F_{i_{k}}\to F_{i_0}\). The transforms are consistent if composing around the cycle gives identity:

\[ \mathbf{T}_{i_0 i_1}\mathbf{T}_{i_1 i_2}\cdots \mathbf{T}_{i_k i_0} = \mathbf{I}_4 . \]

If the product is not identity, at least one transform is incorrect, stale, or noisy. The deviation from identity is quantified by an error transform \( \mathbf{E} \):

\[ \mathbf{E} = \mathbf{T}_{i_0 i_1}\mathbf{T}_{i_1 i_2}\cdots \mathbf{T}_{i_k i_0}, \qquad \mathbf{E}\neq \mathbf{I}_4 \; \Rightarrow \; \text{inconsistency}. \]

Since we are in an intro chapter, we do not use logarithms of \(SE(3)\). Instead, we can measure inconsistency using a simple matrix norm:

\[ \epsilon = \|\mathbf{E}-\mathbf{I}_4\|_F, \]

where \(\|\cdot\|_F\) is the Frobenius norm. A small \(\epsilon\) indicates near-consistency.

Theorem (Cycle Consistency Implies Path Independence). If all cycles in a connected frame graph satisfy \(\mathbf{T}_{cycle}=\mathbf{I}_4\), then the computed transform between any two frames is independent of the chosen path.

Proof.

Let there be two paths \(P_1\) and \(P_2\) from frame \(F_a\) to \(F_b\). Traverse \(P_1\) forward and \(P_2\) backward; this forms a cycle. Denote composed transforms along these paths as \(\mathbf{T}_{ab}^{(1)}\) and \(\mathbf{T}_{ab}^{(2)}\). The cycle product is

\[ \mathbf{T}_{ab}^{(1)}\left(\mathbf{T}_{ab}^{(2)}\right)^{-1} = \mathbf{I}_4 . \]

Multiply on the right by \(\mathbf{T}_{ab}^{(2)}\) to get \(\mathbf{T}_{ab}^{(1)}=\mathbf{T}_{ab}^{(2)}\). Therefore transforms are path independent. \(\square\)

A tree is a special case: it has no cycles, so path independence holds automatically.

In practice, sensors and links provide many pairwise transforms. To avoid ambiguity, we typically select a spanning tree rooted at a chosen world frame.

Let \(G\) be any connected graph of frames. A spanning tree \(T\subseteq G\) can be found (e.g., by BFS or DFS). We store only transforms on edges of \(T\). All other transforms are derived by composition.

Proposition. The spanning-tree transforms define a consistent transform set on all pairs in \(\mathcal{F}\).

Proof.

In a tree there is a unique path between any two nodes. Define \(\mathbf{T}_{ab}\) as the product along that path. Since no alternate path exists, there is no possibility for contradiction. Hence the induced complete set is consistent by construction. \(\square\)

If the original graph contains redundant measurements, consistency checking is done by verifying that each redundant edge agrees with the tree-derived transform.

flowchart TD
  S["Start with connected frame graph"] --> T["Choose root frame"]
  T --> B["Build spanning tree (BFS/DFS)"]
  B --> Store["Store transforms on tree edges"]
  Store --> Derive["Derive any other T_ab by path product"]
  Derive --> Check["For each redundant edge, compute error E"]
  Check --> OK["If ||E - I|| small: consistent"]
  Check --> Bad["Else: locate faulty transform"]
        

Consider frames \(W\) (world), \(B\) (body), and \(C\) (camera). You know \(\mathbf{T}_{WB}\) and \(\mathbf{T}_{BC}\). The derived transform from world to camera is

\[ \mathbf{T}_{WC} = \mathbf{T}_{WB}\mathbf{T}_{BC}. \]

Suppose an additional measurement provides \(\tilde{\mathbf{T}}_{WC}\). The cycle error is

\[ \mathbf{E} = \mathbf{T}_{WB}\mathbf{T}_{BC}\tilde{\mathbf{T}}_{CW}, \qquad \tilde{\mathbf{T}}_{CW} = \tilde{\mathbf{T}}_{WC}^{-1}. \]

If \(\mathbf{E}\neq\mathbf{I}_4\), then the direct measurement \(\tilde{\mathbf{T}}_{WC}\) disagrees with the tree, or some upstream transform is wrong.

We use NumPy to represent homogeneous transforms and verify a 3-node cycle.

import numpy as np

def make_T(R, p):
    T = np.eye(4)
    T[0:3, 0:3] = R
    T[0:3, 3] = p
    return T

def inv_T(T):
    R = T[0:3, 0:3]
    p = T[0:3, 3]
    Tinv = np.eye(4)
    Tinv[0:3, 0:3] = R.T
    Tinv[0:3, 3] = -R.T @ p
    return Tinv

def frob_error(E):
    return np.linalg.norm(E - np.eye(4), ord="fro")

# Example transforms (toy numbers)
R_WB = np.eye(3)
p_WB = np.array([1.0, 0.0, 0.0])
T_WB = make_T(R_WB, p_WB)

R_BC = np.eye(3)
p_BC = np.array([0.0, 2.0, 0.0])
T_BC = make_T(R_BC, p_BC)

# Derived T_WC
T_WC_derived = T_WB @ T_BC

# Direct measurement with slight noise
T_WC_meas = T_WC_derived.copy()
T_WC_meas[0, 3] += 0.05  # small inconsistency

# Cycle error E = T_WB T_BC T_CW_meas
E = T_WB @ T_BC @ inv_T(T_WC_meas)

print("Cycle Frobenius error:", frob_error(E))
      

If the printed error is close to zero, the transforms are consistent. Larger values indicate disagreement.

Eigen is the standard linear algebra library in robotics C++ stacks.

#include <iostream>
#include <Eigen/Dense>

using Eigen::Matrix4d;
using Eigen::Matrix3d;
using Eigen::Vector3d;

Matrix4d makeT(const Matrix3d& R, const Vector3d& p) {
    Matrix4d T = Matrix4d::Identity();
    T.block<3,3>(0,0) = R;
    T.block<3,1>(0,3) = p;
    return T;
}

Matrix4d invT(const Matrix4d& T) {
    Matrix3d R = T.block<3,3>(0,0);
    Vector3d p = T.block<3,1>(0,3);
    Matrix4d Tinv = Matrix4d::Identity();
    Tinv.block<3,3>(0,0) = R.transpose();
    Tinv.block<3,1>(0,3) = -R.transpose() * p;
    return Tinv;
}

double frobError(const Matrix4d& E) {
    return (E - Matrix4d::Identity()).norm(); // Frobenius
}

int main() {
    Matrix3d R_WB = Matrix3d::Identity();
    Vector3d p_WB(1,0,0);
    Matrix4d T_WB = makeT(R_WB, p_WB);

    Matrix3d R_BC = Matrix3d::Identity();
    Vector3d p_BC(0,2,0);
    Matrix4d T_BC = makeT(R_BC, p_BC);

    Matrix4d T_WC_derived = T_WB * T_BC;
    Matrix4d T_WC_meas = T_WC_derived;
    T_WC_meas(0,3) += 0.05;

    Matrix4d E = T_WB * T_BC * invT(T_WC_meas);

    std::cout << "Cycle Frobenius error: " << frobError(E) << std::endl;
    return 0;
}
      

Java robotics stacks often use EJML or Apache Commons Math. Below is a minimal EJML-style snippet (conceptual).

import org.ejml.simple.SimpleMatrix;

public class FrameConsistency {

    static SimpleMatrix makeT(SimpleMatrix R, SimpleMatrix p) {
        SimpleMatrix T = SimpleMatrix.identity(4);
        T.insertIntoThis(0,0,R);
        T.insertIntoThis(0,3,p);
        return T;
    }

    static SimpleMatrix invT(SimpleMatrix T) {
        SimpleMatrix R = T.extractMatrix(0,3,0,3);
        SimpleMatrix p = T.extractMatrix(0,3,3,4);
        SimpleMatrix Tinv = SimpleMatrix.identity(4);
        Tinv.insertIntoThis(0,0,R.transpose());
        Tinv.insertIntoThis(0,3,R.transpose().scale(-1).mult(p));
        return Tinv;
    }

    static double frobError(SimpleMatrix E) {
        return E.minus(SimpleMatrix.identity(4)).normF();
    }

    public static void main(String[] args) {
        SimpleMatrix R_WB = SimpleMatrix.identity(3);
        SimpleMatrix p_WB = new SimpleMatrix(3,1,true, new double[]{1,0,0});
        SimpleMatrix T_WB = makeT(R_WB, p_WB);

        SimpleMatrix R_BC = SimpleMatrix.identity(3);
        SimpleMatrix p_BC = new SimpleMatrix(3,1,true, new double[]{0,2,0});
        SimpleMatrix T_BC = makeT(R_BC, p_BC);

        SimpleMatrix T_WC_derived = T_WB.mult(T_BC);
        SimpleMatrix T_WC_meas = T_WC_derived.copy();
        T_WC_meas.set(0,3, T_WC_meas.get(0,3) + 0.05);

        SimpleMatrix E = T_WB.mult(T_BC).mult(invT(T_WC_meas));
        System.out.println("Cycle Frobenius error: " + frobError(E));
    }
}
      

Matlab represents transforms with 4×4 matrices. In Simulink, these computations map to Matrix Multiply and Matrix Inverse blocks.

function epsilon = check_cycle_consistency(T_WB, T_BC, T_WC_meas)
%CHECK_CYCLE_CONSISTENCY  Frobenius inconsistency over a 3-node cycle.

T_CW_meas = inv(T_WC_meas);
E = T_WB * T_BC * T_CW_meas;
epsilon = norm(E - eye(4), 'fro');
end

% Example usage:
T_WB = eye(4); T_WB(1,4) = 1;
T_BC = eye(4); T_BC(2,4) = 2;
T_WC_derived = T_WB * T_BC;
T_WC_meas = T_WC_derived; T_WC_meas(1,4) = T_WC_meas(1,4) + 0.05;

eps_val = check_cycle_consistency(T_WB, T_BC, T_WC_meas);
disp(eps_val);
      

Simulink hint. Use three blocks: (1) Matrix Multiply for \(T_{WB}T_{BC}\), (2) Matrix Inverse for \(T_{WC}^{meas}\), (3) Multiply again and subtract Identity, then a Norm block.

Problem 1 (Tree Composition): Frames \(W\to B\to S\) form a chain. Given transforms \(\mathbf{T}_{WB}\) and \(\mathbf{T}_{BS}\), derive \(\mathbf{T}_{SW}\).

Solution:

First compute \(\mathbf{T}_{WS}=\mathbf{T}_{WB}\mathbf{T}_{BS}\). Then invert:

\[ \mathbf{T}_{SW} = \mathbf{T}_{WS}^{-1} = (\mathbf{T}_{WB}\mathbf{T}_{BS})^{-1} = \mathbf{T}_{SB}\mathbf{T}_{BW}. \]

Problem 2 (Cycle Consistency Check): Three frames \(A,B,C\) form a cycle. You are given \(\mathbf{T}_{AB}\), \(\mathbf{T}_{BC}\), and \(\mathbf{T}_{CA}\). Show that consistency requires \(\mathbf{T}_{AB}\mathbf{T}_{BC}=\mathbf{T}_{AC}\).

Solution:

Cycle consistency says \(\mathbf{T}_{AB}\mathbf{T}_{BC}\mathbf{T}_{CA}=\mathbf{I}_4\). Multiply on the right by \(\mathbf{T}_{AC}=\mathbf{T}_{CA}^{-1}\):

\[ \mathbf{T}_{AB}\mathbf{T}_{BC} = \mathbf{T}_{CA}^{-1} = \mathbf{T}_{AC}. \]

Problem 3 (Redundant Edge Error): In a frame tree you compute \(\mathbf{T}_{WC}^{tree}\), but a sensor gives \(\tilde{\mathbf{T}}_{WC}\). Define an error measure \(\epsilon\) that is zero iff they agree.

Solution:

Use the cycle error \(\mathbf{E}=\mathbf{T}_{WC}^{tree}\tilde{\mathbf{T}}_{CW}\). Then

\[ \epsilon = \|\mathbf{E}-\mathbf{I}_4\|_F, \qquad \epsilon=0 \Leftrightarrow \mathbf{T}_{WC}^{tree}=\tilde{\mathbf{T}}_{WC}. \]

Problem 4 (Path Independence): Suppose between frames \(F_a\) and \(F_b\) there are two paths in a graph. Prove that if all cycles are consistent, both paths produce the same transform.

Solution:

Let the two path products be \(\mathbf{T}_{ab}^{(1)}\) and \(\mathbf{T}_{ab}^{(2)}\). Traversing path 1 forward and path 2 backward forms a cycle, so \(\mathbf{T}_{ab}^{(1)}(\mathbf{T}_{ab}^{(2)})^{-1}=\mathbf{I}_4\). Right-multiplying yields \(\mathbf{T}_{ab}^{(1)}=\mathbf{T}_{ab}^{(2)}\).

Problem 5 (Design Choice): Explain one practical reason why a robotics framework might forbid cycles even though cycles can be consistent.

Solution:

Cycles require continuous consistency checks and can become inconsistent due to noise, delays, or time-varying measurements. A tree gives a single, unambiguous transform computation and avoids contradictions, so it is safer and simpler for real-time systems.

We modeled frames as a graph with homogeneous transforms on edges, showed that frame trees guarantee unique relative transforms, and formalized consistency through cycle-closure constraints. Trees provide automatic path independence and are therefore standard in robotics software. Simple norm-based checks allow us to detect redundancy errors without advanced kinematics.