Estimation for manipulation and perception: pose, tracking and data association

Localization asks where the robot is. Many robotic tasks also need to know where other things are: the pose of a part on a conveyor so an arm can pick it, the position and velocity of people near a mobile robot, the location of a tool in the gripper. The same probabilistic tools apply, but two problems become central. The first is estimating an object's full position and orientation from sensor data, often by aligning a model with measured points. The second is data association: when the sensor reports several detections and there are several objects, which detection belongs to which object, and which detections are clutter? This lesson covers point-set alignment and its uncertainty, multi-target tracking with Kalman filters, the main data association methods, and how these ideas appear in manipulation.

Estimating a pose by aligning points

Suppose a robot knows a set of model points on an object, \( \mathbf{p}_i \) in the object frame, such as the corners of a box or the centres of fiducial markers, and its sensor measures the corresponding points \( \mathbf{q}_i \) in the camera frame. The object's pose is the rotation R and translation t that best map model points onto measured points, minimizing

When correspondences are known, this problem has a closed-form solution, often called the Kabsch or Umeyama method. Subtract the centroids \( \bar{\mathbf{p}} \) and \( \bar{\mathbf{q}} \) from each set, form the cross-covariance matrix \( \mathbf{W} = \sum_i (\mathbf{q}_i - \bar{\mathbf{q}})(\mathbf{p}_i - \bar{\mathbf{p}})^{\mathsf{T}} \), take its singular value decomposition \( \mathbf{W} = \mathbf{U}\mathbf{D}\mathbf{V}^{\mathsf{T}} \), and set

The determinant term prevents the solution from being a reflection rather than a rotation. When correspondences are unknown, as with raw lidar or depth-camera points, the iterative closest point (ICP) algorithm alternates two steps: match each measured point to its nearest model point under the current pose estimate, then solve the alignment above for those matches, repeating until the pose stops changing. ICP converges quickly from a good initial guess but can lock onto a wrong alignment from a poor one, so it is usually initialized from a coarser method, such as feature matching or a previous estimate. For camera images, where only 2-D projections of known 3-D points are measured, the corresponding problem is perspective-n-point (PnP), solved by algorithms such as EPnP followed by nonlinear refinement; fiducial markers like AprilTags use exactly this to give a full 6-D pose from a single image.

import numpy as np
rng = np.random.default_rng(8)
P = np.array([[0, 0], [0.30, 0], [0.30, 0.20], [0, 0.20], [0.15, 0.25]])   # model points (m)
th = np.radians(25); R_true = np.array([[np.cos(th), -np.sin(th)], [np.sin(th), np.cos(th)]])
t_true = np.array([0.8, 0.4])
def kabsch(P, Q):
    pc, qc = P.mean(0), Q.mean(0)
    U, D, Vt = np.linalg.svd((Q - qc).T @ (P - pc))
    R = U @ np.diag([1, np.linalg.det(U @ Vt)]) @ Vt
    return R, qc - R @ pc
ang, pos = [], []
for trial in range(2000):
    Q = P @ R_true.T + t_true + rng.normal(0, 0.005, P.shape)     # 5 mm measurement noise
    R, t = kabsch(P, Q)
    ang.append(np.degrees(np.arctan2(R[1, 0], R[0, 0])) - 25); pos.append(t - t_true)
ang, pos = np.array(ang), np.array(pos)
print(f"angle error: mean {ang.mean():+.3f} deg, std {ang.std():.3f} deg")
print(f"translation error std (mm): {np.round(pos.std(0)*1000, 2)}")
P4 = P*0.25                                                      # same shape, one quarter the size
a4 = [np.degrees(np.arctan2(*kabsch(P4, P4 @ R_true.T + t_true + rng.normal(0, 0.005, P.shape))[0][[1, 0], 0])) - 25 for _ in range(2000)]
print(f"angle error std for a 4x smaller object: {np.std(a4):.3f} deg")

angle error: mean +0.026 deg, std 0.755 deg
translation error std (mm): [3.24 2.43]
angle error std for a 4x smaller object: 2.949 deg

The Monte Carlo run shows that the estimate is essentially unbiased (a mean angle error of 0.03°) and that 5 mm of point noise gives about 0.76° of angular uncertainty and 2 to 3 mm of translation uncertainty for a 30 cm object, but about 2.9°, almost four times more, for an object a quarter of the size. Angular accuracy scales inversely with the spread of the points, the same geometry that makes a long baseline better for triangulation. This is why grippers pick small parts more reliably with a close-up camera, and why fiducial markers for accurate pose estimation are made as large as practical.

Uncertainty of pose estimates

A pose estimate is only useful for manipulation if its uncertainty is known: an arm inserting a peg into a hole with 0.5 mm clearance needs to know whether the estimated hole position is good to 0.1 mm or 2 mm. Pose covariance can be estimated from the Jacobian of the alignment residuals, \( \boldsymbol{\Sigma} \approx \sigma^2(\mathbf{J}^{\mathsf{T}}\mathbf{J})^{-1} \), the same expression as the information matrix of lesson 10, or by Monte Carlo as above. Rotations need care: covariance is usually expressed in a small three-element rotation error about the estimate rather than in Euler angles or quaternion components. Once a pose and its covariance are available, a Kalman filter can track an object over time, fusing estimates from successive images and propagating with a motion model, constant for a part on a table or constant-velocity for a part on a conveyor.

Tracking multiple targets

A mobile robot moving among people, or a sorting robot watching items on a conveyor, must track many objects at once. A common architecture runs one Kalman filter per track, usually with a constant-velocity model like that of lesson 5, and at each sensor frame performs three tasks: predict every track to the frame time, associate detections with tracks, and update each track with its associated detection. Detections that match no track may start tentative new tracks, and tracks that go unmatched for too long are deleted. A typical confirmation rule is M-of-N: a tentative track is confirmed when it has been matched in at least M of its first N frames, such as 3 of 5, which suppresses tracks started by isolated false detections.

Data association

Data association decides which detection, if any, belongs to each track. The first step is gating, as in lesson 9: a detection is a candidate for a track only if its squared Mahalanobis distance from the track's predicted measurement, using the innovation covariance S, is below a chi-square threshold. Within the gates, several strategies exist. Nearest neighbour assigns each track the closest candidate detection by Mahalanobis distance; it is simple but can assign one detection to two tracks. Global nearest neighbour (GNN) finds the single assignment of detections to tracks, at most one each, that minimizes the total distance, solved efficiently by the Hungarian algorithm or the auction algorithm. GNN works well when targets are well separated and detection is reliable.

When targets come close together or clutter is dense, any hard assignment risks picking the wrong detection, and a single wrong update can swap two tracks or drag a track onto clutter. Probabilistic methods avoid committing. Joint probabilistic data association (JPDA) updates each track with a weighted combination of all detections in its gate, weighted by the probability that each is the correct one, considering all tracks jointly. Multiple hypothesis tracking (MHT) keeps several alternative assignment histories alive and prunes them as later data resolve the ambiguity, much as a particle filter keeps several pose hypotheses. Random finite set methods, such as the PHD filter, treat the set of targets itself as the random variable. For most robot applications GNN with good gating and track management is the starting point, with JPDA or MHT reserved for dense or ambiguous scenes.

Estimation in manipulation

Manipulation adds its own estimation problems. Before grasping, the robot estimates object pose from vision or depth, often with an ICP refinement and a covariance used to choose a grasp robust to the remaining error. During grasping and assembly, vision is often blocked by the gripper itself, so the estimate must be carried by other sensors: joint encoders give the gripper pose through forward kinematics, a wrist force-torque sensor indicates contact, and tactile sensors indicate where the object sits in the hand. A filter can fuse these, for example estimating the position of a peg in the gripper from tactile images, or detecting contact state (free, in contact, sliding, jammed) with a discrete Bayes filter over contact modes, which is the histogram filter of lesson 2 applied to an abstract state. Calibration is itself estimation: hand-eye calibration, which finds the transform between the camera and the gripper, is solved as a least-squares problem over many observed poses of a calibration target, and errors in it appear directly as errors in every grasp.