CHANGELOG

Changelog for package fuse_models

1.1.1 (2024-05-02)

• Required formatting changes for the lastest version of ROS 2 Rolling (#368)

• Contributors: Stephen Williams

1.1.0 (2024-04-20)

• Port support for Ceres 2.1.0 Manifold class into ROS 2 Rolling (#366) * Support gcc12 and ceres 2.1.0 * Add support for the Manifold class when using Ceres Solver version 2.1.0 and above * General clean up for Ceres 2.2.0 support * Updated serialization support to be backwards compatible with previously serialized files * Formatting changes required for ROS 2 Rolling / Ubuntu Noble

• Contributors: Stephen Williams

1.0.1 (2023-03-03)

1.0.0 (2023-03-03)

• Use upstream rclcpp::node_interfaces::NodeInterfaces (#313) * Use upstream rclcpp::node_interfaces::NodeInterfaces * Dereference node arguments to NodeInterfaces ——— Co-authored-by: methylDragon <methylDragon@gmail.com>

• fuse -> ROS 2 fuse_models: Linting (#315)

• Use getParameterName and namespace parameters for publishers (#314)

• Use rclcpp::Clock::wait_until_started (#303)

• fuse -> ROS 2 : Doc Generation (#278) * Port doc generation and fix package.xml for linting * Fix small bugs in package.xml * Use default rosdoc2 settings * Use default rosdoc2 settings * Update fuse_doc for rosdoc2 ——— Co-authored-by: Shane Loretz <sloretz@google.com>

• fuse -> ROS 2 fuse_optimizers: Port fuse_optimizers (#307) Co-authored-by: Shane Loretz <sloretz@osrfoundation.org> Co-authored-by: Shane Loretz <shane.loretz@gmail.com>

• fuse -> ROS 2 fuse_models: Port fuse_models (#304) * Port messages * Port fuse_models * Fix alloc error and some bugs * Wait on result

• fuse -> ROS 2 fuse_publishers : Linting (#305)

• fuse -> ROS 2 fuse_publishers: Port fuse_publishers (#299) Co-authored-by: Shane Loretz <shane.loretz@gmail.com>

• fuse -> ROS 2 fuse_constraints : Linting (#298)

• fuse -> ROS 2 fuse_graphs: Linting (#297)

• fuse -> ROS 2 fuse_variables: Linting (#296) * Migrate to .hpp files * Create redirection headers * Make xmllint and uncrustify happy * Wrap most comment lines * Satisfy cpplint

• fuse -> ROS 2 fuse_loss: Port fuse_loss (#287)

• fuse -> ROS 2 fuse_core: Linting (#292)

• fuse -> ROS 2 fuse_core : Parameters and Tests (#286) Co-authored-by: Shane Loretz <sloretz@osrfoundation.org> Co-authored-by: Ivor Wanders <ivor@iwanders.net>

• fuse -> ROS 2 : Port Time (#283)

• fuse -> ROS 2 : Port Logging (#279) Co-authored-by: Tom Moore <tmoore@locusrobotics.com>

• Fuse -> ROS 2 fuse_core: Partial port of fuse_core (#281) Co-authored-by: Brett Downing

• fuse -> ROS 2: Clean up macro usage warnings (#280)

• fuse -> ROS 2 fuse_msgs : Port package and ignore unported packages for now (#277) Co-authored-by: Tom Moore <tmoore@locusrobotics.com>

• [RST-4186] Fix fuse macro names (#263) * Namespace all macros with the FUSE_ prefix. Mark original macros as deprecated. * Update all fuse objects to use the new macro names

• Adding doxygen to all packages (#241)

• [RST-3451] Delay some transform warnings so startup is less chatty

• Contributors: Shane Loretz, Stephen Williams, Tom Moore, methylDragon

0.4.2 (2021-07-20)

• Adding roslint dependency to fuse_viz (#231) * Adding roslint dependency to fuse_viz * Silence CMP0048 warnings

• Contributors: Tom Moore

0.4.1 (2021-07-13)

• Getting versions in sync

• Substract minimum twist covariance from twist covariance (#222) * Substract min twist cov from twist cov If the twist covariance already had a minimum twist covariance added to it to prevent ill-conditioned covariance matrices, we need a way to substract that minimum twist covariance from it before we compute the pose relative covariance. Otherwise, we cannot get the original pose relative covariance because the minimum twist covariance term is multiplies by the time delta, which could actually make the resulting pose relative covariance ill-conditioned or very small, i.e. overconfident.

• [Issue #223] Add an optional tf_timeout parameter to the sensor models (#224)

• Factorize differential mode processing (#219) * Factorize differential mode processing * Throttle log message when transform message fails

• Target frame optional (#217) * Make target_frame optional * Remove optional target_frame parameters in tests

• Transform message in differential mode (#216) * Transform message in differential mode This is important because the relative transformation is not the same if the sensor and target frame are different. Consider for example the case of an IMU sensor upside down: * The robot base frame is base_link * The IMU sensor frame is imu_link * The imu_link transformation wrt base_link is 180 degrees wrt the y or x axis * The angular velocity around the z axis has opposite sign in the sensor frame wrt the target frame * Require pose_target_frame in differential mode * Roslint * Add pose_target_frame to optimizer test config

• Use fuse_core::getPositiveParam for all ros::Duration parameters (#212) * Use fuse_core::getPositiveParam for ros::Duration * Use fuse_core::getPositiveParam for TF durations

• Add tcp_no_delay parameter to sensor models (#211)

• Add ability to throttle covariance computation (#209)

• Add invert_tf to Odometry2DPublisher (#206) This allows to publish the inverse TF transform. This can be useful to skip the odom->base_link lookupTransform() when braodcasting map->base_link, which must be broadcasted as map->odom because TF tree doesn’t support more than a single parent per frame, base_link in this case. This is particular relevant when predict_to_current_time is enabled, because the lookupTransform() could take a while, causing delays.

• Support throttling serialized graph publisher (#204) * Change sensor proc from gtest to gmock target * Move ThrottledCallback to fuse_core * Support generic callbacks in ThrottledCallback * Throttle graph publishing * Overload getPositiveParam for ros::Duration * Use getPositiveParam for ros::Duration parameters

• Add linear acceleration to synchronizer (#205)

• Use local latest_stamp in notifyCallback (#203)

• Use dedicated spinner for publish timer callback (#201) * Use dedicated spinner for publishTimerCallback * This reduces the jitter in the output topics and TF transform stamp because it allows the notifyCallback and publishTimerCallback to run concurrently. The notifyCallback might take longer than the timer period sometimes, mostly because the covariance computation is an expensive operation. * There is a subtle change of behaviour with this implementation! Before, the publishTimerCallback overwrote the odom_output_ and acceleration_output_ with the predicted state. Now it does not, and if it gets called twice or more times consecutively, it predicts since the last time the state was computed and updated in the notifyCallback. With the notifyCallback and publishTimerCallback running concurrently it is not trivial to keep the previous behaviour efficiently, because we would have to lock the entire callbacks to avoid the publishTimerCallback to overwrite a new state being computed concurrently in the notifyCallback. That being said, the predicted state is likely the same in both implementation. That is, the result is likely the same if we use multiple steps or a single one to predict the last state forward to the current time.

• Add fuse_models::GraphIgnition sensor model (#196)

• Add fuse_models::Transaction sensor model (#195)

• Fix Unicycle2D constructor doxygen (#198)

• Remove deprecated ::Model models leftovers (#194) * Remove fuse_models::twist_2d::Model plugin declaration * Remove empty space in fuse_plugins.xml * Update ::Model names to new names in doxygen comments

• Conditionally test_depend on benchmark (#189)

• Fix typo in jacobian comments (#191)

• Fix throttle (#190) * Update last called time adding throttle period Instead of setting to now, which could be larger than the expected call time. * Init last called time to now the first time * Fix check for init/zero last called time We cannot use isValid because that does not check the last called time is zero, but a completely different thing. We must use isZero.

• Check canTransform output and show error if false (#188) * Check canTransform output and show error if false * Fix pose -> twist typo * Lookup transform directly

• Use std::enable_if_t (#187)

• Fix roslint 0.12.0 (#186) * Fix roslint 0.12.0 include_what_you_use warnings Mostly for: * std::move -> #include <utility> * std::make_shared and similar -> #include <memory> * Remove static string variable not permitted by roslint 0.12.0, using a test fixture where needed.

• Only call generator if motion model history empty (#181) * Only call generator if motion model history empty * Handle dt == 0 special case in motion model * Revert test_timestamp_manager.cpp #154 * Add EmptySingleStamp test

• Add use_twist_covariance ROS param and logic to Imu2D differential orientation measurements (#178) * Move pose into previous_pose_ This makes the Odometry2D do the same as the Imu2D. * Allow Imu2D to use twist covariance For differential orientation measurements. * Move pose relative covariance closer to use

• Validate unicycle 2d (#180) * Remove unused EPSILON constexpr * Validate Unicyle2D state and process noise * Add disable_checks param (defaults to false) * Validate state1 and state2 are finite * Validate process noise covariance (after it’s been scaled and multiplied by dt)

• Fix doxygen comment (#177)

• Delay throttle no valid state message (#175) This requires rosconsole >=1.13.8.

• Throttle (#162) * Support throttling sensor model inputs * Add ThrottledCallback rostest

• Fix motion model history (#168) * Fix the motion model history to maintain at least the requested time interval * Apply a similar fix to the MessageBuffer class

• Get positive param API change (#169) * Change getPositiveParam API In order to match the getParam and getRequiredParam, so the value is not returned, but set in an in/out argument. * Move getPositiveParam and other param related functions to parameter.h from util.h, and updated the ros/unit tests accordingly. * Fix wrapAngle2D expected range to [-Pi, +Pi) Instead of (-Pi, +Pi], and update unit test to reflect that.

• Don’t require frame if empty indices (#166)

• Fix Unicycle2DIgnition set_pose (#154) * Initialize StateHistoryElement::velocity_yaw * Process ignition transactions individually * Call motion model generator with last stamp * Skip optimization cycle if transaction is empty

• C++14 for test_unicycle_2d_state_cost_function (#157)

• Print state history (#156) * Add print method to StateHistoryElement * Add print method to Unicycle2D It only prints the history state for now though.

• Minor typo fixes (#155)

• Get minimum_pose_relative_covariance_diagonal (#150) Regardless of the value of independent, because the fuse_models::Odometry2D sensor model checks for use_twist_covariance before independent, and we could end up with an uninitialized minimum_pose_relative_covariance_diagonal.

• Support ScaledLoss (#141)

• Remove duplicated roslint build_depend (#146)

• Remove old acceleration_2d folder (#145)

• Cleanup validation checks (#139) * Add getCovarianceDiagonalParam helper This allows to load a covariance matrix from the parameter server, provided in a list with the diagonal values. * Add isSymmetric and isPositiveDefinite helper functions

• Use twist covariance for differential dependent (#138) In the fuse_models::Odometry2D sensor model, when differential: true and independent: false, the relative pose covariance should NOT be computed from the consecutive absolute pose covariance matrices because they grow unbounded, so the resulting relative pose covariance suffers from numerical issues. Instead, we can use the twist covariance of the last pose to compute the relative pose covariance, using the time difference between the consecutive absolute poses. The only limitation is that we cannot throttle the input topics, because otherwise the twist covariance from the intermediate/throttled messages is missed. We’ll have to throttle inside the sensor model, by integrating the intermediate messages.

• Support dependent relative pose measurements (#137) * Added a “dependent” covariance calculation option to the “differential” mode * Added an independent param that defaults to true to keep the current behaviour * Added a minimum_pose_relative_covariance_diagonal param that is added to the resulting pose relative covariance in order to guarantee that it’s not zero or ill-conditioned.

• Scale process noise covariance (#130) * Scale process noise covariance This scales the process noise covariance pose by the norm of the current state velocity. A new parameter velocity_norm_min is added, that prevents the process noise scaling from setting the pose components to zero or a very small value that could lead to NaN or a rank deficient Jacobian in the problem solved, due to an ill-condition covariance for the process noise.

• Better validation of partial measurement output (#131) * Relax the default precision when validating the covariance matrix is symmetric. * Print the covariance matrix with Eigen::FullPrecision when the symmetry test fails with isApprox, so we can see the magnitude of the error. * Show source if validation fails * Changes from throwing/crashing to ROS_ERROR. * Add eigenvalues to non-PSD error check * Add disable_checks param to sensor models

• Publish linear acceleration (#129) * Publish linear acceleration * Also use linear acceleration if predicting to the current time if the new param predict_with_acceleration is true (default value).

• Explicitly call boost::range::join (#128) Otherwise we could get a compilation error due to an ambiguous overloaded join function when some additional boost/algorithm headers are included.

• Add fuse_loss pkg with plugin-based loss functions (#118)

• Validate partial measurements (#125)

• Don’t read pose_target_frame if differential (#126) If differential is true, the pose_target_frame is not used.

• Only allow exact timestamp transformations (#123)

• Benchmark unicycle_2d state cost function (#121) The benchmark targets are now only build if CATKIN_ENABLE_TESTING is ON, which means that benchmark is now a test_depend and not a depend. However, the benchmarks are NOT gtests, so they are built directly on catkin build, i.e. there is no need to run make run_tests after. For this reason, the find_package on benchmark is no longer REQUIRED, but QUIET instead. The benchmark is built only if the benchmark package is FOUND.

• Removed the explicit ‘-std=c++14’ compile flag (#119) * Removed the explicit ‘-std=c++14’ compile flag * Changed the CXX_STANDARD setting to be per-target instead of global * Added the CXX_STANDARD_REQUIRED setting to all targets

• Predict jacobians per parameter block (#115)

• fix compilation in Kinetic (#112)

• Wait for reset service existence (#116)

• Publish odometry with timer and allow to predict it (#109)

• Use measurement stamps for transformed variables (#113)

• [RST-2149] Added the configured device_id to the log message (#110)

• [RST-2438] Make ceres params loaders reusable (#104) * Moved the Ceres loadFromROS functions into reusable functions in fuse_core * Load solver parameters for the batch optimizer

• Expose Ceres Solver, Problem and Covariance Options as ROS parameters (#78)

• [RST-2427] Added a ‘source’ field to the constraints. This is an API-breaking change. (#101)

• [RST-2340] Add serialization support to fuse (#98)

• RST-2390 Renaming unicycle_2d (#90) * Renaming unicycle_2d

• Renaming twist_2d (#89)

• Renaming pose_2d (#88)

• Renaming odometry_2d (#87)

• Renaming imu_2d (#86)

• RST-2390 Renaming acceleration_2d (#85) * Renaming acceleration_2d

• Renaming package to fuse_models

• Preparing for move

• Contributors: Davide Faconti, Enrique Fernandez Perdomo, Stephen Williams, Tom Moore, sjphilli

0.4.0 (2019-08-14)

0.3.0 (2019-08-14)

0.2.0 (2019-07-12)

• Get predict_to_current_time ROS param (#17)

• [RST-2202] Catch potential errors when computing the covariances (#18) * Clear the covariance on error

• Remove angles header not used (#16)

• Default to private ~reset and ~set_pose names (#14)

• Depend on sensor_msgs and nav_msgs (#15)

• Depend on libceres-dev instead of ceres-solver (#11) * Depend on libceres-dev instead of ceres-solver * Add missed depend on angles

• Resolve names before subscribing (#10)

• Linter/style changes

• Use std::bind instead of std::bind2nd std::bind2nd is marked as deprecated in C++11 Co-Authored-By: Stephen Williams <stephen.vincent.williams@gmail.com>

• Add sensor_proc test Only for: * mergeIndices * appendPartialMeasurement

• Fix appendPartialMeasurement by merging indices Position and orientation indices are merged together into a single std::vector<size_t> of indices, applying the appropriate offset to the orientation indices. This is passed to appendPartialMeasurement, which should be called only once. It doesn’t need the base_index and offset args anymore.

• [RST-2128] fuse rl ignition sensor (#6)

• [RST-2144] Updated macro calls on all objects to support proper Eigen memory alignment (#8)

• Bug in motion model history (#7)

• Use linear indices for linear velocity (#5) Not angular indices, which is wrong and produces a crash at runtime because an assert fails.

• Fix appendPartialMeasurement assignments (#4)

• Store ros::Subscriber in sensor model attribute (#3) Store ros::Subscriber in sensor model attribute

• Tailor: Creating Jenkinsfile

• Adding base_link_output_frame_id to the 2D odom publisher (#1)

• Contributors: Enrique Fernandez, Enrique Fernandez Perdomo, Enrique Fernández Perdomo, Stephen Williams, Tom Moore, locus-services

0.1.0 (2019-03-18)

• [RST-1625] Use the stamped variable synchronizer (#13)

• Tailor: Updating Jenkinsfile

• Tailor: Updating Jenkinsfile

• Tailor: Updating Jenkinsfile

• Updated package for changes to fuse_core::Transaction (#11)

• Tailor: Creating Jenkinsfile

• Simplifying

• Just using rotation

• Adding tf2 overloads for twist and acceleration

• Updated derived sensors for recent change to the sensor API

• Adding 2D odometry publisher

• Adding 2D IMU sensor model

• Feature/pedantic style change (#7) * White spaaaaaaaaaaaaaaaaaaaaaaace * Moar whitespace

• PR feedback

• Adding support for partial measurements

• Adding 2D odometry sensor

• Enabling partial measurements for fuse_rl

• PR feedback

• PR feedback

• Adding 2d pose sensor

• Adding 2D odometry sensor

• Adding 2D acceleration sensor

• Adding 2D twist sensor

• More comments

• Removing comment

• Using Jacobians to rotate covariances

• PR feedback

• PR feedback

• Adding ability to transform poses

• Adding 2d pose sensor

• Update README.md

• Adding 2D kinematic constraint

• Adding README

• Contributors: David V. Lu!!, Stephen Williams, Tom Moore, locus-services