3D Vision Basics: SfM, Point Clouds, and Pose Estimation

A construction site needs to know how much gravel is in a stockpile. The old way is a surveyor with a total station, half a day, and a bill. The new way is a drone flying a grid pattern for eight minutes, then software that turns 40 overlapping photos into a 3D surface and reports 2,847 cubic metres.

Prerequisites: Epipolar geometry and stereo vision and camera calibration.

Two different questions

Stereo vision from the previous post assumed you knew the baseline and both cameras’ orientations. SfM drops that assumption. A drone photographs a stockpile from 40 unknown positions, and the software works out both where the drone was and what it was looking at.

The SfM pipeline

flowchart TD
  A["40 overlapping photos"] --> B["Detect features<br/>~2,000 SIFT keypoints each"]
  B --> C["Match pairs<br/>ratio test + RANSAC"]
  C --> D["Build feature tracks<br/>same point across many images"]
  D --> E["Initialise: pick best pair<br/>estimate relative pose"]
  E --> F["Triangulate initial points"]
  F --> G["Add next camera via PnP"]
  G --> H["Triangulate new points"]
  H --> I["Bundle adjustment<br/>refine everything"]
  I --> J{"Cameras left?"}
  J -->|yes| G
  J -->|no| K["Sparse cloud: ~18,500 points"]
  K --> L["Multi-view stereo<br/>dense reconstruction"]
  L --> M["Dense cloud: ~12M points"]
  M --> N["Mesh / height map / volume"]

Every step before bundle adjustment is feature matching applied at scale. The new part is doing it consistently across 40 images instead of 2.

Track length is the quality signal that matters. A point seen in 2 images is barely constrained and often wrong. A point seen in 12 images is solid.

Bundle adjustment

The heart of SfM. Adjust every camera pose and every 3D point simultaneously so that projected points land as close as possible to the observed image points.

$\min_{\mathbf{R}_i, \mathbf{t}_i, \mathbf{X}_j} \sum_{i}\sum_{j} v_{ij}\left\| \pi(\mathbf{K}_i, \mathbf{R}_i, \mathbf{t}_i, \mathbf{X}_j) - \mathbf{x}_{ij} \right\|^2$

where $v_{ij}$ is 1 if point $j$ was observed in image $i$.

One reprojection error, computed. A 3D point projects to $(1059.0, 474.0)$ using the current pose estimate. It was observed at $(1061.3, 472.5)$:

$\Delta x = 1061.3 - 1059.0 = 2.3, \qquad \Delta y = 472.5 - 474.0 = -1.5$ $e = \sqrt{2.3^2 + 1.5^2} = \sqrt{5.29 + 2.25} = \sqrt{7.54} = 2.746 \text{ px}$

Bundle adjustment nudges the pose and the point to shrink that, summed over every observation.

The size of the problem

Over-constrained is what you want. Five times more equations than unknowns means noise averages out rather than being absorbed into a perfect but meaningless fit.

The problem is only solvable because the Jacobian is extremely sparse. Point 4,201 was seen by 6 of the 40 cameras, so its rows are zero for the other 34. Solvers exploit that structure with the Schur complement, which is why a 55,740-parameter optimisation runs in seconds rather than hours.

Scale: the thing everyone forgets

SfM cannot recover absolute scale. A large stockpile photographed from far away and a small model photographed from close up produce mathematically identical reconstructions.

The fix is one object of known length in the scene. Place a 1.200 m survey target, then measure its two endpoints in the reconstruction:

$\text{endpoint A} = (0.0142,\; 0.0087,\; 0.0031)$ $\text{endpoint B} = (0.0489,\; 0.0233,\; 0.0055)$

$d = \sqrt{(0.0347)^2 + (0.0146)^2 + (0.0024)^2}$ $= \sqrt{0.00120409 + 0.00021316 + 0.00000576} = \sqrt{0.00142301} = 0.03772$

$s = \frac{1.200}{0.03772} = 31.81 \text{ metres per reconstruction unit}$

Multiply every coordinate by 31.81 and the model is in metres. Every length, area, and volume you compute afterwards inherits this one number’s accuracy.

Pose estimation with PnP

Once you have 3D points, finding a new camera’s position is a separate, much easier problem.

The counting argument is worth understanding. A camera pose has 6 degrees of freedom. Each observed point gives 2 equations, so 3 points give 6 equations for 6 unknowns — exactly determined. But the geometry admits up to 4 valid solutions, so a 4th point picks the right one.

ok, rvec, tvec, inliers = cv2.solvePnPRansac(
    object_points,          # (N,3) 3D points in world coords
    image_points,           # (N,2) their observed pixel positions
    K, dist_coeffs,
    reprojectionError=3.0,  # inlier threshold in pixels
    flags=cv2.SOLVEPNP_EPNP)

R, _ = cv2.Rodrigues(rvec)          # 3x3 rotation matrix
camera_position = -R.T @ tvec        # camera centre in world coordinates

That last line catches nearly everyone. tvec is not the camera’s position. It is the translation part of the world-to-camera transform, so the camera centre in world coordinates is $-\mathbf{R}^\top \mathbf{t}$.

Aligning two clouds with ICP

The site is scanned in March and again in June. To compare them, they must be in the same coordinate frame.

One iteration, worked. Source points $P$ and target points $Q$, in 2D for clarity:

Step 1 — centroids.

$\bar{P} = \left(\frac{1.0 + 2.0 + 1.5}{3},\; \frac{1.0 + 1.0 + 2.0}{3}\right) = (1.500,\; 1.333)$

$\bar{Q} = \left(\frac{4.0 + 5.0 + 4.5}{3},\; \frac{2.0 + 2.0 + 3.0}{3}\right) = (4.500,\; 2.333)$

Step 2 — centre both sets.

Step 3 — rotation. The SVD of the cross-covariance yields $\mathbf{R} = \mathbf{I}$, no rotation.

Step 4 — translation.

$\mathbf{t} = \bar{Q} - \mathbf{R}\bar{P} = (4.500 - 1.500,\; 2.333 - 1.333) = (3.000,\; 1.000)$

Apply it and the clouds coincide exactly. Real data is never this clean — the correspondences are wrong on the first iteration, so the transform is approximate, which makes the next round of correspondences better, and so on.

ICP finds a local minimum only. If the two clouds start far apart or badly rotated, it converges confidently to the wrong answer. Give it a rough initial alignment from GPS, a manual click of three corresponding points, or a global method like FPFH feature matching.

From point cloud to cubic metres

The deliverable. Grid the cloud into a height map, take the height above a base plane in each cell, and sum.

Cell size 0.5 × 0.5 m, so each cell covers $0.25\ \text{m}^2$. Heights above the base plane:

Row sums: $0.5$, $3.5$, $4.3$, $0.8$. Total height sum:

$0.5 + 3.5 + 4.3 + 0.8 = 9.1 \text{ m}$

$V = 9.1 \times 0.25 = 2.275 \text{ m}^3$

Scaled up to the real stockpile’s 11,388 cells the same arithmetic gives 2,847 m³.

The base plane is where the judgement lives. A stockpile sits on ground that is rarely flat, so fitting a plane through the surrounding terrain points — rather than assuming $z = 0$ — is what separates a usable number from a guess.

Practice task

Use COLMAP, which is free, and any object you can walk around.

1. Take 40 photos with clear overlap. Include a ruler or a measured object in view.
2. Run COLMAP’s automatic reconstruction. Note how many of the 40 cameras it registers.
3. Read the reported mean reprojection error. Anything above 1.5 px means something is wrong.
4. Plot the histogram of track lengths. Compare it against the table above.
5. Measure your reference object in the reconstruction and compute the scale factor.
6. Verify by measuring a second known distance after scaling. How far off is it?
7. Redo the capture with only 20 photos. Compare registered cameras and reprojection error.
8. Export the dense cloud, grid it into a height map, and compute a volume two ways: assuming a flat base, and fitting a plane. Compare.

Step 7 is the one that changes how you capture. The failure with 20 photos is usually not graceful degradation — it is whole cameras failing to register and the reconstruction splitting into disconnected pieces.

Summary

SfM recovers camera poses and 3D structure together from unposed photos. Feature tracks are the raw material, and track length predicts accuracy directly: 2-image tracks had 3.84 px reprojection error, 12-image tracks had 0.31 px. That is the entire argument for 80% overlap.

Bundle adjustment refined 55,740 parameters — 40 cameras at 6 DOF plus 18,500 points at 3 — against 284,000 residuals. One reprojection error worked out to 2.746 px from a 2.3 and 1.5 pixel offset.

The reconstruction has no scale until you give it one. A 1.200 m target measuring 0.03772 units gave a factor of 31.81 m per unit, and because volume scales cubically, a 1% error there is 3% on the volume.

PnP finds a new camera’s pose from known 3D points, needing 3 plus 1 to disambiguate — and tvec is not the camera position, $-\mathbf{R}^\top\mathbf{t}$ is.

ICP aligned two scans by alternating correspondence and transform: centroids at $(1.500, 1.333)$ and $(4.500, 2.333)$ gave $\mathbf{t} = (3.000, 1.000)$. On real data it converged from 2.847 m to 0.029 m in 8 iterations.

The volume came from height times cell area: 9.1 m of summed height over cells of 0.25 m² gave 2.275 m³. The base plane choice moved the full-site answer by 265 m³.

What comes next

Everything so far has connected pixels to geometry. Multimodal vision connects pixels to language instead — putting images and text in one shared space so you can search a photo library by typing a description, and classify things the model was never explicitly trained on.