class mola::state_estimation_smoother::Parameters
Overview
Parameters needed by StateEstimationSmoother.
#include <Parameters.h> class Parameters { public: // structs struct Visualization; // fields std::string vehicle_frame_name = "base_link"; std::string reference_frame_name = "map"; std::string enu_frame_name = "enu"; bool publish_map_to_odom_tf = false; std::string map_to_odom_frame_name; std::string map_to_odom_child_frame; bool publish_fused_vehicle_tf = false; std::string fused_vehicle_frame_name = "base_link_fused"; KinematicModel kinematic_model = KinematicModel::ConstantVelocity; double max_time_to_use_velocity_model = 2.0; double sliding_window_length = 5.0; double min_time_difference_to_create_new_frame = 0.01; double time_between_frames_to_warning = 3.0; double gnss_nearby_keyframe_stamp_tolerance = 1.0; double imu_nearby_keyframe_stamp_tolerance = 0.10; double odometry_min_sample_period = 0.0; double odom_motion_model_a1 = 0.01; double odom_motion_model_a2 = 0.05729577951; double odom_motion_model_a3 = 0.017453292519943295; double odom_motion_model_a4 = 0.05; double odom_motion_model_min_std_xy = 1e-3; double odom_motion_model_min_std_phi_deg = 0.1; double sigma_relative_pose_linear = 0.0; double sigma_relative_pose_angular = 0.0; double imu_attitude_sigma_deg = 2.0; double imu_attitude_azimuth_offset_deg = 0.0; double imu_normalized_gravity_alignment_sigma = 0.4; double imu_angular_velocity_sigma = 0.10; bool estimate_geo_reference = false; std::optional<mola::Georeferencing> fixed_geo_reference; double convergence_max_position_sigma = 1.0; double convergence_max_orientation_sigma_deg = 5.0; bool publish_estimated_georef_on_convergence = true; double gnss_huber_threshold = 1.5; uint32_t additional_isam2_update_steps = 3; bool async_backend = false; double fast_predictor_buffer_length = 1.0; std::string do_process_imu_labels_re = ".*"; std::string do_process_odometry_labels_re = ".*"; std::string do_process_gnss_labels_re = ".*"; bool odometry_relative_factors = false; double imu_min_sample_period = 0.0; double sigma_random_walk_acceleration_linear = 0.5; double sigma_random_walk_acceleration_angular = 10.0; bool predict_twist_filter_enabled = true; double predict_twist_filter_time_const = 0.3; double sigma_integrator_position = 0.10; double sigma_integrator_orientation = 0.10; double sigma_twist_from_consecutive_poses_linear = 1.0; double sigma_twist_from_consecutive_poses_angular = 1.0; mrpt::math::TTwist3D initial_twist; double initial_twist_sigma_lin = 0.1; double initial_twist_sigma_ang = 0.1; bool enforce_planar_motion = false; std::optional<double> link_first_pose_to_reference_origin_sigma; Visualization visualization; // methods void loadFrom(const mrpt::containers::yaml& cfg); };
Fields
std::string vehicle_frame_name = "base_link"
Used to publish timely pose updates.
std::string reference_frame_name = "map"
Used to publish timely pose updates. Typically, ‘map’ or ‘odom’, etc. See the docs online.
std::string enu_frame_name = "enu"
The ENU geo-reference frame. See the docs online.
bool publish_map_to_odom_tf = false
If true, spinOnce() additionally advertises the REP-105 correction reference_frame -> map_to_odom_frame_name (i.e. map -> odom), taken directly from the estimator’s own T_map_to_odom_i graph variable. This is time-consistent and smooth by construction, so the ROS bridge can forward it to /tf verbatim instead of composing it from (map->base_link) * (odom->base_link)^-1 (that composition samples the two factors at mismatched timestamps and injects motion-correlated jitter). The primary reference_frame -> vehicle_frame update is still advertised, so the high-rate vehicle-pose topic is unchanged. The extra update carries a distinct method suffix (/map_odom) so the bridge’s TF/odometry source filters can route it independently. Default off.
std::string map_to_odom_frame_name
The odometry frame_id (source) whose T_map_to_odom is published when publish_map_to_odom_tf is true. Empty = auto-select when exactly one odometry source is known (the common single-wheel-odometry case); required if several odometry sources exist. This selects which fused source to read; it is NOT necessarily the ROS /tf child frame name (see map_to_odom_child_frame).
std::string map_to_odom_child_frame
The /tf child frame the map -> odom correction is published under when publish_map_to_odom_tf is true. Empty = use map_to_odom_frame_name (the source’s own frame_id). Set this when the fused source’s internal frame_id differs from the REP-105 odom /tf frame the external wheel driver publishes odom -> base_link for: the two must match for the tree map -> odom -> base_link to connect, and an odometry source’s sensor label (e.g. odom_wheels) is often not that ROS frame (odom).
bool publish_fused_vehicle_tf = false
If true, spinOnce() additionally advertises the fused vehicle pose in a separate child frame fused_vehicle_frame_name (default base_link_fused) at the module rate. Because it is a distinct child frame it never collides with a odom -> base_link chain on /tf, so it gives consumers a high-rate fused pose without touching the canonical robot tree (no sensor frames hang off it). Default off.
std::string fused_vehicle_frame_name = "base_link_fused"
Child frame name for publish_fused_vehicle_tf.
KinematicModel kinematic_model = KinematicModel::ConstantVelocity
Kinematic model to be used in the internal motion model factors. Options: KinematicModel::ConstantVelocity, KinematicModel::Tricycle
double max_time_to_use_velocity_model = 2.0
Valid estimations will be extrapolated only up to this time since the last incorporated observation. If a request is done farther away, an empty estimation will be returned.
double sliding_window_length = 5.0
Time to keep past observations in the filter.
double time_between_frames_to_warning = 3.0
If the time between two keyframes is larger than this, a warning will be emitted; but the algorithm will keep trying its best.
double gnss_nearby_keyframe_stamp_tolerance = 1.0
When adding GNSS observations, specially with consumer grade receivers with errors larger than a few centimeters, we may be more permissive in the temporal distance between the GNSS datum and the associated existing keyframe. This parameter is the extended, alternative value to use instead of “min_time_difference_to_create_new_frame”. [seconds]
double imu_nearby_keyframe_stamp_tolerance = 0.10
When adding IMU observations, this is the temporal distance between the IMU reading and the associated existing keyframe. This applies to gravity-oriented (IMU attitude) and gravity-estimation (accelerometer) only factors, not to high-frequency IMU preintegration.
This parameter is the extended, alternative value to use instead of “min_time_difference_to_create_new_frame”. [seconds]
double odometry_min_sample_period = 0.0
High-rate same-sensor decimation for wheel odometry. If > 0, wheel odometry readings arriving less than this many seconds after the last kept one are MERGED rather than turned into their own keyframe: the reading is dropped without advancing the pose anchor, so the next kept reading fuses the accumulated pose increment together with its accumulated motion-model covariance. This caps the keyframe/factor rate a high-rate odometry stream imposes on the solver without discarding any motion. 0 disables it (every reading creates/updates a keyframe as before). This is independent of, and coarser than, min_time_difference_to_create_new_frame (which only merges near-simultaneous readings from different sensors). [seconds]
double sigma_relative_pose_linear = 0.0
[m] Flat, dt-independent floor added to the position variance of the pose returned by estimated_navstate() (and by the asynchronous fast predictor). 0 disables it, which is the shipped default and reproduces the behavior of every release before this parameter existed.
This is NOT a model of anything the graph knows. It exists because a front end may use the returned covariance as a WEIGHT, not merely as a diagnostic: mola_lidar_odometry turns a non-zero pose.cov_inv into a prior factor inside its ICP solve. The marginal this estimator reports is the graph’s own opinion of its extrapolation, and when the graph is confident that opinion is tight enough to pin the registration to the prediction instead of letting the scan data move it. A floor bounds how hard the estimator is allowed to lean on the front end, independently of how well conditioned the window happens to be.
The lightweight estimator has carried the same knob since it was written (mola_state_estimation_simple, same parameter names, 0.5 m / 0.1 rad), which is why the two estimators hand a front end priors of very different strength. Set this to that value to make the two comparable.
Applied isotropically, so it is invariant to whether the caller reads the rotation block in Euler (yaw, pitch, roll) or Lie-tangent (w_x, w_y, w_z) order: adding the same variance to all three diagonal entries commutes with any permutation of them.
double sigma_relative_pose_angular = 0.0
[rad] Angular counterpart of sigma_relative_pose_linear. 0 disables it.
double imu_attitude_sigma_deg = 2.0
When an IMU provides global attitude measurements (azimuth and gravity aligned), this is the uncertainty or noise sigma [degrees].
double imu_attitude_azimuth_offset_deg = 0.0
When an IMU provides global attitude measurements (azimuth and gravity aligned), this must define the angle (in degrees) to add to IMU yaw orientation to obtain azimuth so 0 deg is North. Note that ENU axes are such vehicle yaw is 0 when pointing East instead. Example cases:
IMU absolute yaw=0 points True North ==> offset=0
IMU absolute yaw=0 points East ==> offset=-90
double imu_normalized_gravity_alignment_sigma = 0.4
When using an IMU with acceleration, use this sigma to estimate the up-vector, hence gravity-align the map. Set to 0 to disable.
double imu_angular_velocity_sigma = 0.10
When an IMU provides angular velocity (gyroscope), add a direct prior on the corresponding keyframe’s body-frame angular-velocity variable, sensor-to-vehicle rotated. Without this, angular velocity is only constrained by the constant-velocity kinematic factor between keyframes plus whatever pose factors happen to be fused, so a genuine, fast rotation can go unrepresented in the graph for as long as those lag (e.g. while ICP is failing and not fusing new poses at all) which then also lags the short-term prediction consumed by front ends as their next ICP prior/initial guess. Sigma is in [rad/s]; set to 0 to disable.
The default (0.10 rad/s ~ 5.7 deg/s) is deliberately loose: this is one raw, instantaneous sample taken as the average angular velocity over a whole keyframe interval, on a platform that vibrates, so its honest uncertainty is several deg/s. A tighter value pins each keyframe’s W hard to its own noisy sample; two such priors on near-simultaneous keyframes then disagree by more than the constant-velocity factor between them allows, and the only way the optimizer can relieve that is by rotating the poses, which can make the window numerically singular. Tune tighter only for a genuinely well-characterized, well-isolated gyro.
bool estimate_geo_reference = false
If true, this estimator will try to estimate the best geo-referencing for {enu} -> {map} from incoming GNSS readings and other sensors. If false, geo-referencing is assumed to be given from either these initial parameters or, if not set, from an external source (e.g. a geo-referenced .mm map loaded in mola_lidar_odometry).
std::optional<mola::Georeferencing> fixed_geo_reference
If estimate_geo_reference is false and this is set, the geo-referencing will be taken from this value and never attempted to be optimized or changed. Other geo-reference information coming from external sources may override this fixed initial value, though.
double convergence_max_position_sigma = 1.0
Maximum position sigma (in meters) to consider state estimation converged. Used when other modules query has_converged_localization(). This applies to both, the map->base_link and enu->map poses.
double convergence_max_orientation_sigma_deg = 5.0
Maximum orientation sigma (in degrees) to consider state estimation converged. Used when other modules query has_converged_localization(). This applies to both, the map->base_link and enu->map poses.
bool publish_estimated_georef_on_convergence = true
If true and estimate_geo_reference is true, once converged, this module will publish the estimated geo-referencing via MapSourceBase.
double gnss_huber_threshold = 1.5
Huber robust cost threshold for GNSS factors [sigmas]. Applied in whitened (normalized) residual space, so units are standard deviations, not meters. Suitable values: ~1.5 (switch to linear loss beyond 1.5σ). Set to 0 to disable robust cost (plain Gaussian).
uint32_t additional_isam2_update_steps = 3
Each new sensor will become a call to isam2.update(), plus this number of additional refining steps. In theory, more steps lead to more accurate results.
bool async_backend = false
If true, the iSAM2 window solve runs in a dedicated backend thread and estimated_navstate() is served by a lock-free FastPredictor re-anchored on the latest backend solution, so a query never runs a solve nor blocks on stateMutex_ (per-query latency stays sub-millisecond). If false (default) the solve runs inline on the caller’s thread, which is deterministic and is what the unit tests and offline batch runs use.
double fast_predictor_buffer_length = 1.0
[s] Only used when async_backend is true : how far back the FastPredictor keeps high-rate observations to extrapolate from the anchor.
std::string do_process_imu_labels_re = ".*"
regex for IMU sensor labels (ROS topics) to accept as IMU readings.
std::string do_process_odometry_labels_re = ".*"
regex for odometry inputs labels (ROS topics) to be accepted as inputs
std::string do_process_gnss_labels_re = ".*"
regex for GNSS (GPS) labels (ROS topics) to be accepted as inputs
bool odometry_relative_factors = false
Fuse each wheel-odometry increment as a RELATIVE constraint between the two keyframes it spans, BetweenFactor(T(kf_prev), T(kf_now), increment, incrementCov), in addition to the absolute pose-in-{odom_i} factor that is always added.
Why it exists: the absolute factor asserts a dead-reckoned pose, so the covariance that must go with it is the whole accumulated dead-reckoning uncertainty which is honest, but by the time it is honest it is also nearly uninformative, and it throws away the one thing wheel odometry is genuinely good at: short-baseline relative motion. The relative factor states exactly that, with exactly the covariance the motion model computes for it, and nothing accumulates.
When enabled, exactly ONE absolute pose-in-{odom_i} factor is ever added, on the first kept reading, because one is all it takes: T_map_to_odom_i is a single rigid variable, so given that anchor plus the relative chain, every later “keyframe k is at odom pose p_k” is already implied. Extra absolute factors do not observe the frame any better; they re-inject the accumulated dead-reckoning error into the map poses.
It also needs no renewal when its keyframe ages out: keyframes leave through the fixed-lag smoother’s marginalization, which folds their factors into a linear marginal on the surviving variables, so the anchor keeps constraining T_map_to_odom_i after its own keyframe is gone.
A relative factor is skipped, rather than forced, when the previous odometry keyframe has already been marginalized out: a factor on a variable the smoother no longer holds would resurrect it as a free state.
Off by default on a measured trade, not out of caution. It is the better of the two ways to fuse on real data (BotanicGarden, better APE on 5 of 7 sequences than the absolute-only formulation), but it costs geo-referencing: the ENU->map rotation error in test-navstate-odom-gnss-fusion grows past that test’s 5 deg gate, where the absolute-only formulation stays inside it. Asserting the dead-reckoned pose absolutely, however weakly, is apparently what makes that yaw well observed. Enable this if you fuse wheel odometry and do not estimate geo-referencing.
double imu_min_sample_period = 0.0
High-rate same-sensor decimation for IMU. If > 0, IMU readings arriving less than this many seconds after the last processed one are skipped. Unlike wheel odometry, IMU attitude/gravity are absolute observations, so dropping intermediate readings simply lowers the redundant-factor rate; there is nothing to accumulate. 0 disables it. [seconds]
double sigma_random_walk_acceleration_angular = 10.0
Angular random-walk sigma for the constant-velocity factor between keyframes. Kept loose on purpose: a tight value makes that factor override the per-keyframe gyro prior, averaging genuine fast rotations away (the optimized W then keeps only a fraction of the measured rate). The downstream predict-twist low-pass damps the extra boundary-node variability this allows.
bool predict_twist_filter_enabled = true
If true, the short-term extrapolation velocity used by estimated_navstate() is a low-pass-filtered version of the newest keyframe’s optimized twist, instead of the raw value. The newest keyframe is the boundary node of the sliding window (constrained on one side only), so its raw velocity is the noisiest state in the graph; extrapolating it un-damped injects jitter into a front end’s motion prior. This mirrors the velocity low-pass of the lightweight estimator. On by default so the prior stays smooth; it is a plain dt-aware EMA, hence deterministic and reproducible run-to-run.
double predict_twist_filter_time_const = 0.3
[s] Time constant of the predict-twist low-pass (predict_twist_filter_enabled). Larger = smoother prior but more lag behind genuine acceleration.
std::optional<double> link_first_pose_to_reference_origin_sigma
If set, the first ever frame will also have an SE(3) edge favoring it to be the identity in the “reference_frame”, with a sigma given by this value. Use a small number, like 1e-6, for initialing the first odometry pose near the map origin. Do not set when using geo-referenced maps.
Methods
void loadFrom(const mrpt::containers::yaml& cfg)
Loads all parameters from a YAML map node.