From 12ffc56121f23c4f90a40af676fab6a5e6222259 Mon Sep 17 00:00:00 2001 From: morten Date: Thu, 25 Jun 2026 10:08:57 +0200 Subject: [PATCH 1/4] remove unused radar params --- mimosa/config/euroc/params.yaml | 2 -- mimosa/config/hornbill/params.yaml | 2 -- mimosa/config/lapwing/params.yaml | 2 -- mimosa/config/magpie/params.yaml | 2 -- mimosa/config/parrot/params.yaml | 2 -- mimosa/include/mimosa/radar/factor.hpp | 11 ++--------- mimosa/include/mimosa/radar/manager.hpp | 2 -- mimosa/src/radar/manager.cpp | 4 +--- 8 files changed, 3 insertions(+), 24 deletions(-) diff --git a/mimosa/config/euroc/params.yaml b/mimosa/config/euroc/params.yaml index 0164f4a..4f70e71 100644 --- a/mimosa/config/euroc/params.yaml +++ b/mimosa/config/euroc/params.yaml @@ -155,9 +155,7 @@ radar: threshold_elevation_deg: 60 filter_min_db: 5 frame_ms: 18.5 - huber_threshold: 1.345 noise_sigma: 0.037527767 - outlier_threshold: 3.0 odometry: sensor_frame: mimosa_odometry diff --git a/mimosa/config/hornbill/params.yaml b/mimosa/config/hornbill/params.yaml index e58d380..ca73718 100644 --- a/mimosa/config/hornbill/params.yaml +++ b/mimosa/config/hornbill/params.yaml @@ -155,9 +155,7 @@ radar: threshold_elevation_deg: 60 filter_min_db: 5 frame_ms: 18.5 - huber_threshold: 1.345 noise_sigma: 0.037527767 - outlier_threshold: 3.0 odometry: sensor_frame: mimosa_odometry diff --git a/mimosa/config/lapwing/params.yaml b/mimosa/config/lapwing/params.yaml index 5acffc6..7306db6 100644 --- a/mimosa/config/lapwing/params.yaml +++ b/mimosa/config/lapwing/params.yaml @@ -154,9 +154,7 @@ radar: threshold_elevation_deg: 60 filter_min_db: 5 frame_ms: 18.5 - huber_threshold: 1.345 noise_sigma: 0.037527767 - outlier_threshold: 3.0 odometry: sensor_frame: mimosa_odometry diff --git a/mimosa/config/magpie/params.yaml b/mimosa/config/magpie/params.yaml index 182b6be..1975759 100644 --- a/mimosa/config/magpie/params.yaml +++ b/mimosa/config/magpie/params.yaml @@ -155,9 +155,7 @@ radar: threshold_elevation_deg: 60 filter_min_db: 5 frame_ms: 18.5 - huber_threshold: 1.345 noise_sigma: 0.037527767 - outlier_threshold: 3.0 odometry: sensor_frame: mimosa_odometry diff --git a/mimosa/config/parrot/params.yaml b/mimosa/config/parrot/params.yaml index 271a378..68e8a72 100644 --- a/mimosa/config/parrot/params.yaml +++ b/mimosa/config/parrot/params.yaml @@ -154,9 +154,7 @@ radar: threshold_elevation_deg: 60 filter_min_db: 5 frame_ms: 18.5 - huber_threshold: 1.345 noise_sigma: 0.037527767 - outlier_threshold: 3.0 odometry: sensor_frame: mimosa_odometry diff --git a/mimosa/include/mimosa/radar/factor.hpp b/mimosa/include/mimosa/radar/factor.hpp index 41c96ff..d50e6b9 100644 --- a/mimosa/include/mimosa/radar/factor.hpp +++ b/mimosa/include/mimosa/radar/factor.hpp @@ -118,8 +118,6 @@ class DopplerHessianFactor : public gtsam::NonlinearFactor gtsam::Vector3 angular_velocity_B_; // angvel from IMU during radar exposure; double noise_sigma_; // in m/s - double huber_threshold_; // in std deviations - double outlier_threshold_; // in std deviations mutable std::vector target_status_; // classification of targets as static or non-static mutable TargetVector static_targets_; @@ -135,15 +133,12 @@ class DopplerHessianFactor : public gtsam::NonlinearFactor DopplerHessianFactor( const TargetVector & targets, const gtsam::Pose3 & pose_R_B, const gtsam::Vector3 & angular_velocity_B, const gtsam::Key key0, const gtsam::Key key1, - const gtsam::Key key2, const double noise_sigma, const double huber_threshold, - const double outlier_threshold) + const gtsam::Key key2, const double noise_sigma) : Base(std::vector{key0, key1, key2}), targets_(targets), pose_R_B_(pose_R_B), angular_velocity_B_(angular_velocity_B), - noise_sigma_(noise_sigma), - huber_threshold_(huber_threshold), - outlier_threshold_(outlier_threshold) + noise_sigma_(noise_sigma) { target_status_.resize(targets_.size()); } @@ -169,8 +164,6 @@ class DopplerHessianFactor : public gtsam::NonlinearFactor << keyFormatter(keys()[1]) << ", " << keyFormatter(keys()[2]) << ")\n" << " Targets: " << targets_.size() << "\n" << " Noise Sigma: " << noise_sigma_ << "\n" - << " Huber Threshold: " << huber_threshold_ << "\n" - << " Outlier Threshold: " << outlier_threshold_ << "\n" << " Pose R_B: " << pose_R_B_ << "\n" << " Angular Velocity B: " << angular_velocity_B_.transpose() << "\n"; } diff --git a/mimosa/include/mimosa/radar/manager.hpp b/mimosa/include/mimosa/radar/manager.hpp index 9d8718d..65da039 100644 --- a/mimosa/include/mimosa/radar/manager.hpp +++ b/mimosa/include/mimosa/radar/manager.hpp @@ -30,8 +30,6 @@ struct ManagerConfig float filter_min_db = 5; float frame_ms = 18.5; float noise_sigma = 0.1; - float huber_threshold = 1.345; - float outlier_threshold = 3.0; }; void declare_config(ManagerConfig & config); diff --git a/mimosa/src/radar/manager.cpp b/mimosa/src/radar/manager.cpp index c9da31c..1a2085f 100644 --- a/mimosa/src/radar/manager.cpp +++ b/mimosa/src/radar/manager.cpp @@ -85,7 +85,7 @@ void Manager::callback(const sensor_msgs::PointCloud2::ConstPtr & msg) gtsam::NonlinearFactorGraph new_factors; auto dhf = std::make_shared( valid_targets_, config_.base.T_B_S, angular_velocity_mean, X(0), V(0), B(0), - config_.noise_sigma, config_.huber_threshold, config_.outlier_threshold); + config_.noise_sigma); new_factors.add(dhf); logger_->debug("Declaring (ts: {})", corrected_ts_); @@ -204,8 +204,6 @@ void declare_config(ManagerConfig & config) field(config.filter_min_db, "filter_min_db", "dB"); field(config.frame_ms, "frame_ms", "float"); field(config.noise_sigma, "noise_sigma", "float"); - field(config.huber_threshold, "huber_threshold", "float"); - field(config.outlier_threshold, "outlier_threshold", "float"); } } } From 9367ce4b682bb9ca0c86fb2df3bd0da13e65f3e1 Mon Sep 17 00:00:00 2001 From: morten Date: Thu, 25 Jun 2026 10:18:00 +0200 Subject: [PATCH 2/4] remove unused factor --- mimosa/include/mimosa/radar/factor.hpp | 79 -------------------------- 1 file changed, 79 deletions(-) diff --git a/mimosa/include/mimosa/radar/factor.hpp b/mimosa/include/mimosa/radar/factor.hpp index d50e6b9..b3d917e 100644 --- a/mimosa/include/mimosa/radar/factor.hpp +++ b/mimosa/include/mimosa/radar/factor.hpp @@ -19,85 +19,6 @@ namespace mimosa { namespace radar { -class DopplerFactor -: public gtsam::NoiseModelFactor3 -{ - gtsam::Point3 point_measurement_; // point in radar frame - double doppler_measurement_; // doppler velocity of point - gtsam::Pose3 pose_R_B_; // pose of radar in B - gtsam::Vector3 angular_velocity_B_; // angvel from IMU during radar exposure -public: - typedef NoiseModelFactor3 Base; - - DopplerFactor( - const gtsam::Point3 & point, const double doppler, const gtsam::Pose3 & pose_R_B, - const gtsam::Vector3 & angular_velocity_B, const gtsam::Key k0, const gtsam::Key k1, - const gtsam::Key k2, const gtsam::SharedNoiseModel & model) - : Base(model, k0, k1, k2), - point_measurement_(point), - doppler_measurement_(doppler), - pose_R_B_(pose_R_B), - angular_velocity_B_(angular_velocity_B) - { - } - - virtual ~DopplerFactor() {} - - // Evaluate error h(x)-z and optionally derivatives - gtsam::Vector evaluateError( - const gtsam::Pose3 & pose_B_W, const gtsam::Vector3 & linear_velocity_W, - const gtsam::imuBias::ConstantBias & imu_bias_B, - boost::optional H0 = boost::none, - boost::optional H1 = boost::none, - boost::optional H2 = boost::none) const - { - const gtsam::Point3 point_hat = point_measurement_.normalized(); // normalize point - const gtsam::Rot3 rot_R_B = pose_R_B_.rotation(); // R from {R} to {B} - const gtsam::Point3 l_R_B = pose_R_B_.translation(); // translation of {R} expressed in {B} - const gtsam::Rot3 rot_B_W = pose_B_W.rotation(); // R from {B} to {W} - - // calculate influence of angular velocity on linear velocity through l_R_B - const gtsam::Vector3 linear_velocity_from_angular_B = - (angular_velocity_B_ - imu_bias_B.gyroscope()).cross(l_R_B); - const gtsam::Vector3 linear_velocity_R = - rot_R_B.transpose() * - (rot_B_W.transpose() * linear_velocity_W + linear_velocity_from_angular_B); - - // residual formulated as h(x) - z - const double doppler_estimate = -point_hat.dot(linear_velocity_R); - const gtsam::Vector residual = - (gtsam::Vector(1) << (doppler_estimate - doppler_measurement_)).finished(); - - // df/dtau - if (H0) { - H0->resize(1, 6); - - (*H0).leftCols(3) = -point_hat.transpose() * rot_R_B.transpose() * - (rot_B_W.transpose() * gtsam::skewSymmetric(linear_velocity_W) * - rot_B_W.matrix()); // rotation - (*H0).rightCols(3) = gtsam::Matrix13::Zero(); // translation - } - - // df/dv - if (H1) { - H1->resize(1, 3); - - *H1 = -point_hat.transpose() * rot_R_B.transpose() * rot_B_W.transpose(); - } - - // df/dBias - if (H2) { - H2->resize(1, 6); - - (*H2).leftCols(3) = gtsam::Matrix13::Zero(); // accelerometer - (*H2).rightCols(3) = - -point_hat.transpose() * rot_R_B.transpose() * gtsam::skewSymmetric(l_R_B); // gyroscope - } - - return residual; - } -}; - // Unary factor which linearizes to a hessian class DopplerHessianFactor : public gtsam::NonlinearFactor { From 95a7c1b6ba351542e16ad87d5ddc3b05cc698581 Mon Sep 17 00:00:00 2001 From: morten Date: Thu, 25 Jun 2026 10:32:53 +0200 Subject: [PATCH 3/4] updating comments --- mimosa/include/mimosa/radar/factor.hpp | 8 ++++---- mimosa/src/radar/manager.cpp | 7 +++---- 2 files changed, 7 insertions(+), 8 deletions(-) diff --git a/mimosa/include/mimosa/radar/factor.hpp b/mimosa/include/mimosa/radar/factor.hpp index b3d917e..4499604 100644 --- a/mimosa/include/mimosa/radar/factor.hpp +++ b/mimosa/include/mimosa/radar/factor.hpp @@ -125,10 +125,10 @@ class DopplerHessianFactor : public gtsam::NonlinearFactor double f = 0.0; int i = -1; - static_targets_.clear(); - static_targets_.reserve(targets_.size()); - dynamic_targets_.clear(); - dynamic_targets_.reserve(targets_.size()); + // static_targets_.clear(); + // static_targets_.reserve(targets_.size()); + // dynamic_targets_.clear(); + // dynamic_targets_.reserve(targets_.size()); for (const TargetData & t : targets_) { i++; diff --git a/mimosa/src/radar/manager.cpp b/mimosa/src/radar/manager.cpp index 1a2085f..460784c 100644 --- a/mimosa/src/radar/manager.cpp +++ b/mimosa/src/radar/manager.cpp @@ -80,8 +80,6 @@ void Manager::callback(const sensor_msgs::PointCloud2::ConstPtr & msg) } debug_msg_.n_points_valid = valid_targets_.size(); - // TODO: Zero velocity check - gtsam::NonlinearFactorGraph new_factors; auto dhf = std::make_shared( valid_targets_, config_.base.T_B_S, angular_velocity_mean, X(0), V(0), B(0), @@ -99,8 +97,9 @@ void Manager::callback(const sensor_msgs::PointCloud2::ConstPtr & msg) initialized_ = true; debug_msg_.t_graph_declare = sw_declare.elapsedMs(); - debug_msg_.n_points_static = dhf->getStatic().size(); - debug_msg_.n_points_dynamic = dhf->getDynamic().size(); + // currently not updating static vs dynamic points + // debug_msg_.n_points_static = dhf->getStatic().size(); + // debug_msg_.n_points_dynamic = dhf->getDynamic().size(); debug_msg_.header.stamp.fromSec(corrected_ts_); debug_msg_.t_full = sw.elapsedMs(); logger_->debug( From 18e0ebebae65643de5880df0fbc6dd869e094127 Mon Sep 17 00:00:00 2001 From: morten Date: Thu, 25 Jun 2026 10:45:49 +0200 Subject: [PATCH 4/4] precommit format --- mimosa/include/mimosa/radar/factor.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mimosa/include/mimosa/radar/factor.hpp b/mimosa/include/mimosa/radar/factor.hpp index 4499604..fa7ed27 100644 --- a/mimosa/include/mimosa/radar/factor.hpp +++ b/mimosa/include/mimosa/radar/factor.hpp @@ -38,7 +38,7 @@ class DopplerHessianFactor : public gtsam::NonlinearFactor gtsam::Pose3 pose_R_B_; // pose of radar in B gtsam::Vector3 angular_velocity_B_; // angvel from IMU during radar exposure; - double noise_sigma_; // in m/s + double noise_sigma_; // in m/s mutable std::vector target_status_; // classification of targets as static or non-static mutable TargetVector static_targets_;