[feat] Epipolar-guided L2R tracking for multicamera mode

TROUBLESHOOTING.md

@@ -495,6 +495,31 @@ See [Step 4: Mask static image areas](#step-4-mask-static-image-areas)
 - For stereo images, try to cut up to 10% from the left border of the left eye and up to 10% from the right border of
   the right eye to keep only the overlapped area.
 
+### Tune multicamera L2R depth range
+
+Applies only to Multicamera mode. cuVSLAM samples scene depth along each epipolar curve to seed initial guesses for
+the left-to-right (L2R) LK tracker. The sampled range must match the rig: too tight and near-camera features are
+dropped, too wide and the candidate list inflates and LK converges on decoys.
+
+Always set these to the actual near/far limits of your scene when you know them — the auto-detected defaults
+are a fallback for when the scene is unknown, not a target to leave in place. Tight bounds around the real depth
+range give shorter candidate lists, faster tracking, and fewer spurious matches.
+
+Auto-detected defaults (used only if you leave the values negative) are derived from the rig baseline:
+
+- Small stereo (~5–10 cm baseline, indoor / robot arm): `[0.1 m, 20 m]`.
+- KITTI-scale outdoor (~0.5 m baseline): `[7 m, 1000 m]`.
+
+Symptoms of a mismatched range: consistently low L2R success on near-camera or far-away features, or a quality
+drop after widening the range too much.
+
+**C++ API**
+
+```cpp
+cuvslam::Odometry::Config::min_depth  // meters; any negative value (e.g. -1) auto-detects
+cuvslam::Odometry::Config::max_depth  // meters; any negative value (e.g. -1) auto-detects
+```
+
 ## Step 9: IMU integration
 
 The cuVSLAM implementation of IMU fusion does not add extra accuracy in scenarios where visual input works, and the IMU can

libs/cuvslam/cuvslam2.cpp

@@ -482,6 +482,8 @@ Odometry::Odometry(const Rig& rig, const Config& cfg) {
   svo_settings.sof_settings.border_left = rig.cameras[0].border_left;
   svo_settings.sof_settings.border_right = rig.cameras[0].border_right;
   svo_settings.sof_settings.box3_prefilter = cfg.use_denoising;
+  svo_settings.sof_settings.min_depth = cfg.min_depth;
+  svo_settings.sof_settings.max_depth = cfg.max_depth;
   if (cfg.rectified_stereo_camera) {
     CheckRectifiedStereoCamera(rig);
     svo_settings.sof_settings.lr_tracker = sof::TrackerType::LKHorizontal;

libs/cuvslam/cuvslam2.h

@@ -509,6 +509,16 @@ class CUVSLAM_API Odometry {
     RGBDSettings rgbd_settings;
     /// Multisensor odometry settings. Used only when odometry_mode == OdometryMode::Multisensor.
     MultisensorSettings multisensor_settings;
+    /// Minimum scene depth (meters) sampled along the epipolar curve when generating LK initial
+    /// guesses for left-to-right (L2R) tracking. ONLY used in Multicamera mode.
+    /// Any negative value (e.g. -1) auto-detects from the pair baseline: small stereo (~7 cm)
+    /// → 0.1 m, KITTI-scale (~0.5 m) → 7 m. Default: -1.f (auto).
+    float min_depth = -1.f;
+    /// Maximum scene depth (meters) sampled along the epipolar curve when generating LK initial
+    /// guesses for left-to-right (L2R) tracking. ONLY used in Multicamera mode.
+    /// Any negative value (e.g. -1) auto-detects from the pair baseline: small stereo (~7 cm)
+    /// → 20 m, KITTI-scale (~0.5 m) → 1000 m. Default: -1.f (auto).
+    float max_depth = -1.f;
   };
 
   // TODO(vikuznetsov): remove when https://gcc.gnu.org/bugzilla/show_bug.cgi?id=88165 is fixed

libs/cuvslam/debug_dump.cpp

@@ -170,6 +170,8 @@ void DumpConfiguration(const std::string& input_dump_root_dir, const Rig& rig, c
     dst_cfg["max_frame_delta_s"] = cfg.max_frame_delta_s;
     dst_cfg["multicam_mode"] = ToUnderlying(cfg.multicam_mode);
     dst_cfg["odometry_mode"] = ToUnderlying(cfg.odometry_mode);
+    dst_cfg["min_depth"] = cfg.min_depth;
+    dst_cfg["max_depth"] = cfg.max_depth;
     dst_cfg["rgbd_settings"]["depth_scale_factor"] = cfg.rgbd_settings.depth_scale_factor;
     dst_cfg["rgbd_settings"]["depth_camera_id"] = cfg.rgbd_settings.depth_camera_id;
     dst_cfg["rgbd_settings"]["enable_depth_stereo_tracking"] = cfg.rgbd_settings.enable_depth_stereo_tracking;

libs/sof/CMakeLists.txt

@@ -19,6 +19,7 @@ set(HEADERS
     basic_image_downscaler.h
     box_blur.h
     convolutor.h
+    epipolar_curves.h
     feature_prediction_interface.h
     feature_tracker.h
     gaussian_coefficients.h
@@ -54,6 +55,7 @@ set(SOURCES
     basic_image_downscaler.cpp
     box_blur.cpp
     convolutor.cpp
+    epipolar_curves.cpp
     gftt.cpp
     gradient_pyramid.cpp
     image_context.cpp

libs/sof/epipolar_curves.cpp

@@ -0,0 +1,165 @@
+
+/*
+ * Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved.
+ *
+ * NVIDIA software released under the NVIDIA Community License is intended to be used to enable
+ * the further development of AI and robotics technologies. Such software has been designed, tested,
+ * and optimized for use with NVIDIA hardware, and this License grants permission to use the software
+ * solely with such hardware.
+ * Subject to the terms of this License, NVIDIA confirms that you are free to commercially use,
+ * modify, and distribute the software with NVIDIA hardware. NVIDIA does not claim ownership of any
+ * outputs generated using the software or derivative works thereof. Any code contributions that you
+ * share with NVIDIA are licensed to NVIDIA as feedback under this License and may be incorporated
+ * in future releases without notice or attribution.
+ * By using, reproducing, modifying, distributing, performing, or displaying any portion or element
+ * of the software or derivative works thereof, you agree to be bound by this License.
+ */
+
+#include "sof/epipolar_curves.h"
+
+#include <algorithm>
+#include <cmath>
+#include <stdexcept>
+
+#include "common/vector_2t.h"
+#include "common/vector_3t.h"
+
+namespace cuvslam::sof {
+namespace {
+constexpr int kNumDepthSamples = 50;
+
+// Auto-detect anchors mapping stereo baseline (m) → sensible (min_depth, max_depth) range (m).
+// Small stereo (7.5 cm avg baseline) is set for close-range indoor use; large stereo (KITTI-
+// scale, ~0.5 m baseline) is set for outdoor / driving use. Baselines above kAutoBaselineLarge
+// clamp to the large-baseline values.
+constexpr float kAutoBaselineSmall = 0.075f;  // ~7.5 cm — small stereo (5–10 cm range midpoint)
+constexpr float kAutoBaselineLarge = 0.5f;    // 0.5 m — KITTI-scale
+constexpr float kAutoMinDepthSmall = 0.1f;    // 10 cm
+constexpr float kAutoMinDepthLarge = 7.f;     // 7 m
+constexpr float kAutoMaxDepthSmall = 20.f;    // 20 m
+constexpr float kAutoMaxDepthLarge = 1000.f;  // 1 km
+
+// Fills any finite negative `min_depth` / `max_depth` with baseline-interpolated defaults, then
+// throws if the resulting range is not finite, strictly positive and monotonic. Only a finite
+// negative value is the "auto" trigger; 0 stays an explicit input, and every non-finite input
+// (NaN, ±inf) falls through to validation rather than being silently replaced. Left unchecked,
+// +inf would reach `std::log` and turn every depth sample into NaN.
+void AutoDetectDepthRange(const float baseline, float& min_depth, float& max_depth) {
+  const float t = std::clamp((baseline - kAutoBaselineSmall) / (kAutoBaselineLarge - kAutoBaselineSmall), 0.f, 1.f);
+  if (std::isfinite(min_depth) && min_depth < 0.f) {
+    min_depth = kAutoMinDepthSmall + t * (kAutoMinDepthLarge - kAutoMinDepthSmall);
+  }
+  if (std::isfinite(max_depth) && max_depth < 0.f) {
+    max_depth = kAutoMaxDepthSmall + t * (kAutoMaxDepthLarge - kAutoMaxDepthSmall);
+  }
+  if (!std::isfinite(min_depth) || !std::isfinite(max_depth) || !(min_depth > 0.f) || !(min_depth < max_depth)) {
+    throw std::invalid_argument(
+        "EpipolarCurves: min_depth / max_depth must be finite and satisfy 0 < min_depth < max_depth "
+        "after auto-detection (pass a negative value to trigger auto-detect).");
+  }
+}
+}  // namespace
+
+void EpipolarCurves::Candidates(const Vector2T& uv_l_base, std::vector<Vector2T>& out) const {
+  out.clear();
+  const float v_f = uv_l_base.y() * inv_scale_;
+  const float u_f = uv_l_base.x() * inv_scale_;
+  // Guard against negative, NaN, or oversized inputs. The `!(x >= 0.f)` form catches NaN too.
+  if (!(v_f >= 0.f) || !(u_f >= 0.f)) {
+    return;
+  }
+  const auto v0 = static_cast<size_t>(v_f);
+  const auto u0 = static_cast<size_t>(u_f);
+  const size_t v1 = v0 + 1;
+  const size_t u1 = u0 + 1;
+  // Need four corners around (u_f, v_f); bail if either right/bottom corner is out of range.
+  if (v1 >= curves_.size() || u1 >= curves_[v0].size()) {
+    return;
+  }
+  const float dv = v_f - static_cast<float>(v0);
+  const float du = u_f - static_cast<float>(u0);
+  const float w00 = (1.0f - du) * (1.0f - dv);
+  const float w01 = du * (1.0f - dv);
+  const float w10 = (1.0f - du) * dv;
+  const float w11 = du * dv;
+  const auto& c00 = curves_[v0][u0];
+  const auto& c01 = curves_[v0][u1];
+  const auto& c10 = curves_[v1][u0];
+  const auto& c11 = curves_[v1][u1];
+  // Interpolate up to the shortest of the four corner curves. Adjacent corners have similar
+  // geometry, so their lengths typically differ by at most a candidate or two.
+  const size_t n = std::min({c00.size(), c01.size(), c10.size(), c11.size()});
+  out.resize(n);
+  for (size_t k = 0; k < n; ++k) {
+    out[k] = w00 * c00[k] + w01 * c01[k] + w10 * c10[k] + w11 * c11[k];
+  }
+}
+
+EpipolarCurves::EpipolarCurves(const camera::ICameraModel& cam_l, const camera::ICameraModel& cam_r,
+                               const Isometry3T& right_from_left, const int top_level, const size_t top_width,
+                               const size_t top_height, float min_depth, float max_depth)
+    : inv_scale_(1.0f / static_cast<float>(1u << top_level)),
+      curves_(top_height + 1, std::vector<std::vector<Vector2T>>(top_width + 1)) {
+  const auto scale = static_cast<float>(1u << top_level);
+
+  AutoDetectDepthRange(right_from_left.translation().norm(), min_depth, max_depth);
+
+  const float log_min = std::log(min_depth);
+  const float log_max = std::log(max_depth);
+  const float log_step = (log_max - log_min) / static_cast<float>(kNumDepthSamples - 1);
+
+  // Iterate CORNERS: (u, v) in [0, top_width] × [0, top_height]. Each corner sits at the base-level
+  // position (u*scale, v*scale) — the top-left corner of top-pixel (u, v) mapped to base.
+  for (size_t v = 0; v <= top_height; ++v) {
+    auto& row = curves_[v];
+    for (size_t u = 0; u <= top_width; ++u) {
+      const Vector2T uv_l_base(static_cast<float>(u) * scale, static_cast<float>(v) * scale);
+      Vector2T xy_l;
+      if (!cam_l.normalizePoint(uv_l_base, xy_l)) {
+        continue;
+      }
+      const Vector3T ray_l(xy_l.x(), xy_l.y(), 1.0f);
+      auto& curve = row[u];
+      // Typical dedup-keeps range from a handful (KITTI-scale, avg ~3) to a few dozen (short
+      // baseline). 128 covers the practical worst case with negligible memory per corner.
+      curve.reserve(128);
+
+      // Distance-based dedup: enforce that consecutive stored entries are ≥ 1 top-level pixel
+      // apart (Euclidean). Prevents storing (u_r, v_r) positions that are within 1 pixel of the
+      // previously kept entry — the LK convergence basin trivially covers sub-pixel gaps.
+      float last_u_top_f = -1e9f;
+      float last_v_top_f = -1e9f;
+      float log_d = log_max;
+      for (int s = 0; s < kNumDepthSamples; ++s, log_d -= log_step) {
+        const Vector3T p_r = right_from_left * (std::exp(log_d) * ray_l);
+        if (p_r.z() <= 0.0f) {
+          continue;
+        }
+        Vector2T uv_r_base;
+        if (!cam_r.denormalizePoint({p_r.x() / p_r.z(), p_r.y() / p_r.z()}, uv_r_base)) {
+          continue;
+        }
+        const float u_r_top_f = uv_r_base.x() * inv_scale_;
+        const float v_r_top_f = uv_r_base.y() * inv_scale_;
+        if (u_r_top_f < 0.0f || v_r_top_f < 0.0f) {
+          continue;
+        }
+        if (u_r_top_f >= static_cast<float>(top_width) || v_r_top_f >= static_cast<float>(top_height)) {
+          continue;
+        }
+        const float du = u_r_top_f - last_u_top_f;
+        const float dv = v_r_top_f - last_v_top_f;
+        if (du * du + dv * dv < 1.0f) {
+          continue;
+        }
+        last_u_top_f = u_r_top_f;
+        last_v_top_f = v_r_top_f;
+        // Store the sub-pixel top-level position (converted to base-level) rather than snapping to
+        // a pixel center — preserves the smooth curve for LK's initial guess.
+        curve.emplace_back(u_r_top_f * scale, v_r_top_f * scale);
+      }
+    }
+  }
+}
+
+}  // namespace cuvslam::sof

libs/sof/epipolar_curves.h

@@ -0,0 +1,63 @@
+
+/*
+ * Copyright (c) 2026, NVIDIA CORPORATION. All rights reserved.
+ *
+ * NVIDIA software released under the NVIDIA Community License is intended to be used to enable
+ * the further development of AI and robotics technologies. Such software has been designed, tested,
+ * and optimized for use with NVIDIA hardware, and this License grants permission to use the software
+ * solely with such hardware.
+ * Subject to the terms of this License, NVIDIA confirms that you are free to commercially use,
+ * modify, and distribute the software with NVIDIA hardware. NVIDIA does not claim ownership of any
+ * outputs generated using the software or derivative works thereof. Any code contributions that you
+ * share with NVIDIA are licensed to NVIDIA as feedback under this License and may be incorporated
+ * in future releases without notice or attribution.
+ * By using, reproducing, modifying, distributing, performing, or displaying any portion or element
+ * of the software or derivative works thereof, you agree to be bound by this License.
+ */
+
+#pragma once
+
+#include <vector>
+
+#include "camera/camera.h"
+#include "common/isometry.h"
+#include "common/vector_2t.h"
+
+namespace cuvslam::sof {
+
+// Precomputed L2R initial-guess table for the epipolar-scan LK tracker. Samples scene depth
+// log-uniformly over the caller-supplied `[min_depth, max_depth]` range along the left viewing
+// ray of each top-level pixel CORNER, projects through the right camera model (with distortion),
+// and dedupes consecutive samples that fall within one top-level pixel of the last kept sample.
+// Internally indexes the stored curves by LEFT top-level pixel corner; publicly,
+// `Candidates(uv_l_base)` accepts a LEFT base-level pixel position and fills a caller-owned
+// vector with RIGHT base-level candidate positions along the epipolar curve — bilinearly
+// interpolated from the four surrounding corner curves, roughly ordered far-to-near (independent
+// per-corner dedup makes the ordering approximate, not strict). Consumers feed those directly to
+// the LK tracker as initial guesses and stop at the first success.
+class EpipolarCurves {
+public:
+  // `min_depth` / `max_depth` — the depth range (meters) sampled along the epipolar curve.
+  // Any negative value (e.g. -1) auto-detects from the pair's baseline; see
+  // `Odometry::Config::min_depth` in the public API for the auto-detection anchors.
+  EpipolarCurves(const camera::ICameraModel& cam_l, const camera::ICameraModel& cam_r,
+                 const Isometry3T& right_from_left, int top_level, size_t top_width, size_t top_height, float min_depth,
+                 float max_depth);
+
+  // For a LEFT base-level pixel, fills `out` with RIGHT base-level candidate positions along the
+  // epipolar curve (roughly far-to-near — see class comment for why "roughly"), bilinearly
+  // interpolated from the four surrounding top-level pixel corners. `out` is resized as needed;
+  // caller should retain the vector between calls/frames to avoid reallocation. Left empty if
+  // `uv_l_base` is out of range or NaN.
+  void Candidates(const Vector2T& uv_l_base, std::vector<Vector2T>& out) const;
+
+private:
+  const float inv_scale_;
+  // curves_[v][u] — for the LEFT top-level pixel CORNER at (u, v), the far-to-near list of RIGHT
+  // base-level pixel positions along the epipolar curve. Corner grid: outer dim is top_height+1,
+  // middle dim is top_width+1 (corners around each top-pixel). Candidates() bilinearly
+  // interpolates between the four surrounding corners for arbitrary sub-corner queries.
+  std::vector<std::vector<std::vector<Vector2T>>> curves_;
+};
+
+}  // namespace cuvslam::sof

libs/sof/internal/sof_multicamera_base.h

@@ -21,17 +21,24 @@
 
 #include <functional>
 #include <list>
+#include <unordered_map>
 #include <vector>
 
 #include "common/camera_id.h"
 #include "odometry/svo_config.h"
 #include "profiler/profiler.h"
 
+#include "sof/epipolar_curves.h"
 #include "sof/kf_selector.h"
 #include "sof/sof.h"
 
 namespace cuvslam::sof {
 
+// Multi-launch L2R scan stops advancing the candidate index once this fraction of observations
+// is tracked. Later candidates are more likely to produce spurious matches for the still-
+// untracked points, so dropping them cleanly beats capturing decoys.
+inline constexpr float kL2REarlyExitFraction = 0.8f;
+
 class MultiSOFBase : public IMultiSOF {
 protected:
 public:
@@ -62,13 +69,32 @@ class MultiSOFBase : public IMultiSOF {
   bool is_keyframe(const MulticamTracksVector& tracks, const int64_t current_timestamp_ns,
                    const odom::KeyFrameSettings& kf_settings);
 
+  // Return the epipolar curve grid for the given (primary, secondary) pair, building it lazily on
+  // first request. Derived classes call this after their per-implementation pyramids are built and
+  // pass in the top-level dimensions from those pyramids. Left and right cameras are assumed to
+  // have the same base resolution (hence the same top-level dimensions). Depth range is taken
+  // from `sof::Settings::min_depth` / `max_depth` captured at construction (any negative
+  // value, e.g. -1, auto-detects from baseline).
+  const EpipolarCurves& GetOrBuildEpipolarCurves(CameraId primary_id, CameraId secondary_id, int top_level,
+                                                 size_t top_width, size_t top_height);
+
+  // LK search-radius cap for L2R initial guesses. With epipolar-guided starting points already
+  // within a few pixels of the true match, LK only needs local refinement. Sized at an
+  // empirically-tuned multiple of top-level pixel size (see .cpp for tuning notes) — large enough
+  // to permit refinement, small enough to prevent lateral drift onto decoy features. Shared by
+  // MultiSOFCPU and MultiSOFGPU so both paths use the same reach.
+  static float CrossCamSearchRadius(int top_level);
+
   camera::Rig rig_;
   camera::FrustumIntersectionGraph fid_;
   bool box_prefilter_ = false;
+  float min_depth_ = -1.f;
+  float max_depth_ = -1.f;
   std::list<std::unique_ptr<IMonoSOF>> mono_sof_;
   KFSelector kf_selector_;
   std::unordered_map<CameraId, TracksVector> last_kf_tracks_;
   int64_t last_kf_timestamp_ = 0;
+  std::unordered_map<CameraId, std::unordered_map<CameraId, EpipolarCurves>> epipolar_curves_by_pair_;
 
   // keep allocated memory for is_keyframe
   TracksVector all_tracks_vec_;