diff --git a/TROUBLESHOOTING.md b/TROUBLESHOOTING.md index a8af6c3..39ed20a 100644 --- a/TROUBLESHOOTING.md +++ b/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 diff --git a/libs/cuvslam/cuvslam2.cpp b/libs/cuvslam/cuvslam2.cpp index 60a5faa..44b75fb 100644 --- a/libs/cuvslam/cuvslam2.cpp +++ b/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; diff --git a/libs/cuvslam/cuvslam2.h b/libs/cuvslam/cuvslam2.h index 271b815..45a3027 100644 --- a/libs/cuvslam/cuvslam2.h +++ b/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 diff --git a/libs/cuvslam/debug_dump.cpp b/libs/cuvslam/debug_dump.cpp index f48dc65..f206cfa 100644 --- a/libs/cuvslam/debug_dump.cpp +++ b/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; diff --git a/libs/sof/CMakeLists.txt b/libs/sof/CMakeLists.txt index 7bfdb89..512d04d 100644 --- a/libs/sof/CMakeLists.txt +++ b/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 diff --git a/libs/sof/epipolar_curves.cpp b/libs/sof/epipolar_curves.cpp new file mode 100644 index 0000000..9dd3c6c --- /dev/null +++ b/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 +#include +#include + +#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& 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(v_f); + const auto u0 = static_cast(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(v0); + const float du = u_f - static_cast(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(1u << top_level)), + curves_(top_height + 1, std::vector>(top_width + 1)) { + const auto scale = static_cast(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(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(u) * scale, static_cast(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(top_width) || v_r_top_f >= static_cast(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 diff --git a/libs/sof/epipolar_curves.h b/libs/sof/epipolar_curves.h new file mode 100644 index 0000000..d579f6e --- /dev/null +++ b/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 + +#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& 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>> curves_; +}; + +} // namespace cuvslam::sof diff --git a/libs/sof/internal/sof_multicamera_base.h b/libs/sof/internal/sof_multicamera_base.h index 10eb14b..974ae3e 100644 --- a/libs/sof/internal/sof_multicamera_base.h +++ b/libs/sof/internal/sof_multicamera_base.h @@ -21,17 +21,24 @@ #include #include +#include #include #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> mono_sof_; KFSelector kf_selector_; std::unordered_map last_kf_tracks_; int64_t last_kf_timestamp_ = 0; + std::unordered_map> epipolar_curves_by_pair_; // keep allocated memory for is_keyframe TracksVector all_tracks_vec_; diff --git a/libs/sof/internal/sof_multicamera_cpu.h b/libs/sof/internal/sof_multicamera_cpu.h index 7de2249..66c0ee2 100644 --- a/libs/sof/internal/sof_multicamera_cpu.h +++ b/libs/sof/internal/sof_multicamera_cpu.h @@ -50,9 +50,27 @@ class MultiSOFCPU : public MultiSOFBase { void reset() final; private: - std::unordered_map // tracker primary -> secondary + // Per-observation winner slot — mirrors GPU's `winner_scratch[i]` (a `TrackData`) so both + // implementations use one scratch vector per observation instead of parallel arrays. + // `tracked` is `uint8_t` to avoid the bit-packed `std::vector` specialization. + struct CPUWinner { + Vector2T uvR; + Matrix2T info; + uint8_t tracked = 0; + }; + + struct PrimaryToSecondaryCPUTracker { + std::unique_ptr tracker; + // Per-frame / per-observation scratch. Kept as members (rather than locals) so allocations + // and inner-vector capacities persist across Launch calls. + std::vector uvL; + std::vector> cands; + std::vector winners; + }; + + std::unordered_map secondary >> secondary_from_primary_sof_; diff --git a/libs/sof/internal/sof_multicamera_gpu.h b/libs/sof/internal/sof_multicamera_gpu.h index c83ca00..0bef335 100644 --- a/libs/sof/internal/sof_multicamera_gpu.h +++ b/libs/sof/internal/sof_multicamera_gpu.h @@ -55,6 +55,12 @@ class MultiSOFGPU : public MultiSOFBase { GPUArrayPinned tracks_data{1000}; Stream stream; bool was_launched = false; + // Per-frame / per-observation scratch. Kept as members (rather than locals) so allocations + // and inner-vector capacities persist across Launch calls. `winners[i].track_status` doubles + // as the "already succeeded?" flag for point i during the multi-launch scan. + std::vector uvL; + std::vector> cands; + std::vector winners; }; std::unordered_map> secondary_from_primary_sof_; - Settings sof_settings_; - void LaunchTrackingPrimaryToSecondary(CameraId primary_id, CameraId secondary_id, const Sources& curr_sources, Images& curr_images, const std::vector& primary_obs, std::vector* secondary_obs = nullptr) final; diff --git a/libs/sof/sof_config.h b/libs/sof/sof_config.h index 48d74d5..d782a8a 100644 --- a/libs/sof/sof_config.h +++ b/libs/sof/sof_config.h @@ -17,9 +17,7 @@ #pragma once -#include #include -#include #include #include "camera/frustum_intersection_graph.h" @@ -49,6 +47,13 @@ struct Settings { // left-to-right tracker (stereo only) TrackerType lr_tracker = TrackerType::LK; + // Depth range (meters) sampled along the epipolar curve when generating LK initial guesses for + // left-to-right (L2R) tracking. ONLY used in multicamera mode; ignored for monocular tracking. + // Any negative value (e.g. -1) auto-detects from the pair baseline. See + // `Odometry::Config::min_depth` in the public API for the auto-detection anchors. + float min_depth = -1.f; + float max_depth = -1.f; + SelectorStereoSettings feature_selection_settings; camera::MulticameraMode multicam_mode = camera::MulticameraMode::Moderate; diff --git a/libs/sof/sof_multicamera_base.cpp b/libs/sof/sof_multicamera_base.cpp index bb418b4..0fc6319 100644 --- a/libs/sof/sof_multicamera_base.cpp +++ b/libs/sof/sof_multicamera_base.cpp @@ -21,7 +21,36 @@ namespace cuvslam::sof { MultiSOFBase::MultiSOFBase(const camera::Rig& rig, const camera::FrustumIntersectionGraph& fid, const Settings& sof_settings, const odom::KeyFrameSettings& keyframe_settings) - : rig_(rig), fid_(fid), box_prefilter_(sof_settings.box3_prefilter), kf_selector_(keyframe_settings) {} + : rig_(rig), + fid_(fid), + box_prefilter_(sof_settings.box3_prefilter), + min_depth_(sof_settings.min_depth), + max_depth_(sof_settings.max_depth), + kf_selector_(keyframe_settings) {} + +float MultiSOFBase::CrossCamSearchRadius(int top_level) { + // Empirical multiplier — smaller values under-reach for jittery interpolation, larger values + // invite decoy matches. 3 gives best KITTI ATE with the current epipolar-curve construction. + constexpr float kMultiplier = 3.f; + return kMultiplier * static_cast(1u << (top_level + 1)); +} + +const EpipolarCurves& MultiSOFBase::GetOrBuildEpipolarCurves(CameraId primary_id, CameraId secondary_id, int top_level, + size_t top_width, size_t top_height) { + auto& row = epipolar_curves_by_pair_[primary_id]; + auto it = row.find(secondary_id); + if (it != row.end()) { + return it->second; + } + const camera::ICameraModel& intr_l = *rig_.intrinsics[primary_id]; + const camera::ICameraModel& intr_r = *rig_.intrinsics[secondary_id]; + const Isometry3T right_from_left = rig_.camera_from_rig[secondary_id] * rig_.camera_from_rig[primary_id].inverse(); + const auto [inserted_it, inserted] = row.emplace( + secondary_id, + EpipolarCurves(intr_l, intr_r, right_from_left, top_level, top_width, top_height, min_depth_, max_depth_)); + EpipolarCurves& epipolar_curves = inserted_it->second; + return epipolar_curves; +} void MultiSOFBase::reset_keyframe_selector() { kf_selector_.reset(); diff --git a/libs/sof/sof_multicamera_cpu.cpp b/libs/sof/sof_multicamera_cpu.cpp index ce7ef09..13ffa15 100644 --- a/libs/sof/sof_multicamera_cpu.cpp +++ b/libs/sof/sof_multicamera_cpu.cpp @@ -15,10 +15,9 @@ * of the software or derivative works thereof, you agree to be bound by this License. */ -#include #include +#include -#include "common/isometry_utils.h" #include "sof/internal/sof_multicamera_cpu.h" #include "sof/sof_create.h" @@ -40,7 +39,7 @@ MultiSOFCPU::MultiSOFCPU(const camera::Rig& rig, const camera::FrustumIntersecti auto& tracker_from_secondary_cam = secondary_from_primary_sof_[primary_cam_id]; for (CameraId secondary_cam_id : secondary_cams) { - tracker_from_secondary_cam[secondary_cam_id] = CreateTracker(sof_settings.lr_tracker); + tracker_from_secondary_cam[secondary_cam_id].tracker = CreateTracker(sof_settings.lr_tracker); } } } @@ -56,49 +55,75 @@ void MultiSOFCPU::LaunchTrackingPrimaryToSecondary(CameraId primary_id, CameraId const camera::ICameraModel& intrinsicsP = *rig_.intrinsics[primary_id]; const camera::ICameraModel& intrinsicsS = *rig_.intrinsics[secondary_id]; - const std::unique_ptr& tracker = secondary_from_primary_sof_[primary_id][secondary_id]; + PrimaryToSecondaryCPUTracker& pair = secondary_from_primary_sof_.at(primary_id).at(secondary_id); + const std::unique_ptr& tracker = pair.tracker; assert(tracker != nullptr); - const Isometry3T secondary_from_primary = - rig_.camera_from_rig[secondary_id] * rig_.camera_from_rig[primary_id].inverse(); - - const float baseline = secondary_from_primary.translation().norm(); - const float avg_focal = 0.5f * (intrinsicsS.getFocal().x() + intrinsicsS.getFocal().y()); - const float cross_cam_search_radius = std::max(20.f, baseline * avg_focal * 2.f); - secondary_image->build_cpu_image_pyramid(secondary_source, box_prefilter_); secondary_image->build_cpu_gradient_pyramid(tracker->isHorizontal()); - for (const camera::Observation& trackL : primary_obs) { - const TrackId& trackId = trackL.id; - const Vector2T& xyL = trackL.xy; - Vector2T uvL; - intrinsicsP.denormalizePoint(xyL, uvL); + const ImagePyramidT& img_l = primary_image->cpu_image_pyramid(); + const ImagePyramidT& img_r = secondary_image->cpu_image_pyramid(); - const Vector3T ray_in_secondary = secondary_from_primary.linear() * xyL.homogeneous(); - const float z = ray_in_secondary.z(); + const int top_l = img_l.getLevelsCount() - 1; + const EpipolarCurves& epipolar_curves = + GetOrBuildEpipolarCurves(primary_id, secondary_id, top_l, img_l[top_l].cols(), img_l[top_l].rows()); - if (z < 1e-8) { - continue; - } + const float cross_cam_search_radius = CrossCamSearchRadius(top_l); - const Vector2T xyR_init(ray_in_secondary.x() / z, ray_in_secondary.y() / z); - Vector2T uvR; - intrinsicsS.denormalizePoint(xyR_init, uvR); + const size_t n = primary_obs.size(); + auto& uvL = pair.uvL; + auto& cands = pair.cands; + auto& winners = pair.winners; + uvL.resize(n); + cands.resize(n); // inner vectors retain their allocation across frames + winners.assign(n, CPUWinner{}); - Matrix2T info; + size_t max_candidates = 0; + for (size_t i = 0; i < n; ++i) { + intrinsicsP.denormalizePoint(primary_obs[i].xy, uvL[i]); + epipolar_curves.Candidates(uvL[i], cands[i]); + max_candidates = std::max(max_candidates, cands[i].size()); + } - if (tracker->trackPoint(primary_image->cpu_gradient_pyramid(), secondary_image->cpu_gradient_pyramid(), - primary_image->cpu_image_pyramid(), secondary_image->cpu_image_pyramid(), uvL, uvR, info, - cross_cam_search_radius)) { - Vector2T xyR; - intrinsicsS.normalizePoint(uvR, xyR); + size_t tracked_count = 0; - if (secondary_obs) { - secondary_obs->push_back( - {secondary_id, trackId, xyR, camera::ObservationInfoUVToXY(intrinsicsS, uvR, xyR, info)}); + // Candidate-index outer loop, observation inner loop — matches GPU semantics: first successful + // candidate wins per observation. + for (size_t k = 0; k < max_candidates; ++k) { + for (size_t i = 0; i < n; ++i) { + if (winners[i].tracked || k >= cands[i].size()) { + continue; + } + Vector2T uvR = cands[i][k]; + Matrix2T info; + if (tracker->trackPoint(primary_image->cpu_gradient_pyramid(), secondary_image->cpu_gradient_pyramid(), img_l, + img_r, uvL[i], uvR, info, cross_cam_search_radius)) { + winners[i].uvR = uvR; + winners[i].info = info; + winners[i].tracked = 1; + ++tracked_count; } } + + // Stop advancing candidate index once we've tracked at least kL2REarlyExitFraction of + // observations (shared with MultiSOFGPU). + if (static_cast(tracked_count) >= kL2REarlyExitFraction * static_cast(n)) { + break; + } + } + + // Publish successful matches in observation order. + if (secondary_obs) { + for (size_t i = 0; i < n; ++i) { + if (!winners[i].tracked) { + continue; + } + Vector2T xyR; + intrinsicsS.normalizePoint(winners[i].uvR, xyR); + secondary_obs->push_back({secondary_id, primary_obs[i].id, xyR, + camera::ObservationInfoUVToXY(intrinsicsS, winners[i].uvR, xyR, winners[i].info)}); + } } } diff --git a/libs/sof/sof_multicamera_gpu.cpp b/libs/sof/sof_multicamera_gpu.cpp index d52ab23..5765c71 100644 --- a/libs/sof/sof_multicamera_gpu.cpp +++ b/libs/sof/sof_multicamera_gpu.cpp @@ -15,9 +15,12 @@ * of the software or derivative works thereof, you agree to be bound by this License. */ +#include #include +#include +#include -#include "common/isometry_utils.h" +#include "common/vector_2t.h" #include "sof/internal/sof_multicamera_gpu.h" #include "sof/sof_create.h" @@ -52,6 +55,12 @@ MultiSOFGPU::MultiSOFGPU(const camera::Rig& rig, const camera::FrustumIntersecti auto& tracker_from_secondary_cam = secondary_from_primary_sof_[primary_cam_id]; for (CameraId secondary_cam_id : secondary_cams) { auto tracker_ptr = CreateGPUTracker(sof_settings.lr_tracker); + // CreateGPUTracker returns nullptr for tracker types with no GPU implementation. Fail here + // rather than on the first Launch: asserts are compiled out in release builds, so the null + // would otherwise surface as a crash inside track_points. + if (tracker_ptr == nullptr) { + throw std::invalid_argument("MultiSOFGPU: lr_tracker type has no GPU implementation"); + } tracker_from_secondary_cam[secondary_cam_id].tracker = std::move(tracker_ptr); } } @@ -66,60 +75,112 @@ void MultiSOFGPU::LaunchTrackingPrimaryToSecondary(CameraId primary_id, CameraId const ImageContextPtr secondary_image = curr_images[secondary_id]; const camera::ICameraModel& intrinsicsP = *rig_.intrinsics[primary_id]; - const camera::ICameraModel& intrinsicsS = *rig_.intrinsics[secondary_id]; - - PrimaryToSecondaryGPUTracker& tracker = secondary_from_primary_sof_[primary_id][secondary_id]; - GPUArrayPinned& tracks_data = tracker.tracks_data; - Stream& stream = tracker.stream; + PrimaryToSecondaryGPUTracker& pair = secondary_from_primary_sof_.at(primary_id).at(secondary_id); + assert(pair.tracker != nullptr); - const Isometry3T secondary_from_primary = - rig_.camera_from_rig[secondary_id] * rig_.camera_from_rig[primary_id].inverse(); + GPUArrayPinned& tracks_data = pair.tracks_data; + Stream& stream = pair.stream; - const float baseline = secondary_from_primary.translation().norm(); - const float avg_focal = 0.5f * (intrinsicsS.getFocal().x() + intrinsicsS.getFocal().y()); - const float cross_cam_search_radius = std::max(20.f, baseline * avg_focal * 2.f); + // Build secondary pyramids. + secondary_image->build_gpu_image_pyramid(secondary_source, box_prefilter_, stream.get_stream()); + secondary_image->build_gpu_gradient_pyramid(true, stream.get_stream()); - for (size_t i = 0; i < primary_obs.size(); i++) { - const camera::Observation& trackL = primary_obs[i]; - const Vector2T& xyL = trackL.xy; + const auto& pyr_l_gpu = primary_image->gpu_image_pyramid(); + const int top_l = static_cast(pyr_l_gpu.getLevelsCount()) - 1; + + const EpipolarCurves& epipolar_curves = + GetOrBuildEpipolarCurves(primary_id, secondary_id, top_l, pyr_l_gpu[top_l].cols(), pyr_l_gpu[top_l].rows()); + + const float cross_cam_search_radius = CrossCamSearchRadius(top_l); + + const size_t n = primary_obs.size(); + auto& uvL = pair.uvL; + auto& cands = pair.cands; + auto& winners = pair.winners; + uvL.resize(n); + cands.resize(n); // inner vectors retain their allocation across frames + // `assign` rather than `resize`: guarantees every slot starts as a zero-init `TrackData`, so a + // newly grown vector element cannot leak stale fields into a later `winners[i] = ...` copy. + winners.assign(n, TrackData{}); + + size_t max_candidates = 0; + for (size_t i = 0; i < n; ++i) { + intrinsicsP.denormalizePoint(primary_obs[i].xy, uvL[i]); + epipolar_curves.Candidates(uvL[i], cands[i]); + max_candidates = std::max(max_candidates, cands[i].size()); + } - TrackData& data = tracks_data[i]; - Vector2T uvL; - intrinsicsP.denormalizePoint(xyL, uvL); + size_t tracked_count = 0; + + // Multi-launch scan: iterate candidate index k = 0..max_candidates-1. Points that have already + // succeeded, or that ran out of candidates, are given a benign (zero) offset — the kernel still + // processes them (batched-launch limitation) but their results are discarded host-side. First + // successful candidate wins, matching CPU semantics. + for (size_t k = 0; k < max_candidates; ++k) { + bool any_active = false; + for (size_t i = 0; i < n; ++i) { + TrackData& data = tracks_data[i]; + data.ncc_threshold = 0.8f; + data.track = {uvL[i].x(), uvL[i].y()}; + data.track_status = false; + data.search_radius_px = cross_cam_search_radius; + // Zero info[] every iteration so a winner copy cannot inherit stale covariance from an + // earlier k iteration for the same point. + std::fill_n(data.info, 4, 0.f); + const std::vector& cands_i = cands[i]; + // Skip already-won points and points past the end of their candidate list. Both get the + // sentinel `{0, 0}` offset; the winner-update loop ignores them. + if (winners[i].track_status || k >= cands_i.size()) { + data.offset = {0.f, 0.f}; + } else { + const Vector2T offset = cands_i[k] - uvL[i]; + data.offset = {offset.x(), offset.y()}; + any_active = true; + } + } - const Vector3T ray_in_secondary = secondary_from_primary.linear() * xyL.homogeneous(); - const float z = ray_in_secondary.z(); + // Early exit: if no point still has an unclaimed real candidate, remaining k values are + // pure sentinel work — skip them entirely. + if (!any_active) { + break; + } - data.track = {uvL.x(), uvL.y()}; - data.track_status = false; - data.ncc_threshold = 0.8f; + tracks_data.copy_top_n(ToGPU, n, stream.get_stream()); + pair.tracker->track_points(primary_image->gpu_gradient_pyramid(), secondary_image->gpu_gradient_pyramid(), + primary_image->gpu_image_pyramid(), secondary_image->gpu_image_pyramid(), tracks_data, n, + stream.get_stream()); + tracks_data.copy_top_n(ToCPU, n, stream.get_stream()); + cudaStreamSynchronize(stream.get_stream()); - if (z < 1e-8) { - data.offset = {0.f, 0.f}; - data.search_radius_px = 0.f; - continue; + for (size_t i = 0; i < n; ++i) { + // Skip already-won points and sentinel results (ran out of candidates). + if (winners[i].track_status || k >= cands[i].size()) { + continue; + } + if (tracks_data[i].track_status) { + winners[i] = tracks_data[i]; + ++tracked_count; + } } - const Vector2T xyR_init(ray_in_secondary.x() / z, ray_in_secondary.y() / z); - Vector2T uvR_init; - intrinsicsS.denormalizePoint(xyR_init, uvR_init); - - const Vector2T offset = uvR_init - uvL; - data.offset = {offset.x(), offset.y()}; - data.search_radius_px = cross_cam_search_radius; + // Stop advancing candidate index once we've tracked at least kL2REarlyExitFraction of + // observations (shared with MultiSOFCPU). + if (static_cast(tracked_count) >= kL2REarlyExitFraction * static_cast(n)) { + break; + } } - tracks_data.copy_top_n(ToGPU, primary_obs.size(), stream.get_stream()); - - secondary_image->build_gpu_image_pyramid(secondary_source, box_prefilter_, stream.get_stream()); - secondary_image->build_gpu_gradient_pyramid(true, stream.get_stream()); - - tracker.tracker->track_points(primary_image->gpu_gradient_pyramid(), secondary_image->gpu_gradient_pyramid(), - primary_image->gpu_image_pyramid(), secondary_image->gpu_image_pyramid(), tracks_data, - primary_obs.size(), stream.get_stream()); - tracks_data.copy_top_n(ToCPU, primary_obs.size(), stream.get_stream()); - tracker.was_launched = true; + // Publish winners so GetTrackingResults sees per-point best track_status + track uv. + for (size_t i = 0; i < n; ++i) { + if (winners[i].track_status) { + tracks_data[i] = winners[i]; + } else { + tracks_data[i].track_status = false; + tracks_data[i].track = {uvL[i].x(), uvL[i].y()}; + } + } + pair.was_launched = true; } void MultiSOFGPU::GetTrackingResults(MulticamObservations& observations) { @@ -134,13 +195,13 @@ void MultiSOFGPU::GetTrackingResults(MulticamObservations& observations) { const auto& secondary_cams = fid_.secondary_cameras(primary_id); for (CameraId secondary_id : secondary_cams) { - PrimaryToSecondaryGPUTracker& tracker = secondary_from_primary_sof_[primary_id][secondary_id]; - if (!tracker.was_launched) { + PrimaryToSecondaryGPUTracker& pair = secondary_from_primary_sof_.at(primary_id).at(secondary_id); + if (!pair.was_launched) { continue; } - GPUArrayPinned& tracks_data = tracker.tracks_data; - Stream& stream = tracker.stream; + GPUArrayPinned& tracks_data = pair.tracks_data; + Stream& stream = pair.stream; const camera::ICameraModel& intrinsicsS = *rig_.intrinsics[secondary_id]; @@ -176,8 +237,8 @@ void MultiSOFGPU::GetTrackingResults(MulticamObservations& observations) { void MultiSOFGPU::StartKeyframe() { for (auto& [_, x] : secondary_from_primary_sof_) { - for (auto& [cam_id, tracker] : x) { - tracker.was_launched = false; + for (auto& [cam_id, pair] : x) { + pair.was_launched = false; } } } diff --git a/libs/sof/test/CMakeLists.txt b/libs/sof/test/CMakeLists.txt index 4440ae4..84e210b 100644 --- a/libs/sof/test/CMakeLists.txt +++ b/libs/sof/test/CMakeLists.txt @@ -25,6 +25,7 @@ set(SOURCES image_test.cpp kf_selector_test.cpp lk_tracker_test.cpp + sof_l2r_test.cpp st_tracker_test.cpp ) @@ -38,6 +39,7 @@ set(LIBS cuvslam_math camera camera_rig_edex + pnp log common profiler @@ -47,6 +49,10 @@ if(USE_CUDA) set(LIBS ${LIBS} cuda_modules) endif() +if(USE_RERUN) + set(LIBS ${LIBS} visualizer rerun::sdk) +endif() + setup_test_project( MODULE_NAME ${MODULE_NAME} HEADERS ${HEADERS} diff --git a/libs/sof/test/sof_l2r_test.cpp b/libs/sof/test/sof_l2r_test.cpp new file mode 100644 index 0000000..192cc6f --- /dev/null +++ b/libs/sof/test/sof_l2r_test.cpp @@ -0,0 +1,162 @@ + +/* + * 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 "camera/frustum_intersection_graph.h" +#include "camera_rig_edex/camera_rig_edex.h" +#include "common/include_gtest.h" +#include "common/rerun.h" +#include "odometry/svo_config.h" +#include "sof/image_manager.h" +#include "sof/sof_create.h" +#ifdef USE_RERUN +#include + +#include "visualizer/visualizer.hpp" +#endif + +namespace test { +using namespace cuvslam; + +namespace { + +#ifdef USE_RERUN +// Logs the left and right frames side by side in a single 2D space, with one line per L2R match +// running from the primary observation to the secondary one (offset by the left image width). +void LogMatches(const std::string& name, const Sources& sources, const Metas& metas, const camera::Rig& rig, + CameraId primary_id, const CameraId secondary_id, const sof::MulticamObservations& observations) { + const size_t w = static_cast(metas[primary_id].shape.width); + const size_t h = static_cast(metas[primary_id].shape.height); + + std::vector composite(2 * w * h); + const auto left = sources[primary_id].as(metas[primary_id].shape); + const auto right = sources[secondary_id].as(metas[secondary_id].shape); + for (size_t y = 0; y < h; ++y) { + for (size_t x = 0; x < w; ++x) { + composite[y * 2 * w + x] = left(y, x); + composite[y * 2 * w + w + x] = right(y, x); + } + } + + std::vector strips; + for (const camera::Observation& obs_l : observations[primary_id]) { + for (const camera::Observation& obs_r : observations[secondary_id]) { + if (obs_l.id != obs_r.id) { + continue; + } + Vector2T uv_l; + Vector2T uv_r; + rig.intrinsics[primary_id]->denormalizePoint(obs_l.xy, uv_l); + rig.intrinsics[secondary_id]->denormalizePoint(obs_r.xy, uv_r); + strips.push_back(rerun::LineStrip2D({{uv_l.x(), uv_l.y()}, {uv_r.x() + static_cast(w), uv_r.y()}})); + break; + } + } + + // Reuse the shared visualizer: it owns the spawned viewer and the blocking flush on shutdown. + const rerun::RecordingStream& rec = visualizer::RerunVisualizer::getInstance().getRecordingStream(); + rec.log(name + "/image", rerun::Image(composite.data(), {static_cast(2 * w), static_cast(h)}, + rerun::datatypes::ColorModel::L)); + rec.log(name + "/image/matches", rerun::LineStrips2D(strips).with_colors(rerun::Color(0, 255, 0)).with_radii(0.5f)); + // The test process exits right after this, so push the data out before teardown. + std::ignore = rec.flush_blocking(); +} +#endif + +// Lower bound on left-to-right matches for test_data/sof/lr_test frame 0 (640x400, 7.5 cm +// baseline, so the epipolar curves auto-detect the [0.1, 20] m depth range). Set below the +// measured count to absorb GPU/driver jitter while still catching a real L2R regression. +constexpr size_t kMinTrackedPoints = 175; + +// Runs the single lr_test frame through the multicamera SOF and returns the number of +// left-to-right tracked points published for the secondary camera. +size_t TrackL2R(sof::Implementation implementation, [[maybe_unused]] const std::string& name) { + const bool use_gpu = implementation == sof::Implementation::kGPU; + + camera_rig_edex::CameraRigEdex edex_rig(std::string(CUVSLAM_TEST_ASSETS) + "sof/lr_test/stereo.edex"); + if (const ErrorCode err = edex_rig.start(); !err) { + ADD_FAILURE() << "CameraRigEdex::start failed: " << err.str(); + return 0; + } + + camera::Rig rig; + rig.num_cameras = static_cast(edex_rig.getCamerasNum()); + for (int32_t cam = 0; cam < rig.num_cameras; ++cam) { + rig.intrinsics[cam] = &edex_rig.getIntrinsic(cam); + rig.camera_from_rig[cam] = edex_rig.getExtrinsic(cam).inverse(); + } + + camera::FigSettings fig_settings; + fig_settings.mode = camera::MulticameraMode::Performance; + const camera::FrustumIntersectionGraph fig(rig, fig_settings); + const CameraId primary_id = fig.primary_cameras().front(); + const CameraId secondary_id = fig.secondary_cameras(primary_id).front(); + + const sof::Settings sof_settings; + constexpr odom::KeyFrameSettings kf_settings; + const std::unique_ptr multi_sof = + CreateMultiSOF(implementation, rig, fig, nullptr, sof_settings, kf_settings); + + Sources sources; + Sources masks; + Metas metas; + DepthSources depths; + if (const ErrorCode err = edex_rig.getFrame(sources, metas, masks, depths); err != ErrorCode::S_True) { + ADD_FAILURE() << "CameraRigEdex::getFrame failed: " << err.str(); + return 0; + } + + sof::ImageManager image_manager; + image_manager.init(metas[0].shape, sources.size(), use_gpu); + sof::Images curr_images(sources.size(), nullptr); + for (size_t cam = 0; cam < sources.size(); ++cam) { + curr_images[cam] = image_manager.acquire(); + curr_images[cam]->set_image_meta(metas[cam]); + } + + odom::TrackPerFrameSettings per_frame; + per_frame.sof = sof_settings; + per_frame.kf = kf_settings; + + // First frame is a keyframe: features are detected in the primary camera and the L2R scan runs. + const sof::Images prev_images(sources.size(), nullptr); + sof::MulticamObservations observations(rig.num_cameras); + sof::FrameState state = sof::FrameState::None; + multi_sof->trackNextFrame(sources, curr_images, prev_images, masks, Isometry3T::Identity(), observations, state, + per_frame); + + RERUN(LogMatches, name, sources, metas, rig, primary_id, secondary_id, observations); + + return observations[secondary_id].size(); +} + +} // namespace + +TEST(SOFL2R, TrackedPointsCPU) { + const size_t tracked = TrackL2R(sof::Implementation::kCPU, "SOFL2R_CPU"); + std::cout << "CPU L2R tracked points: " << tracked << std::endl; + EXPECT_GE(tracked, kMinTrackedPoints); +} + +#ifdef USE_CUDA +TEST(SOFL2R, TrackedPointsGPU) { + const size_t tracked = TrackL2R(sof::Implementation::kGPU, "SOFL2R_GPU"); + std::cout << "GPU L2R tracked points: " << tracked << std::endl; + EXPECT_GE(tracked, kMinTrackedPoints); +} +#endif + +} // namespace test diff --git a/python/cuvslam2.cpp b/python/cuvslam2.cpp index cb46594..90d763a 100644 --- a/python/cuvslam2.cpp +++ b/python/cuvslam2.cpp @@ -471,8 +471,8 @@ NB_MODULE(pycuvslam, m) { nb::class_(odom_cls, "Config") // WARNING: the order of init arguments in this definition must coincide with the order in the structure .def(nb::init(), + float, std::string_view, bool, const Odometry::RGBDSettings&, const Odometry::MultisensorSettings&, + float, float>(), nb::kw_only(), nb::arg("multicam_mode") = Odometry::Config{}.multicam_mode, nb::arg("odometry_mode") = Odometry::Config{}.odometry_mode, nb::arg("use_gpu") = Odometry::Config{}.use_gpu, nb::arg("async_sba") = Odometry::Config{}.async_sba, @@ -486,7 +486,8 @@ NB_MODULE(pycuvslam, m) { nb::arg("debug_dump_directory") = Odometry::Config{}.debug_dump_directory, nb::arg("debug_imu_mode") = Odometry::Config{}.debug_imu_mode, nb::arg("rgbd_settings") = Odometry::Config{}.rgbd_settings, - nb::arg("multisensor_settings") = Odometry::Config{}.multisensor_settings) + nb::arg("multisensor_settings") = Odometry::Config{}.multisensor_settings, + nb::arg("min_depth") = Odometry::Config{}.min_depth, nb::arg("max_depth") = Odometry::Config{}.max_depth) .def_rw("multicam_mode", &Odometry::Config::multicam_mode, "See :class:`Odometry.MulticameraMode`") .def_rw("odometry_mode", &Odometry::Config::odometry_mode, "See :class:`Odometry.OdometryMode`") .def_rw("use_gpu", &Odometry::Config::use_gpu, "Enable to use GPU acceleration") @@ -509,7 +510,13 @@ NB_MODULE(pycuvslam, m) { .def_rw("rgbd_settings", &Odometry::Config::rgbd_settings, "Settings for RGB-D odometry mode. See :class:`Odometry.RGBDSettings`") .def_rw("multisensor_settings", &Odometry::Config::multisensor_settings, - "Settings for Multisensor odometry mode. See :class:`Odometry.MultisensorSettings`"); + "Settings for Multisensor odometry mode. See :class:`Odometry.MultisensorSettings`") + .def_rw("min_depth", &Odometry::Config::min_depth, + "Minimum scene depth (meters) sampled along the epipolar curve for L2R tracking. " + "ONLY used in Multicamera mode. Any negative value (e.g. -1) auto-detects from baseline.") + .def_rw("max_depth", &Odometry::Config::max_depth, + "Maximum scene depth (meters) sampled along the epipolar curve for L2R tracking. " + "ONLY used in Multicamera mode. Any negative value (e.g. -1) auto-detects from baseline."); // Odometry::State binding nb::class_(odom_cls, "State", diff --git a/test_data/sof/lr_test/left/000000.png b/test_data/sof/lr_test/left/000000.png new file mode 100644 index 0000000..b769c2e --- /dev/null +++ b/test_data/sof/lr_test/left/000000.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:e2af4dccddc0cc929292cfed0bd32b440989e4b56118ac9fe06d06abb83fdbdc +size 219075 diff --git a/test_data/sof/lr_test/right/000000.png b/test_data/sof/lr_test/right/000000.png new file mode 100644 index 0000000..a6ba64b --- /dev/null +++ b/test_data/sof/lr_test/right/000000.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:0738b48a8de8de444be425223d7058a1dee8d153a64538fc150dc7180c6abf68 +size 223094 diff --git a/test_data/sof/lr_test/stereo.edex b/test_data/sof/lr_test/stereo.edex new file mode 100644 index 0000000..798d241 --- /dev/null +++ b/test_data/sof/lr_test/stereo.edex @@ -0,0 +1,63 @@ +[ + { + "version": "0.9", + "frame_start": 550, + "frame_end": 550, + "cameras": [ + { + "intrinsics": { + "distortion_model": "polynomial", + "distortion_params": [ + 5.018221378326416, + 1.4885437488555908, + -6.668913556495681e-05, + -1.7411422959412448e-05, + 0.020181948319077492, + 5.383248805999756, + 3.0275461673736572, + 0.19286677241325378 + ], + "focal": [ 283.5061950683594, 283.65777587890625 ], + "principal": [ 322.8827209472656, 195.5970001220703 ], + "size": [ 640, 400 ] + }, + "transform": [ + [ 1.0, 0.0, 0.0, 0.0 ], + [ 0.0, 1.0, 0.0, 0.0 ], + [ 0.0, 0.0, 1.0, 0.0 ] + ] + }, + { + "intrinsics": { + "distortion_model": "polynomial", + "distortion_params": [ + 6.822354316711426, + 2.40677809715271, + 3.5999903047923e-05, + -8.198195246222895e-06, + 0.0508991964161396, + 7.195554733276367, + 4.5601325035095215, + 0.37156370282173157 + ], + "focal": [ 284.34527587890625, 284.4118957519531 ], + "principal": [ 323.3560791015625, 193.4979705810547 ], + "size": [ 640, 400 ] + }, + "transform": [ + [ 0.999837, -0.005045, -0.017307, 0.0750007], + [ 0.005029, 0.999975, -0.000939, 0.0003090], + [ 0.017312, 0.000852, 0.999850, 0.0008054], + [ 0.000000, 0.000000, 0.000000, 1.000000] + ] + } + ] + }, + { + "fps": 60, + "sequence": [ + "left/000000.png", + "right/000000.png" + ] + } +]