Geometry, Mapping, and 3D/4D State › Depth, Pose, and Scene Flow

Recovering geometry from observations involves three coupled estimation problems: depth (where points are), pose (where the sensor or object is), and scene flow (how points move). They are coupled because you usually need one to estimate the others: motion of the camera explains part of the image change, motion of the world explains the rest. To convert a set of depth measurements into a world-frame map you need the sensor pose; to estimate the sensor pose from point clouds you need the geometry of the surfaces around you; to tell whether a surface moved you need to know how much of its apparent motion came from the sensor. There is no fixed order of operations that breaks this loop cleanly. Practical systems break it with approximations, iteration, or priors, and the quality of the resulting geometry depends on how honest the system is about which variable was held fixed while estimating the others.

This section covers two common pose-estimation cases, distinguished by the correspondences available: 3D-to-2D and 3D-to-3D.

• 3D-to-2D (perspective-n-point): given known 3D points and their pixel projections, solve for the camera pose. Each known 3D point together with its observed pixel constrains the pose, because the point must lie on that pixel's ray, and enough rays must intersect at a common camera position and orientation. This is what happens when a robot localizes against a known map, or when it estimates an object's pose from detected keypoints. For a calibrated perspective camera, P3P uses three nondegenerate correspondences but can yield multiple pose solutions, so a fourth correspondence is commonly used to disambiguate them. Other camera models and degenerate point configurations change the requirement. Closed-form and iterative solvers exist, and robust variants handle outliers. In deployed pose estimation, robustness to correspondence outliers is often at least as important as nominal inlier accuracy: a single mis-detected keypoint, if trusted, can drag the whole pose estimate far away, so deployed systems pair the solver with RANSAC-style hypothesis testing that fits the pose to random minimal subsets and keeps the one supported by the most correspondences.
• 3D-to-3D (Procrustes / Kabsch): given two point clouds that are related by a rigid transform, the best-fit rotation and translation have a closed-form solution via SVD. This is the core step inside iterative closest point (ICP) registration and inside many odometry pipelines. ICP illustrates how a chicken-and-egg problem is handled by alternation: correspondences between the clouds are guessed from the current transform estimate, the optimal transform for those correspondences is computed in closed form, and the two steps repeat until convergence. Each half of the loop is easy precisely because the other half is temporarily fixed, an alternation also used by expectation-maximization. An EKF addresses coupling differently: it performs sequential prediction and a joint linearized Bayesian update rather than alternating between fixed optimization blocks.

The Kabsch solution is worth knowing because it recurs everywhere. Given matched source points $P$ and target points $Q$, first compute their centroids and the centered versions $\tilde{P} = P - \bar{P}$ and $\tilde{Q} = Q - \bar{Q}$:

1. Compute the cross-covariance $H = \tilde{P}^\top \tilde{Q}$.
2. Take the SVD $H = U \Sigma V^\top$.
3. The optimal rotation is $R = V D U^\top$, where $D$ corrects the sign if $\det(VU^\top) < 0$ (reflections are not rigid transforms).
4. The translation is $\mathbf{t} = \bar{Q} - R \bar{P}$.

A few remarks on why this works and when it does not. Centering both clouds first removes the translation from the problem, leaving a pure rotation to be found; the cross-covariance $H$ summarizes how source directions map to target directions, and the SVD extracts the orthogonal matrix best aligning them. The determinant correction in step 3 is essential: without it, the least-squares optimum occasionally wants to be a reflection, which is geometrically illegal, and the correction projects back to the nearest legal rotation. Finally, the closed form is only as good as its correspondences. Kabsch assumes each source point is already matched to the correct target point; if the matching is wrong, the transform will be confidently wrong, which is why ICP wraps this solver in an outer loop that revises the matching.

Scene flow is where geometry meets dynamics, and it deserves careful decomposition. When a sensor moves through a static world, all apparent sensor-coordinate change is explained by one rigid transform: the egomotion. This is a remarkably strong structural prior, because a single $SE(3)$ transform accounts for the coordinate change of potentially millions of observed points at once. Camera and object rigid transforms compose rather than adding as independently defined displacements. A precise destination-frame residual transports the time-$t$ point from $C_t$ into $C_{t+1}$ before subtraction:

$$\mathbf{f}^{\mathrm{obj},C_{t+1}}_i := \mathbf{p}^{C_{t+1}}_i(t{+}1) - \left(T_{C_{t+1}C_t}\,\tilde{\mathbf{p}}^{C_t}_i(t)\right)_{1:3},$$

where the homogeneous transform is applied before taking the three-vector difference. A static world point has zero residual under the correct egomotion transform; nonzero residual captures object motion or modeling error in $C_{t+1}$.

A world model without an explicit separation must learn observer-induced change jointly with scene dynamics. It may misattribute that change or generalize poorly unless action or pose conditioning, model capacity, data, and observability make egomotion predictable. Explicitly decomposing a global rigid component from the residual is therefore a useful sanity check in dynamic-scene perception, not a prerequisite for every successful predictor. The residual contains the dynamics of walking people, articulating arms, and falling objects, along with any uncompensated estimation error. It is also, as you might expect from Part III's object-centric chapter, a strong cue for segmentation: points that share a residual motion tend to belong to the same object. Coherent residual motion can support segmentation before object classification, although it does not by itself establish object identity.

Let's estimate a rigid transform from a moving point cloud and look at what remains.