Stereo SLAM initial camera pose: what to supply for your own rig

You are porting a stereo visual SLAM example to your own rig, and it stops at the same line for everyone: the example loads its first camera pose from a file that came with its dataset, and you have no such file. What you have is two synchronised recordings, an exported stereo calibration, and a reconstruction that refuses to start. This guide gives the pose to supply instead, the exact call that defines it, how to confirm the reconstruction is sane, and the fix for the matrix-dimension error most first runs hit at the point-transform step. The same anchor holds for a live stereo camera over GigE or USB3 and for offline AVI pairs.

What you need

• The recorded left and right video, synchronised frame to frame.
• The stereo calibration you exported from the calibration app: intrinsics, distortion, baseline.
• The example code you are porting, with its pose input exposed as a variable.
• A taped distance of 2 m or so for the sanity check later.

What the initial pose anchors

Every stereo SLAM example runs the same pipeline: undistort both frames with the calibration, triangulate map points from the left-right matches of each keyframe, estimate the relative motion between consecutive frames, and accumulate that motion into a trajectory. Accumulation only produces motion relative to a starting pose, so something must state where the camera sat at frame 1 in a world frame. The dataset's authors measured that starting pose and ship it as initialPose.mat, so their whole reconstruction lands in their surveyed frame. Where the origin sits is a convention, not a measurement you are missing — the same choice you make when deciding where Z is measured from in any stereo set-up (Stereo triangulation world origin: choosing where Z is measured from).

The example's helper transforms points in the row-vector form, xyzPoints = xyzPoints * currPose.Rotation + currPose.Translation, and that line is where a first run usually fails. Both conventions describe the same rigid transform:

Supply identity when nothing was measured

If you cannot measure the first pose, the neutral anchor is the identity transform: a 3x3 identity rotation (0° about every axis) and a zero translation ([0 0 0], in your calibration's length unit, normally metres). In the example's pose class that is one call: initialPose = rigid3d(eye(3), [0 0 0]). The eye(3) builds the 3x3 identity; the vector is the zero translation. If your stack uses a different pose type, the contract is identical — rotation identity, translation zero.

The world origin then sits at the first keyframe's camera centre, with the world axes on the camera axes of frame 1. It costs no accuracy: every later pose and map point is a relative transform, so the reconstruction is the same geometry any other anchor would produce, relabelled.

Run it on your own recording

1. Calibrate the pair and export the calibration object. Intrinsics, distortion and baseline set the triangulated scale, so a sloppy calibration shows up as a uniformly wrong map; keep the reported reprojection error under about half a pixel and recapture if it is higher. The ordering problem this prevents is covered in Camera intrinsics for hand-eye calibration: fix the pose first.
2. Record synchronised left and right video. Hardware-sync the shutters; a frame-time skew breaks the stereo match before SLAM starts. Stereo camera recording for visual SLAM: offline workflow walks the capture side.
3. Open both streams and define the anchor in code:

leftReader  = VideoReader('leftframe.avi');   % one reader per stream
rightReader = VideoReader('rightframe.avi');
initialPose = rigid3d(eye(3), [0 0 0]);       % 3x3 identity rotation, zero translation

4. Run the reconstruction loop. Each keyframe pair goes through helperReconstructFromStereo, which triangulates the matched points and returns them with the accumulated pose. Pass initialPose exactly where the example passes its loaded pose.

The map inherits the calibration's length unit; for the identity anchor that is the whole unit story, because the translation you supply is zero.

Worked example: the dimension check at helperReconstructFromStereo line 26

Inputs, with sources: xyzPoints, the triangulated map points of the current keyframe, expected one point per row — an N-by-3 array, taken from the helper's own transform line. currPose.Rotation, a 3x3 matrix, and currPose.Translation, 3x1, taken from the error text. N = 1,000 map points, an illustrative count for one textured indoor keyframe. Dimensions are row and column counts; the only physical unit in the transform is the translation's length unit, which the identity sets to zero.

Rule in words: a matrix product A * B exists only where the column count of A equals the row count of B.

Arithmetic:

• Points one per row: (1,000 x 3) * (3 x 3). Inner dimensions 3 and 3 match. Result: 1,000 x 3, one transformed point per row.
• Points one per column: (3 x 1,000) * (3 x 3). Inner dimensions 1,000 and 3 do not match. Undefined — the error text verbatim, that the columns of the first matrix must match the rows of the second.

Result: the crash reports shapes, not data. Your anchor cannot be the cause here, because eye(3) is its own transpose, so row- and column-vector conventions return the same points at frame 1; only the array orientation, or a pose object built in the opposite convention, can break the product.

Decision: transpose the point array so each row is one point and re-run; the threshold to check before any call is that the first matrix's column count equals 3. If your pose library speaks the column convention, write the transform as R p with points in columns. Do not swap * for .* as the message hints — that computes a different, element-wise result.

Point-array orientation also drives triangulation errors themselves: Stereo triangulation errors: check point data types.

Confirm the map is sane

Three checks take five minutes:

• The trajectory starts at [0 0 0] with zero rotation. That is your anchor doing its job, not a bug.
• Walk the rig along a taped 2 m and read the distance back between the first and last pose. A few percent of 2.0 m is healthy stereo odometry in a textured scene. Reading about 2,000 instead of 2.0 means millimetres entered through the calibration baseline.
• Export the cloud and compare it against the trajectory plot; if the file disagrees with what the viewer showed, sort it out with Why exported point clouds differ from the viewer render before touching the pose.

The honest failure mode is texture: a blank wall gives the matcher nothing, so add matte targets or re-route the capture path.

Four mistakes that cost an afternoon

• Aiming the camera with the anchor. Pre-rotating the initial pose to face the scene gains nothing; the solver estimates orientation from the images, and a hand-built rotation is one more way to corrupt the map. Keep eye(3).
• Baking a second extrinsic into the anchor mid-run. If the map must land in a robot-base or cell frame, compose that transform after the solve. Keeping optical, camera and world frames straight is its own task: Transform depth point clouds between optical frame and camera frame.
• Recalibrating when the pose was the suspect, or vice versa. Identity cannot warp geometry; a sheared or wrongly scaled map is a triangulation problem (rectification, baseline), not an anchor problem.
• Recording a moving rig on rolling shutter. Per-row exposure skew mimics motion and degrades matching. Use global shutter for a moving rig, or hold exposure short enough that skew stays a fraction of a pixel.

When identity is the wrong anchor

Identity is right when you want a self-consistent map. Supply a measured camera-to-world pose instead when the output must align to something that exists: a robot base, a cell origin, surveyed markers. Measured mounts carry real error — hand-eye results on an arm routinely land 1-2 cm off (Hand-eye calibration translation error: is 1-2 cm normal?) — so treat the aligned map at that tolerance, and produce robot-frame poses after the solve (Object pose in camera frame at runtime after hand-eye calibration). For everything else: no file, no measurement, no added error.

Build or buy the stereo front end

Nothing above depends on which camera produced the files. Building the rig: two global-shutter mono area scan cameras with a hardware-synced trigger and a rigid baseline is the standard start; filter by interface and shutter in the camera finder. Buying instead: factory-calibrated 3D sensors output depth and point clouds directly and skip offline reconstruction — the better route when the job is measurement rather than odometry.

Running this on a specific pair? Send the cameras, baseline and mounting through the contact page and we will check the set-up against the catalogue, or step through the camera and lens configurator if you are still selecting hardware. The rest of the pose and frame material sits on the 3D topic page.