Pose estimation and point-cloud registration

Last lesson ended with a calibrated stack: intrinsics that map pixels to rays, and a hand-eye transform that maps camera coordinates into the robot's base frame. You can now turn a pixel plus a depth reading into a 3D point the arm can reach toward. But a point is not an object. To close a gripper on a mug you need position and orientation — the transform $T^{\text{base}}_{\text{obj}} \in SE(3)$ — because next lesson's grasp candidates are defined relative to the object, not to a pixel. Estimating that transform is the last perception problem between you and a working classical pick-and-place. It is also, structurally, a problem you already know: non-convex optimization with a retrieval step inside, where initialization picks your basin and the loss can be confidently low while the answer is wrong.

Geometric Foundations: Rigid Transformations and the Kabsch Proof

Before deriving the alignment algorithm, we must rigorously define the space in which the pose lives. A rigid transformation in 3D space is an element of the Special Euclidean group, denoted $SE(3)$. An element $T \in SE(3)$ is a $4 \times 4$ homogeneous matrix composed of a rotation matrix $R \in SO(3)$ and a translation vector $t \in \mathbb{R}^3$:

The rotation component $R$ belongs to the Special Orthogonal group $SO(3)$. A matrix is in $SO(3)$ if and only if it is orthogonal ($R^T R = I$) and has a determinant of 1 ($\det(R) = 1$). The orthogonality condition ensures that distances and angles are preserved (rigidity), while the determinant condition ensures that the transformation is a proper rotation (preserving chirality) rather than a reflection. We adopt the active transformation convention: $T$ maps coordinates of a fixed physical point from the object frame to the base frame. This is consistent with the code implementation model @ R.T + t, where the model points are rotated into the scene frame.

The Kabsch algorithm minimizes the squared error $E(R, t) = \sum_i w_i \| R a_i + t - b_i \|^2$. After eliminating the translation $t$ by aligning centroids, the problem reduces to maximizing $\mathrm{tr}(R H)$, where $H = \sum_i w_i a_i' b_i'^T$ is the cross-covariance matrix. The standard derivation states that the maximum is achieved when $M = V^T R U = I$. To justify this rigorously, consider the Singular Value Decomposition (SVD) of $H$: $H = U \Sigma V^T$. Substituting this into the trace term gives $\mathrm{tr}(R H) = \mathrm{tr}(R U \Sigma V^T) = \mathrm{tr}(\Sigma V^T R U)$. Let $M = V^T R U$. Since $R, U, V$ are all orthogonal, $M$ is orthogonal. The eigenvalues of an orthogonal matrix lie on the unit circle in the complex plane. The trace of a matrix is the sum of its eigenvalues. For a real orthogonal matrix, the trace is maximized when all eigenvalues are 1, which implies $M = I$. Any deviation from the identity introduces eigenvalues with magnitude 1 but non-zero phase, reducing the real part of the sum.

In point-to-plane ICP, we linearize the rotation for small angles. Let $\theta \in \mathbb{R}^3$ be a small rotation vector. The rotation matrix can be approximated by the first-order Taylor expansion $R \approx I + [\theta]_\times$, where $[\theta]_\times$ is the skew-symmetric matrix defined such that $[\theta]_\times v = \theta \times v$ for any vector $v$. This approximation is valid for $\|\theta\| < 10^\circ$, where higher-order terms are $O(\theta^2)$ and negligible. This linearization converts the non-linear optimization into a linear least-squares problem, significantly accelerating convergence on planar surfaces.

Worked Example: RANSAC Iteration Budget

RANSAC (Random Sample Consensus) is a robust estimation algorithm that iteratively samples minimal subsets of data to fit a model. The number of iterations $N$ required to achieve a confidence level $p$ depends on the inlier ratio $w$ and the sample size $n$. For a 3D rigid transformation, the minimal sample size is $n=3$ because three non-collinear points provide 6 constraints (2 per point), which uniquely determine the 6 degrees of freedom of the pose. The probability that a single sample of size $n$ consists entirely of inliers is $w^n$. The probability that a single sample fails (contains at least one outlier) is $1 - w^n$. The probability that all $N$ samples fail is $(1 - w^n)^N$. We require this failure probability to be less than $1 - p$.

Consider a scenario where the inlier ratio is $w = 0.2$ (20% of correspondences are correct) and we desire a confidence of $p = 0.99$ (99% chance of finding at least one all-inlier sample). The sample size is $n = 3$. First, calculate the probability of an all-inlier sample: $p_{\text{sample}} = w^n = 0.2^3 = 0.008$. The probability of failure for one sample is $1 - 0.008 = 0.992$. We solve for $N$ in the inequality $(0.992)^N \le 1 - 0.99 = 0.01$. Taking the natural logarithm of both sides: $N \ln(0.992) \le \ln(0.01)$. Since $\ln(0.992) \approx -0.008032$ and $\ln(0.01) \approx -4.60517$, we have $N \ge \frac{-4.60517}{-0.008032} \approx 573.35$. Rounding up to the nearest integer, we require $N = 574$ iterations.

From pixels to pose: a three-stage pipeline

Every classical pose estimator on depth data has the same skeleton: segment the object's pixels out of the scene, lift them to a 3D point cloud via last lesson's intrinsics and depth, and register an object model against that cloud to recover the pose. The pipeline view is the debugging architecture: each stage has its own failure signature, visualization, and fix — precisely why Phase 02 builds this instead of jumping to a learned end-to-end detector.

• Segment — isolate the object's pixels. The classical route: fit the table plane with RANSAC (3-point hypotheses, inliers within ~8 mm), delete it — about 90% of a tabletop scene — then Euclidean-cluster what remains. Failure signature: stray table or neighbor-object points in the crop, which registration then dutifully aligns to.
• Lift — back-project each segmented pixel through the intrinsics at its measured depth, then apply the hand-eye transform. An 848×480 depth frame is ~400k points; the object crop is 5–20k; a 3–5 mm voxel downsample leaves 1–5k — all registration needs. Failure signature: holes on dark or specular surfaces, phantom points bleeding off depth edges.
• Register — align a model of the object (a CAD mesh sampled to points, or a template scan captured once) to the lifted cloud. The output is the pose; the residual is your first quality signal. Failure signature: convergence to a plausible-looking wrong pose — the subject of most of this lesson.

Before diving into registration, set the error budget. Typical per-stage contributions on your hardware: intrinsics reprojection ~0.3–0.5 px (sub-millimeter at 60 cm), depth noise 2–5 mm at 60 cm, hand-eye calibration 2–5 mm, registration residual 1–3 mm. Root-sum-square: roughly 4–8 mm of position uncertainty at the object. Hold that number — the WidowX AI's parallel-jaw gripper opening to 40 mm around a 25 mm box has 7.5 mm of clearance per side. Your error budget and your grasp margin are the same order of magnitude, which is why the closing uncertainty section is not optional hygiene.

Known correspondences: PnP and the honest fiducial

Registration is hard because you do not know which observed point corresponds to which model point. So start where you do. If you can identify $n$ known object points $X_i$ (in the object's frame) at image pixels $u_i$, the pose follows from the Perspective-n-Point (PnP) problem: find the rigid transform that reprojects the model points onto their observed pixels.

In a lab, identified correspondences come from fiducials: AprilTag- or ArUco-style markers whose detectors return four sub-pixel corners with identity — correspondence is free, encoded in the bit pattern. Detection costs 5–20 ms per VGA frame on one CPU core, needs no GPU and no depth, and a 40–50 mm tag at 50–60 cm gives sub-millimeter in-plane position noise. Two honest caveats: depth-axis translation and out-of-plane rotation are several times noisier than the in-plane components, and near-fronto-parallel tags have a genuine two-fold flip ambiguity — two orientations reproject almost identically, and a good planar-PnP solver reports both candidates rather than silently picking one.

Here is the judgment call to internalize: taping a fiducial to the object or bin is not cheating — it is fixturing. Phase 02's goal is a debuggable oracle that separates perception, planning, and execution failures, and a tag turns pose estimation into a solved subproblem with known error bars — exactly like mocking a dependency to integration-test everything else. Debug grasping and motion planning against tag poses first; swap in tag-free registration once the downstream stack is trustworthy. Tedrake's geometric pose estimation chapter (opens in a new tab) makes the same progression. The rest of this lesson is about what the tag was hiding.

Unknown correspondences: ICP and the Kabsch step

Without a fiducial, points carry no identities — just coordinates. A chicken-and-egg problem: with the pose known, correspondences are trivial (nearest scene point); with correspondences known, the pose is a closed-form solve. Iterative Closest Point (ICP) alternates the two until fixed point. The closed form is worth deriving — a whiteboard-reconstructable result, like the normal equations. Given matched pairs $(a_i, b_i)$ with weights $w_i$, minimize:

Step 1 — eliminate the translation. $E$ is an unconstrained quadratic in $t$; setting $\partial E / \partial t = 0$ gives $t^\star = \bar{b} - R\,\bar{a}$, with $\bar{a}, \bar{b}$ the weighted centroids: the optimal transform maps centroid to centroid, whatever the rotation. Substituting back and writing $a_i' = a_i - \bar{a}$, $b_i' = b_i - \bar{b}$ reduces the problem to rotation-only alignment of centered sets. Step 2 — expand. Since rotations preserve norms, expanding the square leaves one $R$-dependent term:

Step 3 — solve by SVD. Decompose $H = U \Sigma V^\top$ and substitute: $\mathrm{tr}(R H) = \mathrm{tr}(\Sigma\, V^\top R\, U) = \sum_i \sigma_i m_{ii}$, where $M = V^\top R\, U$ is a product of orthogonal matrices, hence orthogonal, hence has every entry bounded by 1. The trace is at most $\sum_i \sigma_i$, achieved exactly when $M = I$:

That is the Kabsch (or Umeyama, with scale) algorithm: $O(N)$ to accumulate $H$ plus a 3×3 SVD — microseconds for thousands of points. The ICP loop wraps it: transform the model by the current estimate; match each model point to its nearest scene neighbor with a k-d tree; reject pairs beyond a threshold (start at ~2 cm, tighten as you converge); run Kabsch on the survivors and compose the increment; repeat until the RMS residual stops improving. On a 2k-point downsampled cloud: 10–40 iterations, a handful of milliseconds on CPU — comfortably inside a perception tick.

The catch is the loss landscape. ICP's objective is non-convex, and the algorithm is pure local search: for a typical tabletop object the basin of attraction is roughly ±20–30° in rotation and a few centimeters in translation. Initialize outside it and ICP converges — monotonically, confidently — to a wrong pose. Real systems never run ICP cold. Standard warm starts: the segment's centroid plus principal axes (a PCA frame, free from the lift stage), the previous frame's pose when tracking at 30 Hz (millimeters between frames — deep inside the basin), or a coarse global-registration pass we will meet below.

One refinement belongs in every serious pipeline: point-to-plane ICP. Instead of penalizing full 3D distance between matched points, penalize only the component along the scene's surface normal $n_i$ (estimated per point by local PCA over ~20–30 neighbors):

import numpy as np

def kabsch(src, dst, weights=None):
    """Best-fit rigid transform minimizing sum w_i * || R @ src_i + t - dst_i ||^2.

    src, dst: (N, 3) matched point sets (row i of src corresponds to row i of dst).
    Returns R (3, 3), t (3,) such that dst is approximately src @ R.T + t.
    """
    if weights is None:
        weights = np.ones(len(src))
    w = weights / weights.sum()
    src_bar = w @ src                      # weighted centroids
    dst_bar = w @ dst
    A = src - src_bar
    B = dst - dst_bar
    H = (A * w[:, None]).T @ B             # 3x3 cross-covariance
    U, S, Vt = np.linalg.svd(H)
    d = np.sign(np.linalg.det(Vt.T @ U.T)) # reflection guard
    R = Vt.T @ np.diag([1.0, 1.0, d]) @ U.T
    t = dst_bar - R @ src_bar
    return R, t

def icp_point_to_point(model, scene, R0, t0, iters=40, reject=0.02):
    """Register model (M, 3) into scene (N, 3) from initial guess (R0, t0).

    reject: correspondence distance cutoff in meters. Brute-force NN is fine
    for a few thousand downsampled points; swap in a k-d tree beyond that.
    """
    R, t = R0.copy(), t0.copy()
    rms = float("inf")
    for _ in range(iters):
        moved = model @ R.T + t
        d2 = ((moved[:, None, :] - scene[None, :, :]) ** 2).sum(axis=2)
        nn = d2.argmin(axis=1)
        dist = np.sqrt(d2[np.arange(len(model)), nn])
        keep = dist < reject
        if keep.sum() < 3:
            break                          # lost the object: report, do not guess
        dR, dt = kabsch(moved[keep], scene[nn[keep]])
        R = dR @ R                         # compose the increment
        t = dR @ t + dt
        new_rms = float(np.sqrt((dist[keep] ** 2).mean()))
        if abs(rms - new_rms) < 1e-6:
            rms = new_rms
            break
        rms = new_rms
    inlier_frac = float(keep.mean())
    return R, t, rms, inlier_frac

if __name__ == "__main__":
    rng = np.random.default_rng(0)
    model = rng.uniform(-0.03, 0.03, size=(800, 3))     # 6 cm synthetic object
    angle = np.deg2rad(15.0)                            # inside the basin
    c, s = np.cos(angle), np.sin(angle)
    R_true = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]])
    t_true = np.array([0.01, -0.02, 0.005])
    scene = model @ R_true.T + t_true
    scene += rng.normal(0.0, 0.002, scene.shape)        # 2 mm depth-like noise
    R, t, rms, frac = icp_point_to_point(
        model, scene, np.eye(3), np.zeros(3)
    )
    print("rms mm:", round(rms * 1e3, 2), " inliers:", round(frac, 3))
    print("t error mm:", np.round((t - t_true) * 1e3, 2))

Run this at 15° and it recovers the rotation to 0.3°; change angle to 60° and it settles into a minimum more than 80° wrong in rotation while reporting an innocuous 4 mm residual. That one-line experiment calibrates your trust better than any theorem. Note the return signature — pose plus residual plus inlier fraction — because your grasp planner needs evidence, not just an answer.

Robustness: outliers, partial views, and symmetry

The squared loss in Kabsch has the flaw you expect: one mixed-pixel phantom at 10 cm contributes as much as four hundred honest correspondences at 5 mm. Three standard defenses, in increasing machinery: the rejection threshold already in the loop above; trimmed ICP, keeping only the best 70–90% of pairs each iteration (match the ratio to the expected visible fraction); and robust kernels (Huber, Tukey) via iteratively reweighted least squares — free here: the weighted Kabsch you derived is the IRLS inner step, fed weights that shrink with residual size.

For initialization-free global registration, the classical answer is RANSAC over feature correspondences: compute local geometric descriptors (FPFH-style histograms) on model and scene, match them, then repeatedly draw 3 matches, solve Kabsch on the triplet, and count how many other matches the hypothesis explains. The iteration budget is one line of probability: with inlier ratio $w$ and sample size $n$, a sample is all-inlier with probability $w^n$, so failure probability after $N$ draws is $(1 - w^n)^N$. Demanding success probability $p$:

Two failure modes will bite you in week one. Partial views: a camera sees at most half the object, so statistics of the visible cloud are biased — the centroid of the visible 60% of a 60 mm box sits a centimeter off the true center, so PCA initialization starts a centimeter wrong; trimmed ICP matched to the expected overlap is the standard mitigation. Symmetry: a cylinder's rotation about its axis is fundamentally unobservable — that quiz was not hypothetical — and discrete symmetries are subtler: a square container has four poses fitting the data exactly equally, and which one ICP returns depends on initialization noise. Frame-to-frame the estimate can hop between branches, so naively averaging poses over time produces nonsense (the mean of two valid flips is an invalid pose) — and it is why learned-pose papers report the symmetry-aware ADD-S metric for such objects.

Uncertainty: measure it, then spend it

Everything so far produces a point estimate; before the grasp planner spends it you need error bars, and for a system this full of unmodeled effects the honest way to get them is empirical. Run a repeatability study in three tiers, each isolating a slice of the pipeline. Tier 1: freeze the scene and collect 100 estimates — the spread is sensor noise plus algorithm jitter, typically 0.3–1 mm and 0.2–0.5° for a well-conditioned object at 60 cm. Tier 2: re-place the object at the same nominal spot 20 times — segmentation variation and view-dependent effects enter, and the spread doubles or triples. Tier 3: repeat from 2–3 different camera or arm poses — this exposes hand-eye bias, which no fixed-viewpoint repetition can reveal. The finding worth internalizing: variance is usually the small problem — the dominant error is a repeatable millimeters-scale calibration bias, which does not average away; it must be found (tier 3 finds it) and recalibrated away.

Then convert the statistics into the currency that matters: predicted grasp success. The simplest useful model says a grasp succeeds when lateral pose error stays inside the mechanical margin $m$ — half the gripper-opening-minus-object-width, less a friction allowance next lesson refines. With Gaussian lateral error of deviation $\sigma$ and bias $b$:

Numbers like these turn debugging from vibes into arithmetic. If the measured pipeline gives $\sigma = 3$ mm and $b = 1$ mm and the arm still misses 30% of grasps, the miss is not a perception problem — go look at planning or execution. That failure attribution is what Phase 02 exists to enable, and it outlives this phase: when Phase 04's π₀-class policy from the OpenPI stack fails on the same object, this pipeline is the counterfactual that says whether the scene was genuinely hard or the policy blew a decision perception had nailed.

Finally: when should you not use this pipeline? Reach for learned components when its assumptions break — transparent or reflective objects that return no usable depth, heavy clutter where segmentation is the bottleneck (a learned segmenter dropped into stage 1 is often the best of both worlds), category-level tasks with no per-instance CAD model, or RGB-only settings. FoundationPose-style render-and-compare estimators produce impressive 6-DOF poses at 30–200 ms on an RTX-class GPU. What you give up is attribution: a learned estimator's error is a property of a training distribution you cannot inspect; this pipeline's error is a property of geometry you can render, plot, and fix stage by stage.

Where this goes next: Camera models, calibration, and hand-eye gave you trustworthy 3D points; this lesson organized them into object poses with measured error bars. Grasping: friction, closure, and candidates spends that budget — it generates grasp candidates in the object frame you just estimated, and its friction-cone and closure margins are exactly what your pose uncertainty must fit inside. Every pick your classical oracle attempts is now a chain of three auditable claims — pose right, grasp right, motion right — and you own the instruments to test the first.