fuse_core

This package provides the base class interfaces for the various fuse components. Concrete implementations of these interfaces are provided in other packages.

Variable

Within the fuse system, a “variable” is a convenient group of one or more related scalar values. These scalars are treated as a block within the optimizer. If the variable is modeling a time-varying quatity, the an instance of the variable represents value of the state at one specific point in time. The fuse system maintains a history of variable instances, allowing measurements to involve previous states as well as the current state.

When defining a new variable type, there is a balance that must be struct between reusability and convenience. If you define a complex composite state, it is unlikely that any other available components will use that same state definition. If you make the state too granular, then more book-keeping and value lookups will be required to piece together a useful concept from many smallar scalar components.

As an example, let’s consider a 3D pose consisting of a 3D position (x, y, z) and a quaternion orientation (qx, qy, qx, qw). We could define a single state for the 3D pose consisting of all 7 scalar components. Alternatively, we could define two variables types, a 3 dimension position vector and a 4 dimension quaternion. Or we could even define seven variable types, one for each dimension.

Within the fuse_core package, no concrete variables are actually created. We only define the common interface to which all types must adhere. A set of common variable types are provided in the fuse_variables package. And new variable types can be created outside the fuse stack completely. However, similar to how using common ROS messages across nodes allow for code reuse, using common variable types will allow sensor models and motion models to be shared across the community.

Constraint

A Constraint defines a cost function that is connected to one or more variables. This base class defines the required interface of all Constraint objects, and holds the ordered list of involved variable UUIDs. All other functionality is left to the derived classes to implement.

Most importantly, the implementation of the cost function is left to the derived classes, allowing arbitrarily complex sensor models to be implemented outside of the core fuse packages. The cost function must be a valid ceres::CostFunction object. Ceres provides many nice features to make implementing the cost function easier, including an automatic differentiation system. Please see the Ceres documentation for details on creating valid ceres::CostFunction objects (http://ceres-solver.org/nnls_modeling.html). In addition to the cost function itself, an optional loss function may be provided. Loss functions provide a mechanism for reducing the impact of outlier measurements on the final optimization results. Again, see the Ceres documentation for details (http://ceres-solver.org/nnls_modeling.html#lossfunction).

Transaction

A transaction is a group of variable and constraint additions and subtractions that should all be processed at the same time. This arises most often with graph edits, when you want to remove an existing constraint and replace it with one or more new constraints. You don’t want the removal to happen independently of the additions. All graph operations are contained within a Transaction object so that all operations are treated equally.

Changelog for package fuse_core

0.5.0 (2022-02-23)

• [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
• [RST-4390] Allow variables to be held constant during optimization (#243)
  • Add support for holding variables constant
  • Create a 'fixed' landmark
  • Added initial support for marginalizing constant variables
• [RST-4455] Fix C++17 compile issue and simplify matrix serialization at the same time (#244)
• Adding doxygen to all packages (#241)
• Add unstamped 3D point variable (#233) (#239)
  • Add unstamped 3D landmark variable

  * Add landmark test and new uuid generator Co-authored-by: Stephen Williams <<swilliams@locusrobotics.com>> Co-authored-by: Jake McLaughlin <<jake.mclaughlin98@gmail.com>>

• [RST-3474] Created a getConstraintCosts() method
  • Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints
• [RST-2831] Support for optimization bounds (#235)
• Added a time-limited optimization option to the Graph class (#234)
• Contributors: Stephen Williams, Tom Moore

0.6.0 (2023-02-22)

• 0.5.0
• Update changelogs

• Fix Ceres 2.0.0 API support (#273) * Pass kNumResiduals to the internal AutoDiff function. Ceres added this argument in https://github.com/ceres-solver/ceres-solver/commit/e7a30359ee754057f9bd7b349c98c291138d91f4 we need to pass it else template substitution fails. * Pass kLocalSize instead of kGlobalSize Upstream commit made me assume kGlobalSize, but that threw at runtime when the tests ran. This seems to work, also put a using statement there to make roslint happy. Co-authored-by: Ivor Wanders <<ivor@iwanders.net>>

• [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
• [RST-4390] Allow variables to be held constant during optimization (#243)
  • Add support for holding variables constant
  • Create a 'fixed' landmark
  • Added initial support for marginalizing constant variables
• [RST-4455] Fix C++17 compile issue and simplify matrix serialization at the same time (#244)
• Adding doxygen to all packages (#241)
• Add unstamped 3D point variable (#233) (#239)
  • Add unstamped 3D landmark variable

  * Add landmark test and new uuid generator Co-authored-by: Stephen Williams <<swilliams@locusrobotics.com>> Co-authored-by: Jake McLaughlin <<jake.mclaughlin98@gmail.com>>

• [RST-3474] Created a getConstraintCosts() method
  • Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints
• [RST-2831] Support for optimization bounds (#235)
• Added a time-limited optimization option to the Graph class (#234)
• Contributors: Enrique Fernandez Perdomo, Gary Servin, Stephen Williams, Tom Moore

0.7.0 (2023-09-25)

• [RST-7809] Fix optimization errors when the orientation is initialized at +PI (#334)
  • Add some unit tests for the 2D orientation constraints; Create getters/setters for the 2D orientation variable is preparation for a fix.
  • Force the 2D orientation value to be is minimum phase
• Add missed geometry_msgs package (#272)
  • Add geometry_msgs as a test_depend.

  * Make geometry_msgs includable. ---------Co-authored-by: Ivor Wanders <<ivor@iwanders.net>>

• Update devel to build on Ubuntu Jammy (22.04) (#326)
  • Update to C++17 for use with Ubuntu Jammy
  • Include Rviz and Eigen as system includes, which supresses warnings within the included libraries
  • use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter <<wsacul@gmail.com>>
• 0.6.0
• Update changelogs
• 0.5.0
• Update changelogs

• Fix Ceres 2.0.0 API support (#273) * Pass kNumResiduals to the internal AutoDiff function. Ceres added this argument in https://github.com/ceres-solver/ceres-solver/commit/e7a30359ee754057f9bd7b349c98c291138d91f4 we need to pass it else template substitution fails. * Pass kLocalSize instead of kGlobalSize Upstream commit made me assume kGlobalSize, but that threw at runtime when the tests ran. This seems to work, also put a using statement there to make roslint happy. Co-authored-by: Ivor Wanders <<ivor@iwanders.net>>

• [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
• [RST-4390] Allow variables to be held constant during optimization (#243)
  • Add support for holding variables constant
  • Create a 'fixed' landmark
  • Added initial support for marginalizing constant variables
• [RST-4455] Fix C++17 compile issue and simplify matrix serialization at the same time (#244)
• Adding doxygen to all packages (#241)
• Add unstamped 3D point variable (#233) (#239)
  • Add unstamped 3D landmark variable

  * Add landmark test and new uuid generator Co-authored-by: Stephen Williams <<swilliams@locusrobotics.com>> Co-authored-by: Jake McLaughlin <<jake.mclaughlin98@gmail.com>>

• [RST-3474] Created a getConstraintCosts() method
  • Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints
• [RST-2831] Support for optimization bounds (#235)
• Added a time-limited optimization option to the Graph class (#234)
• Contributors: Enrique Fernandez Perdomo, Gary Servin, Stephen Williams, Tom Moore

0.10.0 (2024-09-16)

0.9.0 (2024-06-17)

• Gcc12 ceres 2.1.0 manifold locus (#363)
  • General clean up for Ceres 2.2.0 support

  * Updated serialization support to be backwards compatible with previously serialized files ---------Co-authored-by: Enrique Fernandez Perdomo <<efernandez@clearpath.ai>>

• Support gcc12 and ceres 2.1.0 (#341) Add support for the Manifold class when using Ceres Solver version 2.1.0 and above ---------Co-authored-by: Jake McLaughlin <<jake.mclaughlin98@gmail.com>>
• Contributors: Enrique Fernandez Perdomo, Stephen Williams

0.8.0 (2024-02-02)

• 0.7.0
• Update changelogs
• [RST-7809] Fix optimization errors when the orientation is initialized at +PI (#334)
  • Add some unit tests for the 2D orientation constraints; Create getters/setters for the 2D orientation variable is preparation for a fix.
  • Force the 2D orientation value to be is minimum phase
• Add missed geometry_msgs package (#272)
  • Add geometry_msgs as a test_depend.

  * Make geometry_msgs includable. ---------Co-authored-by: Ivor Wanders <<ivor@iwanders.net>>

• Update devel to build on Ubuntu Jammy (22.04) (#326)
  • Update to C++17 for use with Ubuntu Jammy
  • Include Rviz and Eigen as system includes, which supresses warnings within the included libraries
  • use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter <<wsacul@gmail.com>>
• 0.6.0
• Update changelogs
• 0.5.0
• Update changelogs

• Fix Ceres 2.0.0 API support (#273) * Pass kNumResiduals to the internal AutoDiff function. Ceres added this argument in https://github.com/ceres-solver/ceres-solver/commit/e7a30359ee754057f9bd7b349c98c291138d91f4 we need to pass it else template substitution fails. * Pass kLocalSize instead of kGlobalSize Upstream commit made me assume kGlobalSize, but that threw at runtime when the tests ran. This seems to work, also put a using statement there to make roslint happy. Co-authored-by: Ivor Wanders <<ivor@iwanders.net>>

• [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
• [RST-4390] Allow variables to be held constant during optimization (#243)
  • Add support for holding variables constant
  • Create a 'fixed' landmark
  • Added initial support for marginalizing constant variables
• [RST-4455] Fix C++17 compile issue and simplify matrix serialization at the same time (#244)
• Adding doxygen to all packages (#241)
• Add unstamped 3D point variable (#233) (#239)
  • Add unstamped 3D landmark variable

  * Add landmark test and new uuid generator Co-authored-by: Stephen Williams <<swilliams@locusrobotics.com>> Co-authored-by: Jake McLaughlin <<jake.mclaughlin98@gmail.com>>

• [RST-3474] Created a getConstraintCosts() method
  • Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints
• [RST-2831] Support for optimization bounds (#235)
• Added a time-limited optimization option to the Graph class (#234)
• Contributors: Enrique Fernandez Perdomo, Gary Servin, Stephen Williams, Tom Moore

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)

• 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
• 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.
• Remove variables on hold (#185)
  • Test variables on hold are removed when removing a variable
  • Erase variable on hold when removing variable
• Filter out transactions older than the lag window (#173)
  • Filter out transactions older than the lag window
  • Fix expiration time computation
  • Reset the lag expiration time when the smoother is reset
  • Reorganize class variables by their mutex guard
  • Add a mutex guard for start_time_; use start_time_ as a min time in the lag expiation computation
  • Add minStamp() and maxStamp() accessors to the Transaction class
  • Use the minStamp() and maxStamp() accessors to filter and purge transactions correctly
• Call onStop() directly if !ros::ok() in stop() (#182)
  • Call onStop() directly if !ros::ok() in stop()
  • Stop spinner before calling onStop()
• 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
• Fix doxygen comment (#177)
• Delay throttle no valid state message (#175) This requires rosconsole >=1.13.8.
• Replace ignition_sensors list param with ignition field (#163)
  • Remove ignition_sensors param and use a per-sensor ignition field
• Throttle (#162)
  • Support throttling sensor model inputs
  • Add ThrottledCallback rostest
• Use a static Boost random UUID generator (#171)
• 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.

• Fix thread issue with UUID generation (#167)
  • Add a mutex lock to the random UUID generation. The STL random number generator is not thread-safe.
• Patch Tukey loss for Ceres < 2.0.0 (#159)
  • Patch Tukey loss for Ceres < 2.0.0
  • Create ceres_macros.h header
• 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
• Add evaluate method to graph (#151)
• Support ScaledLoss (#141)
• Const deserialize (#148)
  • Make TransactionDeserializer::deserialize const

  * Make GraphDeserializer::deserialize const This requires the graph_loader_ to be mutable.

• 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
• 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]{.title-ref} when the symmetry test fails with [isApprox]{.title-ref}, 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
• Add fuse_loss pkg with plugin-based loss functions (#118)
• Validate partial measurements (#125)
• 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
• Revert "Fix build with ceres 2.0 with CMake < 3.8 (#106)" (#120) This reverts commit 9933456ecc24ba9b649a8a2885be3f852306efee.
• Predict jacobians per parameter block (#115)
• fix compilation in Kinetic (#112)
• Support ceres 2.0 in tests (#117) In Ceres 2.0 we should call AutoDifferentiate<...>(...) instead of AutoDiff<...>::Differentiate(...).
• Fix build with ceres 2.0 with CMake < 3.8 (#106)
  • Note that while the Ceres 2.0 build completes, there may still be some lingering issues.
• [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
• [RST-1951] speed optimizations (#100)
  • Improved random UUID generator
  • Minor Eigen assignment speed improvements
• [RST-2437] Ensure that all variables are updated by the motion model (#103)
• 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)
• Fix -Wall -Wextra warnings in tests (#80)
• Stamp merged transaction (#79) Set stamp in merged transactions Otherwise, merged transactions don't have a stamp. The stamp used is the maximum stamp of the two transactions merged.
• [RST-2148] Added start() and stop() methods to the MotionModel, SensorModel, and Publisher API (#75)
  • Added start() and stop() methods to the MotionModel, SensorModel, and Publisher API
  • Added the ability to clear the callback queue of the optimizer
  • Refactor the fixed-lag reset callback to use the plugins' stop() and start() methods
• Fix -Wall -Wextra warnings (#77)
• Contributors: Davide Faconti, Enrique Fernandez Perdomo, Stephen Williams

0.4.0 (2019-07-12)

• Comment unused parameters (#76) Otherwise the compilation fails with: -Werror=unused-parameter This happens with these flags: -Wall -Wextra
• Add print method to Graph and HashGraph (#70)
• Depend on libceres-dev instead of ceres-solver (#71)
• Return variable UUID by const reference (#69)
• Added a default implementation for the type() method (#67)
• [RST-2144] Support proper Eigen memory alignment (#65)
• [RST-2128] Added a "reset" service to the fixed lag smoother (#61)
• Modified Variable class to make the UUID immutable (#55)
• [RST-1960] Added a tangent/parameter space flag to the covariance function (#50)
• Some minor header cleanup of fuse_core (#51)
• [RST-1949] Added getConnectedVariables() and getConnectedConstraints() (#45)
• [RST-1746] Remove the marginalizeVariable() methods from the Graph class. (#44)
• [RST-1744] Added a marginal constraint class (#43)
• [RST-1940] Added a localSize() method to the Variable class (#42)
• [RST-1927] Update the local parameterization for the orientation variables (#41)
• [RST-1926] Extend the local parameter definition to include Minus() (#40)
• Contributors: Enrique Fernandez Perdomo, Enrique Fernández Perdomo, Stephen Williams

0.3.0 (2019-03-18)

• [RST-1653] transaction stamps (#37)
  • Moved the set<ros::Time> object that always accompanies a Transaction into the Transaction itself.
  • Updated all related classes to support that change
• [RST-1477] Simplified the sensor<-->optimizer API (#35)
  • Simplified the sensor<-->optimizer API. Moved the implementation details of the optimizer transaction callback into the optimizer where it belongs.
• Contributors: Stephen Williams

0.2.0 (2019-01-16)

• Fix tests for bionic (#34)
• [RST-1554] test depends (#30)
  • Refactored all CMakeLists.txt to avoid path issues when using workspace overlays
• Contributors: Gary Servin, Stephen Williams

0.1.1 (2018-08-15)

0.1.0 (2018-08-12)

• [RST-1121] move optimizers (#25)
  • Added a clone() method to the Transaction object
  • Changed optimizer to unique ownership of the graph. This better captures the usage.
• [RST-1121] Moved the pose publishers (#19)
  • Clean up Eigen depends and includes
• [RST-1121] move publishers (#18)
• [RST-1121] move motion models (#17)
• [RST-1121] move sensor classes (#16)
• [RST-1121] Moved the Graph classes (#15)
• Adding absolute 3d pose
• Converted all Eigen objects to use row-major order (#22)
• Contributors: Stephen Williams, Tom Moore

0.0.2 (2018-07-16)

• Added the Transaction class and unit tests (#2)
• Added missing test depend (#3)
• Contributors: Stephen Williams

0.0.1 (2018-07-05)

• Moved the Variable and Constraint classed into the public fuse repo
• Contributors: Stephen Williams