feat(fastlivo): FAST-LIVO2 odometry module - #3224
Draft
jeff-hykin wants to merge 8 commits into
Draft
Conversation
…eams Runs unmodified upstream FAST-LIVO2 (pinned rev) under a fake-ROS shim header layer: LCM lidar (per-point offset_time)/imu/color_image/camera_info are converted to shim ROS messages and fed into LIVMapper's own callbacks; its odometry publish is hooked back onto LCM, re-stamped from the EKF update time so replayed data keeps its own clock domain. Camera model is built at runtime from the camera_info stream (plumb_bob/equidistant), no camera hardcoding. Extrinsic defaults for the Mid-360 + RealSense rig are derived from the assembly FRAMES (scripts/compute_extrinsics.py). Also: replay_eval.py (db -> LCM feed + odom capture, with IMU uptime-clock fit and stray image stamp-domain repair), compare_trajectories.py (ATE vs reference incl. wall-stamped-estimate time-offset recovery) + tests, and the mid360_realsense_fastlivo blueprint.
…ustness - Defaults = the best full-sequence config (ATE 6.16m vs gtsam reference on huge_loop_realsense; beats the recording's live fastlio at 6.71m): voxel_size 1.0, min_eigen_value 0.005, filter_size_surf 0.2, point_filter_num 3, acc/gyr_cov 0.5/0.3, max_layer 2, map sliding on. - Glue-side visual-map pruning (visual_map_radius): upstream only frees the VIO feature map in its destructor; hour-long runs otherwise grow without bound. Erased between estimation cycles via the public member — upstream still compiled unmodified. - img_stride option; CLI camera model support (recordings without a camera_info stream) incl. distortion coefficients; replay_eval handles missing camera_info streams and repairs foreign-clock image stamps.
…ock-domain and bad-stamp hardening huge_loop_go2 campaign: LIO mode with acc_cov 2.0 / gyr_cov 1.0 ties Point-LIO (13.31m vs 12.95m; live fastlio 81.4m) — the visual updates, not the LIO, are what diverge under legged vibration (373m with camera on). The same high covs hurt on the stable handheld recording (7.68m vs 6.16m), so defaults stay tuned for stable platforms.
…gyr_cov 1.0) Per Jeff: robust-on-legged-platforms defaults; stable-platform values documented on the fields.
…on gating, map export, transport hardening Detector: capture-window peak gyro (primary; blur+jello are rotation- driven) AND-fused with a row-strip shift-variance content check (5 strip profiles phase-correlated against the previous frame; rolling-shutter jello moves top/bottom differently — go2 0.7px at rest vs 13px shaking, ~0.2ms/frame). Laplacian-sharpness metrics cannot detect this damage (jello INFLATES sharpness, corr +0.54 with gyro) — kept only as an optional defocus check, default off. Throttled operator warning on low quality (<=1 per 5s). Soft gating: flagged frames still feed sync_packages (LIVO mode requires images to advance — hard-dropping diverges) but their visual influence is divided by img_bad_frame_penalty via the live vio_manager->img_point_cov. Gated realsense full run: 6.052m ATE (ungated 6.155m) with 1.3% of frames down-weighted. On the go2's continuously-damaged rolling-shutter camera no LIVO configuration survives (6 documented variants diverge); the detector's verdict there is vision-off, and LIO mode scores 9.775m — beating Point-LIO's 12.949m on the same recording. Also: registered-map export (map_out PLY, voxel-deduplicated, from the /cloud_registered hook); LCM buffering guidance — with the default 2MB rcvbuf a transient processing lag silently drops frames (the estimator then NaN-diverges); run with recv_buf_size>=200MB in LCM_DEFAULT_URL and raised net.core.rmem_max on lag-prone hosts.
…os-module-fastlivo cpp/ now holds only a thin flake that pins the module repo and re-exports fastlivo_native; sources moved verbatim (rebuilt binary is bit-identical). module.py build_command/cwd unchanged.
…corded dbs from a raw pcap
…ency (RTT/2), first-packet median locks the offset, stamps published in system time (host_time_sync, default on)
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
FAST-LIVO2 draft (from a few weeks ago)