├── .github └── workflows │ └── main.yaml ├── .gitignore ├── Jenkinsfile ├── LICENSE ├── README.md ├── doc ├── Constraints.md ├── Variables.md ├── constraints-generic_cost_equation.gif ├── constraints-least_squares_equation.gif ├── constraints-observation_model_equation.gif ├── constraints-pose_2d_prior_equation.gif ├── constraints-state_transition_model_equation.gif ├── fuse_graph_1.png ├── fuse_graph_2.png ├── fuse_graph_3.png ├── fuse_graph_4.png ├── fuse_graph_5.png ├── fuse_lightning_talk.pdf ├── fuse_optimizer_1.png ├── fuse_optimizer_2.png ├── fuse_optimizer_3.png ├── fuse_optimizer_4.png ├── fuse_optimizer_5.png ├── fuse_optimizer_6.png ├── fuse_sequence_diagram.png ├── variables-common_property.png ├── variables-multiple_occurrences.png └── variables-time_series.png ├── fuse ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md └── package.xml ├── fuse_constraints ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── benchmark │ ├── benchmark_normal_delta_pose_2d.cpp │ └── benchmark_normal_prior_pose_2d.cpp ├── cmake │ └── FindSuiteSparse.cmake ├── fuse_plugins.xml ├── include │ └── fuse_constraints │ │ ├── absolute_constraint.h │ │ ├── absolute_constraint_impl.h │ │ ├── absolute_orientation_3d_stamped_constraint.h │ │ ├── absolute_orientation_3d_stamped_euler_constraint.h │ │ ├── absolute_pose_2d_stamped_constraint.h │ │ ├── absolute_pose_3d_stamped_constraint.h │ │ ├── fixed_3d_landmark_constraint.h │ │ ├── fixed_3d_landmark_cost_functor.h │ │ ├── marginal_constraint.h │ │ ├── marginal_cost_function.h │ │ ├── marginalize_variables.h │ │ ├── normal_delta.h │ │ ├── normal_delta_orientation_2d.h │ │ ├── normal_delta_orientation_3d_cost_functor.h │ │ ├── normal_delta_pose_2d.h │ │ ├── normal_delta_pose_2d_cost_functor.h │ │ ├── normal_delta_pose_3d_cost_functor.h │ │ ├── normal_prior_orientation_2d.h │ │ ├── normal_prior_orientation_3d_cost_functor.h │ │ ├── normal_prior_orientation_3d_euler_cost_functor.h │ │ ├── normal_prior_pose_2d.h │ │ ├── normal_prior_pose_2d_cost_functor.h │ │ ├── normal_prior_pose_3d_cost_functor.h │ │ ├── relative_constraint.h │ │ ├── relative_constraint_impl.h │ │ ├── relative_orientation_3d_stamped_constraint.h │ │ ├── relative_pose_2d_stamped_constraint.h │ │ ├── relative_pose_3d_stamped_constraint.h │ │ ├── uuid_ordering.h │ │ └── variable_constraints.h ├── package.xml ├── rosdoc.yaml ├── src │ ├── absolute_constraint.cpp │ ├── absolute_orientation_3d_stamped_constraint.cpp │ ├── absolute_orientation_3d_stamped_euler_constraint.cpp │ ├── absolute_pose_2d_stamped_constraint.cpp │ ├── absolute_pose_3d_stamped_constraint.cpp │ ├── fixed_3d_landmark_constraint.cpp │ ├── marginal_constraint.cpp │ ├── marginal_cost_function.cpp │ ├── marginalize_variables.cpp │ ├── normal_delta.cpp │ ├── normal_delta_orientation_2d.cpp │ ├── normal_delta_pose_2d.cpp │ ├── normal_prior_orientation_2d.cpp │ ├── normal_prior_pose_2d.cpp │ ├── relative_constraint.cpp │ ├── relative_orientation_3d_stamped_constraint.cpp │ ├── relative_pose_2d_stamped_constraint.cpp │ ├── relative_pose_3d_stamped_constraint.cpp │ ├── uuid_ordering.cpp │ └── variable_constraints.cpp └── test │ ├── cost_function_gtest.h │ ├── legacy_marginal_version0.txt │ ├── test_absolute_constraint.cpp │ ├── test_absolute_orientation_2d_stamped_constraint.cpp │ ├── test_absolute_orientation_3d_stamped_constraint.cpp │ ├── test_absolute_orientation_3d_stamped_euler_constraint.cpp │ ├── test_absolute_pose_2d_stamped_constraint.cpp │ ├── test_absolute_pose_3d_stamped_constraint.cpp │ ├── test_fixed_3d_landmark_constraint.cpp │ ├── test_marginal_constraint.cpp │ ├── test_marginalize_variables.cpp │ ├── test_normal_delta_pose_2d.cpp │ ├── test_normal_prior_pose_2d.cpp │ ├── test_relative_constraint.cpp │ ├── test_relative_pose_2d_stamped_constraint.cpp │ ├── test_relative_pose_3d_stamped_constraint.cpp │ ├── test_uuid_ordering.cpp │ └── test_variable_constraints.cpp ├── fuse_core ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── include │ └── fuse_core │ │ ├── async_motion_model.h │ │ ├── async_publisher.h │ │ ├── async_sensor_model.h │ │ ├── autodiff_local_parameterization.h │ │ ├── callback_wrapper.h │ │ ├── ceres_macros.h │ │ ├── ceres_options.h │ │ ├── console.h │ │ ├── constraint.h │ │ ├── eigen.h │ │ ├── eigen_gtest.h │ │ ├── fuse_macros.h │ │ ├── graph.h │ │ ├── graph_deserializer.h │ │ ├── local_parameterization.h │ │ ├── loss.h │ │ ├── loss_loader.h │ │ ├── macros.h │ │ ├── manifold.h │ │ ├── manifold_adapter.h │ │ ├── message_buffer.h │ │ ├── message_buffer_impl.h │ │ ├── motion_model.h │ │ ├── parameter.h │ │ ├── publisher.h │ │ ├── sensor_model.h │ │ ├── serialization.h │ │ ├── throttled_callback.h │ │ ├── timestamp_manager.h │ │ ├── transaction.h │ │ ├── transaction_deserializer.h │ │ ├── util.h │ │ ├── uuid.h │ │ └── variable.h ├── package.xml ├── rosdoc.yaml ├── src │ ├── async_motion_model.cpp │ ├── async_publisher.cpp │ ├── async_sensor_model.cpp │ ├── ceres_options.cpp │ ├── constraint.cpp │ ├── fuse_echo.cpp │ ├── graph.cpp │ ├── graph_deserializer.cpp │ ├── loss.cpp │ ├── manifold_adapter.cpp │ ├── serialization.cpp │ ├── timestamp_manager.cpp │ ├── transaction.cpp │ ├── transaction_deserializer.cpp │ ├── uuid.cpp │ └── variable.cpp └── test │ ├── async_motion_model.test │ ├── async_publisher.test │ ├── async_sensor_model.test │ ├── callback_wrapper.test │ ├── example_constraint.h │ ├── example_loss.h │ ├── example_variable.h │ ├── legacy_variable_deserialization.txt │ ├── parameter.test │ ├── parameter.yaml │ ├── test_async_motion_model.cpp │ ├── test_async_publisher.cpp │ ├── test_async_sensor_model.cpp │ ├── test_callback_wrapper.cpp │ ├── test_constraint.cpp │ ├── test_eigen.cpp │ ├── test_local_parameterization.cpp │ ├── test_loss.cpp │ ├── test_message_buffer.cpp │ ├── test_parameter.cpp │ ├── test_throttled_callback.cpp │ ├── test_timestamp_manager.cpp │ ├── test_transaction.cpp │ ├── test_util.cpp │ ├── test_uuid.cpp │ ├── test_variable.cpp │ └── throttled_callback.test ├── fuse_doc ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── doc │ ├── .templates │ │ └── full_globaltoc.html │ ├── conf.py │ ├── getting_started.rst │ ├── images │ │ └── fuse_small.png │ └── index.rst ├── package.xml └── rosdoc.yaml ├── fuse_graphs ├── CHANGELOG.rst ├── CMakeLists.txt ├── benchmark │ └── benchmark_create_problem.cpp ├── fuse_plugins.xml ├── include │ └── fuse_graphs │ │ ├── hash_graph.h │ │ └── hash_graph_params.h ├── package.xml ├── rosdoc.yaml ├── src │ └── hash_graph.cpp └── test │ ├── covariance_constraint.h │ ├── example_constraint.h │ ├── example_loss.h │ ├── example_variable.h │ └── test_hash_graph.cpp ├── fuse_loss ├── CHANGELOG.rst ├── CMakeLists.txt ├── cmake │ └── FindQwt.cmake ├── fuse_plugins.xml ├── include │ └── fuse_loss │ │ ├── arctan_loss.h │ │ ├── cauchy_loss.h │ │ ├── composed_loss.h │ │ ├── dcs_loss.h │ │ ├── fair_loss.h │ │ ├── geman_mcclure_loss.h │ │ ├── huber_loss.h │ │ ├── loss_function.h │ │ ├── qwt_loss_plot.h │ │ ├── scaled_loss.h │ │ ├── softlone_loss.h │ │ ├── tolerant_loss.h │ │ ├── trivial_loss.h │ │ ├── tukey_loss.h │ │ └── welsch_loss.h ├── package.xml ├── rosdoc.yaml ├── src │ ├── arctan_loss.cpp │ ├── cauchy_loss.cpp │ ├── composed_loss.cpp │ ├── dcs_loss.cpp │ ├── fair_loss.cpp │ ├── geman_mcclure_loss.cpp │ ├── huber_loss.cpp │ ├── loss_function.cpp │ ├── scaled_loss.cpp │ ├── softlone_loss.cpp │ ├── tolerant_loss.cpp │ ├── trivial_loss.cpp │ ├── tukey_loss.cpp │ └── welsch_loss.cpp └── test │ ├── test_composed_loss.cpp │ ├── test_huber_loss.cpp │ ├── test_loss_function.cpp │ ├── test_qwt_loss_plot.cpp │ ├── test_scaled_loss.cpp │ └── test_tukey_loss.cpp ├── fuse_models ├── CHANGELOG.rst ├── CMakeLists.txt ├── README.md ├── benchmark │ └── benchmark_unicycle_2d_state_cost_function.cpp ├── fuse_plugins.xml ├── include │ └── fuse_models │ │ ├── acceleration_2d.h │ │ ├── common │ │ ├── sensor_config.h │ │ ├── sensor_proc.h │ │ └── variable_traits.h │ │ ├── graph_ignition.h │ │ ├── imu_2d.h │ │ ├── odometry_2d.h │ │ ├── odometry_2d_publisher.h │ │ ├── parameters │ │ ├── acceleration_2d_params.h │ │ ├── graph_ignition_params.h │ │ ├── imu_2d_params.h │ │ ├── odometry_2d_params.h │ │ ├── odometry_2d_publisher_params.h │ │ ├── parameter_base.h │ │ ├── pose_2d_params.h │ │ ├── transaction_params.h │ │ ├── twist_2d_params.h │ │ └── unicycle_2d_ignition_params.h │ │ ├── pose_2d.h │ │ ├── transaction.h │ │ ├── twist_2d.h │ │ ├── unicycle_2d.h │ │ ├── unicycle_2d_ignition.h │ │ ├── unicycle_2d_predict.h │ │ ├── unicycle_2d_state_cost_function.h │ │ ├── unicycle_2d_state_cost_functor.h │ │ └── unicycle_2d_state_kinematic_constraint.h ├── package.xml ├── rosdoc.yaml ├── src │ ├── acceleration_2d.cpp │ ├── graph_ignition.cpp │ ├── imu_2d.cpp │ ├── odometry_2d.cpp │ ├── odometry_2d_publisher.cpp │ ├── pose_2d.cpp │ ├── transaction.cpp │ ├── twist_2d.cpp │ ├── unicycle_2d.cpp │ ├── unicycle_2d_ignition.cpp │ └── unicycle_2d_state_kinematic_constraint.cpp ├── srv │ ├── SetGraph.srv │ ├── SetPose.srv │ └── SetPoseDeprecated.srv └── test │ ├── example_constraint.h │ ├── example_variable.h │ ├── example_variable_stamped.h │ ├── graph_ignition.test │ ├── test_graph_ignition.cpp │ ├── test_sensor_proc.cpp │ ├── test_unicycle_2d.cpp │ ├── test_unicycle_2d_ignition.cpp │ ├── test_unicycle_2d_predict.cpp │ ├── test_unicycle_2d_state_cost_function.cpp │ └── unicycle_2d_ignition.test ├── fuse_msgs ├── CHANGELOG.rst ├── CMakeLists.txt ├── msg │ ├── SerializedGraph.msg │ └── SerializedTransaction.msg └── package.xml ├── fuse_optimizers ├── CHANGELOG.rst ├── CMakeLists.txt ├── include │ └── fuse_optimizers │ │ ├── batch_optimizer.h │ │ ├── batch_optimizer_params.h │ │ ├── fixed_lag_smoother.h │ │ ├── fixed_lag_smoother_params.h │ │ ├── optimizer.h │ │ └── variable_stamp_index.h ├── package.xml ├── rosdoc.yaml ├── src │ ├── batch_optimizer.cpp │ ├── batch_optimizer_node.cpp │ ├── fixed_lag_smoother.cpp │ ├── fixed_lag_smoother_node.cpp │ ├── optimizer.cpp │ └── variable_stamp_index.cpp └── test │ ├── common.h │ ├── config │ ├── list │ │ ├── common_robot_config.yaml │ │ ├── noisy_motion_model_config.yaml │ │ ├── robot_with_imu_config.yaml │ │ └── serialized_publisher_config.yaml │ └── struct │ │ ├── common_robot_config.yaml │ │ ├── noisy_motion_model_config.yaml │ │ ├── robot_with_imu_config.yaml │ │ └── serialized_publisher_config.yaml │ ├── example_optimizer.h │ ├── fixed_lag_ignition.test │ ├── optimizer.test │ ├── test_fixed_lag_ignition.cpp │ ├── test_optimizer.cpp │ └── test_variable_stamp_index.cpp ├── fuse_publishers ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── fuse_plugins.xml ├── include │ └── fuse_publishers │ │ ├── path_2d_publisher.h │ │ ├── pose_2d_publisher.h │ │ ├── serialized_publisher.h │ │ └── stamped_variable_synchronizer.h ├── package.xml ├── rosdoc.yaml ├── src │ ├── path_2d_publisher.cpp │ ├── pose_2d_publisher.cpp │ └── serialized_publisher.cpp └── test │ ├── path_2d_publisher.test │ ├── pose_2d_publisher.test │ ├── test_path_2d_publisher.cpp │ ├── test_pose_2d_publisher.cpp │ └── test_stamped_variable_synchronizer.cpp ├── fuse_tutorials ├── CHANGELOG.rst ├── CMakeLists.txt ├── config │ ├── fuse_simple_tutorial.rviz │ ├── fuse_simple_tutorial.yaml │ ├── range_sensor_tutorial.rviz │ └── range_sensor_tutorial.yaml ├── data │ └── turtlebot3.bag ├── fuse_plugins.xml ├── include │ └── fuse_tutorials │ │ ├── beacon_publisher.h │ │ ├── range_constraint.h │ │ ├── range_cost_functor.h │ │ └── range_sensor_model.h ├── launch │ ├── fuse_simple_tutorial.launch │ └── range_sensor_tutorial.launch ├── package.xml ├── rosdoc.yaml └── src │ ├── beacon_publisher.cpp │ ├── range_constraint.cpp │ ├── range_sensor_model.cpp │ └── range_sensor_simulator.cpp ├── fuse_variables ├── CHANGELOG.rst ├── CMakeLists.txt ├── LICENSE ├── README.md ├── fuse_plugins.xml ├── include │ └── fuse_variables │ │ ├── acceleration_angular_2d_stamped.h │ │ ├── acceleration_angular_3d_stamped.h │ │ ├── acceleration_linear_2d_stamped.h │ │ ├── acceleration_linear_3d_stamped.h │ │ ├── fixed_size_variable.h │ │ ├── orientation_2d_stamped.h │ │ ├── orientation_3d_stamped.h │ │ ├── pinhole_camera.h │ │ ├── pinhole_camera_fixed.h │ │ ├── point_2d_fixed_landmark.h │ │ ├── point_2d_landmark.h │ │ ├── point_3d_fixed_landmark.h │ │ ├── point_3d_landmark.h │ │ ├── position_2d_stamped.h │ │ ├── position_3d_stamped.h │ │ ├── stamped.h │ │ ├── velocity_angular_2d_stamped.h │ │ ├── velocity_angular_3d_stamped.h │ │ ├── velocity_linear_2d_stamped.h │ │ └── velocity_linear_3d_stamped.h ├── package.xml ├── rosdoc.yaml ├── src │ ├── acceleration_angular_2d_stamped.cpp │ ├── acceleration_angular_3d_stamped.cpp │ ├── acceleration_linear_2d_stamped.cpp │ ├── acceleration_linear_3d_stamped.cpp │ ├── orientation_2d_stamped.cpp │ ├── orientation_3d_stamped.cpp │ ├── pinhole_camera.cpp │ ├── pinhole_camera_fixed.cpp │ ├── point_2d_fixed_landmark.cpp │ ├── point_2d_landmark.cpp │ ├── point_3d_fixed_landmark.cpp │ ├── point_3d_landmark.cpp │ ├── position_2d_stamped.cpp │ ├── position_3d_stamped.cpp │ ├── stamped.cpp │ ├── velocity_angular_2d_stamped.cpp │ ├── velocity_angular_3d_stamped.cpp │ ├── velocity_linear_2d_stamped.cpp │ └── velocity_linear_3d_stamped.cpp └── test │ ├── load_device_id.test │ ├── test_acceleration_angular_2d_stamped.cpp │ ├── test_acceleration_angular_3d_stamped.cpp │ ├── test_acceleration_linear_2d_stamped.cpp │ ├── test_acceleration_linear_3d_stamped.cpp │ ├── test_fixed_size_variable.cpp │ ├── test_load_device_id.cpp │ ├── test_orientation_2d_stamped.cpp │ ├── test_orientation_3d_stamped.cpp │ ├── test_pinhole_camera.cpp │ ├── test_pinhole_camera_fixed.cpp │ ├── test_point_2d_fixed_landmark.cpp │ ├── test_point_2d_landmark.cpp │ ├── test_point_3d_fixed_landmark.cpp │ ├── test_point_3d_landmark.cpp │ ├── test_position_2d_stamped.cpp │ ├── test_position_3d_stamped.cpp │ ├── test_velocity_angular_2d_stamped.cpp │ ├── test_velocity_angular_3d_stamped.cpp │ ├── test_velocity_linear_2d_stamped.cpp │ └── test_velocity_linear_3d_stamped.cpp └── fuse_viz ├── CHANGELOG.rst ├── CMakeLists.txt ├── include └── fuse_viz │ ├── conversions.h │ ├── mapped_covariance_property.h │ ├── mapped_covariance_visual.h │ ├── pose_2d_stamped_property.h │ ├── pose_2d_stamped_visual.h │ ├── relative_pose_2d_stamped_constraint_property.h │ ├── relative_pose_2d_stamped_constraint_visual.h │ └── serialized_graph_display.h ├── package.xml ├── rosdoc.yaml ├── rviz_plugins.xml └── src ├── mapped_covariance_property.cpp ├── mapped_covariance_visual.cpp ├── pose_2d_stamped_property.cpp ├── pose_2d_stamped_visual.cpp ├── relative_pose_2d_stamped_constraint_property.cpp ├── relative_pose_2d_stamped_constraint_visual.cpp └── serialized_graph_display.cpp /.github/workflows/main.yaml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: 6 | - '*' 7 | 8 | jobs: 9 | ci: 10 | strategy: 11 | matrix: 12 | include: 13 | - os: ubuntu-20.04 14 | distro: noetic 15 | fail-fast: false 16 | runs-on: ${{ matrix.os }} 17 | steps: 18 | - uses: actions/checkout@v1 19 | - uses: ros-tooling/setup-ros@0.7.1 20 | with: 21 | required-ros-distributions: ${{ matrix.distro }} 22 | - run: sudo apt remove python3-openssl -y 23 | - uses: ros-tooling/action-ros-ci@0.3.5 24 | with: 25 | target-ros1-distro: ${{ matrix.distro }} 26 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Prerequisites 2 | *.d 3 | 4 | # Compiled Object files 5 | *.slo 6 | *.lo 7 | *.o 8 | *.obj 9 | 10 | # Precompiled Headers 11 | *.gch 12 | *.pch 13 | 14 | # Compiled Dynamic libraries 15 | *.so 16 | *.dylib 17 | *.dll 18 | 19 | # Fortran module files 20 | *.mod 21 | *.smod 22 | 23 | # Compiled Static libraries 24 | *.lai 25 | *.la 26 | *.a 27 | *.lib 28 | 29 | # Executables 30 | *.exe 31 | *.out 32 | *.app 33 | -------------------------------------------------------------------------------- /Jenkinsfile: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env groovy 2 | @Library('tailor-meta@0.1.24')_ 3 | tailorTestPipeline( 4 | // Name of job that generated this test definition. 5 | rosdistro_job: '/ci/rosdistro/master', 6 | // Distribution name 7 | rosdistro_name: 'ros1', 8 | // Release track to test branch against. 9 | release_track: 'hotdog', 10 | // Release label to pull test images from. 11 | release_label: 'hotdog', 12 | // OS distributions to test. 13 | distributions: ['jammy'], 14 | // Version of tailor_meta to build against 15 | tailor_meta: '0.1.24', 16 | // Master or release branch associated with this track 17 | source_branch: 'devel', 18 | // Docker registry where test image is stored 19 | docker_registry: 'https://084758475884.dkr.ecr.us-east-1.amazonaws.com/locus-tailor' 20 | ) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2018, Locus Robotics 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /doc/constraints-generic_cost_equation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/constraints-generic_cost_equation.gif -------------------------------------------------------------------------------- /doc/constraints-least_squares_equation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/constraints-least_squares_equation.gif -------------------------------------------------------------------------------- /doc/constraints-observation_model_equation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/constraints-observation_model_equation.gif -------------------------------------------------------------------------------- /doc/constraints-pose_2d_prior_equation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/constraints-pose_2d_prior_equation.gif -------------------------------------------------------------------------------- /doc/constraints-state_transition_model_equation.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/constraints-state_transition_model_equation.gif -------------------------------------------------------------------------------- /doc/fuse_graph_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_graph_1.png -------------------------------------------------------------------------------- /doc/fuse_graph_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_graph_2.png -------------------------------------------------------------------------------- /doc/fuse_graph_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_graph_3.png -------------------------------------------------------------------------------- /doc/fuse_graph_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_graph_4.png -------------------------------------------------------------------------------- /doc/fuse_graph_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_graph_5.png -------------------------------------------------------------------------------- /doc/fuse_lightning_talk.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_lightning_talk.pdf -------------------------------------------------------------------------------- /doc/fuse_optimizer_1.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_optimizer_1.png -------------------------------------------------------------------------------- /doc/fuse_optimizer_2.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_optimizer_2.png -------------------------------------------------------------------------------- /doc/fuse_optimizer_3.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_optimizer_3.png -------------------------------------------------------------------------------- /doc/fuse_optimizer_4.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_optimizer_4.png -------------------------------------------------------------------------------- /doc/fuse_optimizer_5.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_optimizer_5.png -------------------------------------------------------------------------------- /doc/fuse_optimizer_6.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_optimizer_6.png -------------------------------------------------------------------------------- /doc/fuse_sequence_diagram.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/fuse_sequence_diagram.png -------------------------------------------------------------------------------- /doc/variables-common_property.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/variables-common_property.png -------------------------------------------------------------------------------- /doc/variables-multiple_occurrences.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/variables-multiple_occurrences.png -------------------------------------------------------------------------------- /doc/variables-time_series.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/doc/variables-time_series.png -------------------------------------------------------------------------------- /fuse/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package fuse 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.0 (2022-02-23) 6 | ------------------ 7 | 8 | 0.6.0 (2023-02-22) 9 | ------------------ 10 | * 0.5.0 11 | * Update changelogs 12 | * Contributors: Gary Servin 13 | 14 | 0.7.0 (2023-09-25) 15 | ------------------ 16 | * 0.6.0 17 | * Update changelogs 18 | * 0.5.0 19 | * Update changelogs 20 | * Contributors: Gary Servin 21 | 22 | 0.12.0 (2025-06-06) 23 | ------------------- 24 | 25 | 0.11.0 (2025-02-04) 26 | ------------------- 27 | 28 | 0.10.0 (2024-09-16) 29 | ------------------- 30 | 31 | 0.9.0 (2024-06-17) 32 | ------------------ 33 | 34 | 0.8.0 (2024-02-02) 35 | ------------------ 36 | * 0.7.0 37 | * Update changelogs 38 | * 0.6.0 39 | * Update changelogs 40 | * 0.5.0 41 | * Update changelogs 42 | * Contributors: Gary Servin 43 | 44 | 0.4.2 (2021-07-20) 45 | ------------------ 46 | * Adding roslint dependency to fuse_viz (`#231 `_) 47 | * Adding roslint dependency to fuse_viz 48 | * Silence CMP0048 warnings 49 | * Contributors: Tom Moore 50 | 51 | 0.4.1 (2021-07-13) 52 | ------------------ 53 | * Add support for sphinx documentation (`#228 `_) 54 | * Adding tutorial for basic configuration 55 | * Add fuse_viz pkg with rviz SerializedGraph display (`#99 `_) 56 | * [RST-2340] Add serialization support to fuse (`#98 `_) 57 | * Renaming package to fuse_models 58 | * Contributors: Enrique Fernandez Perdomo, Stephen Williams, Tom Moore 59 | 60 | 0.4.0 (2019-07-12) 61 | ------------------ 62 | 63 | 0.3.0 (2019-03-18) 64 | ------------------ 65 | 66 | 0.2.0 (2019-01-16) 67 | ------------------ 68 | 69 | 0.1.1 (2018-08-15) 70 | ------------------ 71 | 72 | 0.1.0 (2018-08-12) 73 | ------------------ 74 | * [RST-1121] move optimizers (`#25 `_) 75 | * Added fuse_optimizers to the metapackage depends 76 | * [RST-1121] Moved the pose publishers (`#19 `_) 77 | * Added fuse_publishers to the metapackage 78 | * [RST-1121] Moved the Graph classes (`#15 `_) 79 | * Added fuse_graphs to the metapackage 80 | * Contributors: Stephen Williams 81 | 82 | 0.0.2 (2018-07-16) 83 | ------------------ 84 | * Added absolute and relative constraints (`#8 `_) 85 | * Move the fuse_variables package into the public repo (`#4 `_) 86 | * Contributors: Stephen Williams 87 | 88 | 0.0.1 (2018-07-05) 89 | ------------------ 90 | * Moved the Variable and Constraint classes into the public fuse repo 91 | * Contributors: Stephen Williams 92 | -------------------------------------------------------------------------------- /fuse/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fuse) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /fuse/LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2018, Locus Robotics 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /fuse/README.md: -------------------------------------------------------------------------------- 1 | # fuse 2 | This is the metapackage for the entire fuse stack. -------------------------------------------------------------------------------- /fuse/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fuse 4 | 0.12.0 5 | 6 | The fuse metapackage 7 | 8 | 9 | Stephen Williams 10 | Stephen Williams 11 | BSD 12 | 13 | catkin 14 | fuse_constraints 15 | fuse_core 16 | fuse_doc 17 | fuse_graphs 18 | fuse_models 19 | fuse_msgs 20 | fuse_optimizers 21 | fuse_publishers 22 | fuse_variables 23 | fuse_viz 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /fuse_constraints/LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2018, Locus Robotics 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /fuse_constraints/README.md: -------------------------------------------------------------------------------- 1 | # fuse_constraints 2 | This package provides a set of commonly used constraint types, such as direct measurements on state variables (absolute 3 | constraints) or measurements of the state changes (relative constraints). 4 | -------------------------------------------------------------------------------- /fuse_constraints/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | fuse_constraints 7 | 0.12.0 8 | 9 | The fuse_constraints package provides a set of commonly used constraint types, such as direct measurements on 10 | state variables (absolute constraints) or measurements of the state changes (relative constraints). 11 | 12 | 13 | Stephen Williams 14 | Stephen Williams 15 | BSD 16 | 17 | catkin 18 | libceres-dev 19 | eigen 20 | fuse_core 21 | fuse_graphs 22 | fuse_variables 23 | geometry_msgs 24 | pluginlib 25 | roscpp 26 | suitesparse 27 | benchmark 28 | roslint 29 | rostest 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /fuse_constraints/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | 7 | -------------------------------------------------------------------------------- /fuse_constraints/src/absolute_constraint.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | 40 | 41 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint); 42 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint); 43 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteOrientation2DStampedConstraint); 44 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition2DStampedConstraint); 45 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition3DStampedConstraint); 46 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); 47 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); 48 | 49 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, fuse_core::Constraint); 50 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, fuse_core::Constraint); 51 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteOrientation2DStampedConstraint, fuse_core::Constraint); 52 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition2DStampedConstraint, fuse_core::Constraint); 53 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition3DStampedConstraint, fuse_core::Constraint); 54 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, fuse_core::Constraint); 55 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, fuse_core::Constraint); 56 | -------------------------------------------------------------------------------- /fuse_constraints/src/marginal_constraint.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | namespace fuse_constraints 46 | { 47 | void MarginalConstraint::print(std::ostream& stream) const 48 | { 49 | stream << type() << "\n" 50 | << " source: " << source() << "\n" 51 | << " uuid: " << uuid() << "\n" 52 | << " variable:\n"; 53 | for (const auto& variable : variables()) 54 | { 55 | stream << " - " << variable << "\n"; 56 | } 57 | Eigen::IOFormat indent(4, 0, ", ", "\n", " [", "]"); 58 | for (size_t i = 0; i < A().size(); ++i) 59 | { 60 | stream << " A[" << i << "]:\n" << A()[i].format(indent) << "\n" 61 | << " x_bar[" << i << "]:\n" << x_bar()[i].format(indent) << "\n"; 62 | } 63 | stream << " b:\n" << b().format(indent) << "\n"; 64 | 65 | if (loss()) 66 | { 67 | stream << " loss: "; 68 | loss()->print(stream); 69 | } 70 | } 71 | 72 | ceres::CostFunction* MarginalConstraint::costFunction() const 73 | { 74 | #if !CERES_SUPPORTS_MANIFOLDS 75 | return new MarginalCostFunction(A_, b_, x_bar_, local_parameterizations_); 76 | #else 77 | return new MarginalCostFunction(A_, b_, x_bar_, manifolds_); 78 | #endif 79 | } 80 | 81 | } // namespace fuse_constraints 82 | 83 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::MarginalConstraint); 84 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::MarginalConstraint, fuse_core::Constraint); 85 | -------------------------------------------------------------------------------- /fuse_constraints/src/normal_delta.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | 40 | namespace fuse_constraints 41 | { 42 | 43 | NormalDelta::NormalDelta(const fuse_core::MatrixXd& A, const fuse_core::VectorXd& b) : 44 | A_(A), 45 | b_(b) 46 | { 47 | CHECK_GT(b_.rows(), 0); 48 | CHECK_GT(A_.rows(), 0); 49 | CHECK_EQ(b_.rows(), A.cols()); 50 | set_num_residuals(A_.rows()); 51 | mutable_parameter_block_sizes()->push_back(b_.rows()); 52 | mutable_parameter_block_sizes()->push_back(b_.rows()); 53 | } 54 | 55 | bool NormalDelta::Evaluate( 56 | double const* const* parameters, 57 | double* residuals, 58 | double** jacobians) const 59 | { 60 | Eigen::Map x0(parameters[0], parameter_block_sizes()[0]); 61 | Eigen::Map x1(parameters[1], parameter_block_sizes()[1]); 62 | Eigen::Map r(residuals, num_residuals()); 63 | r = A_ * (x1 - x0 - b_); 64 | if (jacobians != nullptr) 65 | { 66 | if (jacobians[0] != nullptr) 67 | { 68 | Eigen::Map(jacobians[0], num_residuals(), parameter_block_sizes()[0]) = -A_; 69 | } 70 | if (jacobians[1] != nullptr) 71 | { 72 | Eigen::Map(jacobians[1], num_residuals(), parameter_block_sizes()[1]) = A_; 73 | } 74 | } 75 | return true; 76 | } 77 | 78 | } // namespace fuse_constraints 79 | -------------------------------------------------------------------------------- /fuse_constraints/src/normal_delta_orientation_2d.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | 38 | 39 | namespace fuse_constraints 40 | { 41 | 42 | NormalDeltaOrientation2D::NormalDeltaOrientation2D(const double A, const double b) : 43 | A_(A), 44 | b_(b) 45 | { 46 | } 47 | 48 | bool NormalDeltaOrientation2D::Evaluate( 49 | double const* const* parameters, 50 | double* residuals, 51 | double** jacobians) const 52 | { 53 | // The following lines should read as 54 | // r = A_ * ((x1 - x0) - b_); 55 | // The wrap function handles the 2_pi wrap around issue with rotations 56 | double angle_diff = fuse_core::wrapAngle2D(parameters[1][0] - parameters[0][0] - b_); 57 | residuals[0] = A_ * angle_diff; 58 | if (jacobians != nullptr) 59 | { 60 | if (jacobians[0] != nullptr) 61 | { 62 | jacobians[0][0] = -A_; 63 | } 64 | if (jacobians[1] != nullptr) 65 | { 66 | jacobians[1][0] = A_; 67 | } 68 | } 69 | return true; 70 | } 71 | 72 | } // namespace fuse_constraints 73 | -------------------------------------------------------------------------------- /fuse_constraints/src/normal_prior_orientation_2d.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | 38 | 39 | namespace fuse_constraints 40 | { 41 | 42 | NormalPriorOrientation2D::NormalPriorOrientation2D(const double A, const double b) : 43 | A_(A), 44 | b_(b) 45 | { 46 | } 47 | 48 | bool NormalPriorOrientation2D::Evaluate( 49 | double const* const* parameters, 50 | double* residuals, 51 | double** jacobians) const 52 | { 53 | // The following lines should read as 54 | // r = A_ * (x - b_); 55 | // The wrap function handles the 2_pi wrap around issue with rotations 56 | double angle_diff = fuse_core::wrapAngle2D(parameters[0][0] - b_); 57 | residuals[0] = A_ * angle_diff; 58 | if ((jacobians != nullptr) && (jacobians[0] != nullptr)) 59 | { 60 | jacobians[0][0] = A_; 61 | } 62 | return true; 63 | } 64 | 65 | } // namespace fuse_constraints 66 | -------------------------------------------------------------------------------- /fuse_constraints/src/relative_constraint.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | 40 | 41 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeAccelerationAngular2DStampedConstraint); 42 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeAccelerationLinear2DStampedConstraint); 43 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeOrientation2DStampedConstraint); 44 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePosition2DStampedConstraint); 45 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePosition3DStampedConstraint); 46 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeVelocityAngular2DStampedConstraint); 47 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativeVelocityLinear2DStampedConstraint); 48 | 49 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeAccelerationAngular2DStampedConstraint, fuse_core::Constraint); 50 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeAccelerationLinear2DStampedConstraint, fuse_core::Constraint); 51 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeOrientation2DStampedConstraint, fuse_core::Constraint); 52 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePosition2DStampedConstraint, fuse_core::Constraint); 53 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePosition3DStampedConstraint, fuse_core::Constraint); 54 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeVelocityAngular2DStampedConstraint, fuse_core::Constraint); 55 | PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativeVelocityLinear2DStampedConstraint, fuse_core::Constraint); 56 | -------------------------------------------------------------------------------- /fuse_constraints/src/uuid_ordering.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | 38 | 39 | namespace fuse_constraints 40 | { 41 | UuidOrdering::UuidOrdering(std::initializer_list uuid_list) : 42 | UuidOrdering(uuid_list.begin(), uuid_list.end()) 43 | { 44 | } 45 | 46 | bool UuidOrdering::empty() const 47 | { 48 | return order_.empty(); 49 | } 50 | 51 | size_t UuidOrdering::size() const 52 | { 53 | return order_.size(); 54 | } 55 | 56 | bool UuidOrdering::exists(const unsigned int index) const 57 | { 58 | return (index < order_.size()); 59 | } 60 | 61 | bool UuidOrdering::exists(const fuse_core::UUID& uuid) const 62 | { 63 | return (order_.right.find(uuid) != order_.right.end()); 64 | } 65 | 66 | bool UuidOrdering::push_back(const fuse_core::UUID& uuid) 67 | { 68 | auto result = order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), uuid)); 69 | return result.second; 70 | } 71 | 72 | const fuse_core::UUID& UuidOrdering::operator[](const unsigned int index) const 73 | { 74 | return order_.left[index].second; 75 | } 76 | 77 | unsigned int UuidOrdering::operator[](const fuse_core::UUID& uuid) 78 | { 79 | auto result = order_.insert(order_.end(), UuidOrderMapping::value_type(order_.size(), uuid)); 80 | return (*result.first).get_left(); 81 | } 82 | 83 | const fuse_core::UUID& UuidOrdering::at(const unsigned int index) const 84 | { 85 | return order_.left.at(index).second; 86 | } 87 | 88 | unsigned int UuidOrdering::at(const fuse_core::UUID& uuid) const 89 | { 90 | return order_.right.at(uuid); 91 | } 92 | 93 | } // namespace fuse_constraints 94 | -------------------------------------------------------------------------------- /fuse_constraints/test/legacy_marginal_version0.txt: -------------------------------------------------------------------------------- 1 | 22 serialization::archive 17 1 0 2 | 0 0 0 4 test 68a8f481-d9e2-434e-96d8-d9c52fac6603 0 0 1 0 d0344d9a-5584-5a24-b972-ea410b25e499 0 1 -1 0 0 1 0 0 0 1 3 5.00000000000000000e+00 6.00000000000000000e+00 7.00000000000000000e+00 0 0 1 1 8.00000000000000000e+00 0 0 1 1 0 1 9 50 fuse_variables::Orientation3DLocalParameterization 1 0 3 | 1 0 0 0 0 1 0 4 1 8.42614976999999987e-01 2.00000000000000011e-01 2.99999999999999989e-01 4.00000000000000022e-01 4 | -------------------------------------------------------------------------------- /fuse_core/LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2018, Locus Robotics 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /fuse_core/include/fuse_core/ceres_macros.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020 Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #ifndef FUSE_CORE_CERES_MACROS_H 35 | #define FUSE_CORE_CERES_MACROS_H 36 | 37 | #include 38 | 39 | /** 40 | * Check for at least Ceres Solver version x.y.z, where: x = major, y = minor and z = revision. 41 | */ 42 | #define CERES_VERSION_AT_LEAST(x, y, z) (CERES_VERSION_MAJOR > x || (CERES_VERSION_MAJOR >= x && \ 43 | (CERES_VERSION_MINOR > y || (CERES_VERSION_MINOR >= y && \ 44 | CERES_VERSION_REVISION >= z)))) 45 | 46 | #define CERES_SUPPORTS_MANIFOLDS CERES_VERSION_AT_LEAST(2, 1, 0) 47 | 48 | #endif // FUSE_CORE_CERES_MACROS_H 49 | -------------------------------------------------------------------------------- /fuse_core/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fuse_core 4 | 0.12.0 5 | 6 | The fuse_core package provides the base class interfaces for the various fuse components. Concrete implementations of these 7 | interfaces are provided in other packages. 8 | 9 | 10 | Stephen Williams 11 | Stephen Williams 12 | BSD 13 | 14 | catkin 15 | libceres-dev 16 | eigen 17 | fuse_msgs 18 | pluginlib 19 | roscpp 20 | rosconsole 21 | roslint 22 | rostest 23 | geometry_msgs 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /fuse_core/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | 7 | -------------------------------------------------------------------------------- /fuse_core/src/constraint.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | #include 40 | #include 41 | 42 | 43 | namespace fuse_core 44 | { 45 | 46 | Constraint::Constraint(const std::string& source, std::initializer_list variable_uuid_list) : 47 | source_(source), 48 | uuid_(uuid::generate()), 49 | variables_(variable_uuid_list) 50 | { 51 | } 52 | 53 | std::ostream& operator <<(std::ostream& stream, const Constraint& constraint) 54 | { 55 | constraint.print(stream); 56 | return stream; 57 | } 58 | 59 | } // namespace fuse_core 60 | -------------------------------------------------------------------------------- /fuse_core/src/loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | 38 | 39 | namespace fuse_core 40 | { 41 | 42 | std::ostream& operator <<(std::ostream& stream, const Loss& loss) 43 | { 44 | loss.print(stream); 45 | return stream; 46 | } 47 | 48 | } // namespace fuse_core 49 | -------------------------------------------------------------------------------- /fuse_core/src/manifold_adapter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2024, Clearpath Robotics, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #if CERES_SUPPORTS_MANIFOLDS 37 | #include 38 | 39 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_core::ManifoldAdapter); 40 | #endif 41 | -------------------------------------------------------------------------------- /fuse_core/src/serialization.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | 40 | namespace fuse_core 41 | { 42 | 43 | MessageBufferStreamSource::MessageBufferStreamSource(const std::vector& data) : 44 | data_(data), 45 | index_(0) 46 | { 47 | } 48 | 49 | std::streamsize MessageBufferStreamSource::read(char_type* s, std::streamsize n) 50 | { 51 | std::streamsize result = std::min(n, static_cast(data_.size() - index_)); 52 | if (result != 0) 53 | { 54 | std::copy(data_.begin() + index_, data_.begin() + index_ + result, s); 55 | index_ += result; 56 | return result; 57 | } 58 | else 59 | { 60 | return -1; // EOF 61 | } 62 | } 63 | 64 | MessageBufferStreamSink::MessageBufferStreamSink(std::vector& data) : 65 | data_(data) 66 | { 67 | } 68 | 69 | std::streamsize MessageBufferStreamSink::write(const char_type* s, std::streamsize n) 70 | { 71 | data_.insert(data_.end(), s, s + n); 72 | return n; 73 | } 74 | 75 | } // namespace fuse_core 76 | -------------------------------------------------------------------------------- /fuse_core/src/variable.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | 38 | 39 | namespace fuse_core 40 | { 41 | 42 | Variable::Variable(const UUID& uuid) : 43 | uuid_(uuid) 44 | { 45 | } 46 | 47 | std::ostream& operator <<(std::ostream& stream, const Variable& variable) 48 | { 49 | variable.print(stream); 50 | return stream; 51 | } 52 | 53 | } // namespace fuse_core 54 | -------------------------------------------------------------------------------- /fuse_core/test/async_motion_model.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fuse_core/test/async_publisher.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fuse_core/test/async_sensor_model.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fuse_core/test/callback_wrapper.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fuse_core/test/legacy_variable_deserialization.txt: -------------------------------------------------------------------------------- 1 | 22 serialization::archive 17 1 0 2 | 0 0 0 5a998fb5-e259-45ca-8e88-957fd8c59a60 4 9.51999999999999957e-01 3.79999999999999991e-02 -1.89000000000000001e-01 2.38999999999999990e-01 3 | -------------------------------------------------------------------------------- /fuse_core/test/parameter.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /fuse_core/test/parameter.yaml: -------------------------------------------------------------------------------- 1 | # Covariance diagonal with expected size (3) and valid: 2 | covariance_diagonal: [1.0e-3, 1.0e-3, 1.0e-3] 3 | 4 | # Covariance diagonal with negative values: 5 | covariance_diagonal_with_negative_values: [1.0e-3, 1.0e-3, -1.0e-3] 6 | 7 | # Covariance diagonal with size 2, smaller than expected (3): 8 | covariance_diagonal_with_size_2: [1.0e-3, 1.0e-3] 9 | 10 | # Covariance diagonal with size 4, larger than expected (3): 11 | covariance_diagonal_with_size_4: [1.0e-3, 1.0e-3, 1.0e-3, 1.0e-3] 12 | 13 | # Covariance diagonal with invalid element type: 14 | covariance_diagonal_with_strings: ["NaN", "NaN", "NaN"] 15 | 16 | # Covariance diagonal with invalid type: 17 | covariance_diagonal_with_string: "NaN" 18 | 19 | # Positive parameter: 20 | positive_parameter: 3.0 21 | 22 | # Negative parameter: 23 | negative_parameter: -3.0 24 | 25 | # Zero parameter: 26 | zero_parameter: 0.0 27 | -------------------------------------------------------------------------------- /fuse_core/test/test_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | 38 | 39 | TEST(Loss, Constructor) 40 | { 41 | const double a{ 0.3 }; 42 | ExampleLoss loss(a); 43 | ASSERT_EQ(a, loss.a); 44 | 45 | auto loss_function = loss.lossFunction(); 46 | 47 | ASSERT_NE(nullptr, loss_function); 48 | 49 | if (fuse_core::Loss::Ownership == ceres::Ownership::TAKE_OWNERSHIP) 50 | { 51 | delete loss_function; 52 | } 53 | } 54 | 55 | int main(int argc, char **argv) 56 | { 57 | testing::InitGoogleTest(&argc, argv); 58 | return RUN_ALL_TESTS(); 59 | } 60 | -------------------------------------------------------------------------------- /fuse_core/test/test_util.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | TEST(Util, wrapAngle2D) 43 | { 44 | // Wrap angle already in [-Pi, +Pi) range 45 | { 46 | const double angle = 0.5; 47 | EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle)); 48 | } 49 | 50 | // Wrap angle equal to +Pi 51 | { 52 | const double angle = M_PI; 53 | EXPECT_EQ(-angle, fuse_core::wrapAngle2D(angle)); 54 | } 55 | 56 | // Wrap angle equal to -Pi 57 | { 58 | const double angle = -M_PI; 59 | EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle)); 60 | } 61 | 62 | // Wrap angle greater than +Pi 63 | { 64 | const double angle = 0.5; 65 | EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle + 3.0 * 2.0 * M_PI)); 66 | } 67 | 68 | // Wrap angle smaller than -Pi 69 | { 70 | const double angle = 0.5; 71 | EXPECT_EQ(angle, fuse_core::wrapAngle2D(angle - 3.0 * 2.0 * M_PI)); 72 | } 73 | } 74 | 75 | int main(int argc, char** argv) 76 | { 77 | testing::InitGoogleTest(&argc, argv); 78 | return RUN_ALL_TESTS(); 79 | } 80 | -------------------------------------------------------------------------------- /fuse_core/test/throttled_callback.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fuse_doc/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package fuse_doc 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.0 (2022-02-23) 6 | ------------------ 7 | * Adding doxygen to all packages (#241) 8 | * Contributors: Tom Moore 9 | 10 | 0.6.0 (2023-02-22) 11 | ------------------ 12 | * 0.5.0 13 | * Update changelogs 14 | * Adding doxygen to all packages (#241) 15 | * Contributors: Gary Servin, Tom Moore 16 | 17 | 0.7.0 (2023-09-25) 18 | ------------------ 19 | * 0.6.0 20 | * Update changelogs 21 | * 0.5.0 22 | * Update changelogs 23 | * Adding doxygen to all packages (#241) 24 | * Contributors: Gary Servin, Tom Moore 25 | 26 | 0.12.0 (2025-06-06) 27 | ------------------- 28 | 29 | 0.11.0 (2025-02-04) 30 | ------------------- 31 | 32 | 0.10.0 (2024-09-16) 33 | ------------------- 34 | 35 | 0.9.0 (2024-06-17) 36 | ------------------ 37 | 38 | 0.8.0 (2024-02-02) 39 | ------------------ 40 | * 0.7.0 41 | * Update changelogs 42 | * 0.6.0 43 | * Update changelogs 44 | * 0.5.0 45 | * Update changelogs 46 | * Adding doxygen to all packages (#241) 47 | * Contributors: Gary Servin, Tom Moore 48 | 49 | 0.4.2 (2021-07-20) 50 | ------------------ 51 | * Adding roslint dependency to fuse_viz (`#231 `_) 52 | * Adding roslint dependency to fuse_viz 53 | * Silence CMP0048 warnings 54 | * Contributors: Tom Moore 55 | 56 | 0.4.1 (2021-07-13) 57 | ------------------ 58 | * Changelogs 59 | * Add support for sphinx documentation (`#228 `_) 60 | * Adding tutorial for basic configuration 61 | * Contributors: Tom Moore 62 | 63 | * Add support for sphinx documentation (`#228 `_) 64 | * Adding tutorial for basic configuration 65 | * Contributors: Tom Moore 66 | 67 | 0.4.0 (2019-07-12) 68 | ------------------ 69 | 70 | 0.3.0 (2019-03-18) 71 | ------------------ 72 | 73 | 0.2.0 (2019-01-16) 74 | ------------------ 75 | 76 | 0.1.1 (2018-08-15) 77 | ------------------ 78 | 79 | 0.1.0 (2018-08-12) 80 | ------------------ 81 | 82 | 0.0.2 (2018-07-16) 83 | ------------------ 84 | 85 | 0.0.1 (2018-07-05) 86 | ------------------ 87 | -------------------------------------------------------------------------------- /fuse_doc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fuse_doc) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | -------------------------------------------------------------------------------- /fuse_doc/LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2021, Locus Robotics 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /fuse_doc/doc/.templates/full_globaltoc.html: -------------------------------------------------------------------------------- 1 |

{{ _('Table Of Contents') }}

2 | {{ toctree(includehidden=True) }} 3 | 4 |

{{ _('Further Documentation') }}

5 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /fuse_doc/doc/images/fuse_small.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/fuse_doc/doc/images/fuse_small.png -------------------------------------------------------------------------------- /fuse_doc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fuse_doc 4 | 0.12.0 5 | 6 | The fuse_doc package provides documentation and examples for the fuse package. 7 | 8 | 9 | Stephen Williams 10 | Stephen Williams 11 | BSD 12 | 13 | catkin 14 | 15 | python-catkin-pkg 16 | python3-catkin-pkg 17 | 18 | 19 | 20 | 21 | 22 | -------------------------------------------------------------------------------- /fuse_doc/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: sphinx 2 | sphinx_root_dir: doc 3 | 4 | -------------------------------------------------------------------------------- /fuse_graphs/fuse_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This is a concrete implementation of the Graph interface using hashmaps to store the constraints and variables. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /fuse_graphs/include/fuse_graphs/hash_graph_params.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019 Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #ifndef FUSE_GRAPHS_HASH_GRAPH_PARAMS_H 35 | #define FUSE_GRAPHS_HASH_GRAPH_PARAMS_H 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | 43 | namespace fuse_graphs 44 | { 45 | 46 | /** 47 | * @brief Defines the set of parameters required by the fuse_graphs::HashGraph class 48 | */ 49 | struct HashGraphParams 50 | { 51 | public: 52 | /** 53 | * @brief Ceres Problem::Options object that controls various aspects of the optimization problem. 54 | * 55 | * See https://ceres-solver.googlesource.com/ceres-solver/+/master/include/ceres/problem.h#123 56 | */ 57 | ceres::Problem::Options problem_options; 58 | 59 | /** 60 | * @brief Method for loading parameter values from ROS. 61 | * 62 | * @param[in] nh - The ROS node handle with which to load parameters 63 | */ 64 | void loadFromROS(const ros::NodeHandle& nh) 65 | { 66 | fuse_core::loadProblemOptionsFromROS(ros::NodeHandle(nh, "problem_options"), problem_options); 67 | } 68 | }; 69 | 70 | } // namespace fuse_graphs 71 | 72 | #endif // FUSE_GRAPHS_HASH_GRAPH_PARAMS_H 73 | -------------------------------------------------------------------------------- /fuse_graphs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | fuse_graphs 7 | 0.12.0 8 | 9 | The fuse_graphs package provides some concrete implementations of the fuse_core::Graph interface. 10 | 11 | 12 | Stephen Williams 13 | Stephen Williams 14 | BSD 15 | 16 | catkin 17 | libceres-dev 18 | fuse_core 19 | pluginlib 20 | roscpp 21 | benchmark 22 | roslint 23 | rostest 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /fuse_graphs/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | 7 | -------------------------------------------------------------------------------- /fuse_loss/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fuse_loss 4 | 0.12.0 5 | 6 | The fuse_loss package provides a set of commonly used loss functions, such as the basic ones provided by Ceres. 7 | 8 | 9 | Stephen Williams 10 | Enrique Fernandez 11 | Stephen Williams 12 | BSD 13 | 14 | catkin 15 | 16 | libceres-dev 17 | fuse_core 18 | pluginlib 19 | roscpp 20 | 21 | qtbase5-dev 22 | libqwt-qt5-dev 23 | roslint 24 | 25 | 26 | 27 | 28 | 29 | 30 | -------------------------------------------------------------------------------- /fuse_loss/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | 7 | -------------------------------------------------------------------------------- /fuse_loss/src/arctan_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | 45 | namespace fuse_loss 46 | { 47 | 48 | ArctanLoss::ArctanLoss(const double a) : a_(a) 49 | { 50 | } 51 | 52 | void ArctanLoss::initialize(const std::string& name) 53 | { 54 | ros::NodeHandle private_node_handle(name); 55 | 56 | private_node_handle.param("a", a_, a_); 57 | } 58 | 59 | void ArctanLoss::print(std::ostream& stream) const 60 | { 61 | stream << type() << "\n" 62 | << " a: " << a_ << "\n"; 63 | } 64 | 65 | ceres::LossFunction* ArctanLoss::lossFunction() const 66 | { 67 | return new ceres::ArctanLoss(a_); 68 | } 69 | 70 | } // namespace fuse_loss 71 | 72 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::ArctanLoss); 73 | PLUGINLIB_EXPORT_CLASS(fuse_loss::ArctanLoss, fuse_core::Loss); 74 | -------------------------------------------------------------------------------- /fuse_loss/src/cauchy_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | 45 | namespace fuse_loss 46 | { 47 | 48 | CauchyLoss::CauchyLoss(const double a) : a_(a) 49 | { 50 | } 51 | 52 | void CauchyLoss::initialize(const std::string& name) 53 | { 54 | ros::NodeHandle private_node_handle(name); 55 | 56 | private_node_handle.param("a", a_, a_); 57 | } 58 | 59 | void CauchyLoss::print(std::ostream& stream) const 60 | { 61 | stream << type() << "\n" 62 | << " a: " << a_ << "\n"; 63 | } 64 | 65 | ceres::LossFunction* CauchyLoss::lossFunction() const 66 | { 67 | return new ceres::CauchyLoss(a_); 68 | } 69 | 70 | } // namespace fuse_loss 71 | 72 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::CauchyLoss); 73 | PLUGINLIB_EXPORT_CLASS(fuse_loss::CauchyLoss, fuse_core::Loss); 74 | -------------------------------------------------------------------------------- /fuse_loss/src/composed_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | #include 45 | #include 46 | 47 | 48 | namespace fuse_loss 49 | { 50 | 51 | ComposedLoss::ComposedLoss(const std::shared_ptr& f_loss, 52 | const std::shared_ptr& g_loss) 53 | : f_loss_(f_loss), g_loss_(g_loss) 54 | { 55 | } 56 | 57 | void ComposedLoss::initialize(const std::string& name) 58 | { 59 | ros::NodeHandle private_node_handle(name); 60 | 61 | f_loss_ = fuse_core::loadLossConfig(private_node_handle, "f_loss"); 62 | g_loss_ = fuse_core::loadLossConfig(private_node_handle, "g_loss"); 63 | } 64 | 65 | void ComposedLoss::print(std::ostream& stream) const 66 | { 67 | stream << type() << "\n"; 68 | 69 | if (f_loss_) 70 | { 71 | stream << " f_loss: " << f_loss_ << "\n"; 72 | } 73 | 74 | if (g_loss_) 75 | { 76 | stream << " g_loss: " << g_loss_ << "\n"; 77 | } 78 | } 79 | 80 | ceres::LossFunction* ComposedLoss::lossFunction() const 81 | { 82 | return new ceres::ComposedLoss(f_loss_ ? f_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership, 83 | g_loss_ ? g_loss_->lossFunction() : TrivialLoss().lossFunction(), Ownership); 84 | } 85 | 86 | } // namespace fuse_loss 87 | 88 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::ComposedLoss); 89 | PLUGINLIB_EXPORT_CLASS(fuse_loss::ComposedLoss, fuse_core::Loss); 90 | -------------------------------------------------------------------------------- /fuse_loss/src/dcs_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | 46 | namespace fuse_loss 47 | { 48 | 49 | DCSLoss::DCSLoss(const double a) : a_(a) 50 | { 51 | } 52 | 53 | void DCSLoss::initialize(const std::string& name) 54 | { 55 | ros::NodeHandle private_node_handle(name); 56 | 57 | private_node_handle.param("a", a_, a_); 58 | } 59 | 60 | void DCSLoss::print(std::ostream& stream) const 61 | { 62 | stream << type() << "\n" 63 | << " a: " << a_ << "\n"; 64 | } 65 | 66 | ceres::LossFunction* DCSLoss::lossFunction() const 67 | { 68 | return new ceres::DCSLoss(a_); 69 | } 70 | 71 | } // namespace fuse_loss 72 | 73 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::DCSLoss); 74 | PLUGINLIB_EXPORT_CLASS(fuse_loss::DCSLoss, fuse_core::Loss); 75 | -------------------------------------------------------------------------------- /fuse_loss/src/fair_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | 46 | namespace fuse_loss 47 | { 48 | 49 | FairLoss::FairLoss(const double a) : a_(a) 50 | { 51 | } 52 | 53 | void FairLoss::initialize(const std::string& name) 54 | { 55 | ros::NodeHandle private_node_handle(name); 56 | 57 | private_node_handle.param("a", a_, a_); 58 | } 59 | 60 | void FairLoss::print(std::ostream& stream) const 61 | { 62 | stream << type() << "\n" 63 | << " a: " << a_ << "\n"; 64 | } 65 | 66 | ceres::LossFunction* FairLoss::lossFunction() const 67 | { 68 | return new ceres::FairLoss(a_); 69 | } 70 | 71 | } // namespace fuse_loss 72 | 73 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::FairLoss); 74 | PLUGINLIB_EXPORT_CLASS(fuse_loss::FairLoss, fuse_core::Loss); 75 | -------------------------------------------------------------------------------- /fuse_loss/src/geman_mcclure_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | 46 | namespace fuse_loss 47 | { 48 | 49 | GemanMcClureLoss::GemanMcClureLoss(const double a) : a_(a) 50 | { 51 | } 52 | 53 | void GemanMcClureLoss::initialize(const std::string& name) 54 | { 55 | ros::NodeHandle private_node_handle(name); 56 | 57 | private_node_handle.param("a", a_, a_); 58 | } 59 | 60 | void GemanMcClureLoss::print(std::ostream& stream) const 61 | { 62 | stream << type() << "\n" 63 | << " a: " << a_ << "\n"; 64 | } 65 | 66 | ceres::LossFunction* GemanMcClureLoss::lossFunction() const 67 | { 68 | return new ceres::GemanMcClureLoss(a_); 69 | } 70 | 71 | } // namespace fuse_loss 72 | 73 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::GemanMcClureLoss); 74 | PLUGINLIB_EXPORT_CLASS(fuse_loss::GemanMcClureLoss, fuse_core::Loss); 75 | -------------------------------------------------------------------------------- /fuse_loss/src/huber_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | 45 | namespace fuse_loss 46 | { 47 | 48 | HuberLoss::HuberLoss(const double a) : a_(a) 49 | { 50 | } 51 | 52 | void HuberLoss::initialize(const std::string& name) 53 | { 54 | ros::NodeHandle private_node_handle(name); 55 | 56 | private_node_handle.param("a", a_, a_); 57 | } 58 | 59 | void HuberLoss::print(std::ostream& stream) const 60 | { 61 | stream << type() << "\n" 62 | << " a: " << a_ << "\n"; 63 | } 64 | 65 | ceres::LossFunction* HuberLoss::lossFunction() const 66 | { 67 | return new ceres::HuberLoss(a_); 68 | } 69 | 70 | } // namespace fuse_loss 71 | 72 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::HuberLoss); 73 | PLUGINLIB_EXPORT_CLASS(fuse_loss::HuberLoss, fuse_core::Loss); 74 | -------------------------------------------------------------------------------- /fuse_loss/src/loss_function.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | 40 | namespace ceres 41 | { 42 | 43 | void DCSLoss::Evaluate(double s, double rho[3]) const 44 | { 45 | if (s > a_) 46 | { 47 | // Outlier region 48 | const double inv = 1.0 / (a_ + s); 49 | const double scale = 2.0 * a_ * inv; 50 | 51 | rho[0] = a_ * (3.0 * s - a_) * inv; 52 | rho[1] = scale * scale; 53 | rho[2] = -2.0 * inv * rho[1]; 54 | } 55 | else 56 | { 57 | // Inlier region 58 | rho[0] = s; 59 | rho[1] = 1.0; 60 | rho[2] = 0.0; 61 | } 62 | } 63 | 64 | void FairLoss::Evaluate(double s, double rho[3]) const 65 | { 66 | const double r = std::sqrt(s); 67 | const double ra = r / a_; 68 | const double sum = 1.0 + ra; 69 | 70 | rho[0] = 2.0 * b_ * (ra - std::log(sum)); 71 | rho[1] = 1.0 / sum; 72 | rho[2] = r == 0.0 ? std::numeric_limits::lowest() : -0.5 / (a_ * r * sum * sum); 73 | } 74 | 75 | void GemanMcClureLoss::Evaluate(double s, double rho[3]) const 76 | { 77 | const double sum = b_ + s; 78 | const double inv = 1.0 / sum; 79 | const double scale = b_ * inv; 80 | 81 | rho[0] = s * scale; 82 | rho[1] = scale * scale; 83 | rho[2] = -2.0 * inv * rho[1]; 84 | } 85 | 86 | void WelschLoss::Evaluate(double s, double rho[3]) const 87 | { 88 | const double exp = std::exp(s * c_); 89 | 90 | rho[0] = b_ * (1 - exp); 91 | rho[1] = exp; 92 | rho[2] = c_ * exp; 93 | } 94 | 95 | } // namespace ceres 96 | -------------------------------------------------------------------------------- /fuse_loss/src/scaled_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | #include 45 | 46 | 47 | namespace fuse_loss 48 | { 49 | 50 | ScaledLoss::ScaledLoss(const double a, const std::shared_ptr& loss) : a_(a), loss_(loss) 51 | { 52 | } 53 | 54 | void ScaledLoss::initialize(const std::string& name) 55 | { 56 | ros::NodeHandle private_node_handle(name); 57 | 58 | private_node_handle.param("a", a_, a_); 59 | 60 | loss_ = fuse_core::loadLossConfig(private_node_handle, "loss"); 61 | } 62 | 63 | void ScaledLoss::print(std::ostream& stream) const 64 | { 65 | stream << type() << "\n" 66 | << " a: " << a_ << "\n"; 67 | 68 | if (loss_) 69 | { 70 | stream << " loss: " << loss_ << "\n"; 71 | } 72 | } 73 | 74 | ceres::LossFunction* ScaledLoss::lossFunction() const 75 | { 76 | return new ceres::ScaledLoss(loss_ ? loss_->lossFunction() : nullptr, a_, Ownership); 77 | } 78 | 79 | } // namespace fuse_loss 80 | 81 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::ScaledLoss); 82 | PLUGINLIB_EXPORT_CLASS(fuse_loss::ScaledLoss, fuse_core::Loss); 83 | -------------------------------------------------------------------------------- /fuse_loss/src/softlone_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | 45 | namespace fuse_loss 46 | { 47 | 48 | SoftLOneLoss::SoftLOneLoss(const double a) : a_(a) 49 | { 50 | } 51 | 52 | void SoftLOneLoss::initialize(const std::string& name) 53 | { 54 | ros::NodeHandle private_node_handle(name); 55 | 56 | private_node_handle.param("a", a_, a_); 57 | } 58 | 59 | void SoftLOneLoss::print(std::ostream& stream) const 60 | { 61 | stream << type() << "\n" 62 | << " a: " << a_ << "\n"; 63 | } 64 | 65 | ceres::LossFunction* SoftLOneLoss::lossFunction() const 66 | { 67 | return new ceres::SoftLOneLoss(a_); 68 | } 69 | 70 | } // namespace fuse_loss 71 | 72 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::SoftLOneLoss); 73 | PLUGINLIB_EXPORT_CLASS(fuse_loss::SoftLOneLoss, fuse_core::Loss); 74 | -------------------------------------------------------------------------------- /fuse_loss/src/tolerant_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | #include 43 | 44 | 45 | namespace fuse_loss 46 | { 47 | 48 | TolerantLoss::TolerantLoss(const double a, const double b) 49 | : a_(a), b_(b) 50 | { 51 | } 52 | 53 | void TolerantLoss::initialize(const std::string& name) 54 | { 55 | ros::NodeHandle private_node_handle(name); 56 | 57 | private_node_handle.param("a", a_, a_); 58 | private_node_handle.param("b", b_, b_); 59 | } 60 | 61 | void TolerantLoss::print(std::ostream& stream) const 62 | { 63 | stream << type() << "\n" 64 | << " a: " << a_ << "\n" 65 | << " b: " << b_ << "\n"; 66 | } 67 | 68 | ceres::LossFunction* TolerantLoss::lossFunction() const 69 | { 70 | return new ceres::TolerantLoss(a_, b_); 71 | } 72 | 73 | } // namespace fuse_loss 74 | 75 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::TolerantLoss); 76 | PLUGINLIB_EXPORT_CLASS(fuse_loss::TolerantLoss, fuse_core::Loss); 77 | -------------------------------------------------------------------------------- /fuse_loss/src/trivial_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | 39 | #include 40 | 41 | #include 42 | 43 | 44 | namespace fuse_loss 45 | { 46 | 47 | void TrivialLoss::print(std::ostream& stream) const 48 | { 49 | stream << type() << "\n"; 50 | } 51 | 52 | ceres::LossFunction* TrivialLoss::lossFunction() const 53 | { 54 | return new ceres::TrivialLoss(); 55 | } 56 | 57 | } // namespace fuse_loss 58 | 59 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::TrivialLoss); 60 | PLUGINLIB_EXPORT_CLASS(fuse_loss::TrivialLoss, fuse_core::Loss); 61 | -------------------------------------------------------------------------------- /fuse_loss/src/welsch_loss.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | #include 43 | #include 44 | 45 | 46 | namespace fuse_loss 47 | { 48 | 49 | WelschLoss::WelschLoss(const double a) : a_(a) 50 | { 51 | } 52 | 53 | void WelschLoss::initialize(const std::string& name) 54 | { 55 | ros::NodeHandle private_node_handle(name); 56 | 57 | private_node_handle.param("a", a_, a_); 58 | } 59 | 60 | void WelschLoss::print(std::ostream& stream) const 61 | { 62 | stream << type() << "\n" 63 | << " a: " << a_ << "\n"; 64 | } 65 | 66 | ceres::LossFunction* WelschLoss::lossFunction() const 67 | { 68 | return new ceres::WelschLoss(a_); 69 | } 70 | 71 | } // namespace fuse_loss 72 | 73 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_loss::WelschLoss); 74 | PLUGINLIB_EXPORT_CLASS(fuse_loss::WelschLoss, fuse_core::Loss); 75 | -------------------------------------------------------------------------------- /fuse_models/README.md: -------------------------------------------------------------------------------- 1 | The `fuse_models` package contains plugins that can be used to produce state estimates for a wide variety of robots. The package is intended to support transitioning from the `robot_localization` package to `fuse`. 2 | -------------------------------------------------------------------------------- /fuse_models/include/fuse_models/common/variable_traits.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #ifndef FUSE_MODELS_COMMON_VARIABLE_TRAITS_H 35 | #define FUSE_MODELS_COMMON_VARIABLE_TRAITS_H 36 | 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | 44 | namespace fuse_models 45 | { 46 | 47 | namespace common 48 | { 49 | 50 | template 51 | struct is_linear_2d 52 | { 53 | static const bool value = false; 54 | }; 55 | 56 | template<> 57 | struct is_linear_2d 58 | { 59 | static const bool value = true; 60 | }; 61 | 62 | template<> 63 | struct is_linear_2d 64 | { 65 | static const bool value = true; 66 | }; 67 | 68 | template<> 69 | struct is_linear_2d 70 | { 71 | static const bool value = true; 72 | }; 73 | 74 | template 75 | struct is_angular_2d 76 | { 77 | static const bool value = false; 78 | }; 79 | 80 | template<> 81 | struct is_angular_2d 82 | { 83 | static const bool value = true; 84 | }; 85 | 86 | template<> 87 | struct is_angular_2d 88 | { 89 | static const bool value = true; 90 | }; 91 | 92 | } // namespace common 93 | 94 | } // namespace fuse_models 95 | 96 | #endif // FUSE_MODELS_COMMON_VARIABLE_TRAITS_H 97 | -------------------------------------------------------------------------------- /fuse_models/include/fuse_models/parameters/transaction_params.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #ifndef FUSE_MODELS_PARAMETERS_TRANSACTION_PARAMS_H 36 | #define FUSE_MODELS_PARAMETERS_TRANSACTION_PARAMS_H 37 | 38 | #include 39 | 40 | #include 41 | #include 42 | 43 | #include 44 | 45 | namespace fuse_models 46 | { 47 | 48 | namespace parameters 49 | { 50 | 51 | /** 52 | * @brief Defines the set of parameters required by the Transaction class 53 | */ 54 | struct TransactionParams : public fuse_models::parameters::ParameterBase 55 | { 56 | public: 57 | /** 58 | * @brief Method for loading parameter values from ROS. 59 | * 60 | * @param[in] nh - The ROS node handle with which to load parameters 61 | */ 62 | void loadFromROS(const ros::NodeHandle& nh) final 63 | { 64 | nh.getParam("queue_size", queue_size); 65 | fuse_core::getParamRequired(nh, "topic", topic); 66 | } 67 | 68 | int queue_size{ 10 }; 69 | std::string topic{}; 70 | }; 71 | 72 | } // namespace parameters 73 | 74 | } // namespace fuse_models 75 | 76 | #endif // FUSE_MODELS_PARAMETERS_TRANSACTION_PARAMS_H 77 | -------------------------------------------------------------------------------- /fuse_models/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | fuse_models 7 | 0.12.0 8 | fuse plugins that implement various kinematic and sensor models 9 | 10 | Tom Moore 11 | Tom Moore 12 | 13 | BSD 14 | 15 | catkin 16 | 17 | boost 18 | libceres-dev 19 | eigen 20 | fuse_constraints 21 | fuse_core 22 | fuse_graphs 23 | fuse_msgs 24 | fuse_publishers 25 | fuse_variables 26 | geometry_msgs 27 | nav_msgs 28 | pluginlib 29 | roscpp 30 | sensor_msgs 31 | std_srvs 32 | tf2 33 | tf2_2d 34 | tf2_geometry_msgs 35 | tf2_ros 36 | 37 | message_generation 38 | roslint 39 | 40 | message_runtime 41 | 42 | 54 | benchmark 55 | rostest 56 | 57 | 58 | 59 | 60 | 61 | 62 | -------------------------------------------------------------------------------- /fuse_models/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | 7 | -------------------------------------------------------------------------------- /fuse_models/src/transaction.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | 35 | #include 36 | 37 | #include 38 | #include 39 | 40 | // Register this sensor model with ROS as a plugin. 41 | PLUGINLIB_EXPORT_CLASS(fuse_models::Transaction, fuse_core::SensorModel) 42 | 43 | namespace fuse_models 44 | { 45 | 46 | Transaction::Transaction() : fuse_core::AsyncSensorModel(1) 47 | { 48 | } 49 | 50 | void Transaction::onInit() 51 | { 52 | // Read settings from the parameter sever 53 | params_.loadFromROS(private_node_handle_); 54 | } 55 | 56 | void Transaction::onStart() 57 | { 58 | subscriber_ = 59 | node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, &Transaction::process, this); 60 | } 61 | 62 | void Transaction::onStop() 63 | { 64 | subscriber_.shutdown(); 65 | } 66 | 67 | void Transaction::process(const fuse_msgs::SerializedTransaction::ConstPtr& msg) 68 | { 69 | // Deserialize and send the transaction to the plugin's parent 70 | sendTransaction(transaction_deserializer_.deserialize(msg).clone()); 71 | } 72 | 73 | } // namespace fuse_models 74 | -------------------------------------------------------------------------------- /fuse_models/srv/SetGraph.srv: -------------------------------------------------------------------------------- 1 | fuse_msgs/SerializedGraph graph 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /fuse_models/srv/SetPose.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseWithCovarianceStamped pose 2 | --- 3 | bool success 4 | string message 5 | -------------------------------------------------------------------------------- /fuse_models/srv/SetPoseDeprecated.srv: -------------------------------------------------------------------------------- 1 | geometry_msgs/PoseWithCovarianceStamped pose 2 | --- 3 | -------------------------------------------------------------------------------- /fuse_models/test/graph_ignition.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fuse_models/test/unicycle_2d_ignition.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fuse_msgs/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package fuse_msgs 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.5.0 (2022-02-23) 6 | ------------------ 7 | 8 | 0.6.0 (2023-02-22) 9 | ------------------ 10 | * 0.5.0 11 | * Update changelogs 12 | * Contributors: Gary Servin 13 | 14 | 0.7.0 (2023-09-25) 15 | ------------------ 16 | * 0.6.0 17 | * Update changelogs 18 | * 0.5.0 19 | * Update changelogs 20 | * Contributors: Gary Servin 21 | 22 | 0.12.0 (2025-06-06) 23 | ------------------- 24 | 25 | 0.11.0 (2025-02-04) 26 | ------------------- 27 | 28 | 0.10.0 (2024-09-16) 29 | ------------------- 30 | 31 | 0.9.0 (2024-06-17) 32 | ------------------ 33 | 34 | 0.8.0 (2024-02-02) 35 | ------------------ 36 | * 0.7.0 37 | * Update changelogs 38 | * 0.6.0 39 | * Update changelogs 40 | * 0.5.0 41 | * Update changelogs 42 | * Contributors: Gary Servin 43 | 44 | 0.4.2 (2021-07-20) 45 | ------------------ 46 | * Adding roslint dependency to fuse_viz (`#231 `_) 47 | * Adding roslint dependency to fuse_viz 48 | * Silence CMP0048 warnings 49 | * Contributors: Tom Moore 50 | 51 | 0.4.1 (2021-07-13) 52 | ------------------ 53 | * Changelogs 54 | * [RST-2340] Add serialization support to fuse (`#98 `_) 55 | * Contributors: Stephen Williams, Tom Moore 56 | 57 | * [RST-2340] Add serialization support to fuse (`#98 `_) 58 | * Contributors: Stephen Williams 59 | 60 | 0.4.0 (2019-07-12) 61 | ------------------ 62 | 63 | 0.3.0 (2019-03-18) 64 | ------------------ 65 | 66 | 0.2.0 (2019-01-16) 67 | ------------------ 68 | 69 | 0.1.1 (2018-08-15) 70 | ------------------ 71 | 72 | 0.1.0 (2018-08-12) 73 | ------------------ 74 | 75 | 0.0.2 (2018-07-16) 76 | ------------------ 77 | 78 | 0.0.1 (2018-07-05) 79 | ------------------ 80 | -------------------------------------------------------------------------------- /fuse_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fuse_msgs) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | message_generation 6 | std_msgs 7 | ) 8 | 9 | add_message_files( 10 | DIRECTORY 11 | msg 12 | FILES 13 | SerializedGraph.msg 14 | SerializedTransaction.msg 15 | ) 16 | 17 | generate_messages( 18 | DEPENDENCIES 19 | std_msgs 20 | ) 21 | 22 | catkin_package( 23 | CATKIN_DEPENDS 24 | message_runtime 25 | std_msgs 26 | ) 27 | -------------------------------------------------------------------------------- /fuse_msgs/msg/SerializedGraph.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header # A timestamp associated with the graph 2 | string plugin_name # The fully-qualified name of the plugin used to serialize/deserialize this graph 3 | uint8[] data # The serialized graph as a raw data buffer 4 | -------------------------------------------------------------------------------- /fuse_msgs/msg/SerializedTransaction.msg: -------------------------------------------------------------------------------- 1 | std_msgs/Header header # A timestamp associated with this transaction 2 | uint8[] data # The serialized transaction as a raw data buffer 3 | -------------------------------------------------------------------------------- /fuse_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fuse_msgs 4 | 0.12.0 5 | 6 | The fuse_msgs package contains messages capable of holding serialized fuse objects 7 | 8 | 9 | Stephen Williams 10 | Stephen Williams 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | std_msgs 17 | 18 | message_generation 19 | 20 | message_runtime 21 | 22 | message_runtime 23 | 24 | -------------------------------------------------------------------------------- /fuse_optimizers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fuse_optimizers 4 | 0.12.0 5 | 6 | The fuse_optimizers package provides a set of optimizer implementations. An optimizer is the object responsible 7 | for coordinating the sensors and motion model inputs, computing the optimal state values, and providing access to 8 | to the optimal state via the publishers. 9 | 10 | 11 | Stephen Williams 12 | Stephen Williams 13 | Tom Moore 14 | BSD 15 | 16 | catkin 17 | diagnostic_updater 18 | fuse_constraints 19 | fuse_core 20 | fuse_graphs 21 | fuse_variables 22 | pluginlib 23 | roscpp 24 | std_msgs 25 | std_srvs 26 | fuse_models 27 | geometry_msgs 28 | nav_msgs 29 | roslint 30 | rostest 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /fuse_optimizers/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | 7 | -------------------------------------------------------------------------------- /fuse_optimizers/src/batch_optimizer_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | #include 36 | #include 37 | 38 | 39 | int main(int argc, char **argv) 40 | { 41 | ros::init(argc, argv, "batch_optimizer_node"); 42 | fuse_optimizers::BatchOptimizer optimizer(fuse_graphs::HashGraph::make_unique()); 43 | ros::spin(); 44 | 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /fuse_optimizers/src/fixed_lag_smoother_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2019, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | int main(int argc, char **argv) 41 | { 42 | ros::init(argc, argv, "fixed_lag_smoother_node"); 43 | ros::NodeHandle private_node_handle("~"); 44 | fuse_graphs::HashGraphParams hash_graph_params; 45 | hash_graph_params.loadFromROS(private_node_handle); 46 | fuse_optimizers::FixedLagSmoother optimizer(fuse_graphs::HashGraph::make_unique(hash_graph_params)); 47 | ros::spin(); 48 | 49 | return 0; 50 | } 51 | -------------------------------------------------------------------------------- /fuse_optimizers/test/config/list/common_robot_config.yaml: -------------------------------------------------------------------------------- 1 | motion_models: 2 | - name: unicycle_2d 3 | type: fuse_models::Unicycle2D 4 | 5 | sensor_models: 6 | - name: unicycle_2d_ignition 7 | type: fuse_models::Unicycle2DIgnition 8 | motion_models: ['unicycle_2d'] 9 | - name: wheel_odometry 10 | type: fuse_models::Odometry2D 11 | motion_models: ['unicycle_2d'] 12 | - name: laser_localization 13 | type: fuse_models::Odometry2D 14 | motion_models: ['unicycle_2d'] 15 | 16 | publishers: 17 | - name: odometry_publisher 18 | type: fuse_models::Odometry2DPublisher 19 | 20 | 21 | unicycle_2d: 22 | process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] 23 | 24 | unicycle_2d_ignition: 25 | initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] 26 | publish_on_startup: false 27 | 28 | wheel_odometry: 29 | topic: odom 30 | differential: true 31 | position_dimensions: ['x', 'y'] 32 | orientation_dimensions: ['yaw'] 33 | 34 | laser_localization: 35 | topic: pose 36 | position_dimensions: ['x', 'y'] 37 | orientation_dimensions: ['yaw'] 38 | -------------------------------------------------------------------------------- /fuse_optimizers/test/config/list/noisy_motion_model_config.yaml: -------------------------------------------------------------------------------- 1 | motion_models: 2 | - name: noisy_unicycle_2d 3 | type: fuse_models::Unicycle2D 4 | # From common_robot_config.yaml: 5 | - name: unicycle_2d 6 | type: fuse_models::Unicycle2D 7 | 8 | 9 | noisy_unicycle_2d: 10 | process_noise_diagonal: [1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1] 11 | -------------------------------------------------------------------------------- /fuse_optimizers/test/config/list/robot_with_imu_config.yaml: -------------------------------------------------------------------------------- 1 | motion_models: 2 | - name: unicycle_2d 3 | type: fuse_models::Unicycle2D 4 | # From noisy_motion_model_config.yaml: 5 | - name: noisy_unicycle_2d 6 | type: fuse_models::Unicycle2D 7 | 8 | sensor_models: 9 | - name: imu 10 | type: fuse_models::Imu2D 11 | motion_models: ['unicycle_2d'] 12 | # From common_robot_config.yaml: 13 | - name: unicycle_2d_ignition 14 | type: fuse_models::Unicycle2DIgnition 15 | motion_models: ['unicycle_2d'] 16 | - name: wheel_odometry 17 | type: fuse_models::Odometry2D 18 | motion_models: ['unicycle_2d'] 19 | - name: laser_localization 20 | type: fuse_models::Odometry2D 21 | motion_models: ['unicycle_2d'] 22 | 23 | 24 | imu: 25 | topic: imu 26 | angular_velocity_dimensions: ['yaw'] 27 | -------------------------------------------------------------------------------- /fuse_optimizers/test/config/list/serialized_publisher_config.yaml: -------------------------------------------------------------------------------- 1 | publishers: 2 | - name: serialized_publisher 3 | type: fuse_publishers::SerializedPublisher 4 | # From common_robot_config.yaml: 5 | - name: odometry_publisher 6 | type: fuse_models::Odometry2DPublisher 7 | -------------------------------------------------------------------------------- /fuse_optimizers/test/config/struct/common_robot_config.yaml: -------------------------------------------------------------------------------- 1 | motion_models: 2 | unicycle_2d: 3 | type: fuse_models::Unicycle2D 4 | 5 | sensor_models: 6 | unicycle_2d_ignition: 7 | type: fuse_models::Unicycle2DIgnition 8 | motion_models: ['unicycle_2d'] 9 | wheel_odometry: 10 | type: fuse_models::Odometry2D 11 | motion_models: ['unicycle_2d'] 12 | laser_localization: 13 | type: fuse_models::Odometry2D 14 | motion_models: ['unicycle_2d'] 15 | 16 | publishers: 17 | odometry_publisher: 18 | type: fuse_models::Odometry2DPublisher 19 | 20 | 21 | unicycle_2d: 22 | process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] 23 | 24 | unicycle_2d_ignition: 25 | initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] 26 | publish_on_startup: false 27 | 28 | wheel_odometry: 29 | topic: odom 30 | differential: true 31 | position_dimensions: ['x', 'y'] 32 | orientation_dimensions: ['yaw'] 33 | 34 | laser_localization: 35 | topic: pose 36 | position_dimensions: ['x', 'y'] 37 | orientation_dimensions: ['yaw'] 38 | -------------------------------------------------------------------------------- /fuse_optimizers/test/config/struct/noisy_motion_model_config.yaml: -------------------------------------------------------------------------------- 1 | motion_models: 2 | noisy_unicycle_2d: 3 | type: fuse_models::Unicycle2D 4 | 5 | 6 | noisy_unicycle_2d: 7 | process_noise_diagonal: [1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1, 1.1] 8 | -------------------------------------------------------------------------------- /fuse_optimizers/test/config/struct/robot_with_imu_config.yaml: -------------------------------------------------------------------------------- 1 | motion_models: 2 | unicycle_2d: 3 | type: fuse_models::Unicycle2D 4 | 5 | sensor_models: 6 | imu: 7 | type: fuse_models::Imu2D 8 | motion_models: ['unicycle_2d'] 9 | 10 | 11 | imu: 12 | topic: imu 13 | angular_velocity_dimensions: ['yaw'] 14 | -------------------------------------------------------------------------------- /fuse_optimizers/test/config/struct/serialized_publisher_config.yaml: -------------------------------------------------------------------------------- 1 | publishers: 2 | serialized_publisher: 3 | type: fuse_publishers::SerializedPublisher 4 | -------------------------------------------------------------------------------- /fuse_optimizers/test/example_optimizer.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2020, Clearpath Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #ifndef FUSE_OPTIMIZERS_TEST_EXAMPLE_OPTIMIZER_H // NOLINT{build/header_guard} 35 | #define FUSE_OPTIMIZERS_TEST_EXAMPLE_OPTIMIZER_H // NOLINT{build/header_guard} 36 | 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | 43 | /** 44 | * @brief Example optimizer that exposes the motion and sensor models, and the publishers, so we can check the expected 45 | * ones are loaded. 46 | */ 47 | class ExampleOptimizer : public fuse_optimizers::Optimizer 48 | { 49 | public: 50 | FUSE_SMART_PTR_DEFINITIONS(ExampleOptimizer); 51 | 52 | ExampleOptimizer(fuse_core::Graph::UniquePtr graph, const ros::NodeHandle& node_handle = ros::NodeHandle(), 53 | const ros::NodeHandle& private_node_handle = ros::NodeHandle("~")) 54 | : fuse_optimizers::Optimizer(std::move(graph), node_handle, private_node_handle) 55 | { 56 | } 57 | 58 | const MotionModels& getMotionModels() const 59 | { 60 | return motion_models_; 61 | } 62 | 63 | const SensorModels& getSensorModels() const 64 | { 65 | return sensor_models_; 66 | } 67 | 68 | const Publishers& getPublishers() const 69 | { 70 | return publishers_; 71 | } 72 | 73 | void transactionCallback( 74 | const std::string& sensor_name, 75 | fuse_core::Transaction::SharedPtr transaction) override 76 | { 77 | } 78 | }; 79 | 80 | #endif // FUSE_OPTIMIZERS_TEST_EXAMPLE_OPTIMIZER_H // NOLINT{build/header_guard} 81 | -------------------------------------------------------------------------------- /fuse_optimizers/test/fixed_lag_ignition.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | optimization_frequency: 2.0 6 | transaction_timeout: 5.0 7 | lag_duration: 5.0 8 | 9 | solver_options: 10 | max_num_iterations: 0 11 | 12 | motion_models: 13 | unicycle_motion_model: 14 | type: fuse_models::Unicycle2D 15 | 16 | sensor_models: 17 | unicycle_ignition_sensor: 18 | type: fuse_models::Unicycle2DIgnition 19 | motion_models: [unicycle_motion_model] 20 | ignition: true 21 | pose_sensor: 22 | type: fuse_models::Pose2D 23 | motion_models: [unicycle_motion_model] 24 | 25 | publishers: 26 | odometry_publisher: 27 | type: fuse_models::Odometry2DPublisher 28 | 29 | unicycle_motion_model: 30 | buffer_length: 5.0 31 | process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] 32 | 33 | unicycle_ignition_sensor: 34 | publish_on_startup: false 35 | initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 36 | initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] 37 | 38 | pose_sensor: 39 | differential: true 40 | topic: relative_pose 41 | position_dimensions: ['x', 'y'] 42 | orientation_dimensions: ['yaw'] 43 | 44 | odometry_publisher: 45 | topic: odom 46 | world_frame_id: map 47 | publish_tf: false 48 | 49 | 50 | 51 | 52 | 53 | -------------------------------------------------------------------------------- /fuse_optimizers/test/optimizer.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /fuse_publishers/LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2018, Locus Robotics 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /fuse_publishers/fuse_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Publisher plugin that publishes all of the Position2DStamped/Orientation2DStamped poses as a 5 | nav_msgs::Path message. 6 | 7 | 8 | 9 | 10 | Publisher that publishes the most recent Position2DStamped/Orientation2DStamped to one or more topics, 11 | including tf. 12 | 13 | 14 | 15 | 16 | Publisher plugin that publishes the transaction and graph as serialized messages. 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /fuse_publishers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fuse_publishers 4 | 0.12.0 5 | 6 | The fuse_publishers package provides a set of common publisher plugins. 7 | 8 | 9 | Stephen Williams 10 | Stephen Williams 11 | 12 | BSD 13 | 14 | catkin 15 | 16 | fuse_core 17 | fuse_variables 18 | geometry_msgs 19 | nav_msgs 20 | pluginlib 21 | roscpp 22 | tf2 23 | tf2_geometry_msgs 24 | tf2_ros 25 | 26 | fuse_constraints 27 | fuse_graphs 28 | roslint 29 | rostest 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /fuse_publishers/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | 7 | -------------------------------------------------------------------------------- /fuse_publishers/test/path_2d_publisher.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fuse_publishers/test/pose_2d_publisher.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /fuse_tutorials/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(fuse_tutorials) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | fuse_constraints 6 | fuse_core 7 | fuse_models 8 | fuse_variables 9 | nav_msgs 10 | roscpp 11 | sensor_msgs 12 | ) 13 | 14 | catkin_package( 15 | INCLUDE_DIRS 16 | include 17 | LIBRARIES 18 | ${PROJECT_NAME} 19 | CATKIN_DEPENDS 20 | fuse_constraints 21 | fuse_core 22 | fuse_models 23 | fuse_variables 24 | nav_msgs 25 | roscpp 26 | sensor_msgs 27 | ) 28 | 29 | ########### 30 | ## Build ## 31 | ########### 32 | add_compile_options(-Wall -Werror) 33 | 34 | ## fuse_tutorial library 35 | add_library(${PROJECT_NAME} 36 | src/beacon_publisher.cpp 37 | src/range_constraint.cpp 38 | src/range_sensor_model.cpp 39 | ) 40 | add_dependencies(${PROJECT_NAME} 41 | ${catkin_EXPORTED_TARGETS} 42 | ) 43 | target_include_directories(${PROJECT_NAME} 44 | PUBLIC 45 | include 46 | ) 47 | target_include_directories(${PROJECT_NAME} 48 | SYSTEM PUBLIC 49 | ${catkin_INCLUDE_DIRS} 50 | ) 51 | target_link_libraries(${PROJECT_NAME} 52 | ${catkin_LIBRARIES} 53 | ) 54 | set_target_properties(${PROJECT_NAME} 55 | PROPERTIES 56 | CXX_STANDARD 17 57 | CXX_STANDARD_REQUIRED YES 58 | ) 59 | 60 | # tutorial_sim executable 61 | add_executable(range_sensor_simulator 62 | src/range_sensor_simulator.cpp 63 | ) 64 | add_dependencies(range_sensor_simulator 65 | ${catkin_EXPORTED_TARGETS} 66 | ) 67 | target_include_directories(range_sensor_simulator 68 | PUBLIC 69 | include 70 | ) 71 | target_include_directories(range_sensor_simulator 72 | SYSTEM PUBLIC 73 | ${catkin_INCLUDE_DIRS} 74 | ) 75 | target_link_libraries(range_sensor_simulator 76 | ${PROJECT_NAME} 77 | ${catkin_LIBRARIES} 78 | ) 79 | set_target_properties(range_sensor_simulator 80 | PROPERTIES 81 | CXX_STANDARD 17 82 | CXX_STANDARD_REQUIRED YES 83 | ) 84 | 85 | ############# 86 | ## Install ## 87 | ############# 88 | 89 | install( 90 | TARGETS ${PROJECT_NAME} 91 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 92 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 93 | RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} 94 | ) 95 | 96 | install( 97 | TARGETS range_sensor_simulator 98 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 99 | ) 100 | 101 | install( 102 | DIRECTORY include/${PROJECT_NAME}/ 103 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 104 | ) 105 | 106 | install( 107 | DIRECTORY launch 108 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 109 | ) 110 | 111 | install( 112 | DIRECTORY config 113 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 114 | ) 115 | 116 | install( 117 | FILES fuse_plugins.xml 118 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 119 | ) 120 | 121 | ############# 122 | ## Testing ## 123 | ############# 124 | 125 | if(CATKIN_ENABLE_TESTING) 126 | find_package(roslint REQUIRED) 127 | 128 | # Lint tests 129 | set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references") 130 | roslint_cpp() 131 | roslint_add_test() 132 | endif() 133 | -------------------------------------------------------------------------------- /fuse_tutorials/config/fuse_simple_tutorial.yaml: -------------------------------------------------------------------------------- 1 | # Fixed-lag smoother configuration 2 | optimization_frequency: 20 3 | transaction_timeout: 0.01 4 | lag_duration: 0.5 5 | 6 | motion_models: 7 | unicycle_motion_model: 8 | type: fuse_models::Unicycle2D 9 | 10 | unicycle_motion_model: 11 | # x y yaw vx vy vyaw ax ay 12 | process_noise_diagonal: [0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.1, 0.1] 13 | 14 | sensor_models: 15 | initial_localization_sensor: 16 | type: fuse_models::Unicycle2DIgnition 17 | motion_models: [unicycle_motion_model] 18 | ignition: true 19 | odometry_sensor: 20 | type: fuse_models::Odometry2D 21 | motion_models: [unicycle_motion_model] 22 | imu_sensor: 23 | type: fuse_models::Imu2D 24 | motion_models: [unicycle_motion_model] 25 | 26 | initial_localization_sensor: 27 | publish_on_startup: true 28 | # x y yaw vx vy vyaw ax ay 29 | initial_state: [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] 30 | initial_sigma: [0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.100] 31 | 32 | odometry_sensor: 33 | topic: 'odom' 34 | twist_target_frame: 'base_footprint' 35 | linear_velocity_dimensions: ['x', 'y'] 36 | angular_velocity_dimensions: ['yaw'] 37 | 38 | imu_sensor: 39 | topic: 'imu' 40 | twist_target_frame: 'base_footprint' 41 | angular_velocity_dimensions: ['yaw'] 42 | 43 | publishers: 44 | filtered_publisher: 45 | type: fuse_models::Odometry2DPublisher 46 | 47 | filtered_publisher: 48 | topic: 'odom_filtered' 49 | base_link_frame_id: 'base_footprint' 50 | odom_frame_id: 'odom' 51 | map_frame_id: 'map' 52 | world_frame_id: 'odom' 53 | publish_tf: true 54 | publish_frequency: 10 55 | -------------------------------------------------------------------------------- /fuse_tutorials/config/range_sensor_tutorial.yaml: -------------------------------------------------------------------------------- 1 | # Fixed-lag smoother configuration 2 | optimization_frequency: 20 3 | transaction_timeout: 0.01 4 | lag_duration: 0.5 5 | 6 | motion_models: 7 | unicycle_motion_model: 8 | type: fuse_models::Unicycle2D 9 | 10 | unicycle_motion_model: 11 | # x y yaw vx vy vyaw ax ay 12 | process_noise_diagonal: [0.100, 0.100, 0.100, 0.100, 0.100, 0.100, 0.1, 0.1] 13 | 14 | sensor_models: 15 | initial_localization_sensor: 16 | type: fuse_models::Unicycle2DIgnition 17 | motion_models: [unicycle_motion_model] 18 | ignition: true 19 | odometry_sensor: 20 | type: fuse_models::Odometry2D 21 | motion_models: [unicycle_motion_model] 22 | imu_sensor: 23 | type: fuse_models::Imu2D 24 | motion_models: [unicycle_motion_model] 25 | range_sensor: 26 | type: fuse_tutorials::RangeSensorModel 27 | motion_models: [unicycle_motion_model] 28 | 29 | initial_localization_sensor: 30 | publish_on_startup: false 31 | 32 | odometry_sensor: 33 | topic: 'wheel_odom' 34 | linear_velocity_dimensions: ['x', 'y'] 35 | angular_velocity_dimensions: ['yaw'] 36 | 37 | imu_sensor: 38 | topic: 'imu' 39 | angular_velocity_dimensions: ['yaw'] 40 | 41 | range_sensor: 42 | {} 43 | 44 | publishers: 45 | filtered_publisher: 46 | type: fuse_models::Odometry2DPublisher 47 | beacon_publisher: 48 | type: fuse_tutorials::BeaconPublisher 49 | 50 | filtered_publisher: 51 | topic: 'odom_filtered' 52 | base_link_frame_id: 'base_link' 53 | odom_frame_id: 'odom' 54 | map_frame_id: 'map' 55 | world_frame_id: 'map' 56 | publish_tf: false 57 | 58 | beacon_publisher: 59 | map_frame_id: 'map' 60 | -------------------------------------------------------------------------------- /fuse_tutorials/data/turtlebot3.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/locusrobotics/fuse/97ec68964713fe2323eb69848451cb0ae3c39e22/fuse_tutorials/data/turtlebot3.bag -------------------------------------------------------------------------------- /fuse_tutorials/fuse_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | Implements a range-only measurement constraint between the robot and a landmark. The main purpose is to 5 | demonstrate how to write your own constraint classes. 6 | 7 | 8 | 9 | 10 | Implements a new sensor model that generates range constraints between the robot and landmarks. The main purpose 11 | is to demonstrate how to write your own sensor model classes. 12 | 13 | 14 | 15 | 16 | Publisher the current position of each beacon in a PointCloud2 message 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /fuse_tutorials/launch/fuse_simple_tutorial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /fuse_tutorials/launch/range_sensor_tutorial.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /fuse_tutorials/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | fuse_tutorials 7 | 0.12.0 8 | 9 | Package containing source code for the fuse tutorials 10 | 11 | 12 | Stephen Williams 13 | Stephen Williams 14 | BSD 15 | 16 | catkin 17 | fuse_constraints 18 | fuse_core 19 | fuse_models 20 | fuse_variables 21 | nav_msgs 22 | roscpp 23 | sensor_msgs 24 | fuse_constraints 25 | fuse_core 26 | fuse_models 27 | fuse_variables 28 | nav_msgs 29 | roscpp 30 | sensor_msgs 31 | fuse_constraints 32 | fuse_core 33 | fuse_models 34 | fuse_optimizers 35 | fuse_publishers 36 | fuse_variables 37 | nav_msgs 38 | roscpp 39 | rviz 40 | sensor_msgs 41 | roslint 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /fuse_tutorials/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | -------------------------------------------------------------------------------- /fuse_variables/LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2018, Locus Robotics 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without 7 | modification, are permitted provided that the following conditions 8 | are met: 9 | 10 | * Redistributions of source code must retain the above copyright 11 | notice, this list of conditions and the following disclaimer. 12 | * Redistributions in binary form must reproduce the above 13 | copyright notice, this list of conditions and the following 14 | disclaimer in the documentation and/or other materials provided 15 | with the distribution. 16 | * Neither the name of the copyright holder nor the names of its 17 | contributors may be used to endorse or promote products derived 18 | from this software without specific prior written permission. 19 | 20 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 21 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 24 | COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 27 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 28 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 29 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 30 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 31 | POSSIBILITY OF SUCH DAMAGE. 32 | -------------------------------------------------------------------------------- /fuse_variables/README.md: -------------------------------------------------------------------------------- 1 | # fuse_variables 2 | This package provides a set of commonly used variable types, such as 2D and 3D positions, orientations, velocities, 3 | and accelerations. 4 | -------------------------------------------------------------------------------- /fuse_variables/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fuse_variables 4 | 0.12.0 5 | 6 | The fuse_variables package provides a set of commonly used variable types, such as 2D and 3D positions, 7 | orientations, velocities, and accelerations. 8 | 9 | 10 | Stephen Williams 11 | Stephen Williams 12 | Tom Moore 13 | BSD 14 | 15 | catkin 16 | libceres-dev 17 | fuse_core 18 | pluginlib 19 | roscpp 20 | roslint 21 | rostest 22 | 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /fuse_variables/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | 7 | -------------------------------------------------------------------------------- /fuse_variables/src/acceleration_angular_2d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | AccelerationAngular2DStamped::AccelerationAngular2DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) : 51 | FixedSizeVariable<1>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 52 | Stamped(stamp, device_id) 53 | { 54 | } 55 | 56 | void AccelerationAngular2DStamped::print(std::ostream& stream) const 57 | { 58 | stream << type() << ":\n" 59 | << " uuid: " << uuid() << "\n" 60 | << " stamp: " << stamp() << "\n" 61 | << " device_id: " << deviceId() << "\n" 62 | << " size: " << size() << "\n" 63 | << " data:\n" 64 | << " - yaw: " << yaw() << "\n"; 65 | } 66 | 67 | } // namespace fuse_variables 68 | 69 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::AccelerationAngular2DStamped); 70 | PLUGINLIB_EXPORT_CLASS(fuse_variables::AccelerationAngular2DStamped, fuse_core::Variable); 71 | -------------------------------------------------------------------------------- /fuse_variables/src/acceleration_angular_3d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | AccelerationAngular3DStamped::AccelerationAngular3DStamped( 51 | const ros::Time& stamp, 52 | const fuse_core::UUID& device_id) : 53 | FixedSizeVariable<3>(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 54 | Stamped(stamp, device_id) 55 | { 56 | } 57 | 58 | void AccelerationAngular3DStamped::print(std::ostream& stream) const 59 | { 60 | stream << type() << ":\n" 61 | << " uuid: " << uuid() << "\n" 62 | << " stamp: " << stamp() << "\n" 63 | << " device_id: " << deviceId() << "\n" 64 | << " size: " << size() << "\n" 65 | << " data:\n" 66 | << " - roll: " << roll() << "\n" 67 | << " - pitch: " << pitch() << "\n" 68 | << " - yaw: " << yaw() << "\n"; 69 | } 70 | 71 | } // namespace fuse_variables 72 | 73 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::AccelerationAngular3DStamped); 74 | PLUGINLIB_EXPORT_CLASS(fuse_variables::AccelerationAngular3DStamped, fuse_core::Variable); 75 | -------------------------------------------------------------------------------- /fuse_variables/src/acceleration_linear_2d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | AccelerationLinear2DStamped::AccelerationLinear2DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) : 51 | FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 52 | Stamped(stamp, device_id) 53 | { 54 | } 55 | 56 | void AccelerationLinear2DStamped::print(std::ostream& stream) const 57 | { 58 | stream << type() << ":\n" 59 | << " uuid: " << uuid() << "\n" 60 | << " stamp: " << stamp() << "\n" 61 | << " device_id: " << deviceId() << "\n" 62 | << " size: " << size() << "\n" 63 | << " data:\n" 64 | << " - x: " << x() << "\n" 65 | << " - y: " << y() << "\n"; 66 | } 67 | 68 | } // namespace fuse_variables 69 | 70 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::AccelerationLinear2DStamped); 71 | PLUGINLIB_EXPORT_CLASS(fuse_variables::AccelerationLinear2DStamped, fuse_core::Variable); 72 | -------------------------------------------------------------------------------- /fuse_variables/src/acceleration_linear_3d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | AccelerationLinear3DStamped::AccelerationLinear3DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) : 51 | FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 52 | Stamped(stamp, device_id) 53 | { 54 | } 55 | 56 | void AccelerationLinear3DStamped::print(std::ostream& stream) const 57 | { 58 | stream << type() << ":\n" 59 | << " uuid: " << uuid() << "\n" 60 | << " stamp: " << stamp() << "\n" 61 | << " device_id: " << deviceId() << "\n" 62 | << " size: " << size() << "\n" 63 | << " data:\n" 64 | << " - x: " << x() << "\n" 65 | << " - y: " << y() << "\n" 66 | << " - z: " << z() << "\n"; 67 | } 68 | 69 | } // namespace fuse_variables 70 | 71 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::AccelerationLinear3DStamped); 72 | PLUGINLIB_EXPORT_CLASS(fuse_variables::AccelerationLinear3DStamped, fuse_core::Variable); 73 | -------------------------------------------------------------------------------- /fuse_variables/src/pinhole_camera_fixed.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Author: Oscar Mendez 5 | * Created: 11.13.2023 6 | * 7 | * Copyright (c) 2023, Locus Robotics 8 | * All rights reserved. 9 | * 10 | * Redistribution and use in source and binary forms, with or without 11 | * modification, are permitted provided that the following conditions 12 | * are met: 13 | * 14 | * * Redistributions of source code must retain the above copyright 15 | * notice, this list of conditions and the following disclaimer. 16 | * * Redistributions in binary form must reproduce the above 17 | * copyright notice, this list of conditions and the following 18 | * disclaimer in the documentation and/or other materials provided 19 | * with the distribution. 20 | * * Neither the name of the copyright holder nor the names of its 21 | * contributors may be used to endorse or promote products derived 22 | * from this software without specific prior written permission. 23 | * 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 25 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 26 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 27 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 28 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 29 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 30 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 31 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 32 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 33 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 34 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 35 | * POSSIBILITY OF SUCH DAMAGE. 36 | */ 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | 44 | #include 45 | 46 | namespace fuse_variables 47 | { 48 | PinholeCameraFixed::PinholeCameraFixed(const uint64_t& camera_id) : 49 | PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) 50 | { 51 | } 52 | 53 | PinholeCameraFixed::PinholeCameraFixed(const uint64_t& camera_id, 54 | const double& fx, const double& fy, 55 | const double& cx, const double& cy) 56 | : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id, fx, fy, cx, cy) 57 | { 58 | } 59 | 60 | } // namespace fuse_variables 61 | 62 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::PinholeCameraFixed); 63 | PLUGINLIB_EXPORT_CLASS(fuse_variables::PinholeCameraFixed, fuse_core::Variable); 64 | -------------------------------------------------------------------------------- /fuse_variables/src/point_2d_fixed_landmark.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2021, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | namespace fuse_variables 44 | { 45 | Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t& landmark_id) : 46 | Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) 47 | { 48 | } 49 | 50 | } // namespace fuse_variables 51 | 52 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Point2DFixedLandmark); 53 | PLUGINLIB_EXPORT_CLASS(fuse_variables::Point2DFixedLandmark, fuse_core::Variable); 54 | -------------------------------------------------------------------------------- /fuse_variables/src/point_2d_landmark.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2021, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | 45 | namespace fuse_variables 46 | { 47 | Point2DLandmark::Point2DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) : 48 | FixedSizeVariable(uuid), 49 | id_(landmark_id) 50 | { 51 | } 52 | 53 | Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) : 54 | Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) 55 | { 56 | } 57 | 58 | void Point2DLandmark::print(std::ostream& stream) const 59 | { 60 | stream << type() << ":\n" 61 | << " uuid: " << uuid() << "\n" 62 | << " size: " << size() << "\n" 63 | << " landmark id: " << id() << "\n" 64 | << " data:\n" 65 | << " - x: " << x() << "\n" 66 | << " - y: " << y() << "\n"; 67 | } 68 | 69 | } // namespace fuse_variables 70 | 71 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Point2DLandmark); 72 | PLUGINLIB_EXPORT_CLASS(fuse_variables::Point2DLandmark, fuse_core::Variable); 73 | -------------------------------------------------------------------------------- /fuse_variables/src/point_3d_fixed_landmark.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2021, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | namespace fuse_variables 44 | { 45 | Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t& landmark_id) : 46 | Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) 47 | { 48 | } 49 | 50 | } // namespace fuse_variables 51 | 52 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Point3DFixedLandmark); 53 | PLUGINLIB_EXPORT_CLASS(fuse_variables::Point3DFixedLandmark, fuse_core::Variable); 54 | -------------------------------------------------------------------------------- /fuse_variables/src/point_3d_landmark.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2021, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | 41 | #include 42 | 43 | #include 44 | 45 | namespace fuse_variables 46 | { 47 | Point3DLandmark::Point3DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) : 48 | FixedSizeVariable(uuid), 49 | id_(landmark_id) 50 | { 51 | } 52 | 53 | Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) : 54 | Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) 55 | { 56 | } 57 | 58 | void Point3DLandmark::print(std::ostream& stream) const 59 | { 60 | stream << type() << ":\n" 61 | << " uuid: " << uuid() << "\n" 62 | << " size: " << size() << "\n" 63 | << " landmark id: " << id() << "\n" 64 | << " data:\n" 65 | << " - x: " << x() << "\n" 66 | << " - y: " << y() << "\n" 67 | << " - z: " << z() << "\n"; 68 | } 69 | 70 | } // namespace fuse_variables 71 | 72 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Point3DLandmark); 73 | PLUGINLIB_EXPORT_CLASS(fuse_variables::Point3DLandmark, fuse_core::Variable); 74 | -------------------------------------------------------------------------------- /fuse_variables/src/position_2d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | Position2DStamped::Position2DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) : 51 | FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 52 | Stamped(stamp, device_id) 53 | { 54 | } 55 | 56 | void Position2DStamped::print(std::ostream& stream) const 57 | { 58 | stream << type() << ":\n" 59 | << " uuid: " << uuid() << "\n" 60 | << " stamp: " << stamp() << "\n" 61 | << " device_id: " << deviceId() << "\n" 62 | << " size: " << size() << "\n" 63 | << " data:\n" 64 | << " - x: " << x() << "\n" 65 | << " - y: " << y() << "\n"; 66 | } 67 | 68 | } // namespace fuse_variables 69 | 70 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Position2DStamped); 71 | PLUGINLIB_EXPORT_CLASS(fuse_variables::Position2DStamped, fuse_core::Variable); 72 | -------------------------------------------------------------------------------- /fuse_variables/src/position_3d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | Position3DStamped::Position3DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) : 51 | FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 52 | Stamped(stamp, device_id) 53 | { 54 | } 55 | 56 | void Position3DStamped::print(std::ostream& stream) const 57 | { 58 | stream << type() << ":\n" 59 | << " uuid: " << uuid() << "\n" 60 | << " device_id: " << deviceId() << "\n" 61 | << " stamp: " << stamp() << "\n" 62 | << " size: " << size() << "\n" 63 | << " data:\n" 64 | << " - x: " << x() << "\n" 65 | << " - y: " << y() << "\n" 66 | << " - z: " << z() << "\n"; 67 | } 68 | 69 | } // namespace fuse_variables 70 | 71 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::Position3DStamped); 72 | PLUGINLIB_EXPORT_CLASS(fuse_variables::Position3DStamped, fuse_core::Variable); 73 | -------------------------------------------------------------------------------- /fuse_variables/src/stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | 42 | 43 | namespace fuse_variables 44 | { 45 | 46 | fuse_core::UUID loadDeviceId( 47 | const ros::NodeHandle& node_handle, 48 | const std::string& uuid_parameter, 49 | const std::string& name_parameter, 50 | bool silent) 51 | { 52 | fuse_core::UUID device_id; 53 | std::string device_str; 54 | if (node_handle.getParam(uuid_parameter, device_str)) 55 | { 56 | device_id = fuse_core::uuid::from_string(device_str); 57 | } 58 | else if (node_handle.getParam(name_parameter, device_str)) 59 | { 60 | device_id = fuse_core::uuid::generate(device_str); 61 | } 62 | else 63 | { 64 | device_id = fuse_core::uuid::NIL; 65 | ROS_WARN_STREAM_COND( 66 | !silent, 67 | "No " << uuid_parameter << " or " << name_parameter << " parameter was provided on the parameter server."); 68 | } 69 | return device_id; 70 | } 71 | 72 | } // namespace fuse_variables 73 | -------------------------------------------------------------------------------- /fuse_variables/src/velocity_angular_2d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | VelocityAngular2DStamped::VelocityAngular2DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) : 51 | FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 52 | Stamped(stamp, device_id) 53 | { 54 | } 55 | 56 | void VelocityAngular2DStamped::print(std::ostream& stream) const 57 | { 58 | stream << type() << ":\n" 59 | << " uuid: " << uuid() << "\n" 60 | << " stamp: " << stamp() << "\n" 61 | << " device_id: " << deviceId() << "\n" 62 | << " size: " << size() << "\n" 63 | << " data:\n" 64 | << " - yaw: " << yaw() << "\n"; 65 | } 66 | 67 | } // namespace fuse_variables 68 | 69 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::VelocityAngular2DStamped); 70 | PLUGINLIB_EXPORT_CLASS(fuse_variables::VelocityAngular2DStamped, fuse_core::Variable); 71 | -------------------------------------------------------------------------------- /fuse_variables/src/velocity_angular_3d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | VelocityAngular3DStamped::VelocityAngular3DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) : 51 | FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 52 | Stamped(stamp, device_id) 53 | { 54 | } 55 | 56 | void VelocityAngular3DStamped::print(std::ostream& stream) const 57 | { 58 | stream << type() << ":\n" 59 | << " uuid: " << uuid() << "\n" 60 | << " stamp: " << stamp() << "\n" 61 | << " device_id: " << deviceId() << "\n" 62 | << " size: " << size() << "\n" 63 | << " data:\n" 64 | << " - roll: " << roll() << "\n" 65 | << " - pitch: " << pitch() << "\n" 66 | << " - yaw: " << yaw() << "\n"; 67 | } 68 | 69 | } // namespace fuse_variables 70 | 71 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::VelocityAngular3DStamped); 72 | PLUGINLIB_EXPORT_CLASS(fuse_variables::VelocityAngular3DStamped, fuse_core::Variable); 73 | -------------------------------------------------------------------------------- /fuse_variables/src/velocity_linear_2d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | VelocityLinear2DStamped::VelocityLinear2DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) : 51 | FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 52 | Stamped(stamp, device_id) 53 | { 54 | } 55 | 56 | void VelocityLinear2DStamped::print(std::ostream& stream) const 57 | { 58 | stream << type() << ":\n" 59 | << " uuid: " << uuid() << "\n" 60 | << " stamp: " << stamp() << "\n" 61 | << " device_id: " << deviceId() << "\n" 62 | << " size: " << size() << "\n" 63 | << " data:\n" 64 | << " - x: " << x() << "\n" 65 | << " - y: " << y() << "\n"; 66 | } 67 | 68 | } // namespace fuse_variables 69 | 70 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::VelocityLinear2DStamped); 71 | PLUGINLIB_EXPORT_CLASS(fuse_variables::VelocityLinear2DStamped, fuse_core::Variable); 72 | -------------------------------------------------------------------------------- /fuse_variables/src/velocity_linear_3d_stamped.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2018, Locus Robotics 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of the copyright holder nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | */ 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | #include 45 | 46 | 47 | namespace fuse_variables 48 | { 49 | 50 | VelocityLinear3DStamped::VelocityLinear3DStamped(const ros::Time& stamp, const fuse_core::UUID& device_id) : 51 | FixedSizeVariable(fuse_core::uuid::generate(detail::type(), stamp, device_id)), 52 | Stamped(stamp, device_id) 53 | { 54 | } 55 | 56 | void VelocityLinear3DStamped::print(std::ostream& stream) const 57 | { 58 | stream << type() << ":\n" 59 | << " uuid: " << uuid() << "\n" 60 | << " stamp: " << stamp() << "\n" 61 | << " device_id: " << deviceId() << "\n" 62 | << " size: " << size() << "\n" 63 | << " data:\n" 64 | << " - x: " << x() << "\n" 65 | << " - y: " << y() << "\n" 66 | << " - z: " << z() << "\n"; 67 | } 68 | 69 | } // namespace fuse_variables 70 | 71 | BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::VelocityLinear3DStamped); 72 | PLUGINLIB_EXPORT_CLASS(fuse_variables::VelocityLinear3DStamped, fuse_core::Variable); 73 | -------------------------------------------------------------------------------- /fuse_variables/test/load_device_id.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | -------------------------------------------------------------------------------- /fuse_viz/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | fuse_viz 4 | 0.12.0 5 | 6 | The fuse_viz package provides visualization tools for fuse. 7 | 8 | 9 | Enrique Fernandez 10 | Enrique Fernandez 11 | BSD 12 | 13 | catkin 14 | 15 | qtbase5-dev 16 | 17 | eigen 18 | fuse_constraints 19 | fuse_core 20 | fuse_msgs 21 | fuse_variables 22 | geometry_msgs 23 | rviz 24 | tf2_geometry_msgs 25 | 26 | roslint 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /fuse_viz/rosdoc.yaml: -------------------------------------------------------------------------------- 1 | - builder: doxygen 2 | output_dir: api 3 | homepage: https://docs.ros.org/en/api/fuse_doc/html/index.html 4 | file_patterns: '*.cpp *.h' 5 | exclude_patterns: '*/test/*' 6 | 7 | -------------------------------------------------------------------------------- /fuse_viz/rviz_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | An rviz display for fuse_msgs::SerializedGraph messages. 5 | 6 | 7 | 8 | --------------------------------------------------------------------------------