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; bool keep_finalized_trajectory = false; 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 relative_pose_increment_sigma_lin = 0.0; double relative_pose_increment_sigma_ang = 0.0; double relative_pose_increment_sigma_per_sqrt_meter = 0.0; double relative_pose_increment_sigma_per_sqrt_rad = 0.0; double pose_robust_huber_threshold = 0.0; 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 fuse_ground_truth_label = false; double pose_min_sample_period = 0.0; 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.
bool keep_finalized_trajectory = false
Keep the pose of every keyframe as it leaves the sliding window, i.e. its final optimized value, and serve them through estimated_trajectory(). Off by default: in a live system nothing reads them and the container would grow without bound. Intended for offline runs that want the smoothed trajectory instead of the front end’s own registered poses.
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 relative_pose_increment_sigma_lin = 0.0
Linear sigma asserted for ONE increment, whatever it spans. Required, must be > 0. Motion-dependent error belongs in relative_pose_increment_sigma_per_sqrt_meter, which scales with the increment and so with pose_min_sample_period. [m]
double relative_pose_increment_sigma_ang = 0.0
Angular counterpart of relative_pose_increment_sigma_lin. Required, must be > 0. [rad]
double relative_pose_increment_sigma_per_sqrt_meter = 0.0
Growth of the per-increment linear uncertainty with the distance the increment spans, as a random walk: the asserted variance is relative_pose_increment_sigma_lin^2 + k^2 * distance. A dead-reckoned error is not a fixed number per reading: slips and skids happen while moving, independently along the path. Because variance, not sigma, grows linearly with distance, the total asserted over a path does not depend on how many increments it is split into (keyframe rate, pose_min_sample_period). relative_pose_increment_sigma_lin is the floor asserted while standing still. 0 disables it. [m/sqrt(m)]
double relative_pose_increment_sigma_per_sqrt_rad = 0.0
Angular counterpart of relative_pose_increment_sigma_per_sqrt_meter, growing with the rotated angle. [rad/sqrt(rad)]
double pose_robust_huber_threshold = 0.0
If > 0, fuse_pose() factors, both absolute and relative, are wrapped in a Huber m-estimator with this threshold, in whitened units, i.e. in sigmas of the covariance supplied for that factor. So it only means something when that covariance is calibrated. A dead-reckoning source produces occasional gross errors (a slipped foot, a skidded wheel), and under a plain Gaussian each one pulls with unbounded weight. Huber is convex: unlike a redescending kernel it cannot switch off a correct reading whose keyframe starts far from it. The one-time anchor factor of a relative source is not wrapped. 0 disables it.
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]. Set to 0 to ignore IMU attitude readings, e.g. for drivers that publish a constant placeholder orientation instead of flagging it as missing.
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 fuse_ground_truth_label = false
Allow fusing CObservationRobotPose observations labeled “ground_truth”, which is how MOLA’s offline dataset sources publish their reference trajectory. Off by default: fusing it makes any accuracy number measured against that same trajectory meaningless.
double pose_min_sample_period = 0.0
High-rate same-sensor decimation for fuse_pose() sources. If > 0, readings of a given frame_id arriving less than this many seconds after the last kept one of that same frame_id are dropped before they reach the graph. A pose source publishing in the hundreds of Hz otherwise packs the sliding window with keyframes that carry no new information, and the solver pays for every one of them.
Nothing is lost for odometry frames: a dropped reading does not advance the source’s chain of increments, so the next kept one asserts the whole merged span as a single increment. A “map” pose stands alone, so there is nothing to accumulate for it either.
It applies to EVERY fuse_pose() source, e.g. LiDAR odometry too. Keep it well below the period of any source that must not be thinned: at about that period, timestamp jitter alone drops every other reading. A dropped reading still refreshes the source’s own-frame pose that estimated_navstate() extrapolates from.
0 disables it. Independent of, and coarser than, min_time_difference_to_create_new_frame. [seconds]
double imu_min_sample_period = 0.0
High-rate same-sensor decimation for IMU. If > 0, IMU readings are averaged over periods of this many seconds, and one averaged reading per period is fused (see mola::imu::ImuAverager). Averaging, rather than keeping one raw reading, keeps vibration faster than this rate from aliasing into the gravity and angular-velocity factors. 0 fuses every reading. [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.