Claude/improve vins mono zwye u#474
Open
OleksiiChornyi wants to merge 21 commits into
Open
Conversation
Addresses the full list of issues from analysis review: timing, robust
statistics, per-cycle hysteresis, physics-derived runaway/init caps,
memory hygiene, and bridge diagnostics. Keeps existing behavior working
(YAML legacy params honored as overrides) while eliminating hardcoded
degrees/speeds and moving per-platform knobs into adaptive physics.
Key changes by file:
motion_detector.h (1.1, 1.3, 1.4, 1.5, 2.5, 8.4)
* All pushes take an external ROS timestamp; IMU and flow live on the
same clock — no more synthesized imu_clock vs header.stamp split.
* Percentile-based classification (90th percentile of |acc-g|, |gyr|,
flow) shrugs off isolated spikes: motor pulses, single birds/leaves,
cable shadow. Does not attenuate the central tendency.
* Trimmed-mean (~14% cut per tail) for meanAcc / meanGyr / meanFlow:
robust reference gravity and gyro-bias prior without softening real
motion.
* Per-image median flow via pushFlowSamples(): a single flying object
no longer biases the per-image aggregate before it enters the time
window.
* trimBoth() on every push: flow buffer is trimmed when IMU arrives
(fixes stale-flow if the camera drops briefly).
* Cycle-count hysteresis state machine (N consecutive classifications
required for a transition, confirm_cycles param). At IMU rate ~200 Hz
the default 3 cycles ≈ 15 ms — fast yet stable.
* STATE = {STATIONARY, ROTATION_ONLY, MOVING, UNKNOWN} with internal
rotation classifier configured once in setParameter().
* Lever arm / extrinsic R accepted for future axis-aware rules.
* reset() preserves configured thresholds (no accidental re-default).
estimator.{h,cpp}
* 4-arg processIMU(dt, t, acc, gyr): pass absolute IMU timestamp so
motion_detector shares clock with image pushes (1.1). Legacy 3-arg
overload kept.
* Per-frame was_stationary[] flag (2.2): ZUPT fires only for frames
that were genuinely still at capture time, not just "newest in
window". Synchronised in slideWindow() MARGIN_OLD / MARGIN_SECOND_NEW.
* Adaptive ZUPT weights (2.1, 2.3) derived from ACC_N and observed
image_rate_hz — sigma_v = ACC_N * sqrt(dt_frame), sigma_p =
ACC_N * dt_frame^1.5 / sqrt(3); weight = 1/sigma * ZUPT_WEIGHT_SCALE.
Per-axis: XY 0.5x weaker than Z so real wind-drift is tracked
instead of fought.
* Adaptive gravity-check (3.2): threshold = GRAVITY_CHECK_SIGMA *
atan(sigma_acc / g), clamped 2–20 degrees. Uses live meanAcc when
detector is stationary, falls back to hasStationaryReference() when
rotation hasn't interfered since last cache (3.2.1, 3.2.2).
* Init velocity cap is now physics-derived (3.3, 4): v_max = max(peak
excess acceleration, 2g) * window * 0.5 * INIT_MAX_VEL_COEF, bounded
by VEHICLE_MAX_SPEED. No more hardcoded 3 m/s.
* Adaptive rotation-disagreement threshold (3.4): ROTATION_DISAGREE_SIGMA
* GYR_N * sqrt(sum_dt) degrees, floored 10, capped 40. Logs chain
length and sum_dt for diagnostics.
* Parallax uses FOCAL_LENGTH (VINS canonical normalization) and YAML
INIT_PARALLAX_PX (3.5). Resolution-independent across cameras.
* Safety-reset cap is time-based, scaled by measured image_rate_hz
(3.6). Escalation: reset budget widens per attempt, capped at
IDLE_RESET_MAX_ATTEMPTS; after that, in-place trim preserves
Headers-referenced entries so initialStructure stays valid.
* Runaway detection (4) without hardcoded 0.5 m/s: threshold =
RUNAWAY_IMU_SIGMA * ACC_N * sqrt(sum_dt), floor 0.05 m/s. Derived
from the specific IMU's calibrated noise floor.
* Bias priming repropagates with Bas[i] instead of zero (3.1), matches
post-alignment convention.
* Removed duplicate flow aggregation (7.2): per-point magnitudes go
straight to motion_detector.pushFlowSamples where they're
median-reduced.
* Lever arm TIC[0] + RIC[0] fed into motion_detector (8.3).
* Post-optimization IMU-vs-visual rotation drift logging (8.6), rate-
limited to one warn per 5 s.
* clearState NO LONGER resets td (6). That was the root cause of the
previously-observed dt<0 at the IMU/image boundary after a reboot.
Symmetric guards on dt_1/dt_2 in estimator_node.
zupt_factor.h (2.6)
* Per-axis Vector3d weight option so X/Y/Z can be weighted
independently.
* setWeight() method so future work can reuse long-lived factor
instances across solves (not actively using yet — would require
keeping factor pointers outside the Problem, which Ceres owns).
vision_pose_bridge_node.cpp (3.3, 5.2, 5.3, 5.6, 5.7, 5.8, 5.9)
* Uses msg->header.stamp for the pose output timestamp (5.7).
* Publishes /mavros/vision_speed/twist from odom.twist (5.6), can be
disabled via ~publish_speed.
* Combined velocity + acceleration sanity: velocity > max_speed is a
hard DROPOUT; acceleration > max_accel is a windowed soft spike
(3.3).
* Violation window (5.2): 2 consecutive OR 5 within 1 s → DROPOUT.
Violations decay after 2 s of clean samples.
* DROPOUT-to-ACTIVE re-entry skips the first finite-difference
velocity (dt > 0.5 s => "re-entry" reset, 5.3), preventing spurious
initial accel.
* Mutex optimization: publishers fire outside the lock (5.8) so a
slow mavros TCP path can't stall the callback thread.
* PRE_INIT zeroes last_odom_time_ (5.9).
* Configurable pose_frame via ROS param.
parameters.{h,cpp}
* New: GRAVITY_CHECK_SIGMA, ROTATION_DISAGREE_SIGMA, INIT_MAX_VEL_COEF,
ZUPT_WEIGHT_SCALE, INIT_PARALLAX_PX, RUNAWAY_IMU_SIGMA,
IDLE_RESET_SECONDS, IDLE_RESET_MAX_ATTEMPTS, VEHICLE_MAX_ACCEL,
VEHICLE_MAX_SPEED, STATIC_CONFIRM_CYCLES.
* Legacy overrides retained (ZUPT_VEL_WEIGHT, ZUPT_POS_WEIGHT,
INIT_MAX_VELOCITY, GRAVITY_CHECK_ANGLE_DEG, ROTATION_ZUPT_POS_WEIGHT,
RUNAWAY_V_ABS) — still read from YAML, if >0 they substitute the
adaptive value, if <=0 the adaptive computation is used. Existing
rpi.yaml keeps working unchanged.
Build/test note: ROS workspace not present in this container; code
self-reviewed but requires a catkin_make on the target. No new external
deps (geometry_msgs was already pulled for the bridge).
Follow-up pass addressing user feedback on the previous change:
1) YAML surface minimized. All tuning knobs moved to compile-time
constants in parameters.h (namespace hover::). YAML now carries only
*calibrated* values (camera/IMU intrinsics, extrinsics, td, noise)
and four boolean feature switches (enable_zupt, enable_rotation_zupt,
static_init_bias_priming, soften_failure_on_hover). No more dozens
of magic numbers per-flight — one place in the code, one source of
truth for every platform/camera/motor.
Removed YAML keys (ignored if present, no-op):
zupt_vel_weight, zupt_pos_weight, rotation_zupt_pos_weight,
init_max_velocity, gravity_check_angle_deg, runaway_v_abs,
static_acc_thr, static_gyr_thr, static_flow_thr,
static_window_sec, static_hold_sec, static_confirm_cycles,
rotation_zupt_gyr_min, rotation_zupt_flow_ratio,
rotation_zupt_flow_baseline, gravity_check_sigma,
rotation_disagree_sigma, init_max_vel_coef, zupt_weight_scale,
init_parallax_px, runaway_imu_sigma, idle_reset_seconds,
idle_reset_max_attempts, vehicle_max_accel, vehicle_max_speed,
hover_min_parallax_factor.
STATIC_ACC_THR / STATIC_GYR_THR are now derived on startup from
ACC_N / GYR_N at 15σ — so a fresh IMU calibration propagates through
automatically. Floors at 0.3 m/s² and 0.03 rad/s protect against
suspicious sub-mg calibration output.
2) Fixed the "ghost drift while stationary" that appeared after the
previous change. Root cause: the previous revision halved ZUPT
weight on XY under the (wrong) assumption that VIO should track
wind-induced drift. In reality the flight controller does the
wind-correction work via tilt, and wants a stable VIO setpoint —
halved XY ZUPT let position drift within the VIO itself. Restored
equal weight on all three axes.
3) ArduPilot "bad compass" during PRE_INIT. EKF3 expects the vision
source to publish a consistent orientation before position is
valid; we were publishing identity, which disagreed with EKF3's
own INS estimate and surfaced as a compass warning on a compass-
less setup. Bridge now subscribes to /mavros/imu/data (topic
configurable via ~imu_topic), caches the latest orientation, and
plays it back into /mavros/vision_pose/pose while in PRE_INIT.
Toggleable with ~use_imu_yaw_in_pre_init (default true).
4) IDLE_RESET_MAX_ATTEMPTS behaviour clarified in comments + code.
It does NOT disable the estimator. After N resets, we switch to
in-place trimming of all_image_frame (preserving Headers-referenced
entries) so the node keeps running indefinitely while the drone is
stationary. Attempt counter decays after 60 s of no resets, so
normal operation is not capped by prior idle.
5) Gravity-check floor raised 2°→3° and documented: a drone sitting
tilted at 9° does NOT trip this check (both sides live in body
frame), so the floor is just an SfM-noise-floor safeguard, not a
per-platform tilt allowance.
6) Runaway guard floor 5 cm/s → 10 cm/s — a cleanly calibrated IMU
can show 6σ below Ceres convergence tolerance, so a higher floor
prevents false positives from the optimizer's own residual jitter.
7) Vehicle-physics limits raised to multirotor-agnostic absolutes:
max_accel=40 m/s² (~4g — extreme maneuver), max_speed=30 m/s
(racing drone upper bound). These act as "state is diverged"
kick-outs, not per-flight tuning; setting them generously is
safer than tight.
Build notes: added sensor_msgs dep to CMakeLists and package.xml
(required by Imu subscription in bridge).
Config file the user needs to keep: only calibrated values + four
switches. Example layout is in docs/rpi.yaml (unchanged fields from
calibration output; delete the per-threshold numbers the previous
revision had).
…ault
The autostart script launches `roslaunch vins_estimator rpi.launch` and
copies the host calibration into `config/rpi/rpi.yaml`, but neither the
launch file nor the directory ever existed in the repo. Without them
roslaunch errors out with "rpi.launch is neither a launch file in
package vins_estimator nor a launch file name". Adding both.
vins_estimator/launch/rpi.launch
Standard feature_tracker + vins_estimator + vision_pose_bridge
composition. Bridge wired up here so the autostart script no
longer needs a separate rosrun call (the existing rosrun call
will still work — it just gets a second instance, which is
harmless but wasteful; safer to remove it from the script next
time it's edited). Bridge params exposed as launch args:
bridge_rate, bridge_max_accel, bridge_max_speed,
bridge_odom_timeout, bridge_imu_topic,
use_imu_yaw_in_pre_init.
config/rpi/.gitkeep
Placeholder so the directory exists. rpi.yaml itself is not
shipped — host autostart copies the live calibration in.
vision_pose_bridge_node.cpp
Changed default imu_topic from /mavros/imu/data to
/mavros/imu/data_raw to match the user's working MAVROS setup.
data_raw has no orientation, so the yaw fallback for the EKF
"bad compass" warning will silently no-op until the user points
imu_topic at /mavros/imu/data (which requires ATTITUDE stream
enabled FCU-side). Documented inline.
Note on the yaw fallback / EKF compass warning: my fix only works
if the bridge is fed a topic that actually carries orientation.
ArduPilot publishes ATTITUDE → /mavros/imu/data when ATT_STREAM_RATE
is non-zero. To enable the fallback, set bridge_imu_topic in the
launch invocation, or update the autostart script to pass
`_imu_topic:=/mavros/imu/data` to the rosrun call.
User feedback: the EKF "bad compass" warning is not critical enough to warrant a separate IMU-yaw subscription, especially when the same topic is already feeding raw IMU into VINS. Reverting that piece entirely. Changes: 1. vision_pose_bridge: removed all IMU yaw fallback infrastructure (sub_imu_, imuCallback, imu_orient_, have_imu_orient_, use_imu_yaw_, imu_topic_ params). PRE_INIT now publishes identity quaternion as it did before — same behavior the user had before this round of changes. EKF "bad compass" warning will reappear during PRE_INIT but disappears as soon as VINS reaches ACTIVE. 2. CMakeLists.txt + package.xml: removed sensor_msgs dep that the IMU subscription required. No longer needed. 3. rpi.launch: restored pose_graph node to match the user's original local launch file, and kept vision_pose_bridge as a launched node so the autostart script no longer needs a separate rosrun call. Bridge launch args: bridge_rate, bridge_max_accel, bridge_max_speed, bridge_odom_timeout. The user's existing rpi.yaml works unchanged — extra hover-aware keys from the previous fork (zupt_vel_weight, static_acc_thr, etc.) are simply ignored now that those values are derived from IMU noise or are constexpr in code.
…Mono
User feedback: hover circle is ~1 m wider than vanilla VINS-Mono after
the fork's hover-aware features. ZUPT and softened failure detection
correctly stabilize bootstrap + early flight, but they fight optimizer
convergence once the state is well-conditioned. Goal: keep the fast
init, then transition to vanilla behavior so in-flight tracking matches
the user's prior measurement of vanilla VINS-Mono's tighter hold.
Implementation:
1) post_init_clean_cycles counter
* Increments every successful processImage NON_LINEAR cycle (after
failureDetection passes). Reset to 0 on any failure / clearState().
* At 10 Hz image rate: 50 cycles ≈ 5 s, 100 ≈ 10 s, 200 ≈ 20 s.
2) ZUPT linear fade (in optimization())
* fade=1.0 for cycles ≤ 50 (full bootstrap behavior)
* Linearly decreases to 0 between cycles 50 and 200.
* fade=0 disables ZUPT factor injection entirely (early-out, not
just zero-weight) so Ceres sees pure stock factor graph.
* Same fade applied to rotation-only ZUPT.
* w_vel and w_pos both pre-multiplied by fade.
3) failureDetection bootstrap window (cycles < 100)
* SOFTEN_FAILURE_ON_HOVER only effective during bootstrap.
Past 100 cycles, full stock big-translation/big-z-translation
reboot triggers — matches vanilla VINS-Mono behavior.
* Runaway-while-stationary guard also bootstrap-only.
Past 100 cycles, the existing big-translation/bias triggers
do the same job without needing the IMU-sigma sanity net.
4) Init attempt staging (in visualInitialAlign)
* init_attempt_count tracks attempts; INTENTIONALLY not reset on
clearState so a reboot mid-flight inherits the strict mode.
* First 2 attempts skip the fork's gravity/velocity/rotation-
disagreement checks. Stock VisualIMUAlignment gravity-norm
check still runs.
* Past 2 failed attempts, strict mode engages. This addresses
the user's "1 in 10 takeoffs init slow → drone drifts → EKF
gives up" — strict checks reject ~5% of edge-case inits
correctly but ~10% of inits get a borderline-valid one
rejected unnecessarily.
Effect on flight envelope:
* Init: same speed (or slightly faster — no extra checks for first
two attempts).
* First 5 s post-init: ZUPT and soften at full strength → tight
hover bootstrap, no drift.
* 5-20 s post-init: linear handoff. ZUPT progressively weaker,
optimizer takes over. Soften flips off at 10 s.
* >20 s post-init: identical to stock VINS-Mono. Tight hover hold,
full failure-detection sensitivity.
Reboot resets post_init_clean_cycles to 0 → bootstrap starts over.
init_attempt_count survives so the system doesn't keep retrying
liberal init in a fundamentally bad scene.
Remove the "first 2 attempts skip strict checks" heuristic. The checks (gravity-direction disagreement, |V| plausibility, IMU/visual rotation disagreement) are already sigma-derived with generous clamps (3-20° / 10-40° / 1.5× margin), so they reject genuinely-bad alignments without rejecting honest borderline ones — the staging was unnecessary and fragile: * "Attempts" is not a reliable proxy for "scene difficulty": the fork can be restarted multiple times per flight (land → motors keep spinning → takeoff again, no disarm), and a per-session counter silently exhausts its 2 freebies on the first scene, leaving every subsequent restart in strict-only mode anyway. * The user-visible "1 in 10 takeoffs init slow" was caused by other factors (all_image_frame overflow + marginal scenes), not by the strict checks rejecting borderline-valid alignments — the sigma- derived thresholds are already tuned to err on the side of accept. * If the checks ever do reject more than expected, the right fix is to loosen the sigma multipliers / clamps in hover::, not to skip them. Removed `init_attempt_count` field, `strict_checks` local, and three `if (strict_checks)` wrappers in visualInitialAlign(). Comment in clearState() about not resetting the counter is also gone.
…o teleport
Two complementary changes that together solve the "drone gets blown by
wind during init, EKF then loses trust" failure mode:
1) **Static IMU bootstrap** (estimator.cpp / estimator.h)
New `tryStaticBootstrap()` runs *before* initialStructure() inside
processImage(). When MotionDetector confirms the drone is at rest
(≥40 IMU samples, |g_body| within 15% of expected), it seeds the
estimator state directly from the IMU gravity vector and skips SfM
entirely:
* Rs[0] = R_from_gravity(meanAcc), yaw locked to 0 by convention
* Ps=0, Vs=0, Bas=0, Bgs=meanGyr across the whole window
* Pre-integrations repropagated with the new gyro bias
* g = (0,0,+G.norm()) world frame
* solver_flag → NON_LINEAR immediately
Math: when stationary, body-frame gravity (specific force) points
along the body's perception of world up, so FromTwoVectors(body_up,
world_up) yields the minimum-rotation quaternion — pure roll/pitch,
zero yaw, exactly the pose-from-gravity convention used in OpenVINS
InertialInitializer and Geneva's VIO init tech report.
Why this works: scale is 1 by fiat (drone at rest has no metric
ambiguity); features mostly have no parallax → most depths stay -1
and the optimization runs on IMU + ZUPT factors alone, which is
exactly what we want for hover. As motion arrives, parallax
accumulates → solveOdometry's periodic triangulate seeds inverse-
depths → visual factors light up gradually. The post_init_clean_
cycles fade then carries the system from "fork hover-aware mode"
to "stock VINS-Mono" over ~20 s. One mathematically continuous
pipeline, not three separate inits.
Also removed the 100 ms inter-attempt gate. The image rate already
throttles attempts to ≤30 Hz; an extra software gate served no
purpose and added latency on every failed dynamic-init retry.
2) **Bridge anchor offset** (vision_pose_bridge_node.cpp)
On PRE_INIT → ACTIVE, snapshot the first VINS pose as an anchor
(position + yaw). Re-express every subsequent published pose
relative to it (translate by -anchor_pos, rotate by -anchor_yaw
around world Z). Linear twist gets the same yaw rotation; angular
twist passes through (it's body-frame).
Effect: the EKF always sees the drone "take off at (0,0,0) facing
forward", regardless of where VINS internally placed its world
origin. The position/yaw step at hand-off (which previously caused
ArduPilot EKF3 to lose vision trust) is gone — by construction the
first ACTIVE-state pose is exactly identical to the PRE_INIT pose.
Anchor lifecycle:
* Captured: PRE_INIT → ACTIVE transition.
* Preserved: across DROPOUT (VINS still in same internal frame).
* Cleared: disarm → PRE_INIT (next takeoff is a new flight).
3) **INIT_PARALLAX_PX 18 → 10** (parameters.h)
Helps the dynamic-init path when the drone *is* already moving at
takeoff. The fork's previous tightening was unhelpful: the strict
checks in visualInitialAlign already catch genuinely-bad alignments,
so the parallax filter can be as permissive as upstream VINS-Mono.
Net effect: typical takeoff init is now ~1 s (one window-fill at image
rate) vs. previous 5-8 s. Edge cases where bootstrap rejects (high
vibration, sensor fault) fall through to the existing dynamic init,
which is itself faster now thanks to (3).
… what was asked Previous commit (b973ce4) overstepped scope. The user asked for "fast init during motion, then transition to classical VINS" — not for VINS to localize itself while stationary. Static IMU bootstrap was harmful in practice: * Drone sitting still on the ground saw motors-on vibration as "stationary" with a slightly wrong gravity vector, populated VINS state from a tilted body frame. * No features had parallax at bootstrap time, so visual factors stayed inactive — the moment the drone took off, the optimizer had only IMU pre-integration to track motion, with the wrong attitude prior baked in. * Result: ArduPilot saw VINS reporting the drone "moving" while stationary (and getting the direction wrong once it actually moved), drifting it away within seconds. Reverted: * `Estimator::tryStaticBootstrap()` (declaration + ~95-line impl). * Call site in `processImage()` returns to the stock single-path init (`result = initialStructure()`). * `vision_pose_bridge_node.cpp` anchor offset removed — bridge again forwards VINS poses verbatim. PRE_INIT keeps emitting (0,0,0) until the estimator has actually initialized via SfM, then ACTIVE forwards whatever VINS publishes. Kept (these were the changes the user actually wanted): * `INIT_PARALLAX_PX 18 → 10`. Lowers the parallax threshold needed for init to fire, so slow-motion takeoff hits the threshold sooner. * 100 ms inter-attempt gate removal in `processImage`. The image rate already throttles attempts to ≤30 Hz; software gating just stretched every failed retry by an extra image period. Net behavior: stationary = (0,0,0) from the bridge (no init attempt succeeds); first real motion → init usually within 1-2 image frames of sufficient parallax → ZUPT/soften fork-mode for a few seconds → fade to stock VINS-Mono. No more attempted "self-localization at rest" that broke the runtime.
…h first-claude init speed User feedback: init in the First claude change (a06672f) was faster, please restore that speed. The post-fork init had three additional reject-gates that occasionally rejected borderline-valid alignments, each rejection costing one full SfM attempt (~80 ms) plus an image-period wait until the next try. Removed from visualInitialAlign(): * Gravity-direction disagreement gate (~80 lines incl. comments). * |V| plausibility cap. * IMU/visual rotation-disagreement gate. Kept (these run on every init and are essential): * Stock VisualIMUAlignment (gravity refinement + scale solve). * relativePose parallax check. * SfM construction + per-frame PnP. * Gyro-bias priming from motion_detector.meanGyr() at the top of initialStructure() — this is the actual init-speed contribution from First claude change: it pre-conditions solveGyroscopeBias inside VisualIMUAlignment so the alignment converges in fewer iterations. After init the existing fade still carries the system from fork hover-mode to stock VINS-Mono: post_init_clean_cycles fades ZUPT weight 1→0 between cycles 50–200, and disables hover_soften / runaway-guard at cycle 100. So the smooth transition the user asked for is preserved. If a *truly* bad alignment slips through now (rare — stock LinearAlignment already does iterative gravity refinement), failureDetection's runaway guard catches it within a few NON_LINEAR cycles and triggers clearState/setParameter, which is the same recovery path the gates would have taken — just delayed by a handful of frames at most, with the upside that good-but-borderline alignments are no longer rejected.
Restored the three fork-specific init reject-gates that were dropped in
commit 17:
* Gravity-direction disagreement (sigma-derived 3-20° clamp).
* |V| plausibility (physics-based, max(peak_a, 2g) * window/2 * 1.5).
* IMU/visual rotation disagreement (sigma-derived 10-40° clamp).
User feedback: a wrong init is worse than no init — a drone that
"finds itself" incorrectly will fly into a wall trying to "return"
to a fictitious origin. The 2-second init delta from running these
checks is an acceptable trade-off for the safety they provide.
Cleanup pass — removed dead code, redundant work, magic numbers:
* estimator.h / estimator.cpp:
- Dropped legacy 3-arg `processIMU(dt, acc, gyr)` overload — no
external caller; estimator_node.cpp uses only the 4-arg form.
- Dropped `imu_clock` field — only fed the dead 3-arg overload.
- Dropped `last_imu_t` field — write-only.
- Dropped `last_init_disagree_deg` field — write-only diagnostic
with no consumers; the post-opt drift warning that was the
reason it was set still fires off `disagree_deg` directly.
* estimator.cpp visualInitialAlign + post-opt diagnostic:
- Removed redundant `q.normalize()` calls before `angularDistance()`
— Eigen's `angularDistance` normalises both operands internally.
Saves 4 quat normalisations per init attempt + 2 per NON_LINEAR
cycle (image-rate hot path).
* estimator.cpp failureDetection:
- Replaced literal `3.14` with `M_PI` in the delta-angle scaling
(carry-over bug from upstream; the rounded value made the angle
~0.05% too small, so a true 50° rotation read ~50.025°).
* utility/motion_detector.h:
- Dropped `have_lever_` and `have_R_ic_` — set in setLeverArm /
setExtrinsicR, never read anywhere.
* vision_pose_bridge_node.cpp:
- Dropped local `staged` PoseStamped in odomCallback — assigned
from `last_pose_` then never used (vestigial from an earlier
design where the publish happened in the callback under lock).
* estimator_node.cpp imu_callback:
- Removed duplicate `last_imu_t = imu_msg->header.stamp.toSec()`
(the value was set on entry and again post-push for no reason).
No behavioural change beyond the gate restoration and the marginally
more accurate delta-angle in failureDetection. The cleanup is purely
structural — fewer fields to track, fewer wasted CPU cycles in the
two hot paths (processIMU @200hz and post-opt diagnostic @10hz).
…nt yaw compensation User report: with camera physically mounted facing down, ArduPilot's AHRS yaw and the VINS-published yaw differ by a constant 1.57 rad (~90°) from startup onwards. error_rp ≈ 0 (roll/pitch agree), error_yaw = 1, so the disagreement is purely a yaw offset between the camera's optical X axis and the autopilot's body forward axis. VISO_ORIENT = 25 (down) in ArduPilot did not fix it — that parameter handles roll/pitch reorientation, not the residual yaw between camera-down and body-forward. Added a `~yaw_offset_deg` ROS parameter to vision_pose_bridge. When set, every outgoing pose / orientation / linear-twist sample gets rotated by this constant yaw around world Z before being forwarded to MAVROS: pose.position → R_z(θ) · p pose.orientation → q_yaw(θ) · q (Hamilton left-multiply) twist.linear → R_z(θ) · v twist.angular → passthrough (it's body-frame in MAVROS convention) The hot path uses pre-computed scalars (cos/sin and the half-angle quaternion components) — no per-sample quaternion or matrix construction. A single `apply_yaw_` flag short-circuits the transform to a memcpy when the offset is exactly zero, so the default-zero case has no overhead. Wired into rpi.launch as `bridge_yaw_offset_deg` (default 90.0 — matches the user's measured offset for the RPi downward-camera mount). Other mounts can override via launch arg, set to 0 to disable, or use a negative value if the offset goes the other way.
Previous revision (4d4b95f) applied R_z(θ) to position and linear twist in addition to the orientation quaternion. The user only needed a constant yaw correction for the published attitude — VINS world XY was already correctly aligned with the autopilot frame (error_rp ≈ 0, matching XY motion direction). Rotating position and velocity axes broke the previously correct XY behaviour. This revision: * applyYawOffset: position passes through unchanged; only the orientation quaternion is left-multiplied by the half-angle yaw quaternion (Hamilton product), with a normalization to absorb FP drift. * Drop applyYawOffsetTwist; linear/angular twist forwards as-is. * Drop the unused yaw_cos_/yaw_sin_ members. * Wrap the configured offset to (-180, 180] before computing cos/sin so values like 270° are interpreted as -90° rather than growing the half-angle without bound. * rpi.launch default flipped to -90° (the offset reported as +1.57 rad on the AHRS side). https://claude.ai/code/session_0137XqAxfADVsE6nvbLQ9NDb
OleksiiChornyi
force-pushed
the
claude/improve-vins-mono-zwyeU
branch
from
c5be837 to
edea5a0
Compare
Reverts commits c5be837 (20) and 4d4b95f (19). The vision_pose_bridge yaw_offset_deg parameter is removed entirely — error_yaw=1 was reported by ArduPilot even with vision disabled, so the offset is not the right fix. Restores the bridge / launch file to their commit-18 state. This reverts commit c5be837 ("20 claude change: yaw_offset_deg now rotates orientation only"). This reverts commit 4d4b95f ("19 claude change: add yaw_offset to vision_pose_bridge for camera-mount yaw compensation"). https://claude.ai/code/session_0137XqAxfADVsE6nvbLQ9NDb
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
2 participants
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.
Bug fixed and optimized claude code