3D visionTroubleshooting

VIO drift with a wide FOV stereo camera: use the fisheye model

Why VIO pose drift starts once a wide-FOV stereo camera moves: the pinhole-model mismatch, the baseline and resolution faults behind it, and the fisheye fix.

LumaSpec Vision11 min read Share on LinkedIn

On this page 9 sections
  1. 01Why does the pose drift as soon as the camera moves?
  2. 02How wrong is a pinhole model on a wide lens?
  3. 03Which camera model should a wide stereo camera use?
  4. 04Which intrinsics go into the config file?
  5. 05What baseline and IMU-to-camera extrinsics should you set?
  6. 06Should VIO use rectified or unrectified images?
  7. 07What else should you check once the model is right?
  8. 08How do you verify the fix in ten minutes?
  9. 09Questions

An odometry stack that initializes cleanly and then tears its own pose graph apart within seconds of movement almost always has a configuration fault, not a tuning fault. This page is for engineers running stereo-inertial VIO on a wide-FOV stereo camera whose transform starts to wander the moment the rig moves. The failing set-up behind this guide declared a pinhole camera model with all four distortion coefficients at 0.0, a guessed 75 mm baseline, and a declared image size that did not match the streamed one. You get the projection arithmetic that explains the drift, the four corrections that stop it, and a ten-minute test sequence that proves the fix before you trust the output.

Why does the pose drift as soon as the camera moves?

Because the estimator projects every feature through the wrong camera model: a pinhole projection only fits a narrow field of view, so on a wide lens it misprojects every feature beyond about 10° off-axis by more than the tracker's 1 px outlier gate.

The front end in the failing config tracked up to 150 corners per frame with a RANSAC gate of F_threshold: 1.0 px and backward optical-flow checking on (flow_back: 1). While the rig holds still, gravity and the IMU hold the solution and nothing contradicts the bad projection. Motion creates parallax, peripheral corners then arrive with residuals of tens of pixels, the gate rejects them or triangulates them wrong, and the published transform starts to walk. Healthy while static, drifting on the first movement is the signature of a projection-model error, so fix the camera model before touching IMU noise or solver settings.

How wrong is a pinhole model on a wide lens?

Past roughly 10° off-axis the projection error passes the 1 px gate; at 30° it reaches about 31 px, at 45° about 123 px, and at 60° the pinhole formula puts the feature 992 px from the image center, 352 px beyond the 640 px half-width of a 1280 px stream.

The gap between the two projections is the tangent itself:

r_eq = f · θ (equidistant projection, θ in radians) r_pin = f · tan θ (pinhole projection) error = f · (tan θ − θ)

Worked example, using the raw left stream's focal length fx = 572.94 px, rounded to f = 573 px (quoted from the CameraInfo in the next section):

θ off-axis r_eq = 573 · θ r_pin = 573 · tan θ error
10° 100 px 101 px 1.0 px
30° 300 px 331 px 31 px
45° 450 px 573 px 123 px
60° 600 px 992 px 392 px

Check the 10° row by hand: 573 × (0.17633 − 0.17453) = 1.0 px, which is where corner tracking starts losing features. At 60° the tangent has grown without bound while the equidistant radius grows linearly with θ, so a zero-coefficient pinhole cannot place the outer half of a wide frame at all. The equidistant form approximates the fitted lens polynomial, so read the table as the error scale rather than an exact per-camera curve.

Which camera model should a wide stereo camera use?

Kannala-Brandt, the equidistant fisheye model: it replaces the tangent with a polynomial in the incidence angle — typically four terms in θ, θ³, θ⁵ and θ⁷ — and it fits equidistant, equisolid and stereographic wide lenses.

The model dates to 2004 and is what fisheye calibration modules in common vision libraries implement. If the node throws an assertion while undistorting, the fisheye undistortImage guide covers matching the model to the right coefficient set.

In the VIO config, the model_type field that held PINHOLE takes the package's Kannala-Brandt option, and the projection block changes from fx and fy to the polynomial coefficients — different parameter names than the pinhole block, which is expected. Feed it the raw, unrectified streams, as the next sections set up.

If you ever need to fit coefficients yourself — swapped lens, custom crop, sub-pixel accuracy — record 20-30 chessboard poses covering the frame corners and fit the fisheye model directly. A distortion_model label the node does not recognize is its own failure mode; the garbage-coefficients guide shows what that looks like.

Which intrinsics go into the config file?

The CameraInfo the driver publishes for the exact stream you subscribe to: for the raw left stream at 1280×720 in this set-up, fx = 572.939 px, fy = 573.308 px, cx = 614.556 px, cy = 366.322 px, distortion_model rational_polynomial, and an eight-term D vector whose first radial term is +0.348.

The failing yaml instead carried fx = 832.269 px with k1, k2, p1 and p2 all 0.0: a focal length 45% longer than the raw stream publishes, the signature of values copied from another stream or resolution, plus no distortion at all. Two rules keep it straight:

  1. Intrinsics belong to one stream at one resolution. Decimate the full sensor by two and fx, fy, cx, cy halve, so 572.939 px becomes 286.5 px; the normalized D coefficients survive that integer decimation.
  2. Copy K and D from the CameraInfo message on the subscribed topic, and set image_width and image_height in the common block to that stream: 1280×720 here, not the 4056×3040 sensor maximum the second config declared.

The 3×3 K matrix follows the standard pinhole algebra, worked end to end in the known-intrinsics calibration guide. Rectified and raw topics carry different K by design; the rectified-versus-raw intrinsics guide shows how to tell which set you are holding.

What baseline and IMU-to-camera extrinsics should you set?

The factory extrinsics, pasted unchanged: for the wide module in question they place the left camera 66.35 mm to one side of the IMU and the right camera 6.41 mm to the other — a 72.8 mm baseline — with both cameras 5.65 mm from the IMU along z.

The failing config guessed a symmetric ±37.5 mm pair: a 75.0 mm baseline, with lever arms wrong by about 29 mm on the left camera and 31 mm on the right. Stereo turns the baseline into metric scale:

d = f · B / Z

With f = 573 px, B = 0.0728 m and a feature at Z = 2.00 m, the expected disparity is 573 × 0.0728 / 2.00 = 20.9 px. Run the same frame with the 75.0 mm guess and every depth inflates by 75.0 / 72.8 = 1.03, so the 2.00 m feature triangulates at 2.06 m. The lever-arm error bites while rotating: at 1 rad/s, a 30 mm error swings the predicted camera position by 30 mm/s, which at 2 m is 573 × 0.030 / 2.00 = 8.6 px/s of corner motion the optimizer can only absorb as pose error.

Keep the rotation part of each matrix in the optical convention (camera z forward, x right, y down) mapped into the IMU frame, and confirm it against the static transform the driver publishes; field reports of this exact symptom include an IMU axis pair that disagreed with the published /tf_static tree.

When the values are trusted, set estimate_extrinsic: 0; the initial-guess mode, estimate_extrinsic: 1, lets the optimizer wander around a bad starting point. The trade-off between factory extrinsics and your own calibration session is covered in the camera-IMU extrinsics guide, and when a stereo pair's baseline and matching window disagree, depth output fails in louder ways covered by the mostly-black disparity guide.

Should VIO use rectified or unrectified images?

Unrectified, whenever the node runs a fisheye model: subscribe the raw left and right topics and let the estimator project through the Kannala-Brandt coefficients, because rectifying a wide lens to a pinhole image crops the periphery where a moving rig finds most of its new features.

The failing config had image0_topic and image1_topic pointed at the rectified streams. Rectification to a pinhole image must either crop the outer field or stretch it back through a fresh distortion, and on wide lenses it crops — the rectification-FOV guide quantifies that loss. The rectified topics also publish their own K, the 832 px-class focal above, so pairing rectified images with raw intrinsics stacks a second silent fault on the first. Pick one pair and stay on it: raw streams with fisheye coefficients for odometry, rectified streams with their own pinhole values for display and 2D inference.

What else should you check once the model is right?

In order of likelihood: IMU noise densities copied instead of measured, an unconverged camera-IMU time offset, flex between camera and IMU, and solver time exhausted on the processor — each has a direct test, and none of them produces the healthy-static, drifting-moving signature the model mismatch does.

  • Driver. Prefer the camera's current maintained driver package over the older example capture node; the maintained one publishes the per-stream CameraInfo and the static transform this whole fix depends on.
  • IMU noise. The example values acc_n 0.1, gyr_n 0.01, acc_w 0.001, gyr_w 0.0001 are starting points in the config's own units. Record two minutes with the rig still, then set acc_n and gyr_n to the per-axis standard deviation of the accelerometer and gyro, times one to two for margin.
  • Time offset. estimate_td: 1 with td: 0.00 works, but watch the estimate settle; a value still walking after a minute of rich motion points at the sync chain.
  • Mounting. Any wobble between camera and IMU during capture breaks the fixed-extrinsic assumption the whole estimator stands on; the mounting-stillness guide shows how much a moving sensor blurs its own evidence.
  • Solver budget. max_solver_time: 0.04 s and max_num_iterations: 8 must fit inside the frame period. On a low-power single-board CPU the 1280×720 stereo pipeline can overrun it; halve the stream to 640×360, where fx becomes 286.5 px and K scales with it, before you drop frames.
  • Feature health. With max_cnt: 150 and min_dist: 30 px on a 1280 px-wide frame, a textured scene at 0.5-5 m should keep well over 60 surviving corners; fewer means texture, exposure or motion blur trouble, not more tuning.

How do you verify the fix in ten minutes?

Three runs settle it: two minutes static, a slow 360° turn in place, and a 5 m out-and-back along a taped line — expect position to hold within a few centimeters, orientation to return within about 2°, and the end point within 5%, that is 0.25 m.

  1. Static hold, 120 s. The pose may wander centimeters; a steady ramp while nothing moves means the bias or noise values are still off.
  2. Rotation in place. Turn 360° over about ten seconds, twice. Orientation should track hand-over-hand and return within roughly 2°; a jump at the start or stop of the turn points at td or the camera-to-IMU rotation.
  3. Walk the line. 5 m out and back beside a taped line in a textured corridor. Scale comes from the stereo baseline, so a correct 72.8 mm baseline returns you within about 0.25 m; systematic overshoot beyond 5% reopens the extrinsic check, the same logic as diagnosing depth-scale differences between identical stereo cameras.

Leave the rig with this checklist in the shipped config:

  • model_type on the fisheye option, raw image0_topic and image1_topic subscribed
  • image_width and image_height equal to the streamed size, K and D copied from that stream's CameraInfo
  • body_T_cam0 and body_T_cam1 translations pasted from the factory static transform
  • estimate_extrinsic: 0 once the axes are confirmed against a slow hand rotation
  • noise parameters measured from a two-minute static log

If you are specifying a camera for a new odometry cell instead of rescuing one, start in the 3D sensors category: stereo and structured-light cameras that publish intrinsics per stream remove the calibration guesswork, and the camera finder filters them by type and series. The system configurator pairs the camera with mounts and cabling, the 3D topic hub collects the surrounding guides, and multi-camera projects can reach us through the contact page.

Questions

Do I still need to calibrate the camera myself?

Usually no. The driver publishes K and D for every listed stream in its CameraInfo messages, and those factory values are the correct input at that resolution. Run your own target calibration only when the lens or focus has been changed, when the node needs a distortion parameterization the factory does not publish, or when you need sub-pixel accuracy at a custom resolution. Twenty to thirty chessboard poses spread across the frame corners are enough to fit a Kannala-Brandt model.

The stream looks fine, so why would the calibration be wrong?

Because a display image and an estimator need different precision. A few pixels of projection error is invisible when you watch the video, but the feature tracker gates residuals at 1 px and the optimizer triangulates from those same projections, so model error your eye never notices becomes tens-of-pixels evidence against the wrong pose. Judge a calibration by residuals and surviving corners, not by how the stream looks.

Can I keep using the rectified image topics for VIO?

Yes, with the intrinsics published for those rectified topics, and at a cost: rectifying a wide lens to a pinhole image crops the outer field, which is where a moving platform picks up most new features. For odometry on a wide-FOV module, raw streams with a fisheye model keep the full field of view and the correct projection in one step; keep rectified streams for display and 2D networks.

What IMU noise values should I start from?

Start from the order of magnitude in the example config — acc_n 0.1, gyr_n 0.01, acc_w 0.001 and gyr_w 0.0001 in the config's own units — then measure: record two minutes with the camera still, take the per-axis standard deviation of the accelerometer and gyroscope, and set acc_n and gyr_n to that deviation multiplied by one to two for margin. Noise densities an order of magnitude too small make the filter trust the IMU and drift hard.

Does a wrong baseline affect scale or only depth accuracy?

Scale, and that is what hurts navigation. Stereo disparity d = f·B/Z converts the baseline into the metric scale of every triangulated landmark, so a 3% baseline error shifts all depths by 3% — with a 75 mm guess on a 72.8 mm camera, a 2.00 m feature reads 2.06 m. The lever-arm error adds an orientation-dependent term on top that peaks while the platform rotates.