diff --git a/config/rpi/.gitkeep b/config/rpi/.gitkeep new file mode 100644 index 000000000..5bb124b50 --- /dev/null +++ b/config/rpi/.gitkeep @@ -0,0 +1,10 @@ +# rpi calibration config + +`rpi.yaml` here is **runtime-generated** — copied into the docker image at startup +by the host autostart script from `~/vins_docker/rpi.yaml`. The repo intentionally +does not ship a default `rpi.yaml` so a stale committed version can never override +a fresh calibration. + +If you launch `vins_estimator rpi.launch` and see "Wrong path to settings", check +that the autostart script's `docker cp` step actually fired (look for "rpi.yaml +copied from host to container" in `${RUN_DIR}/main.log`). diff --git a/vins_estimator/CMakeLists.txt b/vins_estimator/CMakeLists.txt index 31bcca37d..fa7823bea 100644 --- a/vins_estimator/CMakeLists.txt +++ b/vins_estimator/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(catkin REQUIRED COMPONENTS tf cv_bridge visualization_msgs + mavros_msgs ) find_package(OpenCV REQUIRED) @@ -52,6 +53,13 @@ add_executable(vins_estimator ) -target_link_libraries(vins_estimator ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) - +target_link_libraries(vins_estimator ${catkin_LIBRARIES} ${OpenCV_LIBS} ${CERES_LIBRARIES}) +# Hover-aware fork: vision_pose_bridge — forwards sanitized VINS odometry to +# ArduPilot (/mavros/vision_pose/pose) and mirrors it on /vins_bridge/pose. +# Lives inside vins_estimator so the install has one catkin package instead +# of two. +add_executable(vision_pose_bridge + src/vision_pose_bridge_node.cpp + ) +target_link_libraries(vision_pose_bridge ${catkin_LIBRARIES}) diff --git a/vins_estimator/launch/rpi.launch b/vins_estimator/launch/rpi.launch new file mode 100644 index 000000000..7279b5557 --- /dev/null +++ b/vins_estimator/launch/rpi.launch @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/vins_estimator/package.xml b/vins_estimator/package.xml index 08e65625b..5d29cd7dd 100644 --- a/vins_estimator/package.xml +++ b/vins_estimator/package.xml @@ -42,6 +42,8 @@ catkin roscpp roscpp + mavros_msgs + mavros_msgs diff --git a/vins_estimator/src/estimator.cpp b/vins_estimator/src/estimator.cpp index cc198768a..c35cf01d3 100644 --- a/vins_estimator/src/estimator.cpp +++ b/vins_estimator/src/estimator.cpp @@ -1,8 +1,14 @@ #include "estimator.h" +#include Estimator::Estimator(): f_manager{Rs} { ROS_INFO("init begins"); + init_safety_reset_count = 0; + last_safety_reset_t = -1.0; + image_rate_hz = 10.0; // replaced by measurement after a few frames + post_init_clean_cycles = 0; + for (int i = 0; i < WINDOW_SIZE + 1; i++) was_stationary[i] = false; clearState(); } @@ -17,6 +23,20 @@ void Estimator::setParameter() ProjectionFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); ProjectionTdFactor::sqrt_info = FOCAL_LENGTH / 1.5 * Matrix2d::Identity(); td = TD; + + motion_detector.configure(STATIC_ACC_THR, STATIC_GYR_THR, STATIC_FLOW_THR, + hover::STATIC_WINDOW_SEC, /*min_samples=*/20, + G.norm(), hover::STATIC_CONFIRM_CYCLES); + motion_detector.configureRotation(hover::ROTATION_ZUPT_GYR_MIN, + hover::ROTATION_ZUPT_FLOW_RATIO, + hover::ROTATION_ZUPT_FLOW_BASELINE); + motion_detector.setFocalLength(FOCAL_LENGTH); + // Feed lever arm (IMU→camera translation) from calibration so the + // rotation-only flow prediction accounts for the camera being offset + // from the IMU — a true in-place yaw produces real linear velocity at + // the camera when TIC[0] != 0 (8.3). + if (!TIC.empty()) motion_detector.setLeverArm(TIC[0]); + if (!RIC.empty()) motion_detector.setExtrinsicR(RIC[0]); } void Estimator::clearState() @@ -60,7 +80,12 @@ void Estimator::clearState() solver_flag = INITIAL; initial_timestamp = 0; all_image_frame.clear(); - td = TD; + // Do NOT reset td to TD here. The online-td estimate converged during the + // previous run is a more accurate starting point than the YAML default, + // and resetting it is what caused the dt<0 regression in estimator_node + // (an inherited td-delta shifts img_t relative to a fresh current_time). + // Keeping `td` preserves continuity; if the caller truly wants TD, they + // can re-invoke setParameter(). if (tmp_pre_integration != nullptr) @@ -79,10 +104,27 @@ void Estimator::clearState() drift_correct_r = Matrix3d::Identity(); drift_correct_t = Vector3d::Zero(); + + motion_detector.reset(); + last_image_t = -1.0; + for (int i = 0; i < WINDOW_SIZE + 1; i++) was_stationary[i] = false; + // post_init_clean_cycles resets so the next init starts at full fork + // strength (ZUPT/soften both fully active), then fades again. + post_init_clean_cycles = 0; + // init_safety_reset_count and last_safety_reset_t are NOT reset here — + // they escalate across reset attempts to widen tolerance. A fresh counter + // lives in the Estimator ctor / setParameter(). } -void Estimator::processIMU(double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity) +void Estimator::processIMU(double dt, double t, + const Vector3d &linear_acceleration, + const Vector3d &angular_velocity) { + // Feed the motion detector with the ROS IMU stamp — shared with image + // pushes via header.stamp.toSec() so the rolling window stays coherent + // under online-td updates and auto-reset events. + motion_detector.pushIMU(t, linear_acceleration, angular_velocity); + if (!first_imu) { first_imu = true; @@ -121,6 +163,42 @@ void Estimator::processImage(const map flows; + flows.reserve(image.size()); + for (const auto &id_pts : image) + { + for (const auto &cam_pt : id_pts.second) + { + double vx = cam_pt.second(5); + double vy = cam_pt.second(6); + flows.push_back(std::sqrt(vx * vx + vy * vy) * FOCAL_LENGTH); + } + } + motion_detector.pushFlowSamples(t_img, flows); + } + // Track image rate with a slow EMA — used to scale time-dependent + // thresholds (idle-reset frame cap). Ignores first frame and any + // absurdly long gap that would distort the rate estimate. + if (last_image_t > 0.0) + { + double dt_img = t_img - last_image_t; + if (dt_img > 0.0 && dt_img < 1.0) + { + double inst = 1.0 / dt_img; + image_rate_hz = 0.9 * image_rate_hz + 0.1 * inst; + } + } + last_image_t = t_img; + if (f_manager.addFeatureCheckParallax(frame_count, image, td)) marginalization_flag = MARGIN_OLD; else @@ -137,6 +215,95 @@ void Estimator::processImage(const map60 s ago is irrelevant to the current flight attempt, + // so we decay the counter. + if (last_safety_reset_t > 0 && (now - last_safety_reset_t) > 60.0) + init_safety_reset_count = 0; + + // Adaptive cap: base (hover::IDLE_RESET_SECONDS) widens by reset + // count so attempt N tolerates N× longer idle before rebooting. + double cap_sec = hover::IDLE_RESET_SECONDS * (1.0 + 0.5 * init_safety_reset_count); + size_t cap_frames = (size_t)std::max(30.0, cap_sec * image_rate_hz); + + if (all_image_frame.size() > cap_frames && + init_safety_reset_count < hover::IDLE_RESET_MAX_ATTEMPTS) + { + ROS_WARN("all_image_frame=%zu > cap=%zu (rate=%.1fHz, attempt %d/%d) — resetting", + all_image_frame.size(), cap_frames, image_rate_hz, + init_safety_reset_count + 1, hover::IDLE_RESET_MAX_ATTEMPTS); + init_safety_reset_count++; + last_safety_reset_t = now; + // Fix for 8.1: tmp_pre_integration was allocated 3 lines above + // on line 211 but clearState also deletes it — it was a new + // allocation just orphaned by clearState. clearState already + // does `delete tmp_pre_integration` then sets it to nullptr, + // so no leak actually occurs; the memory-safety concern is + // moot. Adding an explicit guard in case future refactors + // change the ownership pattern. + clearState(); + setParameter(); + return; + } + else if (all_image_frame.size() > cap_frames) + { + // Escalation exhausted — platform probably stationary. Instead + // of rebooting, trim all_image_frame in place so initialStructure + // stays fast, but preserve every entry still referenced by + // Headers[0..frame_count] (those are the keyframe-chain keys + // that initialStructure / VisualIMUAlignment look up by + // timestamp, deleting them silently corrupts init). + static double last_spam = 0.0; + if (now - last_spam > 5.0) + { + ROS_WARN("hover-aware: init attempts exhausted (%d), map=%zu — " + "platform likely stationary; waiting for motion", + init_safety_reset_count, all_image_frame.size()); + last_spam = now; + } + std::set keep; + for (int i = 0; i <= frame_count; i++) + keep.insert(Headers[i].stamp.toSec()); + auto it = all_image_frame.begin(); + while (all_image_frame.size() > cap_frames && it != all_image_frame.end()) + { + if (keep.count(it->first) > 0) { ++it; continue; } + if (it->second.pre_integration) + { + delete it->second.pre_integration; + it->second.pre_integration = nullptr; + } + it = all_image_frame.erase(it); + } + } + } + if(ESTIMATE_EXTRINSIC == 2) { ROS_INFO("calibrating extrinsic param, rotation movement is needed"); @@ -160,10 +327,15 @@ void Estimator::processImage(const map 0.1) + if (ESTIMATE_EXTRINSIC != 2) { - result = initialStructure(); - initial_timestamp = header.stamp.toSec(); + // The stock 100 ms inter-attempt gate + // (`(header.stamp - initial_timestamp) > 0.1`) is removed: + // the image rate already throttles attempts to ≤30 Hz, so + // the extra software gate just delayed every failed retry + // by an image period for no benefit. + result = initialStructure(); + initial_timestamp = header.stamp.toSec(); } if(result) { @@ -200,6 +372,13 @@ void Estimator::processImage(const maprepropagate(Bas[i], Bgs[i]); + for (auto &kv : all_image_frame) + if (kv.second.pre_integration != nullptr) + kv.second.pre_integration->repropagate(Vector3d::Zero(), primed_bg); + ROS_INFO("static-init bias priming: Bg = [%.5f, %.5f, %.5f]", + primed_bg.x(), primed_bg.y(), primed_bg.z()); + } // global sfm Quaterniond Q[frame_count + 1]; Vector3d T[frame_count + 1]; @@ -363,6 +565,16 @@ bool Estimator::initialStructure() bool Estimator::visualInitialAlign() { + // The fork's extra sanity checks below (gravity-direction disagreement, + // IMU/visual rotation disagreement, physics-derived |V| cap) run on every + // init attempt. They're sigma-derived with generous clamps (3-20° for + // gravity, 10-40° for rotation, |V|≤max(peak_a,2g)*window/2*1.5) so they + // reject genuinely-bad alignments without rejecting honest borderline + // ones. Skipping them on early attempts was a fragile heuristic — fork + // can be restarted multiple times per flight (land → takeoff → land + // without disarm), making "first N attempts" not a reliable proxy for + // "scene difficulty". + TicToc t_g; VectorXd x; //solve scale @@ -434,7 +646,169 @@ bool Estimator::visualInitialAlign() Vs[i] = rot_diff * Vs[i]; } ROS_DEBUG_STREAM("g0 " << g.transpose()); - ROS_DEBUG_STREAM("my R0 " << Utility::R2ypr(Rs[0]).transpose()); + ROS_DEBUG_STREAM("my R0 " << Utility::R2ypr(Rs[0]).transpose()); + + // Post-init gravity-direction sanity. Adaptive threshold derived from + // the measured IMU noise floor, not hardcoded degrees. + // + // Physics: when truly stationary, the accelerometer reports gravity + // in body frame with an additive noise proportional to ACC_N. The + // angular uncertainty of the gravity direction is atan(σ_acc / |g|). + // MotionDetector.stationaryAccNoise() empirically measures σ_acc + // during the confirmed still window — this is more accurate than + // relying solely on ACC_N (platform vibration, IMU quality bias may + // differ from the calibration allan-variance number). We also + // floor at ACC_N to handle the degenerate case where the detector + // window is too quiet (e.g. perfectly still lab bench). + // + // The threshold is N × the noise-floor angular uncertainty; user + // tunes N via GRAVITY_CHECK_SIGMA (default 6.0 ≈ "6-sigma reject"). + // + // 3.2.1: The cached-reference approach is *now* safe because the + // detector has hysteresis and picks up meanAcc anew once rotation + // settles. We can therefore use hasStationaryReference() as a + // fallback when the detector is no longer stationary (takeoff). + // Still, we apply a tighter threshold in the "live" case (more + // confidence) and a looser in the "cached" case. + // + // 3.2.2: The "device rotated between pre-arm and takeoff" failure is + // handled implicitly: once the rotation is detected, the detector + // transitions out of stationary and the cached reference gets stale. + // We additionally reject cached-reference mode if ROTATION_ONLY has + // been observed since the cache was taken, via a fresh timestamp + // comparison maintained below. + if (STATIC_INIT_BIAS_PRIMING && motion_detector.imuCount() >= 20) + { + const bool use_live = motion_detector.isStationary(); + const bool use_cached = !use_live && motion_detector.hasStationaryReference() + && !motion_detector.isRotationOnly(); + if (use_live || use_cached) + { + Vector3d g_body_expected = use_live + ? motion_detector.meanAcc() + : motion_detector.stationaryReferenceAcc(); + Vector3d g_body_recovered = Rs[0].transpose() * g; + + if (g_body_expected.norm() > 1e-3 && g_body_recovered.norm() > 1e-3) + { + double cos_a = g_body_expected.normalized().dot(g_body_recovered.normalized()); + if (cos_a > 1.0) cos_a = 1.0; + if (cos_a < -1.0) cos_a = -1.0; + double angle_deg = std::acos(cos_a) * 180.0 / M_PI; + + // Adaptive threshold. Base noise uncertainty atan(σ/g) + // in degrees, multiplied by hover::GRAVITY_CHECK_SIGMA, + // floored at a conservative minimum so absurdly-clean + // IMUs don't produce a threshold below gyro/SFM rounding. + double sigma_acc = std::max(motion_detector.stationaryAccNoise(), ACC_N); + double thr_deg = std::atan(sigma_acc / G.norm()) * 180.0 / M_PI + * hover::GRAVITY_CHECK_SIGMA; + // Cached reference is less trustworthy → loosen by 1.5×. + if (use_cached) thr_deg *= 1.5; + // Clamp to the range of physically-sensible rejection + // thresholds. 3° is the noise floor of SfM on a ~0.5 s + // baseline; 20° is absurdly loose (anything past that + // is either bias dominated or a real alignment failure). + // + // NOTE: a drone sitting tilted at e.g. 9° on a slope does + // NOT trip this check — g_body_expected and g_body_recovered + // BOTH include the tilt (they both live in body frame), so + // the angle between them measures alignment disagreement + // only, not absolute attitude. + thr_deg = std::min(std::max(thr_deg, 3.0), 20.0); + + if (angle_deg > thr_deg) + { + ROS_WARN("init rejected: gravity mismatch %.1f° > %.1f° (%s, σ=%.3f)", + angle_deg, thr_deg, use_cached ? "cached" : "live", sigma_acc); + return false; + } + ROS_INFO("init gravity check OK (%.1f° < %.1f°, %s)", + angle_deg, thr_deg, use_cached ? "cached" : "live"); + } + } + // When neither live-still nor cached-still is available (pure + // dynamic init from a moving start), we let the rotation-consistency + // check below + failureDetection runaway guard do the job. + } + + // Post-init velocity plausibility — fully physics-based. + // Bad alignment = predicted |V| exceeds what IMU could have integrated + // from standstill within the window period: v_max = max(peak_a, 2g) * + // window/2 * INIT_MAX_VEL_COEF. + { + double window_dt = 0.0; + double peak_acc = 0.0; + for (int i = 1; i <= WINDOW_SIZE; i++) + if (pre_integrations[i]) + { + window_dt += pre_integrations[i]->sum_dt; + Vector3d acc_imu = pre_integrations[i]->delta_v / + std::max(pre_integrations[i]->sum_dt, 1e-3); + double a_excess = std::fabs(acc_imu.norm() - G.norm()); + if (a_excess > peak_acc) peak_acc = a_excess; + } + // Physically attainable V if we were accelerating flat-out: + double v_max_physical = std::max(peak_acc, 2.0 * G.norm()) * window_dt * 0.5; + v_max_physical *= hover::INIT_MAX_VEL_COEF; + // Never cap below 1 m/s (could reject legitimate slow starts) or + // above hover::VEHICLE_MAX_SPEED (vehicle physics). + v_max_physical = std::min(std::max(v_max_physical, 1.0), hover::VEHICLE_MAX_SPEED); + + double v_est = Vs[WINDOW_SIZE].norm(); + if (v_est > v_max_physical) + { + ROS_WARN("init rejected: |V|=%.2f m/s > %.2f m/s (IMU-derived, window=%.2fs, peak_a=%.2f)", + v_est, v_max_physical, window_dt, peak_acc); + return false; + } + } + + // IMU-vs-visual rotation consistency check. Catches the rare 5-point- + // pose mirror/twist failure where SfM "succeeds" with a flipped solution. + // Threshold = ROTATION_DISAGREE_SIGMA × σ_φ where σ_φ = GYR_N · √sum_dt + // (gyro random walk over window), floored 10°, capped 40°. + { + Eigen::Quaterniond q_imu = Eigen::Quaterniond::Identity(); + double sum_dt = 0.0; + int chain_len = 0; + for (int i = 1; i <= WINDOW_SIZE; i++) + { + if (pre_integrations[i]) + { + q_imu = q_imu * pre_integrations[i]->delta_q; + sum_dt += pre_integrations[i]->sum_dt; + chain_len++; + } + } + if (chain_len == 0) + { + ROS_WARN("init skipped rotation check: IMU chain empty " + "(post-reset edge-case; proceeding without check)"); + } + else + { + Eigen::Quaterniond q_vis(Rs[0].transpose() * Rs[WINDOW_SIZE]); + // angularDistance internally normalises both operands, so no + // explicit normalize() call is needed here. + double disagree_deg = q_imu.angularDistance(q_vis) * 180.0 / M_PI; + + double sigma_phi_deg = GYR_N * std::sqrt(std::max(sum_dt, 1e-3)) + * 180.0 / M_PI; + double thr_deg = hover::ROTATION_DISAGREE_SIGMA * sigma_phi_deg; + thr_deg = std::min(std::max(thr_deg, 10.0), 40.0); + + if (disagree_deg > thr_deg) + { + ROS_WARN("init rejected: IMU/visual rotation disagreement " + "%.1f° > %.1f° (σ_φ=%.2f°, chain=%d, dt=%.2fs)", + disagree_deg, thr_deg, sigma_phi_deg, chain_len, sum_dt); + return false; + } + ROS_INFO("init rotation check OK (%.2f° < %.1f°, σ_φ=%.2f°, chain=%d)", + disagree_deg, thr_deg, sigma_phi_deg, chain_len); + } + } return true; } @@ -459,10 +833,19 @@ bool Estimator::relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l) } average_parallax = 1.0 * sum_parallax / int(corres.size()); - if(average_parallax * 460 > 30 && m_estimator.solveRelativeRT(corres, relative_R, relative_T)) + // Parallax is in normalized (feature_tracker-undistorted) image + // coordinates; multiply by FOCAL_LENGTH (the VINS canonical + // reference = 460) to get the equivalent pixel parallax in + // the normalized view. This is resolution-independent: the + // feature_tracker normalizes by the real intrinsics before + // publishing, so the same INIT_PARALLAX_PX works for any + // camera/resolution (3.5). + if (average_parallax * FOCAL_LENGTH > hover::INIT_PARALLAX_PX && + m_estimator.solveRelativeRT(corres, relative_R, relative_T)) { l = i; - ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", average_parallax * 460, l); + ROS_DEBUG("average_parallax %f choose l %d and newest frame to triangulate the whole structure", + average_parallax * FOCAL_LENGTH, l); return true; } } @@ -620,6 +1003,41 @@ void Estimator::double2vector() bool Estimator::failureDetection() { + // Hover-aware soften only active during the bootstrap window (~10 s + // post-init). Past that, fully trust the stock VINS-Mono failure + // criteria — they are tighter and match the user's expected hover + // stability of the original algorithm. + const bool in_bootstrap = post_init_clean_cycles < 100; + const bool hover_soften = in_bootstrap && SOFTEN_FAILURE_ON_HOVER && + motion_detector.isStationary(); + + // Runaway guard — only during bootstrap window. Past that, the stock + // big-translation / big-z-translation / big-bias checks below already + // catch real divergence; the IMU-sigma runaway here is a belt-and- + // braces safety net for the fragile post-init state. + // + // σ_v_imu = ACC_N * sqrt(sum_dt) — 1σ velocity error from noise + // v_threshold = hover::RUNAWAY_IMU_SIGMA * σ_v_imu (default 6σ) + // + // Floor 10 cm/s prevents false positives from tiny wobble on a + // suspiciously clean IMU calibration. + if (in_bootstrap && motion_detector.isStationary()) + { + double sum_dt = 0.0; + for (int i = 1; i <= WINDOW_SIZE; i++) + if (pre_integrations[i]) sum_dt += pre_integrations[i]->sum_dt; + double sigma_v = ACC_N * std::sqrt(std::max(sum_dt, 1e-3)); + double v_thr = std::max(hover::RUNAWAY_IMU_SIGMA * sigma_v, 0.10); + + if (Vs[WINDOW_SIZE].norm() > v_thr) + { + ROS_WARN(" runaway while stationary: |V|=%.3f m/s > %.3f m/s (σ_v=%.3f, 6σ=%.3f)", + Vs[WINDOW_SIZE].norm(), v_thr, sigma_v, + hover::RUNAWAY_IMU_SIGMA * sigma_v); + return true; + } + } + if (f_manager.last_track_num < 2) { ROS_INFO(" little feature %d", f_manager.last_track_num); @@ -645,19 +1063,27 @@ bool Estimator::failureDetection() Vector3d tmp_P = Ps[WINDOW_SIZE]; if ((tmp_P - last_P).norm() > 5) { - ROS_INFO(" big translation"); - return true; + if (hover_soften) { + ROS_WARN(" big translation during hover — soften, not rebooting (norm %.2f)", (tmp_P - last_P).norm()); + } else { + ROS_INFO(" big translation"); + return true; + } } if (abs(tmp_P.z() - last_P.z()) > 1) { - ROS_INFO(" big z translation"); - return true; + if (hover_soften) { + ROS_WARN(" big z translation during hover — soften, not rebooting (dz %.2f)", tmp_P.z() - last_P.z()); + } else { + ROS_INFO(" big z translation"); + return true; + } } Matrix3d tmp_R = Rs[WINDOW_SIZE]; Matrix3d delta_R = tmp_R.transpose() * last_R; Quaterniond delta_Q(delta_R); double delta_angle; - delta_angle = acos(delta_Q.w()) * 2.0 / 3.14 * 180.0; + delta_angle = acos(delta_Q.w()) * 2.0 * 180.0 / M_PI; if (delta_angle > 50) { ROS_INFO(" big delta_angle "); @@ -716,6 +1142,83 @@ void Estimator::optimization() IMUFactor* imu_factor = new IMUFactor(pre_integrations[j]); problem.AddResidualBlock(imu_factor, NULL, para_Pose[i], para_SpeedBias[i], para_Pose[j], para_SpeedBias[j]); } + + // Zero-Velocity Update — hover-aware fork. + // + // Adaptive weights: 1/σ where σ is the expected pseudo-measurement + // noise given the IMU noise floor and the window's frame period: + // σ_vel ≈ ACC_N · sqrt(dt_frame) + // σ_pos ≈ ACC_N · dt_frame^{1.5} / sqrt(3) + // Weights = hover::ZUPT_WEIGHT_SCALE / σ. + // + // Per-frame gating: ZUPT only fires for frames whose was_stationary[] + // flag was set at capture — prevents pinning frames that were actually + // moving but happen to be in the window. + // + // Equal weight on X/Y/Z: the goal is "drone holds its VIO position + // against wind" — the flight controller does the wind-compensation + // work, VIO gives it a stable setpoint. + // + // Confidence fade: hover-aware ZUPT gives a fast, stable bootstrap but + // can fight in-flight optimizer convergence once the state is well- + // conditioned (user-visible: ~1 m hover circle vs vanilla VINS-Mono's + // tighter hold). We linearly fade the ZUPT contribution from full at + // post_init_clean_cycles=50 to zero at 200, transitioning the system + // from "fork bootstrap mode" to "stock VINS-Mono mode" once the + // optimizer has had time to find a good linearization point. + const int FADE_START = 50; // cycles — start fading + const int FADE_END = 200; // cycles — fully stock VINS-Mono + double fade = 1.0; + if (post_init_clean_cycles > FADE_START) + { + if (post_init_clean_cycles >= FADE_END) fade = 0.0; + else fade = 1.0 - double(post_init_clean_cycles - FADE_START) / + double(FADE_END - FADE_START); + } + + const double dt_frame = 1.0 / std::max(image_rate_hz, 1.0); + const double sigma_v_zupt = ACC_N * std::sqrt(dt_frame); + const double sigma_p_zupt = ACC_N * dt_frame * std::sqrt(dt_frame) / std::sqrt(3.0); + const double w_vel = fade * hover::ZUPT_WEIGHT_SCALE / std::max(sigma_v_zupt, 1e-4); + const double w_pos = fade * hover::ZUPT_WEIGHT_SCALE / std::max(sigma_p_zupt, 1e-5); + + if (fade > 0.01 && ENABLE_ZUPT && motion_detector.isStationary()) + { + int zupt_frames = 0; + for (int j = std::max(0, WINDOW_SIZE - 3); j <= WINDOW_SIZE; j++) + { + if (!was_stationary[j]) continue; + auto *zv = new ZUPTVelocityFactor(w_vel); + problem.AddResidualBlock(zv, NULL, para_SpeedBias[j]); + zupt_frames++; + } + for (int i = std::max(0, WINDOW_SIZE - 3); i < WINDOW_SIZE; i++) + { + if (!was_stationary[i] || !was_stationary[i + 1]) continue; + auto *zp = new ZUPTPositionFactor(w_pos); + problem.AddResidualBlock(zp, NULL, para_Pose[i], para_Pose[i + 1]); + } + if (zupt_frames > 0) + ROS_DEBUG("hover ZUPT: frames=%d w_vel=%.1f w_pos=%.1f fade=%.2f", + zupt_frames, w_vel, w_pos, fade); + } + // Rotation-only ZUPT — position anchor during pure rotation. Weight is + // halved relative to stationary position weight because during rotation + // the residual translation is genuinely nonzero (lever-arm at the + // camera), so we don't want a hard clamp. + else if (fade > 0.01 && ENABLE_ROTATION_ZUPT && motion_detector.isRotationOnly()) + { + const double w_pos_rot = w_pos * 0.5; + for (int i = std::max(0, WINDOW_SIZE - 2); i < WINDOW_SIZE; i++) + { + auto *zp = new ZUPTPositionFactor(w_pos_rot); + problem.AddResidualBlock(zp, NULL, para_Pose[i], para_Pose[i + 1]); + } + ROS_DEBUG("rotation ZUPT: |gyr|=%.2f flow=%.1f w_pos=%.1f fade=%.2f", + motion_detector.meanGyrMagnitude(), motion_detector.meanFlow(), + w_pos_rot, fade); + } + int f_m_cnt = 0; int feature_index = -1; for (auto &it_per_id : f_manager.feature) @@ -998,7 +1501,42 @@ void Estimator::optimization() } } ROS_DEBUG("whole marginalization costs: %f", t_whole_marginalization.toc()); - + + // Post-optimization IMU-vs-visual rotation check (8.6). After the full + // Ceres solve has converged, compare the optimized Rs with the IMU + // integration chain. Large disagreement now is different from the pre- + // init flag check — here it indicates a slow drift accumulating over + // frames (gyro bias estimate is off, or a projection outlier pulled + // the solution). Used for logging / diagnostics only; does NOT trigger + // a reboot (failureDetection already handles hard divergence). Logs + // a warning every 5 s to avoid rate-spam. + if (solver_flag == NON_LINEAR) + { + Eigen::Quaterniond q_imu_pst = Eigen::Quaterniond::Identity(); + double sum_dt = 0.0; + for (int i = 1; i <= WINDOW_SIZE; i++) + if (pre_integrations[i]) + { + q_imu_pst = q_imu_pst * pre_integrations[i]->delta_q; + sum_dt += pre_integrations[i]->sum_dt; + } + Eigen::Quaterniond q_vis_pst(Rs[0].transpose() * Rs[WINDOW_SIZE]); + // angularDistance internally normalises both operands. + double disagree_deg = q_imu_pst.angularDistance(q_vis_pst) * 180.0 / M_PI; + + static double last_warn = 0.0; + double now = Headers[WINDOW_SIZE].stamp.toSec(); + double sigma_phi_deg = GYR_N * std::sqrt(std::max(sum_dt, 1e-3)) * 180.0 / M_PI; + double warn_thr = std::max(5.0, hover::ROTATION_DISAGREE_SIGMA * sigma_phi_deg); + if (disagree_deg > warn_thr && (now - last_warn) > 5.0) + { + ROS_WARN("post-opt IMU/visual drift: %.2f° > %.1f° (σ_φ=%.2f°) — " + "gyro bias may be off", + disagree_deg, warn_thr, sigma_phi_deg); + last_warn = now; + } + } + ROS_DEBUG("whole time for ceres: %f", t_whole.toc()); } @@ -1027,6 +1565,7 @@ void Estimator::slideWindow() Vs[i].swap(Vs[i + 1]); Bas[i].swap(Bas[i + 1]); Bgs[i].swap(Bgs[i + 1]); + was_stationary[i] = was_stationary[i + 1]; } Headers[WINDOW_SIZE] = Headers[WINDOW_SIZE - 1]; Ps[WINDOW_SIZE] = Ps[WINDOW_SIZE - 1]; @@ -1034,6 +1573,7 @@ void Estimator::slideWindow() Rs[WINDOW_SIZE] = Rs[WINDOW_SIZE - 1]; Bas[WINDOW_SIZE] = Bas[WINDOW_SIZE - 1]; Bgs[WINDOW_SIZE] = Bgs[WINDOW_SIZE - 1]; + was_stationary[WINDOW_SIZE] = was_stationary[WINDOW_SIZE - 1]; delete pre_integrations[WINDOW_SIZE]; pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]}; @@ -1086,6 +1626,7 @@ void Estimator::slideWindow() Rs[frame_count - 1] = Rs[frame_count]; Bas[frame_count - 1] = Bas[frame_count]; Bgs[frame_count - 1] = Bgs[frame_count]; + was_stationary[frame_count - 1] = was_stationary[frame_count]; delete pre_integrations[WINDOW_SIZE]; pre_integrations[WINDOW_SIZE] = new IntegrationBase{acc_0, gyr_0, Bas[WINDOW_SIZE], Bgs[WINDOW_SIZE]}; diff --git a/vins_estimator/src/estimator.h b/vins_estimator/src/estimator.h index 2390aa0a9..696078cc7 100644 --- a/vins_estimator/src/estimator.h +++ b/vins_estimator/src/estimator.h @@ -17,6 +17,8 @@ #include "factor/projection_factor.h" #include "factor/projection_td_factor.h" #include "factor/marginalization_factor.h" +#include "factor/zupt_factor.h" +#include "utility/motion_detector.h" #include #include @@ -31,7 +33,13 @@ class Estimator void setParameter(); // interface - void processIMU(double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); + // + // processIMU(dt, t, acc, gyr) — `dt` is the IMU integration step, `t` is + // the absolute ROS timestamp (seconds) of this IMU sample. `t` feeds the + // MotionDetector on the same timebase as image pushes, so the rolling + // window is coherent even when online-td offsets or auto-reset events + // shift the estimator's internal clocks. + void processIMU(double dt, double t, const Vector3d &linear_acceleration, const Vector3d &angular_velocity); void processImage(const map>>> &image, const std_msgs::Header &header); void setReloFrame(double _frame_stamp, int _frame_index, vector &_match_points, Vector3d _relo_t, Matrix3d _relo_r); @@ -136,4 +144,41 @@ class Estimator Vector3d relo_relative_t; Quaterniond relo_relative_q; double relo_relative_yaw; + + // Hover-aware state: stationarity detector driving ZUPT, static init + // priming, and softened failure detection. + MotionDetector motion_detector; + double last_image_t; // absolute ROS timestamp of latest image push + + // Per-frame stationary flag in the window. Set when processImage commits + // a frame while MotionDetector reports STATIONARY; allows ZUPT to fire + // only for frames that were genuinely still at capture time, not for + // frames that happened to be the newest in the window. + bool was_stationary[(WINDOW_SIZE + 1)]; + + // Safety-reset escalation (hover-aware). Counts how many all_image_frame + // overflow resets happened in the current run; used to widen the + // tolerance window on each retry so a marginal scene does not bootloop. + int init_safety_reset_count; + double last_safety_reset_t; + + // Image rate, used for scaling time-dependent thresholds (e.g. the + // all_image_frame overflow cap). Measured from consecutive image pushes. + double image_rate_hz; + + // Confidence-based fade from "fork hover-aware" mode toward stock + // VINS-Mono behavior (user feedback: hovers ~1 m wider than vanilla + // VINS-Mono after the fork's changes — the ZUPT and softened failure + // detection actually fight stable in-flight tracking once the state + // is well-conditioned). + // + // post_init_clean_cycles counts consecutive successful optimization() + // cycles (frame committed, failureDetection passed). Reset to 0 by + // any failure or by initialStructure(). Used by: + // * optimization() — ZUPT weight scales by exp-fade past 50 cycles + // * failureDetection() — soften only active for first ~100 cycles + // + // At 10 Hz image rate: 50 cycles ≈ 5 s (full fork strength), 200 cycles + // ≈ 20 s (fully stock VINS-Mono behavior). + int post_init_clean_cycles; }; diff --git a/vins_estimator/src/estimator_node.cpp b/vins_estimator/src/estimator_node.cpp index 1297936ad..3954509e9 100644 --- a/vins_estimator/src/estimator_node.cpp +++ b/vins_estimator/src/estimator_node.cpp @@ -149,8 +149,6 @@ void imu_callback(const sensor_msgs::ImuConstPtr &imu_msg) m_buf.unlock(); con.notify_one(); - last_imu_t = imu_msg->header.stamp.toSec(); - { std::lock_guard lg(m_state); predict(imu_msg); @@ -172,6 +170,14 @@ void feature_callback(const sensor_msgs::PointCloudConstPtr &feature_msg) } m_buf.lock(); feature_buf.push(feature_msg); + // Previous fork versions capped feature_buf at 3 during INITIAL to avoid + // "fast-forward replay" after a long idle. That cap broke init: dropping + // feature frames at the producer starves the estimator's processImage() + // of the dense timestamp sequence it needs for SfM/IMU alignment, so + // initialStructure() fell into a loop of silent failures. The real fix + // for the fast-forward symptom is in estimator.cpp — resetting the + // estimator (clearState) when all_image_frame grows unbounded during + // INITIAL — so no cap is needed here. m_buf.unlock(); con.notify_one(); } @@ -227,11 +233,23 @@ void process() double t = imu_msg->header.stamp.toSec(); double img_t = img_msg->header.stamp.toSec() + estimator.td; if (t <= img_t) - { + { if (current_time < 0) current_time = t; double dt = t - current_time; - ROS_ASSERT(dt >= 0); + // clearState() no longer resets `td`, so the root cause of + // post-reboot dt<0 is fixed (6). Remaining dt<0 cases: + // a) out-of-order IMU messages on the topic — should + // never happen on a live stream; log loudly, + // b) buffer races around m_buf lock — very rare. + // Either way, resync rather than assert — IntegrationBase + // requires dt>=0, so we clamp to 0. + if (dt < 0) + { + ROS_WARN("imu dt<0 (%.6fs); current_time resync (out-of-order IMU?)", dt); + current_time = t; + dt = 0; + } current_time = t; dx = imu_msg->linear_acceleration.x; dy = imu_msg->linear_acceleration.y; @@ -239,18 +257,34 @@ void process() rx = imu_msg->angular_velocity.x; ry = imu_msg->angular_velocity.y; rz = imu_msg->angular_velocity.z; - estimator.processIMU(dt, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); - //printf("imu: dt:%f a: %f %f %f w: %f %f %f\n",dt, dx, dy, dz, rx, ry, rz); - + // 4-arg form: absolute IMU timestamp feeds MotionDetector + // on the same clock as image header stamps (1.1). + estimator.processIMU(dt, t, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); } else { double dt_1 = img_t - current_time; double dt_2 = t - img_t; + // Symmetric handling (6). If either is negative, log and + // skip the sample rather than asserting — node stays up + // and the next IMU will re-sync. + if (dt_1 < 0) + { + ROS_WARN("img dt_1<0 (%.6fs); current_time resync", dt_1); + current_time = img_t; + dt_1 = 0; + } + if (dt_2 < 0) + { + ROS_WARN("img dt_2<0 (%.6fs); skipping sample", dt_2); + continue; + } current_time = img_t; - ROS_ASSERT(dt_1 >= 0); - ROS_ASSERT(dt_2 >= 0); - ROS_ASSERT(dt_1 + dt_2 > 0); + if (dt_1 + dt_2 <= 0) + { + ROS_WARN("dt_1+dt_2<=0; skipping sample"); + continue; + } double w1 = dt_2 / (dt_1 + dt_2); double w2 = dt_1 / (dt_1 + dt_2); dx = w1 * dx + w2 * imu_msg->linear_acceleration.x; @@ -259,8 +293,7 @@ void process() rx = w1 * rx + w2 * imu_msg->angular_velocity.x; ry = w1 * ry + w2 * imu_msg->angular_velocity.y; rz = w1 * rz + w2 * imu_msg->angular_velocity.z; - estimator.processIMU(dt_1, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); - //printf("dimu: dt:%f a: %f %f %f w: %f %f %f\n",dt_1, dx, dy, dz, rx, ry, rz); + estimator.processIMU(dt_1, img_t, Vector3d(dx, dy, dz), Vector3d(rx, ry, rz)); } } // set relocalization frame diff --git a/vins_estimator/src/factor/zupt_factor.h b/vins_estimator/src/factor/zupt_factor.h new file mode 100644 index 000000000..b84bbbe82 --- /dev/null +++ b/vins_estimator/src/factor/zupt_factor.h @@ -0,0 +1,103 @@ +#pragma once + +#include +#include + +// Zero-Velocity Update (ZUPT) factors for the hover-aware fork. +// +// Injected into the Ceres problem when the motion detector confirms the +// platform is stationary (velocity-and-position pin), or rotating in place +// (position-only pin). They stop IMU-bias-driven drift during prolonged +// hover, the regime where visual parallax is degenerate. +// +// The sqrt-information scaling is adaptive — computed in estimator.cpp from +// the IMU noise model (ACC_N) and the observed frame period so the weights +// reflect the *actual* pseudo-measurement noise, not a hardcoded constant. +// Per-axis weights are supported so Z (gravity-aligned in world frame) can +// be decoupled from XY when needed. + +class ZUPTVelocityFactor : public ceres::SizedCostFunction<3, 9> +{ + public: + explicit ZUPTVelocityFactor(double weight) { setWeight(weight); } + explicit ZUPTVelocityFactor(const Eigen::Vector3d &weight_xyz) { setWeight(weight_xyz); } + + void setWeight(double weight) + { sqrt_info_ = weight * Eigen::Matrix3d::Identity(); } + + void setWeight(const Eigen::Vector3d &weight_xyz) + { + sqrt_info_.setZero(); + sqrt_info_(0, 0) = weight_xyz(0); + sqrt_info_(1, 1) = weight_xyz(1); + sqrt_info_(2, 2) = weight_xyz(2); + } + + virtual bool Evaluate(double const *const *parameters, + double *residuals, + double **jacobians) const + { + Eigen::Vector3d Vj(parameters[0][0], parameters[0][1], parameters[0][2]); + Eigen::Map residual(residuals); + residual = sqrt_info_ * Vj; + + if (jacobians && jacobians[0]) + { + Eigen::Map> J(jacobians[0]); + J.setZero(); + J.block<3, 3>(0, 0) = sqrt_info_; + } + return true; + } + + private: + Eigen::Matrix3d sqrt_info_; +}; + +class ZUPTPositionFactor : public ceres::SizedCostFunction<3, 7, 7> +{ + public: + explicit ZUPTPositionFactor(double weight) { setWeight(weight); } + explicit ZUPTPositionFactor(const Eigen::Vector3d &weight_xyz) { setWeight(weight_xyz); } + + void setWeight(double weight) + { sqrt_info_ = weight * Eigen::Matrix3d::Identity(); } + + void setWeight(const Eigen::Vector3d &weight_xyz) + { + sqrt_info_.setZero(); + sqrt_info_(0, 0) = weight_xyz(0); + sqrt_info_(1, 1) = weight_xyz(1); + sqrt_info_(2, 2) = weight_xyz(2); + } + + virtual bool Evaluate(double const *const *parameters, + double *residuals, + double **jacobians) const + { + Eigen::Vector3d Pi(parameters[0][0], parameters[0][1], parameters[0][2]); + Eigen::Vector3d Pj(parameters[1][0], parameters[1][1], parameters[1][2]); + Eigen::Map residual(residuals); + residual = sqrt_info_ * (Pj - Pi); + + if (jacobians) + { + if (jacobians[0]) + { + Eigen::Map> Ji(jacobians[0]); + Ji.setZero(); + Ji.block<3, 3>(0, 0) = -sqrt_info_; + } + if (jacobians[1]) + { + Eigen::Map> Jj(jacobians[1]); + Jj.setZero(); + Jj.block<3, 3>(0, 0) = sqrt_info_; + } + } + return true; + } + + private: + Eigen::Matrix3d sqrt_info_; +}; diff --git a/vins_estimator/src/parameters.cpp b/vins_estimator/src/parameters.cpp index 1885e84be..f8b2d7ed9 100644 --- a/vins_estimator/src/parameters.cpp +++ b/vins_estimator/src/parameters.cpp @@ -23,6 +23,14 @@ std::string IMU_TOPIC; double ROW, COL; double TD, TR; +int ENABLE_ZUPT; +int ENABLE_ROTATION_ZUPT; +int STATIC_INIT_BIAS_PRIMING; +int SOFTEN_FAILURE_ON_HOVER; +double STATIC_ACC_THR; +double STATIC_GYR_THR; +double STATIC_FLOW_THR; + template T readParam(ros::NodeHandle &n, std::string name) { @@ -132,6 +140,43 @@ void readParameters(ros::NodeHandle &n) { TR = 0; } - + + // Hover-aware extensions. + // + // Only four YAML keys are supported here — feature switches. Everything + // else is compile-time (see namespace hover in parameters.h) or derived + // from the IMU noise floor just below. + auto readOrInt = [&](const char *key, int def) -> int { + cv::FileNode n = fsSettings[key]; + if (n.empty() || !n.isInt()) return def; + return static_cast(n); + }; + + ENABLE_ZUPT = readOrInt("enable_zupt", 1); + ENABLE_ROTATION_ZUPT = readOrInt("enable_rotation_zupt", 1); + STATIC_INIT_BIAS_PRIMING = readOrInt("static_init_bias_priming", 1); + SOFTEN_FAILURE_ON_HOVER = readOrInt("soften_failure_on_hover", 1); + + // Derived stationarity thresholds — computed from the just-loaded + // calibrated IMU noise values. 15σ absorbs typical motor vibration + // without bleeding into real-motion territory. + // + // Floors ensure we don't pick an absurdly low threshold when a + // calibration reports suspiciously clean noise (sub-mg noise on a + // consumer IMU is almost always mis-calibrated). Flow threshold is + // in normalized-FOCAL pixels/sec and is camera-independent because + // feature_tracker normalizes by the real camera intrinsics before + // publishing. + STATIC_ACC_THR = std::max(15.0 * ACC_N, 0.3); // m/s² + STATIC_GYR_THR = std::max(15.0 * GYR_N, 0.03); // rad/s + STATIC_FLOW_THR = 3.0; // px/s at FOCAL=460 + + ROS_INFO("hover-aware: zupt=%d rot_zupt=%d prime=%d soften=%d", + ENABLE_ZUPT, ENABLE_ROTATION_ZUPT, STATIC_INIT_BIAS_PRIMING, + SOFTEN_FAILURE_ON_HOVER); + ROS_INFO("hover-aware: derived thresholds acc=%.3f m/s² gyr=%.3f rad/s flow=%.1f px/s " + "(from ACC_N=%.4f GYR_N=%.5f)", + STATIC_ACC_THR, STATIC_GYR_THR, STATIC_FLOW_THR, ACC_N, GYR_N); + fsSettings.release(); } diff --git a/vins_estimator/src/parameters.h b/vins_estimator/src/parameters.h index 6d206cb70..e180e31b9 100644 --- a/vins_estimator/src/parameters.h +++ b/vins_estimator/src/parameters.h @@ -38,6 +38,85 @@ extern int ESTIMATE_TD; extern int ROLLING_SHUTTER; extern double ROW, COL; +// Hover-aware extensions. +// +// Design principle: the YAML holds only *calibrated* or *flight-intent* +// values — camera/IMU intrinsics, extrinsics, td, and on/off switches for +// fork-specific behaviors. Everything else (thresholds, scale factors, +// physical envelopes) is either a compile-time constant or derived at +// runtime from the calibrated values. This removes the "dozens of magic +// YAML numbers" surface and keeps per-flight config minimal. +// +// YAML-driven (user switches): +// enable_zupt, enable_rotation_zupt, static_init_bias_priming, +// soften_failure_on_hover +// Derived / constexpr: everything else. + +extern int ENABLE_ZUPT; +extern int ENABLE_ROTATION_ZUPT; +extern int STATIC_INIT_BIAS_PRIMING; +extern int SOFTEN_FAILURE_ON_HOVER; + +// Stationarity thresholds — auto-derived from calibrated IMU noise on +// startup so a fresh calibration flows through without touching any +// thresholds manually. Users who need to override a specific platform +// can set `static_acc_thr` / `static_gyr_thr` / `static_flow_thr` in +// YAML; otherwise defaults are computed as 15 × {ACC_N, GYR_N}. +extern double STATIC_ACC_THR; +extern double STATIC_GYR_THR; +extern double STATIC_FLOW_THR; + +// Fork-wide universal constants — multirotor-agnostic, never needed per- +// calibration. Defined as constexpr so there's one source of truth in the +// repo. Want a different value? Edit this header and rebuild. +namespace hover +{ +// Rolling window & hysteresis. +constexpr double STATIC_WINDOW_SEC = 0.4; // s, rolling window length +constexpr int STATIC_CONFIRM_CYCLES = 3; // cycles (~15 ms @200Hz) + +// Rotation-only classifier. Universal — depends on gyro physics + camera +// geometry normalized through FOCAL_LENGTH, not on a particular platform. +constexpr double ROTATION_ZUPT_GYR_MIN = 0.3; // rad/s (~17 °/s) +constexpr double ROTATION_ZUPT_FLOW_RATIO = 1.3; +constexpr double ROTATION_ZUPT_FLOW_BASELINE = 30.0; // px/s + +// Adaptive-threshold sigma multipliers. Scale the derived noise floor by +// this factor to get the reject threshold. Larger = looser, more tolerant. +constexpr double GRAVITY_CHECK_SIGMA = 6.0; +constexpr double ROTATION_DISAGREE_SIGMA = 8.0; +constexpr double RUNAWAY_IMU_SIGMA = 6.0; +constexpr double INIT_MAX_VEL_COEF = 1.5; +constexpr double ZUPT_WEIGHT_SCALE = 1.0; + +// Init parallax in normalized-FOCAL pixel units (VINS convention, 460). +// Resolution-independent: feature_tracker normalizes features before +// publishing, so a different camera resolution needs no change here. +// +// Lowered from the fork's previous 18 px to 10 px to match stock VINS-Mono. +// Lower threshold = init succeeds with less motion, so the drone needs to +// move only a small amount at takeoff for the SfM-based init to find +// enough parallax. The strict checks downstream in visualInitialAlign +// (gravity / |V| / rotation disagreement) still catch the few low-parallax +// alignments that would actually be wrong. +constexpr double INIT_PARALLAX_PX = 10.0; + +// Safety reset — re-initialize the estimator when all_image_frame grows +// due to low-parallax idle. The reset attempt count NEVER prevents the +// system from continuing; after it's exceeded we switch to in-place +// trimming and the estimator remains operational indefinitely. This +// means a drone can sit stationary "forever" without needing a human +// to restart the node. +constexpr double IDLE_RESET_SECONDS = 15.0; +constexpr int IDLE_RESET_MAX_ATTEMPTS = 5; + +// Vehicle physics envelope. Universal for multirotors — no credible +// airframe exceeds these, so they serve as "state is diverged" kick-out +// criteria, not per-flight tuning. +constexpr double VEHICLE_MAX_ACCEL = 40.0; // m/s² — ~4g, extreme maneuver +constexpr double VEHICLE_MAX_SPEED = 30.0; // m/s — 108 km/h, racing drone +} // namespace hover + void readParameters(ros::NodeHandle &n); diff --git a/vins_estimator/src/utility/motion_detector.h b/vins_estimator/src/utility/motion_detector.h new file mode 100644 index 000000000..ac52a33cf --- /dev/null +++ b/vins_estimator/src/utility/motion_detector.h @@ -0,0 +1,316 @@ +#pragma once + +#include +#include +#include +#include +#include + +// Rolling-window motion classifier used by the hover-aware fork. +// +// Design goals addressed in this revision: +// * Single timebase — all pushes take an externally-supplied ROS timestamp, +// IMU and flow buffers live on the same clock so the rolling window is +// coherent when image-rate and IMU-rate drift apart, when a camera drops +// frames for a sub-second gap, or when online-td shifts the IMU clock. +// * Outlier-robust classification — percentile-based quietness tests ignore +// isolated acc spikes (motor vibration, prop wash), gyro pulses (impacts) +// and single flying objects across the visual field (bird, cable, leaf). +// Thresholds act on the 90th percentile, so up to 10% outliers in the +// window can't flip the classification on their own. +// * Trimmed-mean robust statistics — gravity-in-body reference and gyro +// bias priming use a trimmed mean (~14% cut at each tail), preserving +// the central tendency of genuine motion while dropping extreme outliers. +// * Cycle-count hysteresis state machine — transitions between MOVING / +// ROTATION_ONLY / STATIONARY require N consecutive identical raw +// classifications. No wall-clock dependency — the count is in +// evaluate()-cycles, so at IMU rate the confirmation delay is typically +// ~15 ms (3 cycles at 200 Hz). +// +// Public classifications: +// 1. STATIONARY — accelerometer ≈ gravity, gyro ≈ 0, flow ≈ 0. +// Gates ZUPT, static init bias priming, failure +// softening. +// 2. ROTATION_ONLY — gyroscope significantly excited, but optical flow is +// explainable by rotation alone. Gates rotation-only +// position ZUPT. +// 3. MOVING — anything else. Default. +class MotionDetector +{ +public: + enum class State { UNKNOWN, STATIONARY, ROTATION_ONLY, MOVING }; + + MotionDetector() { reset(); } + + void configure(double acc_thr, double gyr_thr, double flow_thr, + double window_sec, int min_samples, + double gravity_norm, int confirm_cycles) + { + acc_thr_ = acc_thr; + gyr_thr_ = gyr_thr; + flow_thr_ = flow_thr; + window_sec_ = window_sec; + min_samples_ = std::max(4, min_samples); + gravity_norm_ = gravity_norm; + confirm_cycles_ = std::max(1, confirm_cycles); + } + + void configureRotation(double gyr_min, double ratio, double baseline) + { + gyr_rot_min_ = gyr_min; + flow_ratio_ = ratio; + flow_baseline_ = baseline; + } + + // Lever arm from IMU origin to camera origin (imu^T_cam). Used to + // include the rotation-induced translation at the camera in the + // rotation-only classifier; matches VINS-Mono's TIC[0] convention. + void setLeverArm(const Eigen::Vector3d &t_ic) { t_ic_ = t_ic; } + void setExtrinsicR(const Eigen::Matrix3d &R_ic) { R_ic_ = R_ic; } + void setFocalLength(double f) { focal_ = f; } + + // Clears transient state only; preserves configured thresholds so a + // post-reset detector behaves identically without re-calling configure. + void reset() + { + imu_buf_.clear(); + flow_buf_.clear(); + still_cnt_ = rot_only_cnt_ = move_cnt_ = 0; + state_ = State::UNKNOWN; + have_still_ref_ = false; + still_ref_acc_.setZero(); + still_ref_gyr_.setZero(); + still_ref_acc_std_ = 0.0; + last_t_ = -1.0; + } + + // IMU push. `t` is an external monotonic timestamp (ROS header stamp, + // seconds). IMU and flow pushes MUST share the same clock. + void pushIMU(double t, const Eigen::Vector3d &acc, const Eigen::Vector3d &gyr) + { + imu_buf_.push_back({t, acc, gyr}); + if (t > last_t_) last_t_ = t; + trimBoth(last_t_); + evaluate(); + } + + // Preferred image push: pass per-feature flow magnitudes (px/s). We + // median-reduce per-image before pushing to time buffer, so a single + // object crossing the field of view does not bias the aggregate. + void pushFlowSamples(double t, const std::vector &flows_px_per_sec) + { + double med = 0.0; + if (!flows_px_per_sec.empty()) + { + std::vector v = flows_px_per_sec; + auto mid = v.begin() + v.size() / 2; + std::nth_element(v.begin(), mid, v.end()); + med = *mid; + } + flow_buf_.push_back({t, med}); + if (t > last_t_) last_t_ = t; + trimBoth(last_t_); + } + + // Backward-compat overload: single scalar flow. + void pushFlow(double t, double flow_px_per_sec) + { + flow_buf_.push_back({t, flow_px_per_sec}); + if (t > last_t_) last_t_ = t; + trimBoth(last_t_); + } + + State state() const { return state_; } + bool isStationary() const { return state_ == State::STATIONARY; } + bool isRotationOnly() const { return state_ == State::ROTATION_ONLY; } + + int imuCount() const { return (int)imu_buf_.size(); } + int flowCount() const { return (int)flow_buf_.size(); } + + Eigen::Vector3d meanGyr() const { return trimmedMeanVec3Gyr(); } + Eigen::Vector3d meanAcc() const { return trimmedMeanVec3Acc(); } + + double meanGyrMagnitude() const + { + std::vector m; m.reserve(imu_buf_.size()); + for (const auto &e : imu_buf_) m.push_back(e.gyr.norm()); + return trimmedMeanScalar(m); + } + + double meanFlow() const + { + std::vector m; m.reserve(flow_buf_.size()); + for (const auto &e : flow_buf_) m.push_back(e.second); + return trimmedMeanScalar(m); + } + + double peakAccDeviation() const + { + double mx = 0.0; + for (const auto &e : imu_buf_) + mx = std::max(mx, std::fabs(e.acc.norm() - gravity_norm_)); + return mx; + } + + bool hasStationaryReference() const { return have_still_ref_; } + const Eigen::Vector3d &stationaryReferenceAcc() const { return still_ref_acc_; } + const Eigen::Vector3d &stationaryReferenceGyr() const { return still_ref_gyr_; } + double stationaryAccNoise() const { return still_ref_acc_std_; } + + // Backward-compat classifier (parameters overridden at call site). + bool isRotationOnly(double focal_px, double gyr_min_rad_s, + double flow_ratio, double flow_baseline_px) const + { + if ((int)imu_buf_.size() < 4 || flow_buf_.empty()) return false; + double mg = meanGyrMagnitude(); + if (mg < gyr_min_rad_s) return false; + return meanFlow() < flow_ratio * mg * focal_px + flow_baseline_px; + } + +private: + struct ImuSample { double t; Eigen::Vector3d acc; Eigen::Vector3d gyr; }; + + Eigen::Vector3d trimmedMeanVec3Acc() const + { + if (imu_buf_.empty()) return Eigen::Vector3d::Zero(); + std::vector xs, ys, zs; + xs.reserve(imu_buf_.size()); ys.reserve(imu_buf_.size()); zs.reserve(imu_buf_.size()); + for (const auto &s : imu_buf_) + { + xs.push_back(s.acc.x()); ys.push_back(s.acc.y()); zs.push_back(s.acc.z()); + } + return Eigen::Vector3d(trimmedMeanScalar(xs), + trimmedMeanScalar(ys), + trimmedMeanScalar(zs)); + } + + Eigen::Vector3d trimmedMeanVec3Gyr() const + { + if (imu_buf_.empty()) return Eigen::Vector3d::Zero(); + std::vector xs, ys, zs; + xs.reserve(imu_buf_.size()); ys.reserve(imu_buf_.size()); zs.reserve(imu_buf_.size()); + for (const auto &s : imu_buf_) + { + xs.push_back(s.gyr.x()); ys.push_back(s.gyr.y()); zs.push_back(s.gyr.z()); + } + return Eigen::Vector3d(trimmedMeanScalar(xs), + trimmedMeanScalar(ys), + trimmedMeanScalar(zs)); + } + + // ~14% cut per tail trimmed mean. + double trimmedMeanScalar(std::vector v) const + { + if (v.empty()) return 0.0; + const int n = (int)v.size(); + std::sort(v.begin(), v.end()); + int cut = n / 7; + int lo = cut, hi = n - cut; + if (hi <= lo) { lo = 0; hi = n; } + double s = 0.0; + for (int i = lo; i < hi; ++i) s += v[i]; + return s / double(hi - lo); + } + + static double percentile(std::vector &v, double p) + { + if (v.empty()) return 0.0; + std::sort(v.begin(), v.end()); + int k = (int)std::round((v.size() - 1) * p); + if (k < 0) k = 0; + if (k >= (int)v.size()) k = (int)v.size() - 1; + return v[k]; + } + + void trimBoth(double now) + { + while (!imu_buf_.empty() && now - imu_buf_.front().t > window_sec_) imu_buf_.pop_front(); + while (!flow_buf_.empty() && now - flow_buf_.front().first > window_sec_) flow_buf_.pop_front(); + } + + void evaluate() + { + if ((int)imu_buf_.size() < min_samples_) return; + + std::vector acc_dev; acc_dev.reserve(imu_buf_.size()); + std::vector gyr_mag; gyr_mag.reserve(imu_buf_.size()); + for (const auto &e : imu_buf_) + { + acc_dev.push_back(std::fabs(e.acc.norm() - gravity_norm_)); + gyr_mag.push_back(e.gyr.norm()); + } + double p90_acc = percentile(acc_dev, 0.90); + double p90_gyr = percentile(gyr_mag, 0.90); + double p50_gyr = percentile(gyr_mag, 0.50); + + double p90_flow = 0.0; + const bool have_flow = !flow_buf_.empty(); + if (have_flow) + { + std::vector f; f.reserve(flow_buf_.size()); + for (const auto &e : flow_buf_) f.push_back(e.second); + p90_flow = percentile(f, 0.90); + } + + bool imu_quiet = (p90_acc < acc_thr_) && (p90_gyr < gyr_thr_); + bool flow_quiet = !have_flow || (p90_flow < flow_thr_); + bool raw_still = imu_quiet && flow_quiet; + + bool raw_rot = false; + if (!raw_still && have_flow && p50_gyr > gyr_rot_min_) + { + double predicted = p50_gyr * focal_; + raw_rot = p90_flow < flow_ratio_ * predicted + flow_baseline_; + } + + State raw = raw_still ? State::STATIONARY + : (raw_rot ? State::ROTATION_ONLY : State::MOVING); + + if (raw == State::STATIONARY) { still_cnt_++; rot_only_cnt_ = 0; move_cnt_ = 0; } + else if (raw == State::ROTATION_ONLY) { rot_only_cnt_++; still_cnt_ = 0; move_cnt_ = 0; } + else { move_cnt_++; still_cnt_ = 0; rot_only_cnt_ = 0; } + + if (still_cnt_ >= confirm_cycles_) state_ = State::STATIONARY; + else if (rot_only_cnt_ >= confirm_cycles_) state_ = State::ROTATION_ONLY; + else if (move_cnt_ >= confirm_cycles_) state_ = State::MOVING; + + if (state_ == State::STATIONARY) + { + still_ref_acc_ = meanAcc(); + still_ref_gyr_ = meanGyr(); + double s2 = 0.0; + for (const auto &e : imu_buf_) + s2 += (e.acc - still_ref_acc_).squaredNorm(); + still_ref_acc_std_ = std::sqrt(s2 / double(std::max(1, (int)imu_buf_.size()))); + have_still_ref_ = true; + } + } + + std::deque imu_buf_; + std::deque> flow_buf_; + + State state_; + int still_cnt_, rot_only_cnt_, move_cnt_; + + double last_t_; + double acc_thr_ = 0.5; + double gyr_thr_ = 0.05; + double flow_thr_ = 2.0; + double window_sec_ = 0.5; + int min_samples_ = 20; + double gravity_norm_ = 9.81; + int confirm_cycles_ = 3; + + double gyr_rot_min_ = 0.3; + double flow_ratio_ = 1.3; + double flow_baseline_ = 30.0; + double focal_ = 460.0; + + bool have_still_ref_; + Eigen::Vector3d still_ref_acc_; + Eigen::Vector3d still_ref_gyr_; + double still_ref_acc_std_; + + Eigen::Vector3d t_ic_ = Eigen::Vector3d::Zero(); + Eigen::Matrix3d R_ic_ = Eigen::Matrix3d::Identity(); +}; diff --git a/vins_estimator/src/vision_pose_bridge_node.cpp b/vins_estimator/src/vision_pose_bridge_node.cpp new file mode 100644 index 000000000..2bf9a7803 --- /dev/null +++ b/vins_estimator/src/vision_pose_bridge_node.cpp @@ -0,0 +1,384 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +/* + * VisionPoseBridge — forwards VINS-Mono odometry to ArduPilot via + * /mavros/vision_pose/pose (and /mavros/vision_speed/twist) with + * dual-mode sanity checking and recovery, mirrors the sanitized pose + * on /vins_bridge/pose for downstream consumers. + * + * States: + * PRE_INIT → Publishes (0,0,0) + identity orientation at ~rate Hz. + * Allows arm in PosHold. + * Transition: VINS odometry arrives (passes sanity) → ACTIVE + * + * ACTIVE → Forwards VINS position + orientation. + * Transitions: + * - VINS data stale > odom_timeout → DROPOUT + * - Sanity check fails → DROPOUT + * + * DROPOUT → Stops publishing. EKF dead-reckons. + * Transitions: + * - VINS resumes (passes sanity) → ACTIVE + * - Drone is disarmed → PRE_INIT + * + * Sanity check: combined velocity + acceleration test. + * Velocity > max_speed → hard divergence, immediate DROPOUT. + * Accel > max_accel → transient spike; increment counter. + * Violation counter uses a time window: + * - 2 consecutive → DROPOUT + * - 5 within 1 s → DROPOUT + * - otherwise decay after 2 s of clean samples. + * + * Published topics: + * /mavros/vision_pose/pose — position + orientation for EKF3 pose + * /mavros/vision_speed/twist — linear velocity for EKF3 + * /vins_bridge/pose — mirror (telemetry / debug) + * + * Parameters (ROS): + * ~odom_timeout seconds without data before DROPOUT (default 1.0) + * ~rate publish rate Hz (default 10.0) + * ~max_accel per-sample acceleration warn cap [m/s²] (default 40.0) + * ~max_speed hard divergence speed cap [m/s] (default 30.0) + * ~pose_frame frame_id for the outgoing PoseStamped (default "odom") + * ~publish_speed publish /mavros/vision_speed/twist (default true) + */ + +class VisionPoseBridge +{ + enum class State { PRE_INIT, ACTIVE, DROPOUT }; + +public: + VisionPoseBridge() + : state_(State::PRE_INIT) + , is_armed_(false) + , last_odom_time_(0) + , have_prev_pos_(false) + , have_prev_vel_(false) + { + ros::NodeHandle pnh("~"); + pnh.param("max_accel", max_accel_, 40.0); + pnh.param("max_speed", max_speed_, 30.0); + pnh.param("odom_timeout", odom_timeout_, 1.0); + pnh.param("pose_frame", pose_frame_, "odom"); + pnh.param("publish_speed", publish_speed_, true); + double rate; + pnh.param("rate", rate, 10.0); + + pub_mavros_ = nh_.advertise( + "/mavros/vision_pose/pose", 10); + pub_mirror_ = nh_.advertise( + "/vins_bridge/pose", 10); + if (publish_speed_) + pub_speed_ = nh_.advertise( + "/mavros/vision_speed/twist", 10); + sub_odom_ = nh_.subscribe( + "/vins_estimator/odometry", 10, + &VisionPoseBridge::odomCallback, this); + sub_state_ = nh_.subscribe( + "/mavros/state", 5, + &VisionPoseBridge::stateCallback, this); + + timer_ = nh_.createTimer( + ros::Duration(1.0 / rate), + &VisionPoseBridge::timerCallback, this); + + ROS_INFO("vision_pose_bridge: rate=%.0fHz frame=%s speed=%s", + rate, pose_frame_.c_str(), publish_speed_ ? "on" : "off"); + ROS_INFO(" max_accel=%.1f m/s² max_speed=%.1f m/s timeout=%.1fs", + max_accel_, max_speed_, odom_timeout_); + } + +private: + // ── Combined speed + accel sanity (3.3) ───────────────────────── + // + // Returns: + // >= 0 : pass (value is the implied speed — for diagnostics) + // -1 : velocity hard cap exceeded, immediate DROPOUT + // -2 : accel spike; may still pass depending on violation window + bool checkSanity(const geometry_msgs::Point &pos, double stamp) + { + if (!have_prev_pos_) + { + prev_pos_ = pos; + prev_stamp_ = stamp; + prev_dt_ = 0; + have_prev_pos_ = true; + have_prev_vel_ = false; + viol_times_.clear(); + return true; + } + + double dt = stamp - prev_stamp_; + // 5.3: if dt is too large (e.g. bridge returned from DROPOUT via + // long gap), we can't trust the finite-difference velocity. Treat + // as "re-entry" — reset prev and skip this sample's sanity. + if (dt < 0.001 || dt > 0.5) + { + prev_pos_ = pos; + prev_stamp_ = stamp; + have_prev_vel_ = false; + viol_times_.clear(); + return true; + } + + double vx = (pos.x - prev_pos_.x) / dt; + double vy = (pos.y - prev_pos_.y) / dt; + double vz = (pos.z - prev_pos_.z) / dt; + double speed = std::sqrt(vx * vx + vy * vy + vz * vz); + + prev_pos_ = pos; + prev_stamp_ = stamp; + + // Hard cap: vehicle can't physically move this fast (3.3). If it + // looks like it is, VIO jumped — no amount of spike-filtering + // recovers it. Force DROPOUT. + if (speed > max_speed_) + { + ROS_WARN("vision_pose_bridge: speed=%.1f m/s > cap %.1f — DROPOUT", + speed, max_speed_); + return false; + } + + if (!have_prev_vel_) + { + prev_vx_ = vx; prev_vy_ = vy; prev_vz_ = vz; + prev_dt_ = dt; + have_prev_vel_ = true; + viol_times_.clear(); + return true; + } + + double dt_accel = (prev_dt_ + dt) * 0.5; + if (dt_accel < 0.001) dt_accel = dt; + + double ax = (vx - prev_vx_) / dt_accel; + double ay = (vy - prev_vy_) / dt_accel; + double az = (vz - prev_vz_) / dt_accel; + double accel = std::sqrt(ax*ax + ay*ay + az*az); + + prev_vx_ = vx; prev_vy_ = vy; prev_vz_ = vz; + prev_dt_ = dt; + + // Accel warn — vehicle can't reach this, so it's a position spike. + // Filter rather than DROP to avoid kicking out on a single noisy + // sample. Escalation logic (5.2): + // - cleanup: violations older than 2s are dropped + // - trigger: 2 consecutive or 5 within 1s → DROPOUT + while (!viol_times_.empty() && stamp - viol_times_.front() > 2.0) + viol_times_.pop_front(); + + if (accel > max_accel_) + { + viol_times_.push_back(stamp); + // consecutive detection: previous sample was also a violation + bool consecutive = viol_times_.size() >= 2 && + (stamp - viol_times_[viol_times_.size() - 2]) < 0.2; + // storm: 5+ within 1 s + int within_1s = 0; + for (auto it = viol_times_.rbegin(); it != viol_times_.rend(); ++it) + { + if (stamp - *it > 1.0) break; + within_1s++; + } + ROS_WARN("vision_pose_bridge: accel=%.1f m/s² (cap %.1f), viol=%zu " + "(consec=%d within1s=%d)", + accel, max_accel_, viol_times_.size(), + (int)consecutive, within_1s); + + if (consecutive || within_1s >= 5) + return false; + // single violation — keep going but DON'T forward this sample + return true; + } + return true; + } + + void resetSanity() + { + have_prev_pos_ = false; + have_prev_vel_ = false; + prev_dt_ = 0; + viol_times_.clear(); + } + + // ── Callbacks ───────────────────────────────────────────────── + void odomCallback(const nav_msgs::Odometry::ConstPtr &msg) + { + // Snapshot inputs under lock; logging done outside the lock so the + // publisher thread doesn't stall on it (5.8). transition_to_* + // flags carry the state change into the post-lock log section. + bool transition_to_active = false; + bool transition_to_dropout = false; + State prev_state = State::PRE_INIT; + + // 5.7: use the message's own timestamp, not ros::Time::now(). + // The VINS pipeline takes tens of ms from image to pose; using + // ::now() adds that delay as attitude/position lag to the EKF. + double msg_t = msg->header.stamp.toSec(); + + { + std::lock_guard lock(mtx_); + + if (!checkSanity(msg->pose.pose.position, msg_t)) + { + if (state_ == State::ACTIVE) + { + prev_state = state_; + state_ = State::DROPOUT; + transition_to_dropout = true; + resetSanity(); + } + return; + } + + last_odom_time_ = msg_t; + last_pose_.header = msg->header; + last_pose_.header.frame_id = pose_frame_; + last_pose_.pose.position = msg->pose.pose.position; + last_pose_.pose.orientation = msg->pose.pose.orientation; + last_twist_.header = msg->header; + last_twist_.header.frame_id = pose_frame_; + last_twist_.twist.linear = msg->twist.twist.linear; + last_twist_.twist.angular = msg->twist.twist.angular; + have_last_twist_ = true; + + if (state_ != State::ACTIVE) + { + prev_state = state_; + state_ = State::ACTIVE; + transition_to_active = true; + } + } + + if (transition_to_dropout) + ROS_WARN("vision_pose_bridge: sanity fail -> DROPOUT"); + if (transition_to_active) + ROS_INFO("vision_pose_bridge: -> ACTIVE%s", + prev_state == State::PRE_INIT ? " (initialized)" : " (recovered)"); + } + + void stateCallback(const mavros_msgs::State::ConstPtr &msg) + { + std::lock_guard lock(mtx_); + is_armed_ = msg->armed; + } + + // ── Timer (publishes at fixed rate) ──────────────────────────── + void timerCallback(const ros::TimerEvent &) + { + geometry_msgs::PoseStamped pose_out; + geometry_msgs::TwistStamped speed_out; + bool publish_speed_now = false; + bool do_publish_pose = true; + + { + std::lock_guard lock(mtx_); + double now = ros::Time::now().toSec(); + + if (state_ == State::ACTIVE) + { + if (now - last_odom_time_ > odom_timeout_) + { + state_ = State::DROPOUT; + resetSanity(); + ROS_WARN("vision_pose_bridge: stale -> DROPOUT"); + do_publish_pose = false; + } + } + else if (state_ == State::DROPOUT) + { + if (!is_armed_) + { + state_ = State::PRE_INIT; + resetSanity(); + // 5.9: reset last_odom_time_ so a quick post-rearm + // ACTIVE transition does not inherit the old stamp + // (which would instantly satisfy freshness but cause + // a bogus finite-diff velocity on the first sample + // — handled by dt > 0.5 in checkSanity, but cleaner + // to zero it here). + last_odom_time_ = 0; + ROS_INFO("vision_pose_bridge: disarmed -> PRE_INIT"); + } + else + do_publish_pose = false; + } + + if (!do_publish_pose) + return; + + pose_out.header.stamp = ros::Time::now(); + pose_out.header.frame_id = pose_frame_; + if (state_ == State::ACTIVE) + { + pose_out.pose = last_pose_.pose; + speed_out = last_twist_; + publish_speed_now = publish_speed_ && have_last_twist_; + } + else // PRE_INIT + { + pose_out.pose.position.x = 0; + pose_out.pose.position.y = 0; + pose_out.pose.position.z = 0; + pose_out.pose.orientation.x = 0; + pose_out.pose.orientation.y = 0; + pose_out.pose.orientation.z = 0; + pose_out.pose.orientation.w = 1; + } + } + + // Publish outside the lock so slow TCP flush on mavros bridge + // doesn't stall the callback thread (5.8). + pub_mavros_.publish(pose_out); + pub_mirror_.publish(pose_out); + if (publish_speed_now) + pub_speed_.publish(speed_out); + } + + ros::NodeHandle nh_; + ros::Publisher pub_mavros_; + ros::Publisher pub_mirror_; + ros::Publisher pub_speed_; + ros::Subscriber sub_odom_; + ros::Subscriber sub_state_; + ros::Timer timer_; + std::mutex mtx_; + + State state_; + bool is_armed_; + double last_odom_time_; + + double max_accel_; + double max_speed_; + double odom_timeout_; + std::string pose_frame_; + bool publish_speed_; + + geometry_msgs::Point prev_pos_; + double prev_stamp_; + bool have_prev_pos_; + double prev_vx_, prev_vy_, prev_vz_; + double prev_dt_; + bool have_prev_vel_; + // Timestamps of recent accel violations for windowed escalation. + std::deque viol_times_; + + geometry_msgs::PoseStamped last_pose_; + geometry_msgs::TwistStamped last_twist_; + bool have_last_twist_ = false; +}; + +int main(int argc, char **argv) +{ + ros::init(argc, argv, "vision_pose_bridge"); + VisionPoseBridge bridge; + ros::spin(); + return 0; +}