From Two Photos to a Point Cloud

Two photos in, a point cloud out. Every read so far was one piece of that sentence. This one runs them all in a row, on every pixel.

There is almost no new maths here, and that is the good news :) The pipeline has four steps, and only one of them measures anything:

• Straighten both photos: undo the lens and turn the pictures until the rows agree.
• Match every pixel of picture 1 along its row in picture 2. This gives a disparity d.
• Clean the matches: neighbours should agree, and doubtful matches are thrown away.
• Lift every (u, v, d) into a 3-D point with one 4×4 matrix.

Brush up on these first ;)

• Stereo Depth — Rectification and Z = fB/d: the rectified rig and Z = fB/d. We meet that division again at the very end, written as a matrix.
• Lens Distortion — Bending Straight Lines Back: where the lens bends a point, and why undoing it on an image is a remap.
• The Pinhole Camera — A Pixel Is a Ray: add a 1, then divide by the last number. That move comes back once more.
• Epipolar Geometry — A Point Becomes a Line: why we undistort first. Epipolar lines are only straight in a pinhole picture.

The rig is the series rig. K has f = 500 px and principal point (320, 240), camera 2 sits at C₂ = (1, 0, 0), B = 1 m to the right, turned 10° toward camera 1, and the point is X = (0.3, −0.2, 4) m. Both lenses have k₁ = −0.2.

1. One Map, Two Jobs

Read 5 straightened a photo with a lookup table: for every pixel of the output picture, it says "go to this spot of the raw photo and copy what you find there". Fill it in once, and every frame after that is just copying. The new part here is that read 10's turn rides in the same table.

step◀1 / 5 · two raw photos▶ Next▶

Camera: focal length and principal point

focal length f (px)

500

principal point cₓ (px)

320

principal point c_y (px)

240

Camera 1: move it and turn it

camera 1 sideways x (m)

0.0

camera 1 height (m, up is −)

0.0

camera 1 forward z (m)

0.0

camera 1 turn left/right (°)

0.0

camera 1 tilt up/down (°)

0.0

Camera 2: move it and turn it

camera 2 sideways x (m)

1.0

camera 2 height (m, up is −)

0.0

camera 2 forward z (m)

0.0

camera 2 turn left/right (°)

10

camera 2 tilt up/down (°)

0.0

the raw photosthe matchthe point cloudthe worked rigReset

Step through the figure. The insets are what each camera records:

• Two raw photos. The lens pulls everything toward the centre, so the faint grid comes out bowed, most at the corners.
• Lens undone. The same points at their pinhole pixels, as read 1 would draw them. Straight lines are straight again.
• Rows lined up. Camera 2 is turned back by 10° about its centre, and X lands on row 215 in both pictures. So does every corner of the target: the dashed rows pass through the same points in both insets.

The second and third stages are one map, not two. For every pixel of the output picture, work backwards:

• Start from the output pixel and undo the turn: that is read 10's H₂ = K R₂ᵀ K⁻¹, run backwards.
• Now you have a pinhole pixel of the real camera. Push it through read 5's lens formula, x_d = x_u(1 + k₁r²), in normalised coordinates, then K.
• That gives the spot of the raw photo to read. Store it in the map.

The map depends only on K, the lens and the turn, none of which change between frames, so it is built once.

Two numbers from the earlier reads, now as entries of the map:

• The lens part. The corner pixel (0, 0) has normalised radius r = 0.8, so L = 1 + k₁r² = 0.872, and the lens puts it at (40.96, 30.72). To fill output pixel (0, 0), the map reads the raw photo there, 51.2 px inward (12.8 % of the radius). Near the middle the lens hardly matters: our worked pixel moves only 0.073 px.
• The turn part. Camera 2's pinhole pixel of X, x₂ = (320.644, 215.374) from read 2, lands at (232.5, 215) after the turn. Row 215, the same row as x₁ = (357.5, 215).

Why undistort before matching? The next step searches along a row, and read 7's epipolar lines are straight only in a pinhole picture. In the raw photo they are curves, and a curve does not stay on one row.

NOTE: Some output pixels have no partner. After the turn, part of camera 2's output picture points outside the raw photo, and those pixels come back black [OPEN24]. It is read 5's trade between black borders and lost pixels again; here stereoRectify takes it as its free scaling parameter, alpha.

Try this: step from "two raw photos" to "lens undone" and watch the bowed grid straighten. The red ring in camera 1's raw picture is (40.96, 30.72), the spot output pixel (0, 0) reads from.

2. Finding the Match Along a Row

Now for the one step that measures something. Picture a strip of wallpaper torn in two. To line the halves up, you slide one along the other until the pattern agrees. That is all a stereo matcher does, one row at a time.

disparity tried (px)

125

▶window size (px)

11

photosclean photoswith noise

Cameras: focal length and baseline

focal length f (px)

500

baseline B (m)

1.0

small windowwith noisestart the scansmall window, with noisethe worked rigReset

The figure is row 215 of both pictures. Picture 2 looks at a wall 4 m away, so its row is picture 1's row slid 125 px to the left:

• The blue window sits around x₁ = (357.5, 215) in picture 1. It stays put.
• The rust window sits d pixels to the left in picture 2. Its brightness is laid over the blue window (dashed), so you can see how well the two agree.
• The plot underneath is the cost at every d from 0 to 200 px. The lowest point is the match.

Let's write the cost down. The plainest one is the sum of absolute differences (SAD), one of the matching costs in Scharstein and Szeliski's survey [SCHA02]. Take every pixel of the window, subtract its partner d pixels to the left, and add up the sizes:

• $I_1,I_2$: the brightness of picture 1 and picture 2, here between 0 and 1.
• $(u_1,v)$: the pixel we want a match for, x₁ = (357.5, 215).
• $k$: runs over the window, $2h+1$ pixels wide (11 px in the figure). A real matcher uses a square block, the same sum over a few rows.
• $d$: the disparity tried, in pixels. $d^{*}$ is the one with the lowest cost.

This is outside the book, which stops once the pictures are lined up (§11.12 [HART04]). On our row the lowest cost is at d* = 125 px, and read 10 turns that into depth: Z = 500 × 1 / 125 = 4 m, X's depth.

Why it exists. It is the one step that measures. Z = fB/d needs a d for every pixel, and this turns "which pixel of row v in picture 2 is this one?" into a cost for every d tried; the lowest wins.

What it assumes. - the same surface point is equally bright in both photos: a surface that looks the same from both cameras, and two cameras with the same gain and exposure ([SCHA02] §2.1). - every pixel in the window has the same d, which means a surface facing the cameras square on ([SCHA02] §3.2). - the match is on the same row v: both pictures are straightened first (section 1). - the true d is inside the range searched. Section 4 turns that range into a nearest depth.

Where it breaks. - One photo is brighter. SAD charges the true match for the difference at every pixel of the window, and a wrong d can win. The standard fixes are costs that ignore gain and offset (gradients, rank, census) or evening out the brightness first ([SCHA02] §3.1); Hirschmüller's mutual information cost is built to tolerate such differences [HIRS08]. - Blank walls and hidden pixels. On a blank wall the cost is flat and every d is about as good as the next; a pixel hidden from camera 2 has no true match at all. Together with depth edges, these are the problem areas Scharstein and Szeliski score on their own ([SCHA02] §5.1). Repeating texture and the window size are the bullets just below and section 3. - Try this: press "with noise". The cost at 125 px is no longer 0 but 0.34, and with the 11 px window the true match still wins. Section 3 shows the same noise with a 3 px window.

The window size is the one knob here, and it cuts both ways:

• A small window is sharp. It sees a thin pole as a thin pole, but a few pixels of a repeating pattern look alike in many places.
• A big window is steady. It covers enough pattern to be unique. But it straddles edges, so it blurs the depth of thin things into the background.

Try this: set d to 0 and press play. The cost dips roughly every 40 px as the ripple of the texture lines up, but only at 125 px does it reach the bottom.

3. Smoothness

disparity tried (px)

125

▶window size (px)

3

photosclean photoswith noise

Cameras: focal length and baseline

focal length f (px)

500

baseline B (m)

1.0

small windowwith noisestart the scansmall window, with noisethe worked rigReset

The same row, with a 3 px window and a little noise on both photos:

• a wrong dip wins, far from 125 px, and the depth tile reports nonsense;
• press "small window": on clean photos the true match still wins, but the "other dips" tile counts several dips nearly as low.

One pixel on its own cannot tell a repeated pattern from the real match.

The fix is to stop deciding each pixel alone. Picture a row of people on uneven ground, each standing at their own height. Small steps between neighbours are normal. A sudden jump of a metre happens too, at the edge of a wall, but it should take real evidence. That is the smoothness rule:

• Pick a d for every pixel, and add up its matching cost.
• Charge a small penalty P1 wherever a neighbour's d differs by 1 px. Slopes and curved surfaces cost a little.
• Charge a bigger penalty P2 wherever it jumps by more. Edges are allowed, but only where the matching costs insist.
• Doing this over the whole picture at once is too slow, so add up the costs along straight paths from several directions across the picture instead. This is Hirschmüller's semi-global matching (SGM): global in spirit, but built from many one-dimensional passes [HIRS08].

Two cleaning steps throw out what is still doubtful:

• A uniqueness test rejects a pixel whose best cost does not beat the second best by a clear margin. That is exactly the small-window failure above.
• A speckle filter drops small islands of disparity that disagree with everything around them.

OpenCV's StereoSGBM puts every one of these ideas behind a parameter [OPEN24b]:

Table. 1 The smoothness and cleaning knobs of StereoSGBM, from the OpenCV 4.x class reference [OPEN24b].

NOTE: OpenCV's matcher is a modified SGM. It matches small blocks rather than single pixels, and it scores them with the Birchfield–Tomasi cost instead of the mutual information of the paper [OPEN24b]. The ideas above are the same; the numbers it computes are not SAD.

4. Reading What the Matcher Hands You

The matcher hands back an image of disparities, and two things about it catch everyone.

step◀4 / 5 · matched▶ Next▶

Camera: focal length and principal point

focal length f (px)

500

principal point cₓ (px)

320

principal point c_y (px)

240

Camera 1: move it and turn it

camera 1 sideways x (m)

0.0

camera 1 height (m, up is −)

0.0

camera 1 forward z (m)

0.0

camera 1 turn left/right (°)

0.0

camera 1 tilt up/down (°)

0.0

Camera 2: move it and turn it

camera 2 sideways x (m)

1.0

camera 2 height (m, up is −)

0.0

camera 2 forward z (m)

0.0

camera 2 turn left/right (°)

10

camera 2 tilt up/down (°)

0.0

the raw photosthe matchthe point cloudthe worked rigReset

The chain again, opened at the "matched" stage. Camera 2 has been turned back, X sits on row 215 in both pictures, and the gap between them is d = 125 px. Now look at what the matcher writes down:

• The numbers are 16 times too big. StereoSGBM (and StereoBM) return a 16-bit image in fixed point, with 4 fractional bits [OPEN24]. A disparity of 125 px arrives as 2000. Divide by 16 before you use it.
• The search range is a depth range. numDisparities is how many disparities the matcher tries, and it must be divisible by 16 [OPEN24b]. With 128 (and the smallest disparity at 0) it tries d = 0 to 127 px, so anything nearer than a certain depth simply cannot be found.

Put both in numbers. Z = fB/d from read 10 [SZEL22] gives the nearest depth directly:

• $D$: the value stored in the disparity image (16 counts per pixel).
• $d$: the disparity in pixels.
• $f$: 500 px; $B$: 1 m.
• $Z_{min}$: the nearest depth the matcher can still find, in metres. The last d tried is 127 px, so the true edge sits a hair further out; the rule is good to one pixel.

For X, D = 16 × 125 = 2000. And with 128 disparities, Z_min = 500 × 1 / 128 = 3.906 m. X at 4 m just fits. The L target's floor corners, at z = 3 to 3.75 m, do not: their matches lie further left than the matcher looks. That is what the hollow red corners say in the next figure.

Where it breaks. - Forgetting the 16. reprojectImageTo3D takes a 16-bit disparity as whole pixels [OPEN24]. Hand it 2000 for X and Q puts the point at (0.01875, −0.0125, 0.25) m: every point of the cloud 16 times too near.

The fractional bits buy something too. A step of 1/16 px in disparity is, by read 10's ΔZ ≈ Z²Δd/(fB), 0.002 m of depth at 4 m. The matcher gets those fractions by fitting a curve through the costs around the best d (a quadratic, in OpenCV [OPEN24b]), so treat it as the resolution of the numbers, not their accuracy.

NOTE: Pixels the matcher could not match do not come back as zero. They get a special value below every real disparity, which the docs call the minimal disparity without printing the number. If you pass handleMissingValues=true to reprojectImageTo3D, those pixels go to a very large Z (10000 at the moment) [OPEN24]. Filter them out before plotting.

Try this: step back to "rows lined up" and forward to "matched". The only new thing on the screen is one number, d = 125 px.

5. Q: Z = fB/d as a Matrix

The last step turns every pixel and its disparity into a 3-D point. By hand, read 10 gives Z = fB/d and read 1 turns (u, v) and Z back into X and Y. OpenCV folds all of it into one 4×4 matrix, Q.

step◀5 / 5 · point cloud▶ Next▶

Camera: focal length and principal point

focal length f (px)

500

principal point cₓ (px)

320

principal point c_y (px)

240

Camera 1: move it and turn it

camera 1 sideways x (m)

0.0

camera 1 height (m, up is −)

0.0

camera 1 forward z (m)

0.0

camera 1 turn left/right (°)

0.0

camera 1 tilt up/down (°)

0.0

Camera 2: move it and turn it

camera 2 sideways x (m)

1.0

camera 2 height (m, up is −)

0.0

camera 2 forward z (m)

0.0

camera 2 turn left/right (°)

10

camera 2 tilt up/down (°)

0.0

the raw photosthe matchthe point cloudthe worked rigReset

The chain once more, at the "point cloud" stage:

• Every wall corner and X come back as solid dots, exactly on the grey ghosts they came from.
• The floor corners come back hollow and red: too near for 128 disparities.

Let's build Q. Put the pixel, its disparity and a 1 into a 4-vector, multiply, and you get a 3-D point in homogeneous form. The layout is the one stereoRectify returns for a horizontal rig [OPEN24]; this is outside the book:

• $c_x,c_y$: the principal point of the rectified camera 1, (320, 240) px.
• $c_x^{\prime }$: the same for the rectified camera 2. In our rig both are 320, so the last entry is 0.
• $f$: 500 px.
• $T_x$: the horizontal shift between the cameras. Read 10 read it off the rectified P₂ʳ: its last column is (−500, 0, 0) = (f·T_x, 0, 0), so T_x = −1. The minus sign only says camera 2 is to the right.

With our numbers, Q = [[1, 0, 0, −320], [0, 1, 0, −240], [0, 0, 0, 500], [0, 0, 1, 0]]. Now feed it x₁ and its disparity:

• The first two rows subtract the principal point: (37.5, −25) px is x₁'s offset from the centre, read 1's first worked number.
• The third row puts f = 500 in the third slot, and the fourth row copies d = 125 into the last slot.
• Divide by the last number, the lesson of read 1, just as read 8 divided by W. Z = 500 / 125 = fB/d with B = 1 m, and X = 37.5 / 125 = 0.3 m.

That is X = (0.3, −0.2, 4) m, the point we started read 1 with. reprojectImageTo3D does exactly this for every pixel of the disparity image, after you divide by 16 [OPEN24].

Why it exists. It lifts a pixel and its d to a 3-D point with one multiply and one division, so a whole disparity image becomes a cloud in one call. It is read 10's Z = fB/d and read 1's back-projection, folded into one 4 × 4 map of homogeneous points (Hartley & Zisserman, Ch. 3 Projective Geometry and Transformations of 3D, §3.1, p. 65).

What it assumes. - a horizontal rig: the cameras are shifted mainly along x. For cameras stacked one above the other, stereoRectify returns a different Q, whose last row has T_y and c_y in place of T_x and c_x [OPEN24]. - d is in pixels, measured in the same rectified pictures this Q came from: the same stereoRectify call, and the same output size passed on to initUndistortRectifyMap [OPEN24]. - the rows truly agree, which needs the lens of read 5 and the K of read 6 to be right first.

Where it breaks. - d = 0. With c_x = c_x′ the last number is −d/T_x, so it is 0 too, and a homogeneous point with a 0 last number is a point at infinity (H&Z Ch. 3 Projective Geometry and Transformations of 3D, §3.1, p. 65; Points at Infinity — Horizons, Vanishing Points and the Floor): the pixel's ray, with no depth on it. Pixels the matcher could not match are the other trap, in the NOTE of section 4. - The error budget. The cloud is only as good as the pieces under it. Every depth at 4 m carries read 10's 0.032 m per px of disparity error, so a match half a pixel off moves the point by 0.016 m. A wrong k₁ bends the rows apart again, and a wrong f scales every depth.

6. In OpenCV

Table. 2 This read in OpenCV 4.x [OPEN24] [OPEN24b].

• The defaults of StereoSGBM::create are numDisparities = 16 and blockSize = 3, with P1 = P2 = 0: set them yourself.
• With the Q of stereoRectify, reprojectImageTo3D returns points in the rectified camera 1's frame [OPEN24]. In our rig camera 1 never turned, so that is the world. stereoRectify turns both cameras, half each (read 10), so with its R1 multiply by R1ᵀ to get back to camera 1's own frame.

7. What Came Next

X = (0.3, −0.2, 4) went a long way in eleven reads. In read 1 it became a pixel, (357.5, 215), and the pixel a ray. Read 2 moved the camera and gave X a second pixel, (320.644, 215.374), in camera 2. Read 3 sent points to infinity and mapped the floor with one 3×3 matrix. Reads 4 to 6 ran the chain backwards: the camera from known points, the lens from straight lines, then K, lens and poses together from a checkerboard. In read 7 the ray became a line in another picture, and in read 8 two rays crossed to give X back. Read 9 found camera 2's pose from the matches alone, up to scale. Read 10 turned the pictures until the match was a single slide along a row, d = 125 px. Here that slide went through Q and came back as (0.3, −0.2, 4), with every other pixel of the photo alongside it. What this series left out is the next step up: many cameras at once, and bundle adjustment, which refines every camera and every point together against read 4's reprojection error. That is another story. If you want to walk this one again, it starts with a single pixel in The Pinhole Camera — A Pixel Is a Ray.

8. References

1. [HART04] R. Hartley, A. Zisserman, Multiple View Geometry in Computer Vision, 2nd ed., Cambridge University Press, 2004. Chapters used in this read: Ch. 3 Projective Geometry and Transformations of 3D; Ch. 11 Computation of the Fundamental Matrix F.
2. [OPEN24] OpenCV, Camera Calibration and 3D Reconstruction (calib3d), OpenCV 4.x documentation.
3. [OPEN24b] OpenCV, cv::StereoSGBM Class Reference, OpenCV 4.x documentation.
4. [SCHA02] D. Scharstein, R. Szeliski, "A Taxonomy and Evaluation of Dense Two-Frame Stereo Correspondence Algorithms", IJCV 47, 2002.
5. [HIRS08] H. Hirschmüller, "Stereo Processing by Semiglobal Matching and Mutual Information", IEEE TPAMI 30(2), 2008.
6. [SZEL22] R. Szeliski, Computer Vision: Algorithms and Applications, 2nd ed., Springer, 2022.