From 073feb69171b024f37d8fb719650157daadb3ce4 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 4 Sep 2024 22:18:38 +0000 Subject: [PATCH 01/46] change to get building with latest rclcpp --- fuse_core/include/fuse_core/callback_wrapper.hpp | 6 +++--- fuse_core/src/callback_wrapper.cpp | 8 ++++---- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/fuse_core/include/fuse_core/callback_wrapper.hpp b/fuse_core/include/fuse_core/callback_wrapper.hpp index 06fbd1209..1a3c127aa 100644 --- a/fuse_core/include/fuse_core/callback_wrapper.hpp +++ b/fuse_core/include/fuse_core/callback_wrapper.hpp @@ -166,7 +166,7 @@ class CallbackAdapter : public rclcpp::Waitable /** * @brief tell the CallbackGroup that this waitable is ready to execute anything */ - bool is_ready(rcl_wait_set_t * wait_set) override; + bool is_ready(rcl_wait_set_t const& wait_set) override; /** @@ -175,11 +175,11 @@ class CallbackAdapter : public rclcpp::Waitable waitable_ptr = std::make_shared(); node->get_node_waitables_interface()->add_waitable(waitable_ptr, (rclcpp::CallbackGroup::SharedPtr) nullptr); */ - void add_to_wait_set(rcl_wait_set_t * wait_set) override; + void add_to_wait_set(rcl_wait_set_t & wait_set) override; std::shared_ptr take_data() override; - void execute(std::shared_ptr & data) override; + void execute(std::shared_ptr const& data) override; void addCallback(const std::shared_ptr & callback); diff --git a/fuse_core/src/callback_wrapper.cpp b/fuse_core/src/callback_wrapper.cpp index f8a157c9d..c86a15de3 100644 --- a/fuse_core/src/callback_wrapper.cpp +++ b/fuse_core/src/callback_wrapper.cpp @@ -59,7 +59,7 @@ size_t CallbackAdapter::get_number_of_ready_guard_conditions() {return 1;} /** * @brief tell the CallbackGroup that this waitable is ready to execute anything */ -bool CallbackAdapter::is_ready(rcl_wait_set_t * wait_set) +bool CallbackAdapter::is_ready(rcl_wait_set_t const& wait_set) { (void) wait_set; return !callback_queue_.empty(); @@ -71,9 +71,9 @@ bool CallbackAdapter::is_ready(rcl_wait_set_t * wait_set) waitable_ptr = std::make_shared(); node->get_node_waitables_interface()->add_waitable(waitable_ptr, (rclcpp::CallbackGroup::SharedPtr) nullptr); */ -void CallbackAdapter::add_to_wait_set(rcl_wait_set_t * wait_set) +void CallbackAdapter::add_to_wait_set(rcl_wait_set_t & wait_set) { - if (RCL_RET_OK != rcl_wait_set_add_guard_condition(wait_set, &gc_, NULL)) { + if (RCL_RET_OK != rcl_wait_set_add_guard_condition(&wait_set, &gc_, NULL)) { RCLCPP_WARN(rclcpp::get_logger("fuse"), "Could not add callback waitable to wait set."); } } @@ -107,7 +107,7 @@ std::shared_ptr CallbackAdapter::take_data() * @brief hook that allows the rclcpp::waitables interface to run the next callback * */ -void CallbackAdapter::execute(std::shared_ptr & data) +void CallbackAdapter::execute(std::shared_ptr const& data) { if (!data) { throw std::runtime_error("'data' is empty"); From 94d2f1537074900ff0a4bcede77d4bfa6e55d8f7 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 6 Sep 2024 20:15:47 -0600 Subject: [PATCH 02/46] uncrustify fixes --- fuse_core/include/fuse_core/callback_wrapper.hpp | 4 ++-- fuse_core/src/callback_wrapper.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/fuse_core/include/fuse_core/callback_wrapper.hpp b/fuse_core/include/fuse_core/callback_wrapper.hpp index 1a3c127aa..0d6532823 100644 --- a/fuse_core/include/fuse_core/callback_wrapper.hpp +++ b/fuse_core/include/fuse_core/callback_wrapper.hpp @@ -166,7 +166,7 @@ class CallbackAdapter : public rclcpp::Waitable /** * @brief tell the CallbackGroup that this waitable is ready to execute anything */ - bool is_ready(rcl_wait_set_t const& wait_set) override; + bool is_ready(rcl_wait_set_t const & wait_set) override; /** @@ -179,7 +179,7 @@ class CallbackAdapter : public rclcpp::Waitable std::shared_ptr take_data() override; - void execute(std::shared_ptr const& data) override; + void execute(std::shared_ptr const & data) override; void addCallback(const std::shared_ptr & callback); diff --git a/fuse_core/src/callback_wrapper.cpp b/fuse_core/src/callback_wrapper.cpp index c86a15de3..454cf6302 100644 --- a/fuse_core/src/callback_wrapper.cpp +++ b/fuse_core/src/callback_wrapper.cpp @@ -59,7 +59,7 @@ size_t CallbackAdapter::get_number_of_ready_guard_conditions() {return 1;} /** * @brief tell the CallbackGroup that this waitable is ready to execute anything */ -bool CallbackAdapter::is_ready(rcl_wait_set_t const& wait_set) +bool CallbackAdapter::is_ready(rcl_wait_set_t const & wait_set) { (void) wait_set; return !callback_queue_.empty(); @@ -107,7 +107,7 @@ std::shared_ptr CallbackAdapter::take_data() * @brief hook that allows the rclcpp::waitables interface to run the next callback * */ -void CallbackAdapter::execute(std::shared_ptr const& data) +void CallbackAdapter::execute(std::shared_ptr const & data) { if (!data) { throw std::runtime_error("'data' is empty"); From 0ad7731f01f5a2359d3dc7b10976d4392b0890f0 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 12 Sep 2024 23:21:44 +0000 Subject: [PATCH 03/46] try disabling -Wstringop-overread --- fuse_constraints/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 8640fe075..ffe83d4e7 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -66,7 +66,7 @@ include(suitesparse-extras.cmake) # 850 | typename plain_matrix_type::type tmp(src); # | ^~~ if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU" AND CMAKE_CXX_COMPILER_VERSION VERSION_GREATER_EQUAL 12.0) - add_compile_options(-Wall -Werror -Wno-array-bounds) + add_compile_options(-Wall -Werror -Wno-array-bounds -Wno-stringop-overread) else() add_compile_options(-Wall -Werror) endif() From def055c34aee2d7d301c10574e8596c7879fd805 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 13 Sep 2024 18:24:59 +0000 Subject: [PATCH 04/46] Merge branch 'models_3d' into merge_3d_models --- fuse.repos | 9 + fuse_constraints/CMakeLists.txt | 5 + fuse_constraints/fuse_plugins.xml | 35 + .../fuse_constraints/absolute_constraint.hpp | 16 + .../absolute_constraint_impl.hpp | 24 + ...olute_pose_3d_stamped_euler_constraint.hpp | 194 +++ ...ormal_delta_pose_3d_euler_cost_functor.hpp | 161 +++ .../normal_prior_orientation_3d.hpp | 100 ++ .../fuse_constraints/normal_prior_pose_3d.hpp | 100 ++ .../normal_prior_pose_3d_euler.hpp | 106 ++ ...ormal_prior_pose_3d_euler_cost_functor.hpp | 133 +++ ...ative_pose_3d_stamped_euler_constraint.hpp | 189 +++ fuse_constraints/src/absolute_constraint.cpp | 16 + .../absolute_pose_3d_stamped_constraint.cpp | 7 +- ...olute_pose_3d_stamped_euler_constraint.cpp | 141 +++ .../src/normal_prior_orientation_3d.cpp | 94 ++ fuse_constraints/src/normal_prior_pose_3d.cpp | 107 ++ .../src/normal_prior_pose_3d_euler.cpp | 95 ++ .../relative_pose_3d_stamped_constraint.cpp | 1 + ...ative_pose_3d_stamped_euler_constraint.cpp | 133 +++ fuse_constraints/test/CMakeLists.txt | 4 + fuse_constraints/test/cost_function_gtest.hpp | 19 +- ...rientation_3d_stamped_euler_constraint.cpp | 7 +- ...olute_pose_3d_stamped_euler_constraint.cpp | 502 ++++++++ .../test/test_normal_prior_pose_3d.cpp | 92 ++ .../test/test_normal_prior_pose_3d_euler.cpp | 197 +++ ...ative_pose_3d_stamped_euler_constraint.cpp | 729 ++++++++++++ fuse_core/include/fuse_core/eigen.hpp | 2 + fuse_core/include/fuse_core/util.hpp | 175 +++ fuse_core/test/test_util.cpp | 168 +++ fuse_models/CMakeLists.txt | 9 + fuse_models/fuse_plugins.xml | 38 + .../fuse_models/common/sensor_config.hpp | 74 ++ .../fuse_models/common/sensor_proc.hpp | 1054 ++++++++++++++++- .../fuse_models/common/variable_traits.hpp | 60 + fuse_models/include/fuse_models/imu_3d.hpp | 195 +++ .../include/fuse_models/odometry_3d.hpp | 186 +++ .../fuse_models/odometry_3d_publisher.hpp | 255 ++++ .../fuse_models/omnidirectional_3d.hpp | 244 ++++ .../omnidirectional_3d_ignition.hpp | 220 ++++ .../omnidirectional_3d_predict.hpp | 647 ++++++++++ ...omnidirectional_3d_state_cost_function.hpp | 292 +++++ .../omnidirectional_3d_state_cost_functor.hpp | 217 ++++ ...ectional_3d_state_kinematic_constraint.hpp | 188 +++ .../fuse_models/parameters/imu_3d_params.hpp | 221 ++++ .../parameters/odometry_3d_params.hpp | 213 ++++ .../odometry_3d_publisher_params.hpp | 249 ++++ .../omnidirectional_3d_ignition_params.hpp | 218 ++++ fuse_models/package.xml | 2 + fuse_models/src/imu_3d.cpp | 320 +++++ fuse_models/src/odometry_3d.cpp | 257 ++++ fuse_models/src/odometry_3d_publisher.cpp | 618 ++++++++++ fuse_models/src/omnidirectional_3d.cpp | 623 ++++++++++ .../src/omnidirectional_3d_ignition.cpp | 459 +++++++ ...ectional_3d_state_kinematic_constraint.cpp | 117 ++ fuse_models/test/CMakeLists.txt | 6 + fuse_models/test/test_omnidirectional_3d.cpp | 400 +++++++ .../test/test_omnidirectional_3d_predict.cpp | 651 ++++++++++ ...omnidirectional_3d_state_cost_function.cpp | 147 +++ fuse_models/test/test_sensor_proc_3d.cpp | 150 +++ fuse_optimizers/src/fixed_lag_smoother.cpp | 3 +- 61 files changed, 11865 insertions(+), 29 deletions(-) create mode 100644 fuse.repos create mode 100644 fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp create mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp create mode 100644 fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp create mode 100644 fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp create mode 100644 fuse_constraints/src/normal_prior_orientation_3d.cpp create mode 100644 fuse_constraints/src/normal_prior_pose_3d.cpp create mode 100644 fuse_constraints/src/normal_prior_pose_3d_euler.cpp create mode 100644 fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp create mode 100644 fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp create mode 100644 fuse_constraints/test/test_normal_prior_pose_3d.cpp create mode 100644 fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp create mode 100644 fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp create mode 100644 fuse_models/include/fuse_models/imu_3d.hpp create mode 100644 fuse_models/include/fuse_models/odometry_3d.hpp create mode 100644 fuse_models/include/fuse_models/odometry_3d_publisher.hpp create mode 100644 fuse_models/include/fuse_models/omnidirectional_3d.hpp create mode 100644 fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp create mode 100644 fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp create mode 100644 fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp create mode 100644 fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp create mode 100644 fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp create mode 100644 fuse_models/include/fuse_models/parameters/imu_3d_params.hpp create mode 100644 fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp create mode 100644 fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp create mode 100644 fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp create mode 100644 fuse_models/src/imu_3d.cpp create mode 100644 fuse_models/src/odometry_3d.cpp create mode 100644 fuse_models/src/odometry_3d_publisher.cpp create mode 100644 fuse_models/src/omnidirectional_3d.cpp create mode 100644 fuse_models/src/omnidirectional_3d_ignition.cpp create mode 100644 fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp create mode 100644 fuse_models/test/test_omnidirectional_3d.cpp create mode 100644 fuse_models/test/test_omnidirectional_3d_predict.cpp create mode 100644 fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp create mode 100644 fuse_models/test/test_sensor_proc_3d.cpp diff --git a/fuse.repos b/fuse.repos new file mode 100644 index 000000000..8ad27e25b --- /dev/null +++ b/fuse.repos @@ -0,0 +1,9 @@ +repositories: + locusrobotics/tf2_2d: + type: git + url: https://github.com/locusrobotics/tf2_2d.git + version: rolling + covariance_geometry_ros: + type: git + url: https://github.com/giafranchini/covariance_geometry_ros.git + version: iron \ No newline at end of file diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index ffe83d4e7..5ccc8e33e 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -78,6 +78,7 @@ add_library(${PROJECT_NAME} src/absolute_orientation_3d_stamped_euler_constraint.cpp src/absolute_pose_2d_stamped_constraint.cpp src/absolute_pose_3d_stamped_constraint.cpp + src/absolute_pose_3d_stamped_euler_constraint.cpp src/marginal_constraint.cpp src/marginal_cost_function.cpp src/marginalize_variables.cpp @@ -85,11 +86,15 @@ add_library(${PROJECT_NAME} src/normal_delta_orientation_2d.cpp src/normal_delta_pose_2d.cpp src/normal_prior_orientation_2d.cpp + src/normal_prior_orientation_3d.cpp src/normal_prior_pose_2d.cpp + src/normal_prior_pose_3d.cpp + src/normal_prior_pose_3d_euler.cpp src/relative_constraint.cpp src/relative_orientation_3d_stamped_constraint.cpp src/relative_pose_2d_stamped_constraint.cpp src/relative_pose_3d_stamped_constraint.cpp + src/relative_pose_3d_stamped_euler_constraint.cpp src/uuid_ordering.cpp src/variable_constraints.cpp ) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index aa6b02c54..71e7b8d9a 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -5,12 +5,24 @@ the 2D angular acceleration. + + + A constraint that represents either prior information about a 3D angular acceleration, or a direct measurement of + the 3D angular acceleration. + + A constraint that represents either prior information about a 2D linear acceleration, or a direct measurement of the 2D linear acceleration. + + + A constraint that represents either prior information about a 3D linear acceleration, or a direct measurement of + the 3D linear acceleration. + + A constraint that represents either prior information about a 2D orientation, or a direct measurement of the @@ -35,12 +47,24 @@ the 2D angular velocity. + + + A constraint that represents either prior information about a 3D angular velocity, or a direct measurement of + the 3D angular velocity. + + A constraint that represents either prior information about a 2D linear velocity, or a direct measurement of the 2D linear velocity. + + + A constraint that represents either prior information about a 3D linear velocity, or a direct measurement of + the 3D linear velocity. + + A constraint that represents either prior information about a 3D orientation, or a direct measurement of the @@ -63,6 +87,12 @@ A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose. + + + A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose, + orientation parametrized in RPY. + + A constraint that represents remaining marginal information on a set of variables. @@ -118,4 +148,9 @@ A constraint that represents a measurement on the difference between two 3D poses. + + + A constraint that represents a measurement on the difference between two 3D poses, orientation parametrized in RPY. + + diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index 8bb4ac4c7..b9e38f4d7 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp @@ -46,11 +46,15 @@ #include #include #include +#include #include +#include #include #include #include #include +#include +#include #include #include @@ -196,27 +200,39 @@ class AbsoluteConstraint : public fuse_core::Constraint // Define unique names for the different variations of the absolute constraint using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationAngular3DStampedConstraint = + AbsoluteConstraint; using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationLinear3DStampedConstraint = + AbsoluteConstraint; using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition3DStampedConstraint = AbsoluteConstraint; using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityAngular3DStampedConstraint = + AbsoluteConstraint; using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityLinear3DStampedConstraint = + AbsoluteConstraint; } // namespace fuse_constraints // Include the template implementation #include BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteOrientation2DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition2DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); #endif // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp index cbb689b54..197db9c5e 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp @@ -155,6 +155,18 @@ inline std::string AbsoluteConstraint +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint"; +} + +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint"; +} + template<> inline std::string AbsoluteConstraint::type() const { @@ -185,6 +197,18 @@ inline std::string AbsoluteConstraint:: return "fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint"; } +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint"; +} + +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint"; +} + } // namespace fuse_constraints #endif // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_IMPL_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp new file mode 100644 index 000000000..63c2d8502 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -0,0 +1,194 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ +#define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents either prior information about a 3D pose, or a direct + * measurement of the 3D pose. + * + * A 3D pose is the combination of a 3D position and a 3D orientation variable. As a convenience, + * this class applies an absolute constraint on both variables at once. This type of constraint + * arises in many situations. In mapping it is common to define the very first pose as the origin. + * Some sensors, such as GPS, provide direct measurements of the robot's pose in the global frame. + * And localization systems often match laserscans or pointclouds to a prior map (scan-to-map + * measurements). This constraint holds the measured 3D pose: orientation is parametrized as roll-pitch-yaw + * Euler angles and the covariance represents the error around each translational and rotational axis. + * This constraint allows measurement of a subset of the pose components given in the variable. + */ + +class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(AbsolutePose3DStampedEulerConstraint) + + /** + * @brief Default constructor + */ + AbsolutePose3DStampedEulerConstraint() = default; + + /** + * @brief Create a constraint using a measurement/prior of the 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the pose + * @param[in] orientation The variable representing the orientation components of the pose + * @param[in] mean The measured/prior pose as a vector + * (6x1 vector: x, y, z, roll, pitch, yaw) + * @param[in] covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) + */ + AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & mean, + const fuse_core::Matrix6d & covariance); + + /** + * @brief Create a constraint using a partial measurement/prior of the 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the pose + * @param[in] orientation The variable representing the orientation components of the pose + * @param[in] partial_mean The measured/prior pose as a vector + * (6x1 vector: x, y, z, roll, pitch, yaw) + * @param[in] partial_covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) + * @param[in] variable_indices The indices of the measured variables + */ + AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & partial_mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices); + + /** + * @brief Destructor + */ + virtual ~AbsolutePose3DStampedEulerConstraint() = default; + + /** + * @brief Read-only access to the measured/prior vector of mean values. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + const fuse_core::Vector6d & mean() const {return mean_;} + + /** + * @brief Read-only access to the square root information matrix. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + fuse_core::Matrix6d covariance() const; + + /** + * @brief Print a human-readable description of the constraint to the provided stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream & stream = std::cout) const override; + + /** + * @brief Construct an instance of this constraint's cost function + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + +protected: + fuse_core::Vector6d mean_; //!< The measured/prior mean vector for this variable + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @brief The Boost Serialize method that serializes all of the data members in to/out of the + * archive + * + * @param[in/out] archive - The archive object that holds the serialized class members + * @param[in] version - The version of the archive being read/written. Generally unused. + */ + template + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & mean_; + archive & sqrt_information_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePose3DStampedEulerConstraint); + +#endif // FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp new file mode 100644 index 000000000..afb16217d --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,161 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ + +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief Implements a cost function that models a difference between 3D pose variables. + * + * A single pose involves two variables: a 3D position and a 3D orientation. This cost function + * computes the difference using standard 3D transformation math: + * + * cost(x) = || A * [ (q1^-1 * (p2 - p1)) - b(0:2) ] ||^2 + * || [ quat2rpy(q1^-1 * q2) - b(3:5) ] || + * + * where p1 and p2 are the position variables, q1 and q2 are the quaternion orientation variables, + * and the matrix A and the vector b are fixed. In case the user is interested in implementing a + * cost function of the form: + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + * + */ +class NormalDeltaPose3DEulerCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + + /** + * @brief Constructor + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (dx, dy, dz, droll, dpitch, dyaw) + * @param[in] b The exposed pose difference in order (dx, dy, dz, droll, dpitch, dyaw) + */ + NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + + /** + * @brief Compute the cost values/residuals using the provided variable/parameter values + */ + template + bool operator()( + const T * const position1, + const T * const orientation1, + const T * const position2, + const T * const orientation2, + T * residual) const; + +private: + fuse_core::MatrixXd A_; + fuse_core::Vector6d b_; +}; + +NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor( + const fuse_core::MatrixXd & A, + const fuse_core::Vector6d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); +} + +template +bool NormalDeltaPose3DEulerCostFunctor::operator()( + const T * const position1, + const T * const orientation1, + const T * const position2, + const T * const orientation2, + T * residual) const +{ + T full_residuals[6]; + T position_delta_rotated[3]; + T orientation_delta[4]; + T orientation_delta_rpy[3]; + + T orientation1_inverse[4] + { + orientation1[0], + -orientation1[1], + -orientation1[2], + -orientation1[3] + }; + + T position_delta[3] + { + position2[0] - position1[0], + position2[1] - position1[1], + position2[2] - position1[2] + }; + + // Compute the position residual + ceres::QuaternionRotatePoint(orientation1_inverse, position_delta, position_delta_rotated); + full_residuals[0] = position_delta_rotated[0] - T(b_(0)); + full_residuals[1] = position_delta_rotated[1] - T(b_(1)); + full_residuals[2] = position_delta_rotated[2] - T(b_(2)); + + // Compute the orientation residual + ceres::QuaternionProduct(orientation1_inverse, orientation2, orientation_delta); + orientation_delta_rpy[0] = fuse_core::getRoll(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[1] = fuse_core::getPitch(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[2] = fuse_core::getYaw(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + full_residuals[3] = orientation_delta_rpy[0] - T(b_(3)); + full_residuals[4] = orientation_delta_rpy[1] - T(b_(4)); + full_residuals[5] = orientation_delta_rpy[2] - T(b_(5)); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> full_residuals_map(full_residuals); + Eigen::Map> residuals_map(residual, A_.rows()); + residuals_map = A_.template cast() * full_residuals_map; + + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ \ No newline at end of file diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp new file mode 100644 index 000000000..4e6c4b60e --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_HPP_ + +#include + +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on a 3D orientation variable (quaternion) + * + * The cost function is of the form: + * + * || ||^2 + * cost(x) = || A * AngleAxis(b^-1 * q) || + * || || + * + * where the matrix A and the vector b are fixed, and q is the variable being measured, represented + * as a quaternion. The AngleAxis function converts a quaternion into a 3-vector of the form + * theta*k, where k is the unit vector axis of rotation and theta is the angle of rotation. The A + * matrix is applied to the angle-axis 3-vector. + * + * In case the user is interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorOrientation3D : public ceres::SizedCostFunction<3, 4> +{ +public: + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (qx, qy, qz) + * @param[in] b The orientation measurement or prior in order (qw, qx, qy, qz) + */ + NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b); + + /** + * @brief Destructor + */ + virtual ~NormalPriorOrientation3D() = default; + + /** + * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided + * variable/parameter values + */ + virtual bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const; + +private: + fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector4d b_; //!< The measured 3D orientation value +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp new file mode 100644 index 000000000..7ec76458c --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_HPP_ + +#include + +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the position and orientation variables at once. + * + * The cost function is of the form: + * + * || [ x - b(0)] ||^2 + * cost(x) = || A * [ y - b(1)] || + * || [ z - b(2)] || + * || [ quat2angleaxis(b(3-6)^-1 * q)] || + * + * In case the user is interested in implementing a cost function of the form: + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3D : public ceres::SizedCostFunction<6, 3, 4> +{ +public: + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The pose measurement or prior in order (x, y, z, qw, qx, qy, qz) + */ + NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + + /** + * @brief Destructor + */ + virtual ~NormalPriorPose3D() = default; + + /** + * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided + * variable/parameter values + */ + virtual bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const; + +private: + fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector7d b_; //!< The measured 3D pose value +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp new file mode 100644 index 000000000..0579b30c7 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -0,0 +1,106 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_HPP_ + +#include + +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the position and orientation variables at once. + * + * The cost function is of the form: + * + * || [ x - b(0)] ||^2 + * cost(x) = || A * [ y - b(1)] || + * || [ z - b(2)] || + * || [ quat2eul(q) - b(3:5) ] || + * + * Here, the matrix A can be of variable size, thereby permitting the computation of errors for + * partial measurements. The vector b is a fixed-size 6x1. In case the user is interested in + * implementing a cost function of the form: + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3DEuler : public ceres::SizedCostFunction +{ +public: + /** + * @brief Construct a cost function instance + * + * The residual weighting matrix can vary in size, as this cost functor can be used to compute + * costs for partial vectors. The number of rows of A will be the number of dimensions for which + * you want to compute the error, and the number of columns in A will be fixed at 6. For example, + * if we just want to use the values of x, y and yaw, then \p A will be of size 3x6. + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The pose measurement or prior in order (x, y, z, roll, pitch, yaw) + */ + NormalPriorPose3DEuler(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + + /** + * @brief Destructor + */ + virtual ~NormalPriorPose3DEuler() = default; + + /** + * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided + * variable/parameter values + */ + virtual bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const; + +private: + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector6d b_; //!< The measured 3D pose value +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp new file mode 100644 index 000000000..085bc2fd6 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,133 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ + +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the 3D position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the 3D position and orientation variables at + * once. + * + * The cost function is of the form: + * + * cost(x) = || A * [ p - b(0:2) ] ||^2 + * || [ quat2eul(q) - b(3:5) ] || + * + * where, the matrix A and the vector b are fixed, p is the position variable, and q is the + * orientation variable. In case the user is interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3DEulerCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The 3D pose measurement or prior in order (x, y, z, roll, pitch, yaw) + */ + NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + */ + template + bool operator()(const T * const position, const T * const orientation, T * residual) const; + +private: + fuse_core::MatrixXd A_; + fuse_core::Vector6d b_; +}; + +NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor( + const fuse_core::MatrixXd & A, + const fuse_core::Vector6d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); +} + +template +bool NormalPriorPose3DEulerCostFunctor::operator()( + const T * const position, const T * const orientation, + T * residual) const +{ + T full_residuals[6]; + T orientation_rpy[3] = { + fuse_core::getRoll(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]) + }; + + // Compute the position residual + full_residuals[0] = position[0] - T(b_(0)); + full_residuals[1] = position[1] - T(b_(1)); + full_residuals[2] = position[2] - T(b_(2)); + // Compute the orientation residual + full_residuals[3] = orientation_rpy[0] - T(b_(3)); + full_residuals[4] = orientation_rpy[1] - T(b_(4)); + full_residuals[5] = orientation_rpy[2] - T(b_(5)); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> full_residuals_map(full_residuals); + Eigen::Map> residuals_map(residual, A_.rows()); + residuals_map = A_.template cast() * full_residuals_map; + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp new file mode 100644 index 000000000..a5f2fdf9d --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp @@ -0,0 +1,189 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ +#define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents a measurement on the difference between two 3D poses. + * + * This type of constraint arises in many situations. Many types of incremental odometry + * measurements (e.g., visual odometry) measure the change in the pose, not the pose directly. This + * constraint holds the measured 3D pose change and the measurement uncertainty/covariance. + */ +class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(RelativePose3DStampedEulerConstraint) + + /** + * @brief Default constructor + */ + RelativePose3DStampedEulerConstraint() = default; + + /** + * @brief Create a constraint using a measurement/prior of the relative 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 The variable representing the position components of the first pose + * @param[in] orientation1 The variable representing the orientation components of the first pose + * @param[in] position2 The variable representing the position components of the second pose + * @param[in] orientation2 The variable representing the orientation components of the second pose + * @param[in] delta The measured change in the pose + * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) + * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, droll, dpitch, dyaw) + */ + RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector6d & delta, + const fuse_core::Matrix6d & covariance); + + /** + * @brief Create a constraint using a measurement/prior of the relative 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 The variable representing the position components of the first pose + * @param[in] orientation1 The variable representing the orientation components of the first pose + * @param[in] position2 The variable representing the position components of the second pose + * @param[in] orientation2 The variable representing the orientation components of the second pose + * @param[in] partial_delta The measured change in the pose + * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) + * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, droll, dpitch, dyaw) + * @param[in] variable_indices The indices of the measured variables + */ + RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector6d & partial_delta, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices); + + /** + * @brief Destructor + */ + virtual ~RelativePose3DStampedEulerConstraint() = default; + + /** + * @brief Read-only access to the measured pose change. + */ + const fuse_core::Vector6d & delta() const {return delta_;} + + /** + * @brief Read-only access to the square root information matrix. + */ + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + */ + fuse_core::Matrix6d covariance() const; + + /** + * @brief Print a human-readable description of the constraint to the provided stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream & stream = std::cout) const override; + + /** + * @brief Access the cost function for this constraint + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + +protected: + fuse_core::Vector6d delta_; //!< The measured pose change (dx, dy, dz, droll, dpitch, dyaw) + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the + //!< covariance matrix) + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @brief The Boost Serialize method that serializes all of the data members in to/out of the + * archive + * + * @param[in/out] archive - The archive object that holds the serialized class members + * @param[in] version - The version of the archive being read/written. Generally unused. + */ + template + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & delta_; + archive & sqrt_information_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::RelativePose3DStampedEulerConstraint); + +#endif // FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index 9aa7593fc..bf4ad6519 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -36,19 +36,29 @@ #include BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteOrientation2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint, + fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint, + fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteOrientation2DStampedConstraint, fuse_core::Constraint); @@ -61,6 +71,12 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint, + fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint, + fuse_core::Constraint); \ No newline at end of file diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index cc64e8d75..324b7fbc6 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,14 +32,13 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include #include #include #include #include -#include +#include #include namespace fuse_constraints @@ -74,8 +74,7 @@ void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsolutePose3DStampedConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalPriorPose3DCostFunctor(sqrt_information_, mean_)); + return new NormalPriorPose3D(sqrt_information_, mean_); } } // namespace fuse_constraints diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..56206e301 --- /dev/null +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,141 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include + +#include + +#include +#include +#include + +namespace fuse_constraints +{ + +AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & mean, + const fuse_core::Matrix6d & covariance) +: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT + mean_(mean), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & partial_mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices) +: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT + mean_(partial_mean) +{ + // Compute the partial sqrt information matrix of the provided cov matrix + fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + + // Assemble a sqrt information matrix from the provided values, but in proper Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); + for (size_t i = 0; i < variable_indices.size(); ++i) + { + sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); + } +} + +fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const +{ + if (sqrt_information_.rows() == 6) + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + // Otherwise we need to compute the pseudoinverse + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + +void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position variable: " << variables().at(0) << "\n" + << " orientation variable: " << variables().at(1) << "\n" + << " mean: " << mean().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; + + if (loss()) { + stream << " loss: "; + loss()->print(stream); + } +} + +ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const +{ + return new NormalPriorPose3DEuler(sqrt_information_, mean_); + + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could + // use automatic differentiation as follows: + + // return new ceres::AutoDiffCostFunction( + // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), + // sqrt_information_.rows()); + + // And including the followings: + // #include + // #include +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePose3DStampedEulerConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePose3DStampedEulerConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/normal_prior_orientation_3d.cpp b/fuse_constraints/src/normal_prior_orientation_3d.cpp new file mode 100644 index 000000000..34269cedc --- /dev/null +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -0,0 +1,94 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include + +namespace fuse_constraints +{ + +NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b) +: A_(A), + b_(b) +{ +} + +bool NormalPriorOrientation3D::Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const +{ + double variable[4] = + { + parameters[0][0], + parameters[0][1], + parameters[0][2], + parameters[0][3], + }; + + double observation_inverse[4] = + { + b_(0), + -b_(1), + -b_(2), + -b_(3) + }; + + double difference[4]; + double j_product[16]; + double j_quat2angle[12]; + + // TODO(giafranchini): these jacobians should be populated only if jacobians[1] != nullptr + fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); + fuse_core::quaternionToAngleAxis(difference, residuals, j_quat2angle); // orientation angle-axis + + // Scale the residuals by the square root information matrix to account for the measurement + // uncertainty. + Eigen::Map residuals_map(residuals); + residuals_map.applyOnTheLeft(A_); + + if (jacobians != nullptr) { + if (jacobians[0] != nullptr) { + Eigen::Map> j_map(jacobians[0]); + Eigen::Map j_product_map(j_product); + Eigen::Map> j_quat2angle_map(j_quat2angle); + j_map = A_ * j_quat2angle_map * j_product_map; + } + } + return true; +} + +} // namespace fuse_constraints diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp new file mode 100644 index 000000000..581e455dd --- /dev/null +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include + +namespace fuse_constraints +{ + +NormalPriorPose3D::NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b) +: A_(A), + b_(b) +{ +} + +bool NormalPriorPose3D::Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const +{ + + double variable[4] = + { + parameters[1][0], + parameters[1][1], + parameters[1][2], + parameters[1][3], + }; + + double observation_inverse[4] = + { + b_(3), + -b_(4), + -b_(5), + -b_(6) + }; + + double difference[4]; + double j_product[16]; + double j_quat2angle[12]; + + // Compute position residuals + residuals[0] = parameters[0][0] - b_[0]; + residuals[1] = parameters[0][1] - b_[1]; + residuals[2] = parameters[0][2] - b_[2]; + // Compute orientation residuals + + // TODO(giafranchini): this is repeated code, it should be better to include normal_prior_orientation_3d.hpp + // and use that to compute residuals and jacobians. + fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); + fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis + + // Scale the residuals by the square root information matrix to account for the measurement + // uncertainty. + Eigen::Map residuals_map(residuals); + residuals_map.applyOnTheLeft(A_); + + if (jacobians != nullptr) { + // Jacobian of the residuals wrt position parameter block + if (jacobians[0] != nullptr) { + Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); + } + // Jacobian of the residuals wrt orientation parameter block + if (jacobians[1] != nullptr) { + Eigen::Map j_product_map(j_product); + Eigen::Map> j_quat2angle_map(j_quat2angle); + Eigen::Map j1_map(jacobians[1], num_residuals(), 4); + j1_map = A_.rightCols<3>() * j_quat2angle_map * j_product_map; + } + } + return true; +} + +} // namespace fuse_constraints diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp new file mode 100644 index 000000000..c4871aa3b --- /dev/null +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -0,0 +1,95 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include + +#include +#include +#include + +namespace fuse_constraints +{ + +NormalPriorPose3DEuler::NormalPriorPose3DEuler(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); + set_num_residuals(A_.rows()); +} + +bool NormalPriorPose3DEuler::Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const +{ + fuse_core::Vector6d full_residuals; + double orientation_rpy[3]; + double j_quat2rpy[12]; + + fuse_core::quaternion2rpy(parameters[1], orientation_rpy, j_quat2rpy); + + // Compute the position residual + full_residuals(0) = parameters[0][0] - b_(0); + full_residuals(1) = parameters[0][1] - b_(1); + full_residuals(2) = parameters[0][2] - b_(2); + + // Compute the orientation residual + full_residuals(3) = orientation_rpy[0] - b_(3); + full_residuals(4) = orientation_rpy[1] - b_(4); + full_residuals(5) = orientation_rpy[2] - b_(5); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> residuals_map(residuals, A_.rows()); + residuals_map = A_ * full_residuals; + + if (jacobians != nullptr) { + // Jacobian wrt position + if (jacobians[0] != nullptr) { + Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); + } + // Jacobian wrt orientation + if (jacobians[1] != nullptr) { + Eigen::Map> j_quat2angle_map(j_quat2rpy); + Eigen::Map(jacobians[1], num_residuals(), 4) = + A_.rightCols<3>() * j_quat2angle_map; + } + } + return true; +} + +} // namespace fuse_constraints diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp index 7d627e117..1aefeb5fe 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -74,6 +74,7 @@ void RelativePose3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * RelativePose3DStampedConstraint::costFunction() const { + // TODO(giafranchini): implement cost function with analytical derivatives return new ceres::AutoDiffCostFunction( new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_)); } diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..0c4e7eee3 --- /dev/null +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,133 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include + +#include +#include +#include +#include + +namespace fuse_constraints +{ + +RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector6d & delta, + const fuse_core::Matrix6d & covariance) +: fuse_core::Constraint( + source, + {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT + delta_(delta), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector6d & partial_delta, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices) +: fuse_core::Constraint( + source, + {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT + delta_(partial_delta) +{ + // Compute the partial sqrt information matrix of the provided cov matrix + fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + + // Assemble a sqrt information matrix from the provided values, but in proper Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); + for (size_t i = 0; i < variable_indices.size(); ++i) + { + sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); + } +} + +fuse_core::Matrix6d RelativePose3DStampedEulerConstraint::covariance() const +{ + // We need to compute the pseudoinverse + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + +void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position1 variable: " << variables().at(0) << "\n" + << " orientation1 variable: " << variables().at(1) << "\n" + << " position2 variable: " << variables().at(2) << "\n" + << " orientation2 variable: " << variables().at(3) << "\n" + << " delta: " << delta().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; +} + +ceres::CostFunction * RelativePose3DStampedEulerConstraint::costFunction() const +{ + // TODO(giafranchini): implement cost function with analytical Jacobians + return new ceres::AutoDiffCostFunction( + new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_), + sqrt_information_.rows()); +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePose3DStampedEulerConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::RelativePose3DStampedEulerConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/test/CMakeLists.txt b/fuse_constraints/test/CMakeLists.txt index 426322c5c..ec69a351b 100644 --- a/fuse_constraints/test/CMakeLists.txt +++ b/fuse_constraints/test/CMakeLists.txt @@ -6,13 +6,17 @@ set(TEST_TARGETS test_absolute_orientation_3d_stamped_euler_constraint test_absolute_pose_2d_stamped_constraint test_absolute_pose_3d_stamped_constraint + test_absolute_pose_3d_stamped_euler_constraint test_marginal_constraint test_marginalize_variables test_normal_delta_pose_2d test_normal_prior_pose_2d + test_normal_prior_pose_3d + test_normal_prior_pose_3d_euler test_relative_constraint test_relative_pose_2d_stamped_constraint test_relative_pose_3d_stamped_constraint + test_relative_pose_3d_stamped_euler_constraint test_uuid_ordering test_variable_constraints ) diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index 3687d0504..02cae90e3 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -38,6 +38,7 @@ #include #include +#include /** * @brief A helper function to compare a expected and actual cost function. @@ -68,8 +69,22 @@ static void ExpectCostFunctionsAreEqual( } std::unique_ptr parameters(new double[num_parameters]); - for (size_t i = 0; i < num_parameters; ++i) { - parameters[i] = static_cast(i) + 1.0; + if ((num_parameters == 7) && (parameter_block_sizes[0] == 3) && + (parameter_block_sizes[1] == 4)) + { + // Special case for parameters[1] as quaternion + for (size_t i = 0; i < 3; i++) { + parameters[i] = static_cast(i) + 1.0; + } + Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom(); + parameters[3] = q.w(); + parameters[4] = q.x(); + parameters[5] = q.y(); + parameters[6] = q.z(); + } else { + for (size_t i = 0; i < num_parameters; ++i) { + parameters[i] = static_cast(i) + 1.0; + } } std::unique_ptr residuals(new double[num_residuals]); diff --git a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp index 9787d83f6..9d775cb5e 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -150,7 +151,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) ceres::Solve(options, &problem, &summary); // Check - Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * + Eigen::Quaterniond expected = + Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); @@ -229,7 +231,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) ceres::Solve(options, &problem, &summary); // Check - Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * + Eigen::Quaterniond expected = + Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..4e0b0457a --- /dev/null +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,502 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; +using fuse_constraints::AbsolutePose3DStampedEulerConstraint; + + +TEST(AbsolutePose3DStampedEulerConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + AbsolutePose3DStampedEulerConstraint constraint( + "test", position_variable, orientation_variable, + mean, cov)); +} + +TEST(AbsolutePose3DStampedEulerConstraint, ConstructorPartial) +{ + // Construct a constraint just to make sure it compiles. + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector6d mean_partial; + mean_partial << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; + + fuse_core::Matrix5d cov_partial; + /* *INDENT-OFF* */ + cov_partial << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + AbsolutePose3DStampedEulerConstraint constraint( + "test", position_variable, orientation_variable, + mean_partial, cov_partial, variable_indices)); +} + +TEST(AbsolutePose3DStampedEulerConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( + "mo")); + + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, + cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix6d expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + /* *INDENT-ON* */ + fuse_core::Matrix6d expected_cov = cov; + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); +} + +TEST(AbsolutePose3DStampedEulerConstraint, CovariancePartial) +{ + // Verify the covariance <--> sqrt information conversions are correct + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( + "mo")); + + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector6d mean; + mean << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, + mean, cov, variable_indices); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, // NOLINT + 0. , 0.0, 3.82674687, 2.80341172 , -2.68168479, -2.88943844, // NOLINT + 0. , 0.0, 0. , 1.83006791 , -0.69691741, -1.17412835, // NOLINT + 0. , 0.0, 0. , 0. , 0.95330283, -0.76965441, // NOLINT + 0. , 0.0, 0. , 0. , 0. , 0.68147774; // NOLINT + /* *INDENT-ON* */ + + fuse_core::Matrix6d expected_cov; + /* *INDENT-OFF* */ + expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-8); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-8); +} + +TEST(AbsolutePose3DStampedEulerConstraint, Optimization) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are + // generated. Create the variables + auto position_variable = Position3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = -3.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + // Create an absolute pose constraint + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.0, 0.0, 0.0; + + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( + "test", + *position_variable, + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + position_variable->data(), + position_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position_variable->localParameterization()); +#else + position_variable->manifold()); +#endif + + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(2.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); + covariance.GetCovarianceBlock( + position_variable->data(), + position_variable->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + // High tolerance here, but also high values of covariance + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); +} + +TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are + // generated. Create the variables. Version for partial measurements + auto position_variable = Position3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = 1.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + // Create an absolute pose constraint + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector6d mean; + mean << 1.0, 0.0, 3.0, 0.0, 0.0, 0.0; + + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 1.0, 0.2, 0.3, 0.4, 0.5, + 0.2, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( + "test", + *position_variable, + *orientation_variable, + mean, + cov, + variable_indices); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + position_variable->data(), + position_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position_variable->localParameterization()); +#else + position_variable->manifold()); +#endif + + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(1.0, position_variable->y(), 1.0e-5); // This is not measured so it will not change + EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); + covariance.GetCovarianceBlock( + position_variable->data(), + position_variable->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.2, 0.3, 0.4, 0.5, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.2, 0.0, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.0, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.0, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.0, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + // High tolerance here, but also high values of covariance + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); +} + +TEST(AbsolutePose3DStampedEulerConstraint, Serialization) +{ + // Construct a constraint + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedEulerConstraint expected("test", position_variable, orientation_variable, mean, + cov); + + // Serialize the constraint into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new constraint from that same stream + AbsolutePose3DStampedEulerConstraint actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_MATRIX_EQ(expected.mean(), actual.mean()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); +} diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp new file mode 100644 index 000000000..8a778c092 --- /dev/null +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -0,0 +1,92 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include + +#include +#include + +#include "cost_function_gtest.hpp" +#include +#include +#include + +/** + * @brief Test fixture that initializes a full pose 3d mean and sqrt information matrix. + */ +class NormalPriorPose3DTestFixture : public ::testing::Test +{ +public: + //!< The automatic differentiation cost function type for the pose 3d cost functor + using AutoDiffNormalPriorPose3D = + ceres::AutoDiffCostFunction; + + /** + * @brief Constructor + */ + NormalPriorPose3DTestFixture() + { + full_sqrt_information = covariance.inverse().llt().matrixU(); + } + + const fuse_core::Matrix6d covariance = + fuse_core::Vector6d(1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components + Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{1.0, 2.0, 1.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean + //!< components: x, y z, + //!< qw, qx, qy, qz +}; + +TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) +{ + // Create cost function + auto q = Eigen::Quaterniond::UnitRandom(); + full_mean << 1.0, 2.0, 1.0, q.w(), q.x(), q.y(), q.z(); // Create automatic differentiation cost function + const fuse_constraints::NormalPriorPose3D cost_function{full_sqrt_information, full_mean}; + const auto num_residuals = full_sqrt_information.rows(); + + AutoDiffNormalPriorPose3D autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DCostFunctor(full_sqrt_information, full_mean), + num_residuals); + + // Compare the expected, automatic differentiation, cost function and the actual one + // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function + // and the second argument is the actual cost function + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); +} \ No newline at end of file diff --git a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp new file mode 100644 index 000000000..5b9f08f64 --- /dev/null +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -0,0 +1,197 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include + +#include +#include + +#include "cost_function_gtest.hpp" +#include +#include +#include + +/** + * @brief Test fixture that initializes a full pose 3d mean and sqrt information matrix. + */ +class NormalPriorPose3DEulerTestFixture : public ::testing::Test +{ +public: + //!< The automatic differentiation cost function type for the pose 3d cost functor + using AutoDiffNormalPriorPose3DEuler = + ceres::AutoDiffCostFunction; + + /** + * @brief Constructor + */ + NormalPriorPose3DEulerTestFixture() + { + full_sqrt_information = covariance.inverse().llt().matrixU(); + } + + const fuse_core::Matrix6d covariance = + fuse_core::Vector6d(1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components + Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{1.0, 2.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean + //!< components: x, y z, + //!< roll, pitch, and yaw +}; + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) +{ + // Create cost function + auto rpy = Eigen::Vector3d::Random(); + full_mean << 1.0, 2.0, 1.0, rpy.x(), rpy.y(), rpy.z(); + const fuse_constraints::NormalPriorPose3DEuler cost_function{full_sqrt_information, full_mean}; + const auto num_residuals = full_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(full_sqrt_information, full_mean), + num_residuals); + + // Compare the expected, automatic differentiation, cost function and the actual one + // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function + // and the second argument is the actual cost function + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); +} + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialPositionResiduals) +{ + // Create cost function for a subset of residuals + // Version with y position = 0 + std::vector indices = {0, 2, 3, 4, 5}; + auto rpy = Eigen::Vector3d::Random(); + full_mean << 1.0, 0.0, 1.0, rpy.x(), rpy.y(), rpy.z(); + fuse_core::Matrix partial_sqrt_information; + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + std::cout << "full_mean: " << full_mean << std::endl; + std::cout << "partial_sqrt_information: " << partial_sqrt_information << std::endl; + + const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); +} + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialOrientationResiduals) +{ + // Create cost function for a subset of residuals + // Version with roll, pitch = 0 + std::vector indices = {0, 1, 2, 5}; + Eigen::Vector3d rpy = Eigen::Vector3d::Random(); + rpy(0) = 0.0; + rpy(1) = 0.0; + full_mean << 1.0, 0.0, 1.0, rpy.x(), rpy.y(), rpy.z(); + fuse_core::Matrix partial_sqrt_information; + + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); +} + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsPositionOnly) +{ + // Create cost function for a subset of residuals + // Version with z = 0, orientation = 0 + std::vector indices = {0, 1}; + Eigen::Vector3d rpy {0.0, 0.0, 0.0}; + full_mean << 0.1, 0.5, 0.0, rpy.x(), rpy.y(), rpy.z(); + fuse_core::Matrix partial_sqrt_information; + + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); +} + +TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForPartialResidualsOrientationOnly) +{ + // Create cost function for a subset of residuals + // Version with position = 0, roll = 0 + std::vector indices = {4, 5}; + Eigen::Vector3d rpy = Eigen::Vector3d::Random(); + rpy(0) = 0.0; + full_mean << 0.0, 0.0, 0.0, rpy.x(), rpy.y(), rpy.z(); + fuse_core::Matrix partial_sqrt_information; + + for (size_t i = 0; i < indices.size(); ++i) { + partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); + } + + const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + + // Create automatic differentiation cost function + const auto num_residuals = partial_sqrt_information.rows(); + + AutoDiffNormalPriorPose3DEuler autodiff_cost_function( + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), + num_residuals); + + ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); +} diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..8f266107a --- /dev/null +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,729 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; +using fuse_constraints::AbsolutePose3DStampedConstraint; +using fuse_constraints::AbsolutePose3DStampedEulerConstraint; +using fuse_constraints::RelativePose3DStampedEulerConstraint; + + +TEST(RelativePose3DStampedEulerConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + RelativePose3DStampedEulerConstraint constraint( + "test", position1, orientation1, position2, + orientation2, delta, cov)); +} + +TEST(RelativePose3DStampedEulerConstraint, ConstructorPartial) +{ + // Construct a constraint just to make sure it compiles. + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + std::vector indices {0, 1, 3, 4, 5}; + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + RelativePose3DStampedEulerConstraint constraint( + "test", position1, orientation1, position2, + orientation2, delta, cov, indices)); +} + +TEST(RelativePose3DStampedEulerConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + RelativePose3DStampedEulerConstraint constraint( + "test", + position1, + orientation1, + position2, + orientation2, + delta, + cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix6d expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + /* *INDENT-ON* */ + fuse_core::Matrix6d expected_cov = cov; + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); +} + +TEST(RelativePose3DStampedEulerConstraint, CovariancePartial) +{ + // Verify the covariance <--> sqrt information conversions are correct + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + std::vector indices {0, 1, 3, 4, 5}; + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + RelativePose3DStampedEulerConstraint constraint( + "test", + position1, + orientation1, + position2, + orientation2, + delta, + cov, + indices); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 1.7687, -0.1286, 0.0, -1.3877, -0.6508, 0.6747, // NOLINT + 0.0, 1.3117, 0.0, -0.3361, -0.0415, -0.4332, // NOLINT + 0.0, 0.0, 0.0, 1.8301, -0.6969, -1.1741, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.9533, -0.7697, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.6815; // NOLINT + /* *INDENT-ON* */ + /* *INDENT-ON* */ + fuse_core::Matrix6d expected_cov; + + expected_cov << 2.0847236144069, 1.10752598122138, 0.0, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.0, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.96120532313878, 1.35471905449013, 0.0, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 0.0, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 0.0, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-4); +} + +TEST(RelativePose3DStampedEulerConstraint, Optimization) +{ + // Optimize a two-pose system with a pose prior and a relative pose constraint + // Verify the expected poses and covariances are generated. + // Create two poses + auto position1 = + Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position1->x() = 1.5; + position1->y() = -3.0; + position1->z() = 10.0; + + auto orientation1 = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation1->w() = 0.952; + orientation1->x() = 0.038; + orientation1->y() = -0.189; + orientation1->z() = 0.239; + + auto position2 = + Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + position2->x() = -1.5; + position2->y() = 3.0; + position2->z() = -10.0; + + auto orientation2 = Orientation3DStamped::make_shared( + rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + orientation2->w() = 0.944; + orientation2->x() = -0.128; + orientation2->y() = 0.145; + orientation2->z() = -0.269; + + // Create an absolute pose constraint at the origin + fuse_core::Vector7d mean_origin; + mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); + auto prior = AbsolutePose3DStampedConstraint::make_shared( + "test", + *position1, + *orientation1, + mean_origin, + cov_origin); + + // Create a relative pose constraint for 1m in the x direction + fuse_core::Vector6d mean_delta; + mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_delta = fuse_core::Matrix6d::Identity(); + auto relative = RelativePose3DStampedEulerConstraint::make_shared( + "test", + *position1, + *orientation1, + *position2, + *orientation2, + mean_delta, + cov_delta); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation1->data(), + orientation1->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation1->localParameterization()); +#else + orientation1->manifold()); +#endif + problem.AddParameterBlock( + position1->data(), + position1->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position1->localParameterization()); +#else + position1->manifold()); +#endif + problem.AddParameterBlock( + orientation2->data(), + orientation2->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation2->localParameterization()); +#else + orientation2->manifold()); +#endif + problem.AddParameterBlock( + position2->data(), + position2->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position2->localParameterization()); +#else + position2->manifold()); +#endif + std::vector prior_parameter_blocks; + prior_parameter_blocks.push_back(position1->data()); + prior_parameter_blocks.push_back(orientation1->data()); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); + std::vector relative_parameter_blocks; + relative_parameter_blocks.push_back(position1->data()); + relative_parameter_blocks.push_back(orientation1->data()); + relative_parameter_blocks.push_back(position2->data()); + relative_parameter_blocks.push_back(orientation2->data()); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position1->x(), 1.0e-5); + EXPECT_NEAR(0.0, position1->y(), 1.0e-5); + EXPECT_NEAR(0.0, position1->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation1->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->z(), 1.0e-3); + EXPECT_NEAR(1.0, position2->x(), 1.0e-5); + EXPECT_NEAR(0.0, position2->y(), 1.0e-5); + EXPECT_NEAR(0.0, position2->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation2->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->z(), 1.0e-3); + + // Compute the marginal covariance for pose1 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position1->data(), position1->data()); + covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); + covariance_blocks.emplace_back(position1->data(), orientation1->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position1->size(), position1->size()); + covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(3, 3); + covariance.GetCovarianceBlockInTangentSpace( + orientation1->data(), + orientation1->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->size(), 3); + covariance.GetCovarianceBlockInTangentSpace( + position1->data(), + orientation1->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); + } + + // Compute the marginal covariance for pose2 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position2->data(), position2->data()); + covariance_blocks.emplace_back(position2->data(), orientation2->data()); + covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position2->size(), position2->size()); + covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(3, 3); + covariance.GetCovarianceBlockInTangentSpace( + orientation2->data(), + orientation2->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->size(), 3); + covariance.GetCovarianceBlockInTangentSpace( + position2->data(), + orientation2->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, + 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; + /* *INDENT-ON* */ + + // High tolerance here, but also high values of covariance + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); + } +} + +TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) +{ + // Optimize a two-pose system with a pose prior and a relative pose constraint + // Verify the expected poses and covariances are generated. + // Create two poses + auto position1 = + Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position1->x() = 1.5; + position1->y() = -3.0; + position1->z() = 10.0; + + auto orientation1 = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation1->w() = 0.952; + orientation1->x() = 0.038; + orientation1->y() = -0.189; + orientation1->z() = 0.239; + + auto position2 = + Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + position2->x() = -1.5; + position2->y() = 3.0; + position2->z() = -10.0; + + auto orientation2 = Orientation3DStamped::make_shared( + rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + orientation2->w() = 0.944; + orientation2->x() = -0.128; + orientation2->y() = 0.145; + orientation2->z() = -0.269; + + std::vector indices {0, 1, 3, 4, 5}; + + // Create an absolute pose constraint at the origin + fuse_core::Vector7d mean_origin; + mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); + auto prior = AbsolutePose3DStampedConstraint::make_shared( + "test", + *position1, + *orientation1, + mean_origin, + cov_origin); + + // Create a relative pose constraint for 1m in the x direction + fuse_core::Vector6d mean_delta; + mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + fuse_core::Matrix5d cov_delta = fuse_core::Matrix5d::Identity(); + auto relative = RelativePose3DStampedEulerConstraint::make_shared( + "test", + *position1, + *orientation1, + *position2, + *orientation2, + mean_delta, + cov_delta, + indices); + + // Create a relative pose constraint for 1m in the y direction + std::vector indices_y {1, 2, 3, 4, 5}; + fuse_core::Vector6d mean_delta_y; + mean_delta_y << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0; + fuse_core::Matrix5d cov_delta_y = fuse_core::Matrix5d::Identity(); + auto relative_y = RelativePose3DStampedEulerConstraint::make_shared( + "test", + *position1, + *orientation1, + *position2, + *orientation2, + mean_delta_y, + cov_delta_y, + indices_y); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation1->data(), + orientation1->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation1->localParameterization()); +#else + orientation1->manifold()); +#endif + problem.AddParameterBlock( + position1->data(), + position1->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position1->localParameterization()); +#else + position1->manifold()); +#endif + problem.AddParameterBlock( + orientation2->data(), + orientation2->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation2->localParameterization()); +#else + orientation2->manifold()); +#endif + problem.AddParameterBlock( + position2->data(), + position2->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position2->localParameterization()); +#else + position2->manifold()); +#endif + std::vector prior_parameter_blocks; + prior_parameter_blocks.push_back(position1->data()); + prior_parameter_blocks.push_back(orientation1->data()); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); + std::vector relative_parameter_blocks; + relative_parameter_blocks.push_back(position1->data()); + relative_parameter_blocks.push_back(orientation1->data()); + relative_parameter_blocks.push_back(position2->data()); + relative_parameter_blocks.push_back(orientation2->data()); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); + std::vector relative_parameter_blocks_y; + relative_parameter_blocks_y.push_back(position1->data()); + relative_parameter_blocks_y.push_back(orientation1->data()); + relative_parameter_blocks_y.push_back(position2->data()); + relative_parameter_blocks_y.push_back(orientation2->data()); + problem.AddResidualBlock( + relative_y->costFunction(), + relative_y->lossFunction(), + relative_parameter_blocks_y); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position1->x(), 1.0e-5); + EXPECT_NEAR(0.0, position1->y(), 1.0e-5); + EXPECT_NEAR(0.0, position1->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation1->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->z(), 1.0e-3); + EXPECT_NEAR(1.0, position2->x(), 1.0e-5); + EXPECT_NEAR(0.5, position2->y(), 1.0e-5); + EXPECT_NEAR(0.0, position2->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation2->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->z(), 1.0e-3); + + // Compute the marginal covariance for pose1 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position1->data(), position1->data()); + covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); + covariance_blocks.emplace_back(position1->data(), orientation1->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position1->size(), position1->size()); + covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation1->data(), + orientation1->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position1->data(), + orientation1->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); + } + + // Compute the marginal covariance for pose2 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position2->data(), position2->data()); + covariance_blocks.emplace_back(position2->data(), orientation2->data()); + covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position2->size(), position2->size()); + covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation2->data(), + orientation2->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position2->data(), + orientation2->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 2.25, -0.5, 0.0, 0.0, 0.0, -0.5, + -0.5, 2.5, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 3.25, 0.5, -1.0, 0.0, + 0.0, 0.0, 0.5, 1.5, 0.0, 0.0, + 0.0, 0.0, -1.0, 0.0, 1.5, 0.0, + -0.5, 1.0, 0.0, 0.0, 0.0, 1.5; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); + } +} + +TEST(RelativePose3DStampedEulerConstraint, Serialization) +{ + // Construct a constraint + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + RelativePose3DStampedEulerConstraint expected("test", position1, orientation1, position2, orientation2, + delta, cov); + + // Serialize the constraint into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new constraint from that same stream + RelativePose3DStampedEulerConstraint actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_MATRIX_EQ(expected.delta(), actual.delta()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); +} diff --git a/fuse_core/include/fuse_core/eigen.hpp b/fuse_core/include/fuse_core/eigen.hpp index 6d9ced923..447325539 100644 --- a/fuse_core/include/fuse_core/eigen.hpp +++ b/fuse_core/include/fuse_core/eigen.hpp @@ -54,6 +54,7 @@ using Vector6d = Eigen::Matrix; using Vector7d = Eigen::Matrix; using Vector8d = Eigen::Matrix; using Vector9d = Eigen::Matrix; +using Vector15d = Eigen::Matrix; using MatrixXd = Eigen::Matrix; using Matrix1d = Eigen::Matrix; @@ -65,6 +66,7 @@ using Matrix6d = Eigen::Matrix; using Matrix7d = Eigen::Matrix; using Matrix8d = Eigen::Matrix; using Matrix9d = Eigen::Matrix; +using Matrix15d = Eigen::Matrix; template using Matrix = Eigen::Matrix; diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index b3bdc4a55..5766b349e 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -35,6 +35,7 @@ #define FUSE_CORE__UTIL_HPP_ #include +#include #include #include @@ -150,6 +151,180 @@ Eigen::Matrix rotationMatrix2D(const T angle) return rotation; } +/** + * @brief Compute roll, pitch, and yaw from a quaternion + * + * @param[in] q Pointer to the quaternion array (4x1) + * @param[in] rpy Pointer to the roll, pitch, yaw array (3x1) + * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) + */ +static inline void quaternion2rpy(const double * q, double * rpy, double * jacobian = nullptr) +{ + rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); + rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); + rpy[2] = fuse_core::getYaw(q[0], q[1], q[2], q[3]); + + if (jacobian) { + Eigen::Map> jacobian_map(jacobian); + const double qw = q[0]; + const double qx = q[1]; + const double qy = q[2]; + const double qz = q[3]; + const double discr = qw * qy - qx * qz; + const double gl_limit = 0.5 - 2.0 * std::numeric_limits::epsilon(); + + if (discr > gl_limit) { + // pitch = 90 deg + jacobian_map.setZero(); + jacobian_map(2, 0) = (2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); + jacobian_map(2, 1) = -2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); + return; + } else if (discr < -gl_limit) { + // pitch = -90 deg + jacobian_map.setZero(); + jacobian_map(2, 0) = (-2.0 * qx) / (qw * qw * ((qx * qx / qw * qw) + 1.0)); + jacobian_map(2, 1) = 2.0 / (qw * ((qx * qx / qw * qw) + 1.0)); + return; + } else { + // Non-degenerate case: + jacobian_map(0, 0) = + -(2.0 * qx) / + ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + + 1.0) * + (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); + jacobian_map(0, 1) = + -((2.0 * qw) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - + (4.0 * qx * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / + (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); + jacobian_map(0, 2) = + -((2.0 * qz) / (2.0 * qx * qx + 2.0 * qy * qy - 1.0) - + (4.0 * qy * (2.0 * qw * qx + 2.0 * qy * qz)) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0)) / + (std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + 1.0); + jacobian_map(0, 3) = + -(2.0 * qy) / + ((std::pow((2.0 * qw * qx + 2.0 * qy * qz), 2.0) / std::pow((2.0 * qx * qx + 2.0 * qy * qy - 1.0), 2.0) + + 1.0) * + (2.0 * qx * qx + 2.0 * qy * qy - 1.0)); + + jacobian_map(1, 0) = (2.0 * qy) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map(1, 1) = -(2.0 * qz) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map(1, 2) = (2.0 * qw) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + jacobian_map(1, 3) = -(2.0 * qx) / std::sqrt(1.0 - std::pow((2.0 * qw * qy - 2.0 * qx * qz), 2.0)); + + jacobian_map(2, 0) = + -(2.0 * qz) / + ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + + 1.0) * + (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); + jacobian_map(2, 1) = + -(2.0 * qy) / + ((std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + + 1.0) * + (2.0 * qy * qy + 2.0 * qz * qz - 1.0)); + jacobian_map(2, 2) = + -((2.0 * qx) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - + (4.0 * qy * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / + (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); + jacobian_map(2, 3) = + -((2.0 * qw) / (2.0 * qy * qy + 2.0 * qz * qz - 1.0) - + (4.0 * qz * (2.0 * qw * qz + 2.0 * qx * qy)) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0)) / + (std::pow((2.0 * qw * qz + 2.0 * qx * qy), 2.0) / std::pow((2.0 * qy * qy + 2.0 * qz * qz - 1.0), 2.0) + 1.0); + } + } +} + +/** + * @brief Compute product of two quaternions and the function jacobian + * TODO(giafranchini): parametric jacobian computation? Atm this function is only used in + * normal_prior_pose_3d cost function. There we only need the derivatives wrt quaternion W, + * so at the time we are only computing the jacobian wrt W + * + * @param[in] z Pointer to the first quaternion array (4x1) + * @param[in] w Pointer to the second quaternion array (4x1) + * @param[in] zw Pointer to the output quaternion array (4x1) that will be populated with the result of z * w + * @param[in] jacobian Pointer to the jacobian of zw with respect to w (4x4, optional) + */ +static inline void quaternionProduct(const double * z, const double * w, double * zw, double * jacobian = nullptr) +{ + ceres::QuaternionProduct(z, w, zw); + if (jacobian) { + Eigen::Map> jacobian_map(jacobian); + jacobian_map << + z[0], -z[1], -z[2], -z[3], + z[1], z[0], -z[3], z[2], + z[2], z[3], z[0], -z[1], + z[3], -z[2], z[1], z[0]; + } +} + +/** + * @brief Compute quaternion to AngleAxis conversion and the function jacobian + * + * @param[in] q Pointer to the quaternion array (4x1) + * @param[in] angle_axis Pointer to the angle_axis array (3x1) + * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) + */ +static inline void quaternionToAngleAxis(const double * q, double * angle_axis, double * jacobian = nullptr) +{ + ceres::QuaternionToAngleAxis(q, angle_axis); + if (jacobian) { + Eigen::Map> jacobian_map(jacobian); + const double & q0 = q[0]; + const double & q1 = q[1]; + const double & q2 = q[2]; + const double & q3 = q[3]; + const double q_sum2 = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; + const double sin_theta2 = q1 * q1 + q2 * q2 + q3 * q3; + const double sin_theta = std::sqrt(sin_theta2); + const double cos_theta = q0; + + if (std::fpclassify(sin_theta) != FP_ZERO) { + const double two_theta = 2.0 * + (cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta)); + jacobian_map(0, 0) = -2.0 * q1 / q_sum2; + jacobian_map(0, 1) = + two_theta / sin_theta + + (2.0 * q0 * q1 * q1) / (sin_theta2 * q_sum2) - + (q1 * q1 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(0, 2) = + (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - + (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(0, 3) = + (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - + (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + + jacobian_map(1, 0) = -2.0 * q2 / q_sum2; + jacobian_map(1, 1) = + (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - + (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(1, 2) = + two_theta / sin_theta + + (2.0 * q0 * q2 * q2) / (sin_theta2 * q_sum2) - + (q2 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(1, 3) = + (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - + (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + + jacobian_map(2, 0) = -2.0 * q3 / q_sum2; + jacobian_map(2, 1) = + (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - + (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(2, 2) = + (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - + (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(2, 3) = + two_theta / sin_theta + + (2.0 * q0 * q3 * q3) / (sin_theta2 * q_sum2) - + (q3 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + } else { + jacobian_map.setZero(); + jacobian_map(1, 1) = 2.0; + jacobian_map(2, 2) = 2.0; + jacobian_map(3, 3) = 2.0; + } + } +} + /** * @brief Create a compound ROS topic name from two components * diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 49851fe4b..1ce51eb79 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -36,7 +36,50 @@ #include #include +#include #include +#include + +struct Quat2RPY { + template + bool operator()(const T * const q, T * rpy) const { + rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); + rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); + rpy[2] = fuse_core::getYaw(q[0], q[1], q[2], q[3]); + return true; + } + + static ceres::CostFunction* Create() { + return (new ceres::AutoDiffCostFunction(new Quat2RPY())); + } +}; + +struct QuatProd { + template + bool operator()( + const T * const q1, + const T * const q2, + T * q_out) const { + ceres::QuaternionProduct(q1, q2, q_out); + return true; + } + + static ceres::CostFunction* Create() { + return (new ceres::AutoDiffCostFunction(new QuatProd())); + } +}; + +struct Quat2AngleAxis { + template + bool operator()(const T * const q, T * aa) const { + ceres::QuaternionToAngleAxis(q, aa); + return true; + } + + static ceres::CostFunction* Create() { + return (new ceres::AutoDiffCostFunction(new Quat2AngleAxis())); + } +}; TEST(Util, wrapAngle2D) { @@ -81,3 +124,128 @@ TEST(Util, wrapAngle2D) EXPECT_EQ("~b", fuse_core::joinTopicName("a/", "~b")); } } + +TEST(Util, quaternion2rpy) +{ + // Test correct conversion from quaternion to roll-pitch-yaw + double q[4] = {1.0, 0.0, 0.0, 0.0}; + double rpy[3]; + fuse_core::quaternion2rpy(q, rpy); + EXPECT_EQ(0.0, rpy[0]); + EXPECT_EQ(0.0, rpy[1]); + EXPECT_EQ(0.0, rpy[2]); + + q[0] = 0.9818562; + q[1] = 0.0640713; + q[2] = 0.0911575; + q[3] = -0.1534393; + + fuse_core::quaternion2rpy(q, rpy); + EXPECT_NEAR(0.1, rpy[0], 1e-6); + EXPECT_NEAR(0.2, rpy[1], 1e-6); + EXPECT_NEAR(-0.3, rpy[2], 1e-6); + + // Test correct quaternion to roll-pitch-yaw function jacobian + const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); + double J_analytic[12], J_autodiff[12]; + q[0] = q_eigen.w(); + q[1] = q_eigen.x(); + q[2] = q_eigen.y(); + q[3] = q_eigen.z(); + + fuse_core::quaternion2rpy(q, rpy, J_analytic); + + double * jacobians[1] = {J_autodiff}; + const double * parameters[1] = {q}; + + ceres::CostFunction * quat2rpy_cf = Quat2RPY::Create(); + double rpy_autodiff[3]; + quat2rpy_cf->Evaluate(parameters, rpy_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); +} + +TEST(Util, quaternionProduct) +{ + // Test correct quaternion product function jacobian + const Eigen::Quaterniond q1_eigen = Eigen::Quaterniond::UnitRandom(); + const Eigen::Quaterniond q2_eigen = Eigen::Quaterniond::UnitRandom(); + double q_out[4]; + double q1[4] + { + q1_eigen.w(), + q1_eigen.x(), + q1_eigen.y(), + q1_eigen.z() + }; + + double q2[4] + { + q2_eigen.w(), + q2_eigen.x(), + q2_eigen.y(), + q2_eigen.z() + }; + + // Atm only the jacobian wrt the second quaternion is implemented. If the computation will be + // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. + double J_analytic_q1[16], J_analytic_q2[16]; // Analytical Jacobians wrt first and second quaternion + double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion + + fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); + + double * jacobians[2]; + jacobians[0] = J_autodiff_q1; + jacobians[1] = J_autodiff_q2; + + const double * parameters[2]; + parameters[0] = q1; + parameters[1] = q2; + + ceres::CostFunction * quat_prod_cf = QuatProd::Create(); + double q_out_autodiff[4]; + quat_prod_cf->Evaluate(parameters, q_out_autodiff, jacobians); + + Eigen::Map> J_autodiff_q1_map(jacobians[0]); + Eigen::Map> J_autodiff_q2_map(jacobians[1]); + + // Eigen::Map> J_analytic_q1_map(J_analytic_q1); + Eigen::Map> J_analytic_q2_map(J_analytic_q2); + + EXPECT_TRUE(J_analytic_q2_map.isApprox(J_autodiff_q2_map)); +} + +TEST(Util, quaternionToAngleAxis) +{ + // Test correct quaternion to angle-axis function jacobian + const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); + double angle_axis[3]; + double q[4] + { + q_eigen.w(), + q_eigen.x(), + q_eigen.y(), + q_eigen.z() + }; + + double J_analytic[12]; + double J_autodiff[12]; + + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + + double * jacobians[1] = {J_autodiff}; + const double * parameters[1] = {q}; + + ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); + double angle_axis_autodiff[3]; + quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); +} + diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index fe3bc9452..0df2d164d 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -13,6 +13,7 @@ endif() find_package(ament_cmake_ros REQUIRED) +find_package(covariance_geometry_ros REQUIRED) find_package(fuse_constraints REQUIRED) find_package(fuse_core REQUIRED) find_package(fuse_graphs REQUIRED) @@ -43,20 +44,27 @@ add_library(${PROJECT_NAME} src/acceleration_2d.cpp src/graph_ignition.cpp src/imu_2d.cpp + src/imu_3d.cpp src/odometry_2d.cpp src/odometry_2d_publisher.cpp + src/odometry_3d.cpp + src/odometry_3d_publisher.cpp src/pose_2d.cpp src/transaction.cpp src/twist_2d.cpp src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp src/unicycle_2d_state_kinematic_constraint.cpp + src/omnidirectional_3d.cpp + src/omnidirectional_3d_ignition.cpp + src/omnidirectional_3d_state_kinematic_constraint.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" ) target_link_libraries(${PROJECT_NAME} + covariance_geometry_ros::covariance_geometry_ros Ceres::ceres fuse_constraints::fuse_constraints fuse_core::fuse_core @@ -115,6 +123,7 @@ ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) ament_export_dependencies( ament_cmake_ros + covariance_geometry_ros fuse_constraints fuse_core fuse_graphs diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index f6c860934..7830e9a81 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -12,6 +12,19 @@ + + + A class that represents a kinematic constraint between 3D states at two different times. + + + + + + A fuse_models 3D kinematic model that generates kinematic constraints between provided time stamps, and adds + those constraints to the fuse graph. + + + Publisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts a tf transform for optimized 2D @@ -20,6 +33,14 @@ + + + Publisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts a tf transform for optimized 3D + state data (combination of Position3DStamped, Orientation3DStamped, VelocityLinear3DStamped, and + VelocityAngular3DStamped). + + + An adapter-type sensor that produces 2D linear acceleration constraints from information published by @@ -40,12 +61,24 @@ acceleration constraints from IMU sensor data published by another node + + + An adapter-type sensor that produces 3D orientation (relative or absolute), angular velocity, and linear + acceleration constraints from IMU sensor data published by another node + + An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data published by another node + + + An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data + published by another node + + An adapter-type sensor that produces absolute or relative pose constraints from information published by @@ -68,4 +101,9 @@ A fuse_models ignition sensor designed to be used in conjunction with the unicycle 2D motion model. + + + A fuse_models ignition sensor designed to be used in conjunction with the Omnidirectional 3D motion model. + + diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index b9f2b3505..d9eeb4b5b 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -44,10 +44,14 @@ #include #include +#include #include +#include #include #include +#include #include +#include #include @@ -90,6 +94,28 @@ std::enable_if_t::value, size_t> toIndex(const std::string & dim return 0u; } +/** + * @brief Method that converts from 3D linear axis dimension names to index values + * + * This method is enabled only for variables that contain _only_ 3D linear quantities + * + * @param[in] dimension - The dimension name to convert + * @return the index of the enumerated dimension for that type + * @throws runtime_error if the dimension name is invalid + */ +template +std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +{ + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "x") {return static_cast(T::X);} + if (lower_dim == "y") {return static_cast(T::Y);} + if (lower_dim == "z") {return static_cast(T::Z);} + + throwDimensionError(dimension); + + return 0u; +} + /** * @brief Method that converts from 2D angular axis dimension names to index values * @@ -112,6 +138,54 @@ std::enable_if_t::value, size_t> toIndex(const std::string & di return 0u; } +/** + * @brief Method that converts from 3D angular axis dimension names to index values + * + * This method is enabled only for variables that contain _only_ 3D angular quantities + * + * @param[in] dimension - The dimension name to convert + * @return the index of the enumerated dimension for that type + * @throws runtime_error if the dimension name is invalid + */ +template +std::enable_if_t::value && !is_orientation::value, size_t> toIndex(const std::string & dimension) +{ + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "roll" || lower_dim == "x") { + return static_cast(T::ROLL); + } + if (lower_dim == "pitch" || lower_dim == "y") { + return static_cast(T::PITCH); + } + if (lower_dim == "yaw" || lower_dim == "z") { + return static_cast(T::YAW); + } + + throwDimensionError(dimension); + + return 0u; +} + +template +std::enable_if_t::value && is_orientation::value, size_t> toIndex(const std::string & dimension) +{ + // Trick to get roll, pitch, yaw indexes as 0, 1, 2 + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "roll" || lower_dim == "x") { + return static_cast(fuse_variables::Orientation3DStamped::Euler::ROLL) - 4UL; + } + if (lower_dim == "pitch" || lower_dim == "y") { + return static_cast(fuse_variables::Orientation3DStamped::Euler::PITCH) - 4UL; + } + if (lower_dim == "yaw" || lower_dim == "z") { + return static_cast(fuse_variables::Orientation3DStamped::Euler::YAW) - 4UL; + } + + throwDimensionError(dimension); + + return 0u; +} + /** * @brief Utility method to convert a vector of dimension names to a vector of dimension indices * diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 40bb4bc14..4a851cd9b 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -45,18 +46,28 @@ #include #include +#include #include +#include +#include #include +#include +#include #include #include #include #include #include #include +#include #include +#include #include +#include #include +#include #include +#include #include #include @@ -69,6 +80,11 @@ #include #include +#include "covariance_geometry/pose_composition.hpp" +#include "covariance_geometry/pose_covariance_representation.hpp" +#include "covariance_geometry/pose_covariance_composition.hpp" +#include "covariance_geometry_ros/utils.hpp" + #include @@ -194,6 +210,27 @@ inline void populatePartialMeasurement( } } +/** + * @brief Method to create covariances of sub-measurements from covariances of full measurements and append + * them to existing partial covariances + * + * @param[in] covariance_full - The full covariance matrix from which we will generate the covariances of the + * sub-measurement + * @param[in] indices - The indices we want to include in the sub-measurement + * @param[in,out] covariance_partial - The partial measurement covariance to which we want to append + */ +inline void populatePartialMeasurement( + const fuse_core::MatrixXd & covariance_full, + const std::vector & indices, + fuse_core::MatrixXd & covariance_partial) +{ + for (size_t r = 0; r < indices.size(); ++r) { + for (size_t c = 0; c < indices.size(); ++c) { + covariance_partial(r, c) = covariance_full(indices[r], indices[c]); + } + } +} + /** * @brief Method to validate partial measurements, that checks for finite values and covariance * properties @@ -224,6 +261,28 @@ inline void validatePartialMeasurement( } } +inline void validateMeasurement( + const fuse_core::VectorXd & mean, + const fuse_core::MatrixXd & covariance, + const double precision = Eigen::NumTraits::dummy_precision()) +{ + if (!mean.allFinite()) { + throw std::runtime_error("Invalid mean " + fuse_core::to_string(mean)); + } + + if (!fuse_core::isSymmetric(covariance, precision)) { + throw std::runtime_error( + "Non-symmetric covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } + + if (!fuse_core::isPositiveDefinite(covariance)) { + throw std::runtime_error( + "Non-positive-definite covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } +} + /** * @brief Transforms a ROS geometry message from its frame to the frame of the output message * @@ -357,7 +416,7 @@ inline bool processAbsolutePoseWithCovariance( if (validate) { try { - validatePartialMeasurement(pose_mean_partial, pose_covariance_partial); + validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -387,6 +446,165 @@ inline bool processAbsolutePoseWithCovariance( return true; } +/** + * @brief Extracts 3D pose data from a PoseWithCovarianceStamped message and adds that data to a + * fuse Transaction + * + * This method effectively adds two variables (3D position and 3D orientation) and a 3D pose + * constraint to the given \p transaction. The pose data is extracted from the \p pose message. + * The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] pose - The PoseWithCovarianceStamped message from which we will extract the pose data + * @param[in] loss - The loss function for the 3D pose constraint generated + * @param[in] target_frame - The frame ID into which the pose data will be transformed before it is + * used + * @param[in] position_indices - The indices of the position variables to be added to the transaction + * @param[in] orientation_indices - The indices of the orientation variables to be added to the transaction + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processAbsolutePose3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const fuse_core::Loss::SharedPtr & loss, + const std::string & target_frame, + const std::vector & position_indices, + const std::vector & orientation_indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + if (position_indices.empty() && orientation_indices.empty()) { + return false; + } + + geometry_msgs::msg::PoseWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = pose; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform pose message with stamp " << rclcpp::Time( + pose.header.stamp).nanoseconds() << ". Cannot create constraint."); + return false; + } + } + // Create the pose variable + auto position = fuse_variables::Position3DStamped::make_shared(pose.header.stamp, device_id); + position->x() = transformed_message.pose.pose.position.x; + position->y() = transformed_message.pose.pose.position.y; + position->z() = transformed_message.pose.pose.position.z; + auto orientation = fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); + orientation->w() = transformed_message.pose.pose.orientation.w; + orientation->x() = transformed_message.pose.pose.orientation.x; + orientation->y() = transformed_message.pose.pose.orientation.y; + orientation->z() = transformed_message.pose.pose.orientation.z; + + if (position_indices.size() == 3 && orientation_indices.size() == 3) { + // Full pose measurement, no need to create a partial one + fuse_core::Vector7d pose_mean; + pose_mean << transformed_message.pose.pose.position.x, + transformed_message.pose.pose.position.y, + transformed_message.pose.pose.position.z, + transformed_message.pose.pose.orientation.w, + transformed_message.pose.pose.orientation.x, + transformed_message.pose.pose.orientation.y, + transformed_message.pose.pose.orientation.z; + + Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); + + if (validate) { + try { + validatePartialMeasurement(pose_mean, pose_covariance, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); + return false; + } + } + + auto constraint = fuse_constraints::AbsolutePose3DStampedConstraint::make_shared( + source, + *position, + *orientation, + pose_mean, + pose_covariance); + + constraint->loss(loss); + + transaction.addVariable(position); + transaction.addVariable(orientation); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose.header.stamp); + + return true; + } + + // Convert the ROS message into tf2 transform + tf2::Transform tf2_pose; + tf2::fromMsg(transformed_message.pose.pose, tf2_pose); + // Fill eigen pose in RPY representation + fuse_core::Vector6d pose_mean_partial; + pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean_partial(3), pose_mean_partial(4), pose_mean_partial(5)); + + Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); + + // Set the components which are not measured to zero + const auto indices = mergeIndices(position_indices, orientation_indices, 3); + std::replace_if( + pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), + [&indices, &pose_mean_partial](const double & value) { + return std::find(indices.begin(), indices.end(), &value - pose_mean_partial.data()) == indices.end(); + }, 0.0); + fuse_core::MatrixXd pose_covariance_partial(indices.size(), indices.size()); + populatePartialMeasurement( + pose_covariance, + indices, + pose_covariance_partial); + + if (validate) { + try { + validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); + return false; + } + } + + auto constraint = fuse_constraints::AbsolutePose3DStampedEulerConstraint::make_shared( + source, + *position, + *orientation, + pose_mean_partial, + pose_covariance_partial, + indices); + + constraint->loss(loss); + + transaction.addVariable(position); + transaction.addVariable(orientation); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose.header.stamp); + + return true; +} + /** * @brief Extracts relative 2D pose data from a PoseWithCovarianceStamped and adds that data to a * fuse Transaction @@ -750,6 +968,274 @@ inline bool processDifferentialPoseWithCovariance( return true; } +/** + * @brief Extracts relative 3D pose data from a PoseWithCovarianceStamped and adds that data to a + * fuse Transaction + * + * This method computes the delta between two poses and creates the required fuse variables and + * constraints, and then adds them to the given \p transaction. The pose delta is calculated as + * + * pose_relative = pose_absolute1^-1 * pose_absolute2 + * + * Additionally, the covariance of each pose message is rotated into the robot's base frame at the + * time of pose_absolute1. They are then added in the constraint if the pose measurements are + * independent. Otherwise, if the pose measurements are dependent, the covariance of pose_absolute1 + * is substracted from the covariance of pose_absolute2. A small minimum relative covariance is + * added to avoid getting a zero or ill-conditioned covariance. This could happen if both covariance + * matrices are the same or very similar, e.g. when pose_absolute1 == pose_absolute2, it's possible + * that the covariance is the same for both poses. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] pose1 - The first (and temporally earlier) PoseWithCovarianceStamped message + * @param[in] pose2 - The second (and temporally later) PoseWithCovarianceStamped message + * @param[in] independent - Whether the pose measurements are independent or not + * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always + * added to the resulting pose relative covariance + * @param[in] loss - The loss function for the 3D pose constraint generated + * @param[in] position_indices - The indices of the position variables to be added to the transaction + * @param[in] orientation_indices - The indices of the orientation variables to be added to the transaction + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processDifferentialPose3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, + const bool independent, + const fuse_core::Matrix6d & minimum_pose_relative_covariance, + const fuse_core::Loss::SharedPtr & loss, + const std::vector & position_indices, + const std::vector & orientation_indices, + const bool validate, + fuse_core::Transaction & transaction) +{ + // Create the pose variables + auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); + auto orientation1 = + fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); + position1->x() = pose1.pose.pose.position.x; + position1->y() = pose1.pose.pose.position.y; + position1->z() = pose1.pose.pose.position.z; + orientation1->x() = pose1.pose.pose.orientation.x; + orientation1->y() = pose1.pose.pose.orientation.y; + orientation1->z() = pose1.pose.pose.orientation.z; + orientation1->w() = pose1.pose.pose.orientation.w; + + auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); + auto orientation2 = + fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); + position2->x() = pose2.pose.pose.position.x; + position2->y() = pose2.pose.pose.position.y; + position2->z() = pose2.pose.pose.position.z; + orientation2->x() = pose2.pose.pose.orientation.x; + orientation2->y() = pose2.pose.pose.orientation.y; + orientation2->z() = pose2.pose.pose.orientation.z; + orientation2->w() = pose2.pose.pose.orientation.w; + + // Here we are using covariance_geometry types to compute the relative pose covariance: + // PoseQuaternionCovarianceRPY is std::pair, Covariance> + // Position is Eigen::Vector3d + // Quaternion is Eigen::Quaterniond + // Covariance is Eigen::Matrix6d + + // N.B. covariance_geometry implements functions for pose composition and covariance propagation + // which are based on "A tutorial on SE(3) transformation parameterizations and on-manifold optimization" + // by José Luis Blanco Claraco (https://arxiv.org/abs/2103.15980) + + // Convert from ROS msg to covariance geometry types + covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; + covariance_geometry::fromROS(pose1.pose, p1); + covariance_geometry::fromROS(pose2.pose, p2); + + // Create the delta for the constraint + if (independent) { + covariance_geometry::ComposePoseQuaternionCovarianceRPY( + covariance_geometry::InversePose3DQuaternionCovarianceRPY(p1), + p2, + p12 + ); + } else { + // If covariances of p1 and p2 are the same, then it's possible that p12 covariance will be + // zero or ill-conditioned. To avoid this, we skip the expensive following calculations and + // instead we just add a minimum covariance later + if (p1.second.isApprox(p2.second, 1e-9)) { + covariance_geometry::ComposePose3DQuaternion( + covariance_geometry::InversePose(p1.first), + p2.first, + p12.first + ); + p12.second.setZero(); + } else { + // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. + // We know cov1 and cov2 and we should substract the first to the second in order + // to obtain the relative pose covariance. But first the 2 of them have to be in the + // same reference frame, moreover we need to rotate the result in p12 reference frame + // The covariance propagation of p2 = p1 * p12 is: + // + // C2 = J_p1 * C1 * J_p1^T + J_p12 * C12 * J_p12^T + // + // where C1, C2, C12 are the covariance matrices of p1, p2 and dp, respectively, and J_p1 and + // J_p12 are the jacobians of the equation (pose composition) wrt p1 and p12, respectively. + // + // Therefore, the covariance C12 of the relative pose p12 is: + // + // C12 = J_p12^-1 * (C2 - J_p1 * C1 * J_p1^T) * J_p12^-T + + // First we need to convert covariances from RPY (6x6) to quaternion (7x7) + covariance_geometry::PoseQuaternionCovariance p1_q, p2_q, p12_q; + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( + p1, + p1_q + ); + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( + p2, + p2_q + ); + // Then we need to compute the delta pose + covariance_geometry::ComposePose3DQuaternion( + covariance_geometry::InversePose(p1_q.first), + p2_q.first, + p12_q.first + ); + // Now we have to compute pose composition jacobians so we can rotate covariances + Eigen::Matrix7d j_p1, j_p12, j_p12_inv; + Eigen::Matrix4d j_qn; + + covariance_geometry::jacobianQuaternionNormalization(p12_q.first.second, j_qn); + + covariance_geometry::JacobianPosePoseCompositionA(p1_q.first, p12_q.first, j_p1); + j_p1.block<4, 4>(3, 3).applyOnTheLeft(j_qn); + j_p1.block<4, 3>(3, 0).setZero(); + + covariance_geometry::JacobianPosePoseCompositionB(p1_q.first, j_p12); + j_p12.block<4, 4>(3, 3).applyOnTheLeft(j_qn); + j_p12.block<3, 4>(0, 3).setZero(); + j_p12.block<4, 3>(3, 0).setZero(); + + // TODO(giafranchini): check if faster to use j12.llt().solve() instead + j_p12_inv = j_p12.colPivHouseholderQr().solve(Eigen::Matrix7d::Identity()); + p12_q.second = j_p12_inv * (p2_q.second - j_p1 * p1_q.second * j_p1.transpose()) * j_p12_inv.transpose(); + + // Now again convert the delta pose covariance back to RPY(6x6) + covariance_geometry::Pose3DQuaternionCovarianceTo3DQuaternionCovarianceRPY( + p12_q, + p12 + ); + } + } + + if (position_indices.size() == 3 && orientation_indices.size() == 3) { + // Full pose measurement, no need to create a partial one + fuse_core::Vector7d pose_relative_mean; + pose_relative_mean << + p12.first.first.x(), p12.first.first.y(), p12.first.first.z(), + p12.first.second.w(), p12.first.second.x(), p12.first.second.y(), + p12.first.second.z(); + fuse_core::Matrix6d pose_relative_covariance = p12.second + minimum_pose_relative_covariance; + + if (validate) { + try { + validateMeasurement( + pose_relative_mean, pose_relative_covariance, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean, + pose_relative_covariance); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; + } + + // Convert the poses into RPY representation + fuse_core::Vector6d pose_relative_mean_partial; + + tf2::Quaternion q12(p12.first.second.x(), p12.first.second.y(), p12.first.second.z(), p12.first.second.w()); + pose_relative_mean_partial.head<3>() << p12.first.first.x(), p12.first.first.y(), p12.first.first.z(); + tf2::Matrix3x3(q12).getRPY(pose_relative_mean_partial(3), pose_relative_mean_partial(4), pose_relative_mean_partial(5)); + + fuse_core::Matrix6d pose_relative_covariance = p12.second + minimum_pose_relative_covariance; + + const auto indices = mergeIndices(position_indices, orientation_indices, 3); + + fuse_core::MatrixXd pose_relative_covariance_partial(indices.size(), indices.size()); + + // Set the components which are not measured to zero + std::replace_if( + pose_relative_mean_partial.data(), pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), + [&indices, &pose_relative_mean_partial](const double & value) { + return std::find( + indices.begin(), + indices.end(), + &value - pose_relative_mean_partial.data()) == indices.end(); + }, 0.0); + + populatePartialMeasurement( + pose_relative_covariance, + indices, + pose_relative_covariance_partial); + + if (validate) { + try { + validateMeasurement( + pose_relative_mean_partial, pose_relative_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean_partial, + pose_relative_covariance_partial, + indices); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; +} + /** * @brief Extracts relative 2D pose data from a PoseWithCovarianceStamped and adds that data to a * fuse Transaction @@ -948,37 +1434,238 @@ inline bool processDifferentialPoseWithTwistCovariance( } /** - * @brief Extracts velocity data from a TwistWithCovarianceStamped and adds that data to a fuse - * Transaction + * @brief Extracts relative 3D pose data from a PoseWithCovarianceStamped and adds that data to a + * fuse Transaction * - * This method effectively adds two variables (2D linear velocity and 2D angular velocity) and their - * respective constraints to the given \p transaction. The velocity data is extracted from the \p - * twist message. Only 2D data is used. The data will be automatically transformed into the \p - * target_frame before it is used. + * This method computes the delta between two poses and creates the required fuse variables and + * constraints, and then adds them to the given \p transaction. 3D data is used. The pose delta + * is calculated as + * + * pose_relative = pose_absolute1^-1 * pose_absolute2 + * + * Additionally, the twist covariance of the last message is used to compute the relative pose + * covariance using the time difference between the pose_absolute2 and pose_absolute1 time stamps. + * This assumes the pose measurements are dependent. A small minimum relative covariance is added to + * avoid getting a zero or ill-conditioned covariance. This could happen if the twist covariance is + * very small, e.g. when the twist is zero. * * @param[in] source - The name of the sensor or motion model that generated this constraint * @param[in] device_id - The UUID of the machine - * @param[in] twist - The TwistWithCovarianceStamped message from which we will extract the twist - * data - * @param[in] linear_velocity_loss - The loss function for the 2D linear velocity constraint - * generated - * @param[in] angular_velocity_loss - The loss function for the 2D angular velocity constraint - * generated - * @param[in] target_frame - The frame ID into which the twist data will be transformed before it is - * used - * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] pose1 - The first (and temporally earlier) PoseWithCovarianceStamped message + * @param[in] pose2 - The second (and temporally later) PoseWithCovarianceStamped message + * @param[in] twist - The second (and temporally later) TwistWithCovarianceStamped message + * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always + * added to the resulting pose relative covariance + * @param[in] twist_covariance_offset - The twist covariance offset that was added to the twist + * covariance and must be substracted from it before computing + * the pose relative covariance from it + * @param[in] loss - The loss function for the 3D pose constraint generated * @param[in] validate - Whether to validate the measurements or not. If the validation fails no * constraint is added * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processTwistWithCovariance( +inline bool processDifferentialPose3DWithTwistCovariance( const std::string & source, const fuse_core::UUID & device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const fuse_core::Loss::SharedPtr & linear_velocity_loss, - const fuse_core::Loss::SharedPtr & angular_velocity_loss, - const std::string & target_frame, + const fuse_core::Matrix6d & minimum_pose_relative_covariance, + const fuse_core::Matrix6d & twist_covariance_offset, + const fuse_core::Loss::SharedPtr & loss, + const std::vector & position_indices, + const std::vector & orientation_indices, + const bool validate, + fuse_core::Transaction & transaction) +{ + if (position_indices.empty() && orientation_indices.empty()) { + return false; + } + + // Convert the poses into tf2 transforms + tf2::Transform pose1_tf2, pose2_tf2; + tf2::fromMsg(pose1.pose.pose, pose1_tf2); + tf2::fromMsg(pose2.pose.pose, pose2_tf2); + + // Create the pose variables + auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); + position1->x() = pose1_tf2.getOrigin().x(); + position1->y() = pose1_tf2.getOrigin().y(); + position1->z() = pose1_tf2.getOrigin().z(); + auto orientation1 = + fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); + orientation1->x() = pose1_tf2.getRotation().x(); + orientation1->y() = pose1_tf2.getRotation().y(); + orientation1->z() = pose1_tf2.getRotation().z(); + orientation1->w() = pose1_tf2.getRotation().w(); + + auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); + position2->x() = pose2_tf2.getOrigin().x(); + position2->y() = pose2_tf2.getOrigin().y(); + position2->z() = pose2_tf2.getOrigin().z(); + auto orientation2 = + fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); + orientation2->x() = pose2_tf2.getRotation().x(); + orientation2->y() = pose2_tf2.getRotation().y(); + orientation2->z() = pose2_tf2.getRotation().z(); + orientation2->w() = pose2_tf2.getRotation().w(); + + // Create the delta for the constraint + const auto delta = pose1_tf2.inverseTimes(pose2_tf2); + + // Create the covariance components for the constraint + Eigen::Map cov(twist.twist.covariance.data()); + + const auto dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); + + if (dt < 1e-6) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Very small time difference " << dt << "s from '" << source << "' source."); + return false; + } + + fuse_core::Matrix6d j_twist; + j_twist.setIdentity(); + j_twist *= dt; + + fuse_core::Matrix6d pose_relative_covariance = + j_twist * (cov - twist_covariance_offset) * j_twist.transpose() + + minimum_pose_relative_covariance; + + if (position_indices.size() == 3 && orientation_indices.size() == 3) { + // Full pose measurement, no need to create a partial one + fuse_core::Vector7d pose_relative_mean; + pose_relative_mean << delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(), + delta.getRotation().w(), delta.getRotation().x(), delta.getRotation().y(), delta.getRotation().z(); + + if (validate) { + try { + validatePartialMeasurement( + pose_relative_mean, pose_relative_covariance, + 1e-4); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement using the twist covariance from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean, + pose_relative_covariance); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; + } + + // Fill eigen pose in RPY representation + fuse_core::Vector6d pose_relative_mean_partial; + pose_relative_mean_partial.head<3>() << delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(); + tf2::Matrix3x3(delta.getRotation()).getRPY( + pose_relative_mean_partial(3), pose_relative_mean_partial(4), pose_relative_mean_partial(5)); + + // Set the components which are not measured to zero + const auto indices = mergeIndices(position_indices, orientation_indices, 3); + std::replace_if( + pose_relative_mean_partial.data(), pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), + [&indices, &pose_relative_mean_partial](const double & value) { + return std::find(indices.begin(), indices.end(), &value - pose_relative_mean_partial.data()) == indices.end(); + }, 0.0); + fuse_core::MatrixXd pose_relative_covariance_partial(indices.size(), indices.size()); + populatePartialMeasurement( + pose_relative_covariance, + indices, + pose_relative_covariance_partial); + + if (validate) { + try { + validatePartialMeasurement( + pose_relative_mean_partial, pose_relative_covariance_partial, + 1e-4); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement using the twist covariance from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean_partial, + pose_relative_covariance_partial, + indices); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; +} + +/** + * @brief Extracts velocity data from a TwistWithCovarianceStamped and adds that data to a fuse + * Transaction + * + * This method effectively adds two variables (2D linear velocity and 2D angular velocity) and their + * respective constraints to the given \p transaction. The velocity data is extracted from the \p + * twist message. Only 2D data is used. The data will be automatically transformed into the \p + * target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] twist - The TwistWithCovarianceStamped message from which we will extract the twist + * data + * @param[in] linear_velocity_loss - The loss function for the 2D linear velocity constraint + * generated + * @param[in] angular_velocity_loss - The loss function for the 2D angular velocity constraint + * generated + * @param[in] target_frame - The frame ID into which the twist data will be transformed before it is + * used + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processTwistWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const fuse_core::Loss::SharedPtr & linear_velocity_loss, + const fuse_core::Loss::SharedPtr & angular_velocity_loss, + const std::string & target_frame, const std::vector & linear_indices, const std::vector & angular_indices, const tf2_ros::Buffer & tf_buffer, @@ -1114,6 +1801,191 @@ inline bool processTwistWithCovariance( return constraints_added; } +/** + * @brief Extracts velocity data from a TwistWithCovarianceStamped and adds that data to a fuse + * Transaction + * + * This method effectively adds two variables (3D linear velocity and 3D angular velocity) and their + * respective constraints to the given \p transaction. The velocity data is extracted from the \p + * twist message. The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] twist - The TwistWithCovarianceStamped message from which we will extract the twist + * data + * @param[in] linear_velocity_loss - The loss function for the 3D linear velocity constraint + * generated + * @param[in] angular_velocity_loss - The loss function for the 3D angular velocity constraint + * generated + * @param[in] target_frame - The frame ID into which the twist data will be transformed before it is + * used + * @param[in] linear_indices - The indices of the linear velocity vector to use. If empty, no + * linear velocity constraint is added + * @param[in] angular_indices - The indices of the angular velocity vector to use. If empty, no + * angular velocity constraint is added + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processTwist3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const fuse_core::Loss::SharedPtr & linear_velocity_loss, + const fuse_core::Loss::SharedPtr & angular_velocity_loss, + const std::string & target_frame, + const std::vector & linear_indices, + const std::vector & angular_indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + // Make sure we actually have work to do + if (linear_indices.empty() && angular_indices.empty()) { + return false; + } + + geometry_msgs::msg::TwistWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = twist; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform twist message with stamp " << rclcpp::Time( + twist.header.stamp).nanoseconds() << ". Cannot create constraint."); + return false; + } + } + + bool constraints_added = false; + + // Create two absolute constraints + if (!linear_indices.empty()) { + auto velocity_linear = + fuse_variables::VelocityLinear3DStamped::make_shared(twist.header.stamp, device_id); + velocity_linear->x() = transformed_message.twist.twist.linear.x; + velocity_linear->y() = transformed_message.twist.twist.linear.y; + velocity_linear->z() = transformed_message.twist.twist.linear.z; + + // Create the mean twist vectors for the constraints + fuse_core::Vector3d linear_vel_mean; + linear_vel_mean << transformed_message.twist.twist.linear.x, + transformed_message.twist.twist.linear.y, + transformed_message.twist.twist.linear.z; + + // Create the covariance for the constraint + Eigen::Map linear_vel_covariance_map( + transformed_message.twist.covariance.data()); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd linear_vel_mean_partial(linear_indices.size()); + + fuse_core::MatrixXd linear_vel_covariance_partial(linear_vel_mean_partial.rows(), + linear_vel_mean_partial.rows()); + + populatePartialMeasurement( + linear_vel_mean, + linear_vel_covariance_map.block<3, 3>(0, 0), + linear_indices, + linear_vel_mean_partial, + linear_vel_covariance_partial); + + bool add_constraint = true; + + if (validate) { + try { + validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear velocity measurement from '" + << source << "' source: " << ex.what()); + add_constraint = false; + } + } + + if (add_constraint) { + auto linear_vel_constraint = + fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( + source, *velocity_linear, linear_vel_mean_partial, linear_vel_covariance_partial, + linear_indices); + + linear_vel_constraint->loss(linear_velocity_loss); + + transaction.addVariable(velocity_linear); + transaction.addConstraint(linear_vel_constraint); + constraints_added = true; + } + } + + if (!angular_indices.empty()) { + // Create the twist variables + auto velocity_angular = + fuse_variables::VelocityAngular3DStamped::make_shared(twist.header.stamp, device_id); + velocity_angular->roll() = transformed_message.twist.twist.angular.x; + velocity_angular->pitch() = transformed_message.twist.twist.angular.y; + velocity_angular->yaw() = transformed_message.twist.twist.angular.z; + + fuse_core::Vector3d angular_vel_mean; + angular_vel_mean << transformed_message.twist.twist.angular.x, + transformed_message.twist.twist.angular.y, + transformed_message.twist.twist.angular.z; + + // Create the covariance for the constraint + Eigen::Map angular_vel_cov_map(transformed_message.twist.covariance.data()); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd angular_vel_mean_partial(angular_indices.size()); + fuse_core::MatrixXd angular_vel_covariance_partial(angular_vel_mean_partial.rows(), + angular_vel_mean_partial.rows()); + + populatePartialMeasurement( + angular_vel_mean, + angular_vel_cov_map.block<3, 3>(3, 3), + angular_indices, + angular_vel_mean_partial, + angular_vel_covariance_partial); + + bool add_constraint = true; + + if (validate) { + try { + validatePartialMeasurement(angular_vel_mean_partial, angular_vel_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Invalid partial angular velocity measurement from '" + << source << "' source: " << ex.what()); + add_constraint = false; + } + } + + if (add_constraint) { + auto angular_vel_constraint = + fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( + source, *velocity_angular, angular_vel_mean_partial, angular_vel_covariance_partial, angular_indices); + + angular_vel_constraint->loss(angular_velocity_loss); + + transaction.addVariable(velocity_angular); + transaction.addConstraint(angular_vel_constraint); + constraints_added = true; + } + } + + if (constraints_added) { + transaction.addInvolvedStamp(twist.header.stamp); + } + + return constraints_added; +} + /** * @brief Extracts linear acceleration data from an AccelWithCovarianceStamped and adds that data to * a fuse Transaction @@ -1225,6 +2097,117 @@ inline bool processAccelWithCovariance( return true; } +/** + * @brief Extracts linear acceleration data from an AccelWithCovarianceStamped and adds that data to + * a fuse Transaction + * + * This method effectively adds a linear acceleration variable and constraint to the given to the + * given \p transaction. The acceleration data is extracted from the \p acceleration message. + * The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] acceleration - The AccelWithCovarianceStamped message from which we will extract the + * acceleration data + * @param[in] loss - The loss function for the 3D linear acceleration constraint generated + * @param[in] indices - The indices of the linear acceleration vector to use. If empty, no + * linear acceleration constraint is added + * @param[in] target_frame - The frame ID into which the acceleration data will be transformed + * before it is used + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processAccel3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::AccelWithCovarianceStamped & acceleration, + const fuse_core::Loss::SharedPtr & loss, + const std::string & target_frame, + const std::vector & indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + // Make sure we actually have work to do + if (indices.empty()) { + return false; + } + + geometry_msgs::msg::AccelWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = acceleration; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Failed to transform acceleration message with stamp " << + rclcpp::Time(acceleration.header.stamp).nanoseconds() + << ". Cannot create constraint."); + return false; + } + } + + // Create the full mean vector and covariance for the constraint + fuse_core::Vector3d accel_mean; + accel_mean << + transformed_message.accel.accel.linear.x, + transformed_message.accel.accel.linear.y, + transformed_message.accel.accel.linear.z; + + Eigen::Map accel_covariance_map(transformed_message.accel.covariance.data()); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd accel_mean_partial(indices.size()); + fuse_core::MatrixXd accel_covariance_partial(accel_mean_partial.rows(), + accel_mean_partial.rows()); + + populatePartialMeasurement( + accel_mean, accel_covariance_map, indices, accel_mean_partial, + accel_covariance_partial); + + if (validate) { + try { + validatePartialMeasurement(accel_mean_partial, accel_covariance_partial, 1e-4); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear acceleration measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // Create the acceleration variables + auto acceleration_linear = + fuse_variables::AccelerationLinear3DStamped::make_shared(acceleration.header.stamp, device_id); + acceleration_linear->x() = transformed_message.accel.accel.linear.x; + acceleration_linear->y() = transformed_message.accel.accel.linear.y; + acceleration_linear->z() = transformed_message.accel.accel.linear.z; + + // Create the constraint + auto linear_accel_constraint = + fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( + source, + *acceleration_linear, + accel_mean_partial, + accel_covariance_partial, + indices); + + linear_accel_constraint->loss(loss); + + transaction.addVariable(acceleration_linear); + transaction.addConstraint(linear_accel_constraint); + transaction.addInvolvedStamp(acceleration.header.stamp); + + return true; +} + /** * @brief Scales the process noise covariance pose by the norm of the velocity * @@ -1266,6 +2249,37 @@ inline void scaleProcessNoiseCovariance( velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); } +/** + * @brief Scales the process noise covariance pose by the norm of the velocity + * + * @param[in, out] process_noise_covariance - The process noise covariance to scale. Only the pose + * components (x, y, z, roll, pitch, yaw) are scaled, and they are assumed to be in the + * top left 6x6 corner + * @param[in] velocity_linear - The linear velocity + * @param[in] velocity_angular - The angular velocity + * @param[in] velocity_linear_norm_min - The minimum linear velocity norm + * @param[in] velocity_angular_norm_min - The minimum angular velocity norm + */ +inline void scaleProcessNoiseCovariance( + fuse_core::Matrix15d & process_noise_covariance, + const fuse_core::Vector3d & velocity_linear, + const fuse_core::Vector3d & velocity_angular, + const double velocity_linear_norm_min, + const double velocity_angular_norm_min) +{ + fuse_core::Matrix6d velocity; + velocity.setIdentity(); + velocity.topLeftCorner<3, 3>().diagonal() *= + std::max( + velocity_linear_norm_min, + velocity_linear.norm()); + velocity.bottomRightCorner<3, 3>().diagonal() *= + std::max( + velocity_angular_norm_min, + velocity_angular.norm()); + process_noise_covariance.topLeftCorner<6, 6>() = + velocity * process_noise_covariance.topLeftCorner<6, 6>() * velocity.transpose(); +} } // namespace common } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/common/variable_traits.hpp b/fuse_models/include/fuse_models/common/variable_traits.hpp index b5a77de98..19eef89e8 100644 --- a/fuse_models/include/fuse_models/common/variable_traits.hpp +++ b/fuse_models/include/fuse_models/common/variable_traits.hpp @@ -35,10 +35,15 @@ #define FUSE_MODELS__COMMON__VARIABLE_TRAITS_HPP_ #include +#include #include +#include #include +#include #include +#include #include +#include namespace fuse_models @@ -71,6 +76,30 @@ struct is_linear_2d static const bool value = true; }; +template +struct is_linear_3d +{ + static const bool value = false; +}; + +template<> +struct is_linear_3d +{ + static const bool value = true; +}; + +template<> +struct is_linear_3d +{ + static const bool value = true; +}; + +template<> +struct is_linear_3d +{ + static const bool value = true; +}; + template struct is_angular_2d { @@ -89,6 +118,37 @@ struct is_angular_2d static const bool value = true; }; +template +struct is_angular_3d +{ + static const bool value = false; +}; +template<> +struct is_angular_3d +{ + static const bool value = true; +}; +template<> +struct is_angular_3d +{ + static const bool value = true; +}; + +template +struct is_orientation +{ + static const bool value = false; +}; +template<> +struct is_orientation +{ + static const bool value = true; +}; +template<> +struct is_orientation +{ + static const bool value = true; +}; } // namespace common } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp new file mode 100644 index 000000000..8dc570000 --- /dev/null +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -0,0 +1,195 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__IMU_3D_HPP_ +#define FUSE_MODELS__IMU_3D_HPP_ + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief An adapter-type sensor that produces orientation (relative or absolute), angular velocity, + * and linear acceleration constraints from IMU sensor data published by another node + * + * This sensor subscribes to a sensor_msgs::msg::Imu topic and: + * 1. Creates relative or absolute orientation and constraints. If the \p differential parameter + * is set to false (the default), the orientation measurement will be treated as an absolute + * constraint. If it is set to true, consecutive measurements will be used to generate relative + * orientation constraints. + * 2. Creates 3D velocity variables and constraints. + * + * This sensor really just separates out the orientation, angular velocity, and linear acceleration + * components of the message, and processes them just like the Pose3D, Twist3D, and Acceleration3D + * classes. + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - queue_size (int, default: 10) The subscriber queue size for the pose messages + * - topic (string) The topic to which to subscribe for the pose messages + * - differential (bool, default: false) Whether we should fuse orientation measurements + * absolutely, or to create relative orientation constraints + * using consecutive measurements. + * - remove_gravitational_acceleration (bool, default: false) Whether we should remove acceleration + * due to gravity from the acceleration + * values produced by the IMU before + * fusing + * - gravitational_acceleration (double, default: 9.80665) Acceleration due to gravity, in + * meters/sec^2. This value is only used if + * \p remove_gravitational_acceleration is + * true + * - orientation_target_frame (string) Orientation data will be transformed into this frame before + * it is fused. + * - twist_target_frame (string) Twist/velocity data will be transformed into this frame before it + * is fused. + * - acceleration_target_frame (string) Acceleration data will be transformed into this frame + * before it is fused. + * + * Subscribes: + * - \p topic (sensor_msgs::msg::Imu) IMU data at a given timestep + */ +class Imu3D : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Imu3D) + using ParameterType = parameters::Imu3DParams; + + /** + * @brief Default constructor + */ + Imu3D(); + + /** + * @brief Destructor + */ + virtual ~Imu3D() = default; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Callback for pose messages + * @param[in] msg - The IMU message to process + */ + void process(const sensor_msgs::msg::Imu & msg); + +protected: + fuse_core::UUID device_id_; //!< The UUID of this device + + /** + * @brief Perform any required initialization for the sensor model + * + * This could include things like reading from the parameter server or subscribing to topics. The + * class's node handles will be properly initialized before SensorModel::onInit() is called. + * Spinning of the callback queue will not begin until after the call to SensorModel::onInit() + * completes. + */ + void onInit() override; + + /** + * @brief Subscribe to the input topic to start sending transactions to the optimizer + */ + void onStart() override; + + /** + * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer + */ + void onStop() override; + + /** + * @brief Process a pose message in differential mode + * + * @param[in] pose - The pose message to process in differential mode + * @param[in] twist - The twist message used in case the twist covariance is used in differential + * mode + * @param[in] validate - Whether to validate the pose and twist coavriance or not + * @param[out] transaction - The generated variables and constraints are added to this transaction + */ + void processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const bool validate, + fuse_core::Transaction & transaction); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; + + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_; + + // NOTE(CH3): Unique ptr to defer till we have the node interfaces from initialize() + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr sub_; + + using ImuThrottledCallback = fuse_core::ThrottledMessageCallback; + ImuThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__IMU_3D_HPP_ diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp new file mode 100644 index 000000000..8a888f3a8 --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -0,0 +1,186 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__ODOMETRY_3D_HPP_ +#define FUSE_MODELS__ODOMETRY_3D_HPP_ + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief An adapter-type sensor that produces pose (relative or absolute) and velocity constraints + * from sensor data published by another node + * + * This sensor subscribes to a nav_msgs::msg::Odometry topic and: + * 1. Creates relative or absolute pose variables and constraints. If the \p differential parameter + * is set to false (the default), the measurement will be treated as an absolute constraint. If + * it is set to true, consecutive measurements will be used to generate relative pose + * constraints. + * 2. Creates 3D velocity variables and constraints. + * + * This sensor really just separates out the pose and twist components of the message, and processes + * them just like the Pose3D and Twist3D classes. + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - queue_size (int, default: 10) The subscriber queue size for the pose messages + * - topic (string) The topic to which to subscribe for the pose messages + * - differential (bool, default: false) Whether we should fuse measurements absolutely, or to + * create relative pose constraints using consecutive + * measurements. + * - pose_target_frame (string) Pose data will be transformed into this frame before it is fused. + * This frame should be a world-fixed frame, typically 'odom' or + * 'map'. + * - twist_target_frame (string) Twist/velocity data will be transformed into this frame before it + * is fused. This frame should be a body-relative frame, typically + * 'base_link'. + * + * Subscribes: + * - \p topic (nav_msgs::msg::Odometry) Odometry information at a given timestep + */ +class Odometry3D : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Odometry3D) + using ParameterType = parameters::Odometry3DParams; + + /** + * @brief Default constructor + */ + Odometry3D(); + + /** + * @brief Destructor + */ + virtual ~Odometry3D() = default; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Callback for pose messages + * @param[in] msg - The pose message to process + */ + void process(const nav_msgs::msg::Odometry & msg); + +protected: + fuse_core::UUID device_id_; //!< The UUID of this device + + /** + * @brief Perform any required initialization for the sensor model + * + * This could include things like reading from the parameter server or subscribing to topics. The + * class's node handles will be properly initialized before SensorModel::onInit() is called. + * Spinning of the callback queue will not begin until after the call to SensorModel::onInit() + * completes. + */ + void onInit() override; + + /** + * @brief Subscribe to the input topic to start sending transactions to the optimizer + */ + void onStart() override; + + /** + * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer + */ + void onStop() override; + + /** + * @brief Process a pose message in differential mode + * + * @param[in] pose - The pose message to process in differential mode + * @param[in] twist - The twist message used in case the twist covariance is used in differential + * mode + * @param[in] validate - Whether to validate the pose and twist coavriance or not + * @param[out] transaction - The generated variables and constraints are added to this transaction + */ + void processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const bool validate, + fuse_core::Transaction & transaction); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; + + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_; + + // NOTE(CH3): Unique ptr to defer till we have the node interfaces from initialize() + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr sub_; + + using OdometryThrottledCallback = fuse_core::ThrottledMessageCallback; + OdometryThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__ODOMETRY_3D_HPP_ diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp new file mode 100644 index 000000000..f1c70f441 --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -0,0 +1,255 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__ODOMETRY_3D_PUBLISHER_HPP_ +#define FUSE_MODELS__ODOMETRY_3D_PUBLISHER_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @class Odometry3DPublisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts + * a tf transform for optimized 3D state data (combination of Position3DStamped, + * Orientation3DStamped, VelocityLinear3DStamped, VelocityAngular3DStamped, and + * AccelerationLinear3DStamped). + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - publish_tf (bool, default: true) Whether to publish the generated pose data as a transform to + * the tf tree + * - predict_to_current_time (bool, default: false) The tf publication happens at a fixed rate. + * This parameter specifies whether we should + * predict, using the 3D omnidirectional model, the state + * at the time of the tf publication, rather than + * the last posterior (optimized) state. + * - publish_frequency (double, default: 10.0) How often, in Hz, we publish the filtered state + * data and broadcast the transform + * - tf_cache_time (double, default: 10.0) The length of our tf cache (only used if the + * world_frame_id and the map_frame_id are the same) + * - tf_timeout (double, default: 0.1) Our tf lookup timeout period (only used if the + * world_frame_id and the map_frame_id are the same) + * - queue_size (int, default: 1) The size of our ROS publication queue + * - map_frame_id (string, default: "map") Our map frame_id + * - odom_frame_id (string, default: "odom") Our odom frame_id + * - base_link_frame_id (string, default: "base_link") Our base_link (body) frame_id + * - world_frame_id (string, default: "odom") The frame_id that will be published as the parent + * frame for the output. Must be either the + * map_frame_id or the odom_frame_id. + * - topic (string, default: "odometry/filtered") The ROS topic to which we will publish the + * filtered state data + * + * Publishes: + * - odometry/filtered (nav_msgs::msg::Odometry) The most recent optimized state, gives as an + * odometry message + * - tf (via a tf2_ros::TransformBroadcaster) The most recent optimized state, as a tf transform + * + * Subscribes: + * - tf, tf_static (tf2_msgs::msg::TFMessage) Subscribes to tf data to obtain the requisite + * odom->base_link transform, but only if the + * world_frame_id is set to the value of the + * map_frame_id. + */ +class Odometry3DPublisher : public fuse_core::AsyncPublisher +{ +public: + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(Odometry3DPublisher) + using ParameterType = parameters::Odometry3DPublisherParams; + + /** + * @brief Constructor + */ + Odometry3DPublisher(); + + /** + * @brief Destructor + */ + virtual ~Odometry3DPublisher() = default; + + /** + * @brief Shadowing extension to the AsyncPublisher::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) override; + +protected: + /** + * @brief Perform any required post-construction initialization, such as advertising publishers or + * reading from the parameter server. + */ + void onInit() override; + + /** + * @brief Fires whenever an optimized graph has been computed + * + * @param[in] transaction A Transaction object, describing the set of variables that have been + * added and/or removed + * @param[in] graph A read-only pointer to the graph object, allowing queries to be + * performed whenever needed + */ + void notifyCallback( + fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; + + /** + * @brief Perform any required operations before the first call to notify() occurs + */ + void onStart() override; + + /** + * @brief Perform any required operations to stop publications + */ + void onStop() override; + + /** + * @brief Retrieves the given variable values at the requested time from the graph + * @param[in] graph The graph from which we will retrieve the state + * @param[in] stamp The time stamp at which we want the state + * @param[in] device_id The device ID for which we want the given variables + * @param[out] position_uuid The UUID of the position variable that gets extracted from the graph + * @param[out] orientation_uuid The UUID of the orientation variable that gets extracted from the + * graph + * @param[out] velocity_linear_uuid The UUID of the linear velocity variable that gets extracted + * from the graph + * @param[out] velocity_angular_uuid The UUID of the angular velocity variable that gets extracted + * from the graph + * @param[out] acceleration_linear_uuid The UUID of the linear acceleration variable that gets + * extracted from the graph + * @param[out] odometry All of the fuse pose and velocity variable values get packed into this + * structure + * @param[out] acceleration All of the fuse acceleration variable values get packed into this + * structure + * @return true if the checks pass, false otherwise + */ + bool getState( + const fuse_core::Graph & graph, + const rclcpp::Time & stamp, + const fuse_core::UUID & device_id, + fuse_core::UUID & position_uuid, + fuse_core::UUID & orientation_uuid, + fuse_core::UUID & velocity_linear_uuid, + fuse_core::UUID & velocity_angular_uuid, + fuse_core::UUID & acceleration_linear_uuid, + nav_msgs::msg::Odometry & odometry, + geometry_msgs::msg::AccelWithCovarianceStamped & acceleration); + + /** + * @brief Timer callback method for the filtered state publication and tf broadcasting + * @param[in] event The timer event parameters that are associated with the given invocation + */ + void publishTimerCallback(); + + /** + * @brief Object that searches for the most recent common timestamp for a set of variables + */ + using Synchronizer = fuse_publishers::StampedVariableSynchronizer< + fuse_variables::Orientation3DStamped, + fuse_variables::Position3DStamped, + fuse_variables::VelocityLinear3DStamped, + fuse_variables::VelocityAngular3DStamped, + fuse_variables::AccelerationLinear3DStamped>; + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Timers, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncPublisher interfaces_ + + fuse_core::UUID device_id_; //!< The UUID of this device + rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The publisher's logger + + ParameterType params_; + + rclcpp::Time latest_stamp_; + rclcpp::Time latest_covariance_stamp_; + bool latest_covariance_valid_{false}; //!< Whether the latest covariance computed is valid or not + nav_msgs::msg::Odometry odom_output_; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output_; + + //!< Object that tracks the latest common timestamp of multiple variables + Synchronizer synchronizer_; + + std::unique_ptr tf_buffer_; + + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr acceleration_pub_; + + std::shared_ptr tf_broadcaster_ = nullptr; + std::unique_ptr tf_listener_; + + fuse_core::DelayedThrottleFilter delayed_throttle_filter_{10.0}; //!< A ros::console filter to + //!< print delayed throttle + //!< messages, that can be reset + //!< on start + + rclcpp::TimerBase::SharedPtr publish_timer_; + + std::mutex mutex_; //!< A mutex to protect the access to the attributes used concurrently by the + //!< notifyCallback and publishTimerCallback methods: + //!< latest_stamp_, latest_covariance_stamp_, odom_output_ and + //!< acceleration_output_ +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__ODOMETRY_3D_PUBLISHER_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp new file mode 100644 index 000000000..f8e4ccab3 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -0,0 +1,244 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__UNICYCLE_3D_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_HPP_ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fuse_models +{ + +/** + * @brief A fuse_models 3D kinematic model that generates kinematic constraints between provided + * time stamps, and adds those constraints to the fuse graph. + * + * This class uses a unicycle kinematic model for the robot. It is equivalent to the motion model + * in the robot_localization state estimation nodes. + * + * Parameters: + * - ~device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - ~device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - ~buffer_length (double) The length of the graph state buffer and state history, in seconds + * - ~process_noise_diagonal (vector of doubles) An 15-dimensional vector containing the diagonal + * values for the process noise covariance matrix. + * Variable order is (x, y, z, roll, pitch, yaw, + * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc). + */ +class Omnidirectional3D : public fuse_core::AsyncMotionModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(Omnidirectional3D) + + /** + * @brief Default constructor + * + * All plugins are required to have a constructor that accepts no arguments + */ + Omnidirectional3D(); + + /** + * @brief Destructor + */ + ~Omnidirectional3D() = default; + + /** + * @brief Shadowing extension to the AsyncMotionModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) override; + + void print(std::ostream & stream = std::cout) const; + +protected: + /** + * @brief Structure used to maintain a history of "good" pose estimates + */ + struct StateHistoryElement + { + fuse_core::UUID position_uuid; //!< The uuid of the associated position variable + fuse_core::UUID orientation_uuid; //!< The uuid of the associated orientation variable + fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable + fuse_core::UUID vel_angular_uuid; //!< The uuid of the associated angular velocity variable + fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration + //!< variable + fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position + Eigen::Quaterniond orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); //!< Map-frame orientation (quaternion) + fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities + fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities + fuse_core::Vector3d acc_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations + + void print(std::ostream & stream = std::cout) const; + + /** + * @brief Validate the state components: pose, linear velocities, angular velocities and linear + * accelerations. + * + * This validates the state components are finite. It throws an exception if any validation + * check fails. + */ + void validate() const; + }; + using StateHistory = std::map; + + /** + * @brief Augment a transaction structure such that the provided timestamps are connected by + * motion model constraints. + * @param[in] stamps The set of timestamps that should be connected by motion model + * constraints + * @param[out] transaction The transaction object that should be augmented with motion model + * constraints + * @return True if the motion models were generated successfully, false otherwise + */ + bool applyCallback(fuse_core::Transaction & transaction) override; + + /** + * @brief Generate a single motion model segment between the specified timestamps. + * + * This function is used by the timestamp manager to generate just the new motion model segments + * required to fulfill a query. + * + * @param[in] beginning_stamp The beginning timestamp of the motion model constraints to be + * generated. \p beginning_stamp is guaranteed to be less than \p + * ending_stamp. + * @param[in] ending_stamp The ending timestamp of the motion model constraints to be + * generated. \p ending_stamp is guaranteed to be greater than \p + * beginning_stamp. + * @param[out] constraints One or more motion model constraints between the requested + * timestamps. + * @param[out] variables One or more variables at both the \p beginning_stamp and \p + * ending_stamp. The variables should include initial values for the + * optimizer. + */ + void generateMotionModel( + const rclcpp::Time & beginning_stamp, + const rclcpp::Time & ending_stamp, + std::vector & constraints, + std::vector & variables); + + /** + * @brief Callback fired in the local callback queue thread(s) whenever a new Graph is received + * from the optimizer + * @param[in] graph A read-only pointer to the graph object, allowing queries to be performed + * whenever needed. + */ + void onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) override; + + /** + * @brief Perform any required initialization for the kinematic model + */ + void onInit() override; + + /** + * @brief Reset the internal state history before starting + */ + void onStart() override; + + /** + * @brief Update all of the estimated states in the state history container using the optimized + * values from the graph + * @param[in] graph The graph object containing updated variable values + * @param[in] state_history The state history object to be updated + * @param[in] buffer_length States older than this in the history will be pruned + */ + static void updateStateHistoryEstimates( + const fuse_core::Graph & graph, + StateHistory & state_history, + const rclcpp::Duration & buffer_length); + + /** + * @brief Validate the motion model state #1, state #2 and process noise covariance + * + * This validates the motion model states and process noise covariance are valid. It throws an + * exception if any validation check fails. + * + * @param[in] state1 The first/oldest state + * @param[in] state2 The second/newest state + * @param[in] process_noise_covariance The process noise covariance, after it is scaled and + * multiplied by dt + */ + static void validateMotionModel( + const StateHistoryElement & state1, const StateHistoryElement & state2, + const fuse_core::Matrix15d & process_noise_covariance); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + rclcpp::Duration buffer_length_; //!< The length of the state history + fuse_core::UUID device_id_; //!< The UUID of the device to be published + fuse_core::TimestampManager timestamp_manager_; //!< Tracks timestamps and previously created + //!< motion model segments + fuse_core::Matrix15d process_noise_covariance_; //!< Process noise covariance matrix + bool scale_process_noise_{false}; //!< Whether to scale the process noise + //!< covariance pose by the norm of the current + //!< state twist + double velocity_linear_norm_min_{1e-3}; //!< The minimum linear velocity norm allowed when + //!< scaling the process noise covariance + double velocity_angular_norm_min_{1e-3}; //!< The minimum twist norm allowed when + //!< scaling the process noise covariance + bool disable_checks_{false}; //!< Whether to disable the validation checks for the current and + //!< predicted state, including the process noise covariance after + //!< it is scaled and multiplied by dt + StateHistory state_history_; //!< History of optimized graph pose estimates +}; + +std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & unicycle_2d); + +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp new file mode 100644 index 000000000..ecbf39028 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -0,0 +1,220 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\ + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief A fuse_models ignition sensor designed to be used in conjunction with the Omnidirectional 3D + * motion model. + * + * This class publishes a transaction that contains a prior on each state subvariable used in the + * Omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction + * with the configured initial state and covariance. + * Additionally, whenever a pose is received, either on the set_pose service or the topic, this + * ignition sensor resets the optimizer then publishes a new transaction with a prior at the + * specified pose. Priors on velocities and accelerations continue to use the values + * configured on the parameter server. + * + * Parameters: + * - ~device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - ~device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - ~initial_sigma (vector of doubles) A 15-dimensional vector containing the standard deviations + * for the initial state values. The covariance matrix is + * created placing the squared standard deviations along the + * diagonal of an 15x15 matrix. Variable order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc). + * - ~initial_state (vector of doubles) A 15-dimensional vector containing the initial values for + * the state. Variable order is (x, y, z, + * qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc). + * - ~queue_size (int, default: 10) The subscriber queue size for the pose messages + * - ~reset_service (string, default: "~/reset") The name of the reset service to call before + * sending a transaction + * - ~set_pose_deprecated_service (string, default: "set_pose_deprecated") The name of the + * set_pose_deprecated + * service + * - ~set_pose_service (string, default: "set_pose") The name of the set_pose service to advertise + * - ~topic (string, default: "set_pose") The topic name for received PoseWithCovarianceStamped + * messages + */ +class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Omnidirectional3DIgnition) + using ParameterType = parameters::Omnidirectional3DIgnitionParams; + + /** + * @brief Default constructor + * + * All plugins are required to have a constructor that accepts no arguments + */ + Omnidirectional3DIgnition(); + + /** + * @brief Destructor + */ + ~Omnidirectional3DIgnition() = default; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Subscribe to the input topic to start sending transactions to the optimizer + * + * As a very special case, we are overriding the start() method instead of providing an onStart() + * implementation. This is because the Omnidirectional3DIgnition sensor calls reset() on the optimizer, + * which in turn calls stop() and start(). If we used the AsyncSensorModel implementations of + * start() and stop(), the system would hang inside of one callback function while waiting for + * another callback to complete. + */ + void start() override; + + /** + * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer + * + * As a very special case, we are overriding the stop() method instead of providing an onStop() + * implementation. This is because the Omnidirectional3DIgnition sensor calls reset() on the optimizer, + * which in turn calls stop() and start(). If we used the AsyncSensorModel implementations of + * start() and stop(), the system would hang inside of one callback function while waiting for + * another callback to complete. + */ + void stop() override; + + /** + * @brief Triggers the publication of a new prior transaction at the supplied pose + */ + void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + + /** + * @brief Triggers the publication of a new prior transaction at the supplied pose + */ + bool setPoseServiceCallback( + rclcpp::Service::SharedPtr service, + std::shared_ptr, + const fuse_msgs::srv::SetPose::Request::SharedPtr req); + + /** + * @brief Triggers the publication of a new prior transaction at the supplied pose + */ + bool setPoseDeprecatedServiceCallback( + rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); + +protected: + /** + * @brief Perform any required initialization for the kinematic ignition sensor + */ + void onInit() override; + + /** + * @brief Process a received pose from one of the ROS comm channels + * + * This method validates the input pose, resets the optimizer, then constructs and sends the + * initial state constraints (by calling sendPrior()). + * + * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) + */ + void process( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + std::function post_process = nullptr); + + /** + * @brief Create and send a prior transaction based on the supplied pose + * + * The omnidirectional 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc) will use the initial state values and standard deviations configured on the + * parameter server. + * + * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) + */ + void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Graph, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Services, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + std::atomic_bool started_; //!< Flag indicating the sensor has been started + bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already + fuse_core::UUID device_id_; //!< The UUID of this device + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; //!< Object containing all of the configuration parameters + + //!< Service client used to call the "reset" service on the optimizer + rclcpp::Client::SharedPtr reset_client_; + rclcpp::Service::SharedPtr set_pose_service_; + rclcpp::Service::SharedPtr set_pose_deprecated_service_; + + rclcpp::Subscription::SharedPtr sub_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_IGNITION_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp new file mode 100644 index 000000000..1a9a3501d --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -0,0 +1,647 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ + +#include + +#include +#include + +namespace fuse_models +{ +/** + * @brief Given a state and time delta, predicts a new state + * @param[in] position1_x - First X position + * @param[in] position1_y - First Y position + * @param[in] position1_z - First Z position + * @param[in] orientation1_r - First roll orientation + * @param[in] orientation1_p - First pitch orientation + * @param[in] orientation1_y - First yaw orientation + * @param[in] vel_linear1_x - First X velocity + * @param[in] vel_linear1_y - First Y velocity + * @param[in] vel_linear1_z - First Z velocity + * @param[in] vel_angular1_r - First roll velocity + * @param[in] vel_angular1_p - First pitch velocity + * @param[in] vel_angular1_y - First yaw velocity + * @param[in] acc_linear1_x - First X acceleration + * @param[in] acc_linear1_y - First Y acceleration + * @param[in] acc_linear1_z - First Z acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2_x - Second X position + * @param[out] position2_y - Second Y position + * @param[out] position2_z - Second Z position + * @param[out] orientation2_r - Second roll orientation + * @param[out] orientation2_p - Second pitch orientation + * @param[out] orientation2_y - Second yaw orientation + * @param[out] vel_linear2_x - Second X velocity + * @param[out] vel_linear2_y - Second Y velocity + * @param[out] vel_linear2_z - Second Z velocity + * @param[out] vel_angular_r - Second roll velocity + * @param[out] vel_angular_p - Second pitch velocity + * @param[out] vel_angular_y - Second yaw velocity + * @param[out] acc_linear2_x - Second X acceleration + * @param[out] acc_linear2_y - Second Y acceleration + * @param[out] acc_linear2_z - Second Z acceleration + */ +template +inline void predict( + const T position1_x, + const T position1_y, + const T position1_z, + const T orientation1_r, + const T orientation1_p, + const T orientation1_y, + const T vel_linear1_x, + const T vel_linear1_y, + const T vel_linear1_z, + const T vel_angular1_r, + const T vel_angular1_p, + const T vel_angular1_y, + const T acc_linear1_x, + const T acc_linear1_y, + const T acc_linear1_z, + const T dt, + T & position2_x, + T & position2_y, + T & position2_z, + T & orientation2_r, + T & orientation2_p, + T & orientation2_y, + T & vel_linear2_x, + T & vel_linear2_y, + T & vel_linear2_z, + T & vel_angular2_r, + T & vel_angular2_p, + T & vel_angular2_y, + T & acc_linear2_x, + T & acc_linear2_y, + T & acc_linear2_z) +{ + // 3D material point projection model which matches the one used by r_l. + const T sr = ceres::sin(orientation1_r); + const T cr = ceres::cos(orientation1_r); + const T sp = ceres::sin(orientation1_p); + const T cp = ceres::cos(orientation1_p); + const T sy = ceres::sin(orientation1_y); + const T cy = ceres::cos(orientation1_y); + const T cpi = T(1.0) / cp; + const T dt2 = T(0.5) * dt * dt; + + // Project the state + position2_x = position1_x + + ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + (cy * sp * cr + sy * sr) * vel_linear1_z) * dt + + ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + (cy * sp * cr + sy * sr) * acc_linear1_z) * dt2; + position2_y = position1_y + + ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + (sy * sp * cr - cy * sr) * vel_linear1_z) * dt + + ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + (sy * sp * cr - cy * sr) * acc_linear1_z) * dt2; + position2_z = position1_z + + ((-sp) * vel_linear1_x + (cp * sr) * vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + + ((-sp) * acc_linear1_x + (cp * sr) * acc_linear1_y + (cp * cr) * acc_linear1_z) * dt2; + + orientation2_r = orientation1_r + + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; + orientation2_p = orientation1_p + + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; + orientation2_y = orientation1_y + + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; + + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; + vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; + vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; + + vel_angular2_r = vel_angular1_r; + vel_angular2_p = vel_angular1_p; + vel_angular2_y = vel_angular1_y; + + acc_linear2_x = acc_linear1_x; + acc_linear2_y = acc_linear1_y; + acc_linear2_z = acc_linear1_z; + + fuse_core::wrapAngle2D(orientation2_r); + fuse_core::wrapAngle2D(orientation2_p); + fuse_core::wrapAngle2D(orientation2_y); +} + +/** + * @brief Given a state and time delta, predicts a new state + * @param[in] position1_x - First X position + * @param[in] position1_y - First Y position + * @param[in] position1_z - First Z position + * @param[in] orientation1_r - First roll orientation + * @param[in] orientation1_p - First pitch orientation + * @param[in] orientation1_y - First yaw orientation + * @param[in] vel_linear1_x - First X velocity + * @param[in] vel_linear1_y - First Y velocity + * @param[in] vel_linear1_z - First Z velocity + * @param[in] vel_angular1_r - First roll velocity + * @param[in] vel_angular1_p - First pitch velocity + * @param[in] vel_angular1_y - First yaw velocity + * @param[in] acc_linear1_x - First X acceleration + * @param[in] acc_linear1_y - First Y acceleration + * @param[in] acc_linear1_z - First Z acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2_x - Second X position + * @param[out] position2_y - Second Y position + * @param[out] position2_z - Second Z position + * @param[out] orientation2_r - Second roll orientation + * @param[out] orientation2_p - Second pitch orientation + * @param[out] orientation2_y - Second yaw orientation + * @param[out] vel_linear2_x - Second X velocity + * @param[out] vel_linear2_y - Second Y velocity + * @param[out] vel_linear2_z - Second Z velocity + * @param[out] vel_angular_r - Second roll velocity + * @param[out] vel_angular_p - Second pitch velocity + * @param[out] vel_angular_y - Second yaw velocity + * @param[out] acc_linear2_x - Second X acceleration + * @param[out] acc_linear2_y - Second Y acceleration + * @param[out] acc_linear2_z - Second Z acceleration + * @param[out] jacobians - Jacobians wrt the state + */ +inline void predict( + const double position1_x, + const double position1_y, + const double position1_z, + const double orientation1_r, + const double orientation1_p, + const double orientation1_y, + const double vel_linear1_x, + const double vel_linear1_y, + const double vel_linear1_z, + const double vel_angular1_r, + const double vel_angular1_p, + const double vel_angular1_y, + const double acc_linear1_x, + const double acc_linear1_y, + const double acc_linear1_z, + const double dt, + double & position2_x, + double & position2_y, + double & position2_z, + double & orientation2_r, + double & orientation2_p, + double & orientation2_y, + double & vel_linear2_x, + double & vel_linear2_y, + double & vel_linear2_z, + double & vel_angular2_r, + double & vel_angular2_p, + double & vel_angular2_y, + double & acc_linear2_x, + double & acc_linear2_y, + double & acc_linear2_z, + double ** jacobians, + double * jacobian_quat2rpy) +{ + // 3D material point projection model which matches the one used by r_l. + const double sr = ceres::sin(orientation1_r); + const double cr = ceres::cos(orientation1_r); + const double sp = ceres::sin(orientation1_p); + const double cp = ceres::cos(orientation1_p); + const double sy = ceres::sin(orientation1_y); + const double cy = ceres::cos(orientation1_y); + const double cpi = 1.0 / cp; + const double dt2 = 0.5 * dt * dt; + + // Project the state + position2_x = position1_x + + ((cp * cy) * vel_linear1_x + (sr * sp * cy - cr * sy) * vel_linear1_y + (cr * sp * cy + sr * sy) * vel_linear1_z) * dt + + ((cp * cy) * acc_linear1_x + (sr * sp * cy - cr * sy) * acc_linear1_y + (cr * sp * cy + sr * sy) * acc_linear1_z) * dt2; + position2_y = position1_y + + ((cp * sy) * vel_linear1_x + (sr * sp * sy + cr * cy) * vel_linear1_y + (cr * sp * sy - sr * cy) * vel_linear1_z) * dt + + ((cp * sy) * acc_linear1_x + (sr * sp * sy + cr * cy) * acc_linear1_y + (cr * sp * sy - sr * cy) * acc_linear1_z) * dt2; + position2_z = position1_z + + ((-sp) * vel_linear1_x + (sr * cp) * vel_linear1_y + (cr * cp) * vel_linear1_z) * dt + + ((-sp) * acc_linear1_x + (sr * cp) * acc_linear1_y + (cr * cp) * acc_linear1_z) * dt2; + + orientation2_r = orientation1_r + + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; + orientation2_p = orientation1_p + + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; + orientation2_y = orientation1_y + + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; + + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; + vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; + vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; + + vel_angular2_r = vel_angular1_r; + vel_angular2_p = vel_angular1_p; + vel_angular2_y = vel_angular1_y; + + acc_linear2_x = acc_linear1_x; + acc_linear2_y = acc_linear1_y; + acc_linear2_z = acc_linear1_z; + + fuse_core::wrapAngle2D(orientation2_r); + fuse_core::wrapAngle2D(orientation2_p); + fuse_core::wrapAngle2D(orientation2_y); + + if (jacobians) { + // Jacobian wrt position1 + if (jacobians[0]) { + Eigen::Map> jacobian(jacobians[0]); + jacobian.setZero(); + // partial derivatives of position2 wrt orientation1 + jacobian(0, 0) = 1.0; + jacobian(1, 1) = 1.0; + jacobian(2, 2) = 1.0; + } + // Jacobian wrt orientation1 + if (jacobians[1]) { + Eigen::Map> jacobian(jacobians[1]); + Eigen::Map> j_quat2rpy_map(jacobian_quat2rpy); + fuse_core::Matrix j_tmp; + jacobian.setZero(); + j_tmp.setZero(); + + // partial derivatives of position2_x wrt orientation1 + j_tmp(0, 0) = + ((cr * sp * cy + sr * sy) * vel_linear1_y + (- sr * sp * cy + cr * sy) * vel_linear1_z) * dt + + ((cr * sp * cy + sr * sy) * acc_linear1_y + (- sr * sp * cy + cr * sy) * acc_linear1_z) * dt2; + j_tmp(0, 1) = + ((-sp * cy) * vel_linear1_x + (sr * cp * cy) * vel_linear1_y + (cr * cp * cy) * vel_linear1_z) * dt + + ((-sp * cy) * acc_linear1_x + (sr * cp * cy) * acc_linear1_y + (cr * cp * cy) * acc_linear1_z) * dt2; + j_tmp(0, 2) = + ((-cp * sy) * vel_linear1_x + (- sr * sp * sy - cr * cy) * vel_linear1_y + (- cr * sp * sy + sr * cy) * vel_linear1_z) * dt + + ((-cp * sy) * acc_linear1_x + (- sr * sp * sy - cr * cy) * acc_linear1_y + (- cr * sp * sy + sr * cy) * acc_linear1_z) * dt2; + + // partial derivatives of position2_y wrt orientation1 + j_tmp(1, 0) = + ((- sr * cy + cr * sp * sy) * vel_linear1_y + (- cr * cy - sr * sp * sy) * vel_linear1_z) * dt + + ((- sr * cy + cr * sp * sy) * acc_linear1_y + (- cr * cy - sr * sp * sy) * acc_linear1_z) * dt2; + j_tmp(1, 1) = + ((-sp * sy) * vel_linear1_x + (sr * cp * sy) * vel_linear1_y + (cr * cp * sy) * vel_linear1_z) * dt + + ((-sp * sy) * acc_linear1_x + (sr * cp * sy) * acc_linear1_y + (cr * cp * sy) * acc_linear1_z) * dt2; + j_tmp(1, 2) = + ((cp * cy) * vel_linear1_x + (- cr * sy + sr * sp * cy) * vel_linear1_y + (sr * sy + cr * sp * cy) * vel_linear1_z) * dt + + ((cp * cy) * acc_linear1_x + (- cr * sy + sr * sp * cy) * acc_linear1_y + (sr * sy + cr * sp * cy) * acc_linear1_z) * dt2; + + // partial derivatives of position2_z wrt orientation1 + j_tmp(2, 0) = + ((cr * cp) * vel_linear1_y - (sr * cp) * vel_linear1_z) * dt + + ((cr * cp) * acc_linear1_y - (sr * cp) * acc_linear1_z) * dt2; + j_tmp(2, 1) = + (-cp * vel_linear1_x - (sr * sp) * vel_linear1_y - (cr * sp) * vel_linear1_z) * dt + + (-cp * acc_linear1_x - (sr * sp) * acc_linear1_y - (cr * sp) * acc_linear1_z) * dt2; + + // partial derivatives of orientation2_r wrt orientation1 + j_tmp(3, 0) = 1.0 + + ((cr * sp * cpi) * vel_angular1_p - (sr * sp * cpi) * vel_angular1_y) * dt; + j_tmp(3, 1) = + ((sr + sr * sp * sp * cpi * cpi) * vel_angular1_p + (cr + cr *sp * sp * cpi * cpi) * vel_angular1_y) * dt; + + // partial derivatives of orientation2_p wrt orientation1 + j_tmp(4, 0) = (-sr * vel_angular1_p - cr * vel_angular1_y) * dt; + j_tmp(4, 1) = 1.0; + + // partial derivatives of orientation2_y wrt orientation1 + j_tmp(5, 0) = ((cr * cpi) * vel_angular1_p - (sr * cpi) * vel_angular1_y) * dt; + j_tmp(5, 1) = ((sp * sr * cpi * cpi) * vel_angular1_p + (cr * sp * cpi * cpi) * vel_angular1_y) * dt; + j_tmp(5, 2) = 1.0; + + jacobian.block<6, 4>(0, 0) = j_tmp * j_quat2rpy_map; + } + // Jacobian wrt vel_linear1 + if (jacobians[2]) { + Eigen::Map> jacobian(jacobians[2]); + jacobian.setZero(); + + // partial derivatives of position1_x wrt vel_linear1 + jacobian(0, 0) = cp * cy * dt; + jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt; + // partial derivatives of position1_y wrt vel_linear1 + jacobian(1, 0) = cp * sy * dt; + jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt; + jacobian(1, 2) = (cr * sp * sy - sr * cy) * dt; + // partial derivatives of position1_z wrt vel_linear1 + jacobian(2, 0) = -sp * dt; + jacobian(2, 1) = sr * cp * dt; + jacobian(2, 2) = cr * cp * dt; + // partial derivatives of vel_linear2_x wrt vel_linear1 + jacobian(6, 0) = 1.0; + // partial derivatives of vel_linear2_y wrt vel_linear1 + jacobian(7, 1) = 1.0; + // partial derivatives of vel_linear2_z wrt vel_linear1 + jacobian(8, 2) = 1.0; + } + + // Jacobian wrt vel_angular1 + if (jacobians[3]) { + Eigen::Map> jacobian(jacobians[3]); + jacobian.setZero(); + + // partial derivatives of orientation2_r wrt vel_angular1 + jacobian(3, 0) = dt; + jacobian(3, 1) = sr * sp * cpi * dt; + jacobian(3, 2) = cr * sp * cpi * dt; + // partial derivatives of orientation2_p wrt vel_angular1 + jacobian(4, 1) = cr * dt; + jacobian(4, 2) = -sr * dt; + // partial derivatives of orientation2_y wrt vel_angular1 + jacobian(5, 1) = sr * cpi * dt; + jacobian(5, 2) = cr * cpi * dt; + // partial derivatives of vel_angular2_r wrt vel_angular1 + jacobian(9, 0) = 1.0; + // partial derivatives of vel_angular2_p wrt vel_angular1 + jacobian(10, 1) = 1.0; + // partial derivatives of vel_angular2_y wrt vel_angular1 + jacobian(11, 2) = 1.0; + } + + // Jacobian wrt acc_linear1 + if (jacobians[4]) { + Eigen::Map> jacobian(jacobians[4]); + jacobian.setZero(); + // partial derivatives of position1_x wrt acc_linear1 + jacobian(0, 0) = cp * cy * dt2; + jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt2; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt2; + // partial derivatives of position1_y wrt acc_linear1 + jacobian(1, 0) = cp * sy * dt2; + jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt2; + jacobian(1, 2) = (cr * sp * sy - sr * cy) * dt2; + // partial derivatives of position1_z wrt acc_linear1 + jacobian(2, 0) = -sp * dt2; + jacobian(2, 1) = sr * cp * dt2; + jacobian(2, 2) = cr * cp * dt2; + // partial derivatives of vel_linear2_x wrt acc_linear1 + jacobian(6, 0) = dt; + // partial derivatives of vel_linear2_y wrt acc_linear1 + jacobian(7, 1) = dt; + // partial derivatives of vel_linear2_z wrt acc_linear1 + jacobian(8, 2) = dt; + // partial derivatives of acc_linear2_x wrt acc_linear1 + jacobian(12, 0) = 1.0; + // partial derivatives of acc_linear2_y wrt acc_linear1 + jacobian(13, 1) = 1.0; + // partial derivatives of acc_linear2_z wrt acc_linear1 + jacobian(14, 2) = 1.0; + } + } +} + +/** + * @brief Given a state and time delta, predicts a new state + * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) + * @param[in] orientation1 - First orientation (array with roll at index 0, pitch at index 1, yaw at index 2) + * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) + * @param[in] vel_angular1 - First angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, z at index 2) + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2 - Second position (array with x at index 0, y at index 1, z at index 2) + * @param[out] orientation2 - Second orientation (array with roll at index 0, pitch at index 1, yaw at index 2) + * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1, z at index 2) + * @param[out] vel_angular2 - Second yaw velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) + */ +template +inline void predict( + const T * const position1, + const T * const orientation1, + const T * const vel_linear1, + const T * const vel_angular1, + const T * const acc_linear1, + const T dt, + T * const position2, + T * const orientation2, + T * const vel_linear2, + T * const vel_angular2, + T * const acc_linear2) +{ + predict( + position1[0], + position1[1], + position1[2], + orientation1[0], + orientation1[1], + orientation1[2], + vel_linear1[0], + vel_linear1[1], + vel_linear1[2], + vel_angular1[0], + vel_angular1[1], + vel_angular1[2], + acc_linear1[0], + acc_linear1[1], + acc_linear1[2], + dt, + position2[0], + position2[1], + position2[2], + orientation2[0], + orientation2[1], + orientation2[2], + vel_linear2[0], + vel_linear2[1], + vel_linear2[2], + vel_angular2[0], + vel_angular2[1], + vel_angular2[2], + acc_linear2[0], + acc_linear2[1], + acc_linear2[2]); +} +/** + * @brief Given a state and time delta, predicts a new state + * @param[in] position1 - First position + * @param[in] orientation1 - First orientation (quaternion) + * @param[in] vel_linear1 - First linear velocity + * @param[in] vel_angular1 - First angular velocity + * @param[in] acc_linear1 - First linear acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2 - Second position + * @param[out] orientation2 - Second orientation (quaternion) + * @param[out] vel_linear2 - Second linear velocity + * @param[out] vel_angular2 - Second angular velocity + * @param[out] acc_linear2 - Second linear acceleration + */ +inline void predict( + const fuse_core::Vector3d & position1, + const Eigen::Quaterniond & orientation1, + const fuse_core::Vector3d & vel_linear1, + const fuse_core::Vector3d & vel_angular1, + const fuse_core::Vector3d & acc_linear1, + const double dt, + fuse_core::Vector3d & position2, + Eigen::Quaterniond & orientation2, + fuse_core::Vector3d & vel_linear2, + fuse_core::Vector3d & vel_angular2, + fuse_core::Vector3d & acc_linear2) +{ + fuse_core::Vector3d rpy( + fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) + ); + + predict( + position1.x(), + position1.y(), + position1.z(), + rpy.x(), + rpy.y(), + rpy.z(), + vel_linear1.x(), + vel_linear1.y(), + vel_linear1.z(), + vel_angular1.x(), + vel_angular1.y(), + vel_angular1.z(), + acc_linear1.x(), + acc_linear1.y(), + acc_linear1.z(), + dt, + position2.x(), + position2.y(), + position2.z(), + rpy.x(), + rpy.y(), + rpy.z(), + vel_linear2.x(), + vel_linear2.y(), + vel_linear2.z(), + vel_angular2.x(), + vel_angular2.y(), + vel_angular2.z(), + acc_linear2.x(), + acc_linear2.y(), + acc_linear2.z()); + + // Convert back to quaternion + orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); +} + +/** + * @brief Given a state and time delta, predicts a new state + * @param[in] position1 - First position + * @param[in] orientation1 - First orientation (quaternion) + * @param[in] vel_linear1 - First linear velocity + * @param[in] vel_angular1 - First angular velocity + * @param[in] acc_linear1 - First linear acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2 - Second position + * @param[out] orientation2 - Second orientation (quaternion) + * @param[out] vel_linear2 - Second linear velocity + * @param[out] vel_angular2 - Second angular velocity + * @param[out] acc_linear2 - Second linear acceleration + * @param[out] jacobian - Jacobian wrt the state + */ +inline void predict( + const fuse_core::Vector3d & position1, + const Eigen::Quaterniond & orientation1, + const fuse_core::Vector3d & vel_linear1, + const fuse_core::Vector3d & vel_angular1, + const fuse_core::Vector3d & acc_linear1, + const double dt, + fuse_core::Vector3d & position2, + Eigen::Quaterniond & orientation2, + fuse_core::Vector3d & vel_linear2, + fuse_core::Vector3d & vel_angular2, + fuse_core::Vector3d & acc_linear2, + fuse_core::Matrix15d & jacobian) +{ + + double quat[4] = {orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()}; + double rpy[3]; + double jacobian_quat2rpy[12]; + fuse_core::quaternion2rpy(quat, rpy, jacobian_quat2rpy); + + // fuse_core::Matrix15d is Eigen::RowMajor, so we cannot use pointers to the columns where each + // parameter block starts. Instead, we need to create a vector of Eigen::RowMajor matrices per + // parameter block and later reconstruct the fuse_core::Matrix15d with the full jacobian. The + // parameter blocks have the following sizes: {position1: 3, orientation1: 4, vel_linear1: 3, + // vel_angular1: 3, acc_linear1: 3} + + static constexpr size_t num_residuals{15}; + static constexpr size_t num_parameter_blocks{5}; + static const std::array block_sizes = {3, 4, 3, 3, 3}; + + std::array J; + std::array jacobians; + + for (size_t i = 0; i < num_parameter_blocks; ++i) { + J[i].resize(num_residuals, block_sizes[i]); + jacobians[i] = J[i].data(); + } + + predict( + position1.x(), + position1.y(), + position1.z(), + rpy[0], + rpy[1], + rpy[2], + vel_linear1.x(), + vel_linear1.y(), + vel_linear1.z(), + vel_angular1.x(), + vel_angular1.y(), + vel_angular1.z(), + acc_linear1.x(), + acc_linear1.y(), + acc_linear1.z(), + dt, + position2.x(), + position2.y(), + position2.z(), + rpy[0], + rpy[1], + rpy[2], + vel_linear2.x(), + vel_linear2.y(), + vel_linear2.z(), + vel_angular2.x(), + vel_angular2.y(), + vel_angular2.z(), + acc_linear2.x(), + acc_linear2.y(), + acc_linear2.z(), + jacobians.data(), + jacobian_quat2rpy); + + jacobian << J[0], J[1], J[2], J[3], J[4]; + + // Convert back to quaternion + orientation2 = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()); +} + +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_PREDICT_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp new file mode 100644 index 000000000..730d48def --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp @@ -0,0 +1,292 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ + +#include + +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief Create a cost function for a 3D state vector + * + * The state vector includes the following quantities, given in this order: + * position (x, y, z) + * orientation (roll, pitch, yaw) + * linear velocity (x, y, z) + * angular velocity (roll, pitch, yaw) + * linear acceleration (x, y, z) + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the entire state vector. + * + * The cost function is of the form: + * + * || [ x_t2 - proj(x_t1) ] ||^2 + * || [ y_t2 - proj(y_t1) ] || + * || [ z_t2 - proj(z_t1) ] || + * || [ quat2eul(rpy_t2) - proj(quat2eul(rpy_t1)) ] || + * cost(x) = || A *[ x_vel_t2 - proj(x_vel_t1) ] || + * || [ y_vel_t2 - proj(y_vel_t1) ] || + * || [ z_vel_t2 - proj(z_vel_t1) ] || + * || [ roll_vel_t2 - proj(roll_vel_t1) ] || + * || [ pitch_vel_t2 - proj(pitch_vel_t1) ] || + * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || + * || [ x_acc_t2 - proj(x_acc_t1) ] || + * || [ y_acc_t2 - proj(y_acc_t1) ] || + * || [ z_acc_t2 - proj(z_acc_t1) ] || + * + * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and + * proj is a function that projects the state variables from time t1 to time t2. In case the user is + * interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3, 4, 3, 3, 3, 3, 4, 3, 3, 3> +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + + /** + * @brief Construct a cost function instance + * + * @param[in] dt The time delta across which to generate the kinematic model cost + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) + */ + Omnidirectional3DStateCostFunction(const double dt, const fuse_core::Matrix15d & A); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + * + * @param[in] parameters - Parameter blocks: + * 0 : position1 - First position (array with x at index 0, y at index 1, + * z at index 2) + * 1 : orientation1 - First orientation (array with w at index 0, x at index 1, + * y at index 2, z at index 3) + * 2 : vel_linear1 - First linear velocity (array with x at index 0, y at + * index 1, z at index 2) + * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, + * vpitch at index 1, vyaw at index 2) + * 4 : acc_linear1 - First linear acceleration (array with x at index 0, y + * at index 1, z at index 2) + * 5 : position2 - Second position (array with x at index 0, y at index 1, + * z at index 2) + * 6 : orientation2 - Second orientation (array with w at index 0, x at index 1, + * y at index 2, z at index 3) + * 7 : vel_linear2 - Second linear velocity (array with x at index 0, y at + * index 1, z at index 2) + * 8 : vel_angular2 - Second angular velocity (array with vroll at index 0, + * vpitch at index 1, vyaw at index 2) + * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at + * index 1) + * @param[out] residual - The computed residual (error) + * @param[out] jacobians - Jacobians of the residuals wrt the parameters. Only computed if not + * NULL, and only computed for the parameters where jacobians[i] is not + * NULL. + * @return The return value indicates whether the computation of the residuals and/or jacobians + * was successful or not. + */ + bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const override + { + double position_pred[3]; + double orientation_pred_rpy[3]; + double vel_linear_pred[3]; + double vel_angular_pred[3]; + double acc_linear_pred[3]; + double orientation1_rpy[3]; + double orientation2_rpy[3]; + double j1_quat2rpy[12]; + double j2_quat2rpy[12]; + fuse_core::quaternion2rpy(parameters[1], orientation1_rpy, j1_quat2rpy); + fuse_core::quaternion2rpy(parameters[6], orientation2_rpy, j2_quat2rpy); + + predict( + parameters[0][0], // position1_x + parameters[0][1], // position1_y + parameters[0][2], // position1_z + orientation1_rpy[0], // orientation1_roll + orientation1_rpy[1], // orientation1_pitch + orientation1_rpy[2], // orientation1_yaw + parameters[2][0], // vel_linear1_x + parameters[2][1], // vel_linear1_y + parameters[2][2], // vel_linear1_z + parameters[3][0], // vel_angular1_roll + parameters[3][1], // vel_angular1_pitch + parameters[3][2], // vel_angular1_yaw + parameters[4][0], // acc_linear1_x + parameters[4][1], // acc_linear1_y + parameters[4][2], // acc_linear1_z + dt_, + position_pred[0], + position_pred[1], + position_pred[2], + orientation_pred_rpy[0], + orientation_pred_rpy[1], + orientation_pred_rpy[2], + vel_linear_pred[0], + vel_linear_pred[1], + vel_linear_pred[2], + vel_angular_pred[0], + vel_angular_pred[1], + vel_angular_pred[2], + acc_linear_pred[0], + acc_linear_pred[1], + acc_linear_pred[2], + jacobians, + j1_quat2rpy); + + residuals[0] = parameters[5][0] - position_pred[0]; + residuals[1] = parameters[5][1] - position_pred[1]; + residuals[2] = parameters[5][2] - position_pred[2]; + residuals[3] = orientation2_rpy[0] - orientation_pred_rpy[0]; + residuals[4] = orientation2_rpy[1] - orientation_pred_rpy[1]; + residuals[5] = orientation2_rpy[2] - orientation_pred_rpy[2]; + residuals[6] = parameters[7][0] - vel_linear_pred[0]; + residuals[7] = parameters[7][1] - vel_linear_pred[1]; + residuals[8] = parameters[7][2] - vel_linear_pred[2]; + residuals[9] = parameters[8][0] - vel_angular_pred[0]; + residuals[10] = parameters[8][1] - vel_angular_pred[1]; + residuals[11] = parameters[8][2] - vel_angular_pred[2]; + residuals[12] = parameters[9][0] - acc_linear_pred[0]; + residuals[13] = parameters[9][1] - acc_linear_pred[1]; + residuals[14] = parameters[9][2] - acc_linear_pred[2]; + + fuse_core::wrapAngle2D(residuals[3]); + fuse_core::wrapAngle2D(residuals[4]); + fuse_core::wrapAngle2D(residuals[5]); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map residuals_map(residuals); + residuals_map.applyOnTheLeft(A_); + + if (jacobians) { + // Update jacobian wrt position1 + if (jacobians[0]) { + Eigen::Map> jacobian(jacobians[0]); + jacobian.applyOnTheLeft(-A_); + } + + // Update jacobian wrt orientation1 + if (jacobians[1]) { + Eigen::Map> jacobian(jacobians[1]); + jacobian.applyOnTheLeft(-A_); + } + + // Update jacobian wrt vel_linear1 + if (jacobians[2]) { + Eigen::Map> jacobian(jacobians[2]); + jacobian.applyOnTheLeft(-A_); + } + + // Update jacobian wrt vel_yaw1 + if (jacobians[3]) { + Eigen::Map> jacobian(jacobians[3]); + jacobian.applyOnTheLeft(-A_); + } + + // Update jacobian wrt acc_linear1 + if (jacobians[4]) { + Eigen::Map> jacobian(jacobians[4]); + jacobian.applyOnTheLeft(-A_); + } + + // Jacobian wrt position2 + if (jacobians[5]) { + Eigen::Map> jacobian(jacobians[5]); + jacobian = A_.block<15, 3>(0, 0); + } + + // Jacobian wrt orientation2 + if (jacobians[6]) { + Eigen::Map> jacobian(jacobians[6]); + Eigen::Map> j2_quat2rpy_map(j2_quat2rpy); + jacobian = A_.block<15, 3>(0, 3) * j2_quat2rpy_map; + } + + // Jacobian wrt vel_linear2 + if (jacobians[7]) { + Eigen::Map> jacobian(jacobians[7]); + jacobian = A_.block<15, 3>(0, 6); + } + + // Jacobian wrt vel_angular2 + if (jacobians[8]) { + Eigen::Map> jacobian(jacobians[8]); + jacobian = A_.block<15, 3>(0, 9); + } + + // Jacobian wrt acc_linear2 + if (jacobians[9]) { + Eigen::Map> jacobian(jacobians[9]); + jacobian = A_.block<15, 3>(0, 12); + } + } + + return true; + } + +private: + double dt_; + fuse_core::Matrix15d A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix +}; + +Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction( + const double dt, + const fuse_core::Matrix15d & A) +: dt_(dt), + A_(A) +{ +} + +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTION_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp new file mode 100644 index 000000000..d95523a01 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp @@ -0,0 +1,217 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ + +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief Create a cost function for a 3D state vector + * + * The state vector includes the following quantities, given in this order: + * position (x, y, z) + * orientation (roll, pitch, yaw) + * linear velocity (x, y, z) + * angular velocity (roll, pitch, yaw) + * linear acceleration (x, y, z) + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the entire state vector. + * + * The cost function is of the form: + * + * || [ x_t2 - proj(x_t1) ] ||^2 + * || [ y_t2 - proj(y_t1) ] || + * || [ z_t2 - proj(z_t1) ] || + * || [ quat2eul(rpy_t2) - proj(quat2eul(rpy_t1)) ] || + * cost(x) = || A *[ x_vel_t2 - proj(x_vel_t1) ] || + * || [ y_vel_t2 - proj(y_vel_t1) ] || + * || [ z_vel_t2 - proj(z_vel_t1) ] || + * || [ roll_vel_t2 - proj(roll_vel_t1) ] || + * || [ pitch_vel_t2 - proj(pitch_vel_t1) ] || + * || [ yaw_vel_t2 - proj(yaw_vel_t1) ] || + * || [ x_acc_t2 - proj(x_acc_t1) ] || + * || [ y_acc_t2 - proj(y_acc_t1) ] || + * || [ z_acc_t2 - proj(z_acc_t1) ] || + * + * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and + * proj is a function that projects the state variables from time t1 to time t2. In case the user is + * interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class Omnidirectional3DStateCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + + /** + * @brief Construct a cost function instance + * + * @param[in] dt The time delta across which to generate the kinematic model cost + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) + */ + Omnidirectional3DStateCostFunctor(const double dt, const fuse_core::Matrix15d & A); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) + * @param[in] orientation1 - First orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) + * @param[in] vel_angular1 - First angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, z at index 2) + * @param[in] dt - The time delta across which to predict the state + * @param[out] position2 - Second position (array with x at index 0, y at index 1, z at index 2) + * @param[out] orientation2 - Second orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1, z at index 2) + * @param[out] vel_angular2 - Second angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) + */ + template + bool operator()( + const T * const position1, + const T * const orientation1, + const T * const vel_linear1, + const T * const vel_angular1, + const T * const acc_linear1, + const T * const position2, + const T * const orientation2, + const T * const vel_linear2, + const T * const vel_angular2, + const T * const acc_linear2, + T * residual) const; + +private: + double dt_; + fuse_core::Matrix15d A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix +}; + +Omnidirectional3DStateCostFunctor::Omnidirectional3DStateCostFunctor( + const double dt, + const fuse_core::Matrix15d & A) +: dt_(dt), + A_(A) +{ +} + +template +bool Omnidirectional3DStateCostFunctor::operator()( + const T * const position1, + const T * const orientation1, + const T * const vel_linear1, + const T * const vel_angular1, + const T * const acc_linear1, + const T * const position2, + const T * const orientation2, + const T * const vel_linear2, + const T * const vel_angular2, + const T * const acc_linear2, + T * residual) const +{ + T position_pred[3]; + T orientation_pred[3]; + T vel_linear_pred[3]; + T vel_angular_pred[3]; + T acc_linear_pred[3]; + + // Convert orientation variables from quaternion to roll-pitch-yaw + const T orientation1_rpy[3] { + fuse_core::getRoll(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), + fuse_core::getPitch(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), + fuse_core::getYaw(orientation1[0], orientation1[1], orientation1[2], orientation1[3]) + }; + const T orientation2_rpy[3] { + fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), + fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), + fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]) + }; + + predict( + position1, + orientation1_rpy, + vel_linear1, + vel_angular1, + acc_linear1, + T(dt_), + position_pred, + orientation_pred, + vel_linear_pred, + vel_angular_pred, + acc_linear_pred); + + Eigen::Map> residuals_map(residual); + residuals_map(0) = position2[0] - position_pred[0]; + residuals_map(1) = position2[1] - position_pred[1]; + residuals_map(2) = position2[2] - position_pred[2]; + residuals_map(3) = orientation2_rpy[0] - orientation_pred[0]; + residuals_map(4) = orientation2_rpy[1] - orientation_pred[1]; + residuals_map(5) = orientation2_rpy[2] - orientation_pred[2]; + residuals_map(6) = vel_linear2[0] - vel_linear_pred[0]; + residuals_map(7) = vel_linear2[1] - vel_linear_pred[1]; + residuals_map(8) = vel_linear2[2] - vel_linear_pred[2]; + residuals_map(9) = vel_angular2[0] - vel_angular_pred[0]; + residuals_map(10) = vel_angular2[1] - vel_angular_pred[1]; + residuals_map(11) = vel_angular2[2] - vel_angular_pred[2]; + residuals_map(12) = acc_linear2[0] - acc_linear_pred[0]; + residuals_map(13) = acc_linear2[1] - acc_linear_pred[1]; + residuals_map(14) = acc_linear2[2] - acc_linear_pred[2]; + + fuse_core::wrapAngle2D(residuals_map(3)); + fuse_core::wrapAngle2D(residuals_map(4)); + fuse_core::wrapAngle2D(residuals_map(5)); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + residuals_map.applyOnTheLeft(A_.template cast()); + + return true; +} + +} // namespace fuse_models + +#endif // FUSE_MODELS__UNICYCLE_3D_STATE_COST_FUNCTOR_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp new file mode 100644 index 000000000..a61de7bab --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp @@ -0,0 +1,188 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ +#define FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief A class that represents a kinematic constraint between 3D states at two different times + * + * The fuse_models 3D state is a combination of 3D position, 3D orientation, 3D linear velocity, 3D + * angular velocity, and 3D linear acceleration. + */ +class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(Omnidirectional3DStateKinematicConstraint) + + /** + * @brief Default constructor + */ + Omnidirectional3DStateKinematicConstraint() = default; + + /** + * @brief Create a constraint using a time delta and a kinematic model cost functor + * + * The constraint is created between two states. The state is broken up into multiple fuse + * variable types. + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 Position component variable of the fist state + * @param[in] orientation1 Orientation component variable of the first state + * @param[in] velocity_linear1 Linear velocity component variable of the first state + * @param[in] velocity_angular1 Angular velocity component variable of the first state + * @param[in] acceleration_linear1 Linear acceleration component variable of the first state + * @param[in] position2 Position component variable of the second state + * @param[in] orientation2 Orientation component variable of the second state + * @param[in] velocity_linear2 Linear velocity component variable of the second state + * @param[in] velocity_angular2 Angular velocity component variable of the second state + * @param[in] acceleration_linear2 Linear acceleration component variable of the second state + * @param[in] covariance The covariance matrix used to weight the constraint. Order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) + */ + Omnidirectional3DStateKinematicConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::VelocityLinear3DStamped & velocity_linear1, + const fuse_variables::VelocityAngular3DStamped & velocity_angular1, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_variables::VelocityLinear3DStamped & velocity_linear2, + const fuse_variables::VelocityAngular3DStamped & velocity_angular2, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, + const fuse_core::Matrix15d & covariance); + + /** + * @brief Destructor + */ + virtual ~Omnidirectional3DStateKinematicConstraint() = default; + + /** + * @brief Read-only access to the time delta between the first and second state (really, between + * the position1 and position2 variables in the constructor) + */ + double dt() const {return dt_;} + + /** + * @brief Read-only access to the square root information matrix. + * + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) + */ + const fuse_core::Matrix15d & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc) + */ + fuse_core::Matrix15d covariance() const + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + + /** + * @brief Print a human-readable description of the constraint to the provided stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream & stream = std::cout) const override; + + /** + * @brief Construct an instance of this constraint's cost function + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + +protected: + double dt_; //!< The time delta for the constraint + fuse_core::Matrix15d sqrt_information_; //!< The square root information matrix + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @brief The Boost Serialize method that serializes all of the data members in to/out of the + * archive + * + * @param[in/out] archive - The archive object that holds the serialized class members + * @param[in] version - The version of the archive being read/written. Generally unused. + */ + template + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & dt_; + archive & sqrt_information_; + } +}; + +} // namespace fuse_models + +BOOST_CLASS_EXPORT_KEY(fuse_models::Omnidirectional3DStateKinematicConstraint); + +#endif // FUSE_MODELS__UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp new file mode 100644 index 000000000..1c0dda5c5 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -0,0 +1,221 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__PARAMETERS__IMU_3D_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__IMU_3D_PARAMS_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Imu3D class + */ +struct Imu3DParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + angular_velocity_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_dimensions")); + linear_acceleration_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "linear_acceleration_dimensions")); + orientation_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "orientation_dimensions")); + + differential = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "differential"), + differential); + disable_checks = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "disable_checks"), + disable_checks); + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_timeout"), tf_timeout, + false); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_period"), throttle_period, + false); + throttle_use_wall_time = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_use_wall_time"), + throttle_use_wall_time); + + remove_gravitational_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, "remove_gravitational_acceleration"), remove_gravitational_acceleration); + gravitational_acceleration = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "gravitational_acceleration"), + gravitational_acceleration); + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + if (differential) { + independent = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "independent"), + independent); + use_twist_covariance = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + } + + acceleration_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "acceleration_target_frame"), + acceleration_target_frame); + orientation_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "orientation_target_frame"), + orientation_target_frame); + twist_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "twist_target_frame"), + twist_target_frame); + + pose_loss = + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + angular_velocity_loss = + fuse_core::loadLossConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_loss")); + linear_acceleration_loss = + fuse_core::loadLossConfig( + interfaces, + fuse_core::joinParameterName(ns, "linear_acceleration_loss")); + } + + bool differential {false}; + bool disable_checks {false}; + bool independent {true}; + bool use_twist_covariance {true}; + fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance + //!< matrix + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + bool remove_gravitational_acceleration {false}; + int queue_size {10}; + rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds + bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not + double gravitational_acceleration {9.80665}; + std::string acceleration_target_frame {}; + std::string orientation_target_frame {}; + std::string topic {}; + std::string twist_target_frame {}; + std::vector angular_velocity_indices; + std::vector linear_acceleration_indices; + std::vector orientation_indices; + fuse_core::Loss::SharedPtr pose_loss; + fuse_core::Loss::SharedPtr angular_velocity_loss; + fuse_core::Loss::SharedPtr linear_acceleration_loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__IMU_3D_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp new file mode 100644 index 000000000..1e01a073c --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -0,0 +1,213 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Odometry3D class + */ +struct Odometry3DParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + position_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "position_dimensions")); + orientation_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "orientation_dimensions")); + linear_velocity_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "linear_velocity_dimensions")); + angular_velocity_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_dimensions")); + + differential = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "differential"), + differential); + disable_checks = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "disable_checks"), + disable_checks); + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_timeout"), tf_timeout, + false); + + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_period"), throttle_period, + false); + throttle_use_wall_time = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_use_wall_time"), + throttle_use_wall_time); + + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + twist_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "twist_target_frame"), + twist_target_frame); + pose_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "pose_target_frame"), + pose_target_frame); + + if (differential) { + independent = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "independent"), + independent); + use_twist_covariance = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + } + + pose_loss = + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + linear_velocity_loss = + fuse_core::loadLossConfig( + interfaces, fuse_core::joinParameterName( + ns, + "linear_velocity_loss")); + angular_velocity_loss = + fuse_core::loadLossConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_loss")); + } + + bool differential {false}; + bool disable_checks {false}; + bool independent {true}; + bool use_twist_covariance {true}; + fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance + //!< matrix + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + int queue_size {10}; + rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds + bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not + std::string topic {}; + std::string pose_target_frame {}; + std::string twist_target_frame {}; + std::vector position_indices; + std::vector orientation_indices; + std::vector linear_velocity_indices; + std::vector angular_velocity_indices; + fuse_core::Loss::SharedPtr pose_loss; + fuse_core::Loss::SharedPtr linear_velocity_loss; + fuse_core::Loss::SharedPtr angular_velocity_loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp new file mode 100644 index 000000000..64d2d1380 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -0,0 +1,249 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PUBLISHER_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PUBLISHER_PARAMS_HPP_ + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Odometry3DPublisher class + */ +struct Odometry3DPublisherParams : public ParameterBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + publish_tf = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "publish_tf"), + publish_tf); + invert_tf = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "invert_tf"), + invert_tf); + predict_to_current_time = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "predict_to_current_time"), + predict_to_current_time); + predict_with_acceleration = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "predict_with_acceleration"), + predict_with_acceleration); + publish_frequency = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "publish_frequency"), + publish_frequency); + + process_noise_covariance = fuse_core::getCovarianceDiagonalParam<15>( + interfaces, fuse_core::joinParameterName( + ns, + "process_noise_diagonal"), 0.0); + scale_process_noise = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "scale_process_noise"), + scale_process_noise); + velocity_linear_norm_min_ = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "velocity_linear_norm_min"), + velocity_linear_norm_min_); + velocity_angular_norm_min_ = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "velocity_angular_norm_min"), + velocity_angular_norm_min_); + fuse_core::getPositiveParam( + interfaces, + fuse_core::joinParameterName( + ns, + "covariance_throttle_period"), covariance_throttle_period, + false); + + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_cache_time"), tf_cache_time, + false); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_timeout"), tf_timeout, + false); + + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + + map_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "map_frame_id"), + map_frame_id); + odom_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "odom_frame_id"), + odom_frame_id); + base_link_frame_id = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "base_link_frame_id"), + base_link_frame_id); + base_link_output_frame_id = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "base_link_output_frame_id"), + base_link_output_frame_id); + world_frame_id = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "world_frame_id"), + world_frame_id); + + const bool frames_valid = + map_frame_id != odom_frame_id && + map_frame_id != base_link_frame_id && + map_frame_id != base_link_output_frame_id && + odom_frame_id != base_link_frame_id && + odom_frame_id != base_link_output_frame_id && + (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); + + if (!frames_valid) { + RCLCPP_FATAL_STREAM( + interfaces.get_node_logging_interface()->get_logger(), + "Invalid frame configuration! Please note:\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " + << "unique\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " + << "unique\n" + << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); + + assert(frames_valid); + } + + topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + acceleration_topic = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "acceleration_topic"), + acceleration_topic); + + fuse_core::loadCovarianceOptionsFromROS(interfaces, covariance_options, "covariance_options"); + } + + bool publish_tf {true}; //!< Whether to publish/broadcast the TF transform or not + bool invert_tf{false}; //!< Whether to broadcast the inverse of the TF transform or not. When + //!< the inverse is broadcasted, the transform is inverted and the + //!< header.frame_id and child_frame_id are swapped, i.e. the odometry + //!< output header.frame_id is set to the base_link_output_frame_id and + //!< the child_frame_id to the world_frame_id + bool predict_to_current_time {false}; + bool predict_with_acceleration {false}; + double publish_frequency {10.0}; + fuse_core::Matrix15d process_noise_covariance; //!< Process noise covariance matrix + bool scale_process_noise{false}; + double velocity_linear_norm_min_{1e-3}; + double velocity_angular_norm_min_{1e-3}; + rclcpp::Duration covariance_throttle_period {0, 0}; //!< The throttle period duration in seconds + //!< to compute the covariance + rclcpp::Duration tf_cache_time {10, 0}; + rclcpp::Duration tf_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + int queue_size {1}; + std::string map_frame_id {"map"}; + std::string odom_frame_id {"odom"}; + std::string base_link_frame_id {"base_link"}; + std::string base_link_output_frame_id {base_link_frame_id}; + std::string world_frame_id {odom_frame_id}; + std::string topic {"odometry/filtered"}; + std::string acceleration_topic {"acceleration/filtered"}; + ceres::Covariance::Options covariance_options; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PUBLISHER_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp new file mode 100644 index 000000000..053ac5572 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp @@ -0,0 +1,218 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Omnidirectional3DIgnition class + */ +struct Omnidirectional3DIgnitionParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + publish_on_startup = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "publish_on_startup"), + publish_on_startup); + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + reset_service = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "reset_service"), + reset_service); + set_pose_service = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "set_pose_service"), + set_pose_service); + set_pose_deprecated_service = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "set_pose_deprecated_service"), + set_pose_deprecated_service); + topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + std::vector sigma_vector; + sigma_vector = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "initial_sigma"), + sigma_vector); + if (!sigma_vector.empty()) { + if (sigma_vector.size() != 15) { + throw std::invalid_argument( + "The supplied initial_sigma parameter must be length 15, but " + "is actually length " + + std::to_string(sigma_vector.size())); + } + auto is_sigma_valid = [](const double sigma) + { + return std::isfinite(sigma) && (sigma > 0); + }; + if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { + throw std::invalid_argument( + "The supplied initial_sigma parameter must contain valid floating point values. " + "NaN, Inf, and values <= 0 are not acceptable."); + } + initial_sigma.swap(sigma_vector); + } + + std::vector state_vector; + state_vector = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "initial_state"), + state_vector); + if (!state_vector.empty()) { + if (state_vector.size() != 15) { + throw std::invalid_argument( + "The supplied initial_state parameter must be length 15, but is actually length " + + std::to_string(state_vector.size())); + } + auto is_state_valid = [](const double state) + { + return std::isfinite(state); + }; + if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { + throw std::invalid_argument( + "The supplied initial_state parameter must contain valid floating point values. " + "NaN, Inf, etc are not acceptable."); + } + initial_state.swap(state_vector); + } + + loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); + } + + + /** + * @brief Flag indicating if an initial state transaction should be sent on startup, or only in + * response to a set_pose service call or topic message. + */ + bool publish_on_startup {true}; + + /** + * @brief The size of the subscriber queue for the set_pose topic + */ + int queue_size {10}; + + /** + * @brief The name of the reset service to call before sending transactions to the optimizer + */ + std::string reset_service {"~/reset"}; + + /** + * @brief The name of the set_pose service to advertise + */ + std::string set_pose_service {"set_pose"}; + + /** + * @brief The name of the deprecated set_pose service without return codes + */ + std::string set_pose_deprecated_service {"set_pose_deprecated"}; + + /** + * @brief The topic name for received PoseWithCovarianceStamped messages + */ + std::string topic {"set_pose"}; + + /** + * @brief The uncertainty of the initial state value + * + * Standard deviations are provided as an 15-dimensional vector in the order: + * (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) + * The covariance matrix is created placing the squared standard deviations along the diagonal of + * an 15x15 matrix. + */ + std::vector initial_sigma {1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 + }; + + /** + * @brief The initial value of the 15-dimension state vector (x, y, z, roll, pitch, yaw, + * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) + */ + std::vector initial_state {0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0}; + /** + * @brief Loss function + */ + fuse_core::Loss::SharedPtr loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__UNICYCLE_3D_IGNITION_PARAMS_HPP_ diff --git a/fuse_models/package.xml b/fuse_models/package.xml index 28d6bf95c..60343f0b1 100644 --- a/fuse_models/package.xml +++ b/fuse_models/package.xml @@ -17,6 +17,7 @@ libceres-dev eigen + covariance_geometry_ros fuse_constraints fuse_core fuse_graphs @@ -35,6 +36,7 @@ tf2_geometry_msgs tf2_ros + covariance_geometry_ros fuse_constraints fuse_core fuse_graphs diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp new file mode 100644 index 000000000..9fdc11d4f --- /dev/null +++ b/fuse_models/src/imu_3d.cpp @@ -0,0 +1,320 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// Register this sensor model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Imu3D, fuse_core::SensorModel) + +namespace fuse_models +{ + +Imu3D::Imu3D() +: fuse_core::AsyncSensorModel(1), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")), + throttled_callback_(std::bind(&Imu3D::process, this, std::placeholders::_1)) +{ +} + +void Imu3D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + +void Imu3D::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(clock_); + } + + if (params_.orientation_indices.empty() && + params_.linear_acceleration_indices.empty() && + params_.angular_velocity_indices.empty()) + { + RCLCPP_WARN_STREAM( + logger_, + "No dimensions were specified. Data from topic " << params_.topic + << " will be ignored."); + } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); +} + +void Imu3D::onStart() +{ + if (!params_.orientation_indices.empty() || + !params_.linear_acceleration_indices.empty() || + !params_.angular_velocity_indices.empty()) + { + previous_pose_.reset(); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + params_.topic, + params_.queue_size, + std::bind( + &ImuThrottledCallback::callback, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); + } +} + +void Imu3D::onStop() +{ + sub_.reset(); +} + +void Imu3D::process(const sensor_msgs::msg::Imu & msg) +{ + // Create a transaction object + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(msg.header.stamp); + + // Handle the orientation data (treat it as a pose, but with only orientation indices used) + auto pose = std::make_unique(); + pose->header = msg.header; + pose->pose.pose.orientation = msg.orientation; + pose->pose.covariance[21] = msg.orientation_covariance[0]; + pose->pose.covariance[22] = msg.orientation_covariance[1]; + pose->pose.covariance[23] = msg.orientation_covariance[2]; + pose->pose.covariance[27] = msg.orientation_covariance[3]; + pose->pose.covariance[28] = msg.orientation_covariance[4]; + pose->pose.covariance[29] = msg.orientation_covariance[5]; + pose->pose.covariance[33] = msg.orientation_covariance[6]; + pose->pose.covariance[34] = msg.orientation_covariance[7]; + pose->pose.covariance[35] = msg.orientation_covariance[8]; + + geometry_msgs::msg::TwistWithCovarianceStamped twist; + twist.header = msg.header; + twist.twist.twist.angular = msg.angular_velocity; + twist.twist.covariance[21] = msg.angular_velocity_covariance[0]; + twist.twist.covariance[22] = msg.angular_velocity_covariance[1]; + twist.twist.covariance[23] = msg.angular_velocity_covariance[2]; + twist.twist.covariance[27] = msg.angular_velocity_covariance[3]; + twist.twist.covariance[28] = msg.angular_velocity_covariance[4]; + twist.twist.covariance[29] = msg.angular_velocity_covariance[5]; + twist.twist.covariance[33] = msg.angular_velocity_covariance[6]; + twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; + twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; + + const bool validate = !params_.disable_checks; + + if (params_.differential) { + processDifferential(*pose, twist, validate, *transaction); + } else { + common::processAbsolutePose3DWithCovariance( + name(), + device_id_, + *pose, + params_.pose_loss, + params_.orientation_target_frame, + {}, + params_.orientation_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + } + + // Handle the twist data (only include indices for angular velocity) + common::processTwist3DWithCovariance( + name(), + device_id_, + twist, + nullptr, + params_.angular_velocity_loss, + params_.twist_target_frame, + {}, + params_.angular_velocity_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + + // Handle the acceleration data + geometry_msgs::msg::AccelWithCovarianceStamped accel; + accel.header = msg.header; + accel.accel.accel.linear = msg.linear_acceleration; + accel.accel.covariance[0] = msg.linear_acceleration_covariance[0]; + accel.accel.covariance[1] = msg.linear_acceleration_covariance[1]; + accel.accel.covariance[2] = msg.linear_acceleration_covariance[2]; + accel.accel.covariance[6] = msg.linear_acceleration_covariance[3]; + accel.accel.covariance[7] = msg.linear_acceleration_covariance[4]; + accel.accel.covariance[8] = msg.linear_acceleration_covariance[5]; + accel.accel.covariance[12] = msg.linear_acceleration_covariance[6]; + accel.accel.covariance[13] = msg.linear_acceleration_covariance[7]; + accel.accel.covariance[14] = msg.linear_acceleration_covariance[8]; + + // Optionally remove the acceleration due to gravity + if (params_.remove_gravitational_acceleration) { + geometry_msgs::msg::Vector3 accel_gravity; + accel_gravity.z = params_.gravitational_acceleration; + geometry_msgs::msg::TransformStamped orientation_trans; + tf2::Quaternion imu_orientation; + tf2::fromMsg(msg.orientation, imu_orientation); + orientation_trans.transform.rotation = tf2::toMsg(imu_orientation.inverse()); + tf2::doTransform(accel_gravity, accel_gravity, orientation_trans); // Doesn't use the stamp + accel.accel.accel.linear.x -= accel_gravity.x; + accel.accel.accel.linear.y -= accel_gravity.y; + accel.accel.accel.linear.z -= accel_gravity.z; + } + + common::processAccel3DWithCovariance( + name(), + device_id_, + accel, + params_.linear_acceleration_loss, + params_.acceleration_target_frame, + params_.linear_acceleration_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + + // Send the transaction object to the plugin's parent + sendTransaction(transaction); +} + +void Imu3D::processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const bool validate, + fuse_core::Transaction & transaction) +{ + auto transformed_pose = std::make_unique(); + transformed_pose->header.frame_id = + params_.orientation_target_frame.empty() ? pose.header.frame_id : params_. + orientation_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " << rclcpp::Time( + pose.header.stamp).nanoseconds() + << " to orientation target frame " << + params_.orientation_target_frame); + return; + } + + if (!previous_pose_) { + previous_pose_ = std::move(transformed_pose); + return; + } + + if (params_.use_twist_covariance) { + geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; + transformed_twist.header.frame_id = + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " << rclcpp::Time( + twist.header.stamp).nanoseconds() + << " to twist target frame " << + params_.twist_target_frame); + } else { + common::processDifferentialPose3DWithTwistCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + twist, + params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, + params_.pose_loss, + {}, + params_.orientation_indices, + validate, + transaction); + } + } else { + common::processDifferentialPose3DWithCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + params_.independent, + params_.minimum_pose_relative_covariance, + params_.pose_loss, + {}, + params_.orientation_indices, + validate, + transaction); + } + + previous_pose_ = std::move(transformed_pose); +} + +} // namespace fuse_models diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp new file mode 100644 index 000000000..85cb1cc7f --- /dev/null +++ b/fuse_models/src/odometry_3d.cpp @@ -0,0 +1,257 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Register this sensor model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Odometry3D, fuse_core::SensorModel) + +namespace fuse_models +{ + +Odometry3D::Odometry3D() +: fuse_core::AsyncSensorModel(1), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")), + throttled_callback_(std::bind(&Odometry3D::process, this, std::placeholders::_1)) +{ +} + +void Odometry3D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + +void Odometry3D::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(clock_); + } + + if (params_.position_indices.empty() && params_.orientation_indices.empty() && + params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) + { + RCLCPP_WARN_STREAM( + logger_, + "No dimensions were specified. Data from topic " << params_.topic + << " will be ignored."); + } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); +} + +void Odometry3D::onStart() +{ + if (!params_.position_indices.empty() || !params_.orientation_indices.empty() || + !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) + { + previous_pose_.reset(); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + params_.topic, + params_.queue_size, + std::bind( + &OdometryThrottledCallback::callback< + const nav_msgs::msg::Odometry &>, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); + } +} + +void Odometry3D::onStop() +{ + sub_.reset(); +} + +void Odometry3D::process(const nav_msgs::msg::Odometry & msg) +{ + // Create a transaction object + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(msg.header.stamp); + + // Handle the pose data + auto pose = std::make_unique(); + pose->header = msg.header; + pose->pose = msg.pose; + + geometry_msgs::msg::TwistWithCovarianceStamped twist; + twist.header = msg.header; + twist.header.frame_id = msg.child_frame_id; + twist.twist = msg.twist; + + const bool validate = !params_.disable_checks; + + if (params_.differential) { + processDifferential(*pose, twist, validate, *transaction); + } else { + common::processAbsolutePose3DWithCovariance( + name(), + device_id_, + *pose, + params_.pose_loss, + params_.pose_target_frame, + params_.position_indices, + params_.orientation_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + } + + // Handle the twist data + common::processTwist3DWithCovariance( + name(), + device_id_, + twist, + params_.linear_velocity_loss, + params_.angular_velocity_loss, + params_.twist_target_frame, + params_.linear_velocity_indices, + params_.angular_velocity_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + + // Send the transaction object to the plugin's parent + sendTransaction(transaction); +} + +void Odometry3D::processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const bool validate, + fuse_core::Transaction & transaction) +{ + auto transformed_pose = std::make_unique(); + transformed_pose->header.frame_id = + params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time( + pose.header.stamp).nanoseconds() << " to pose target frame " << params_.pose_target_frame); + return; + } + + if (!previous_pose_) { + previous_pose_ = std::move(transformed_pose); + return; + } + + if (params_.use_twist_covariance) { + geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; + transformed_twist.header.frame_id = + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " << rclcpp::Time( + twist.header.stamp).nanoseconds() + << " to twist target frame " << + params_.twist_target_frame); + } else { + common::processDifferentialPose3DWithTwistCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + transformed_twist, + params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, + params_.pose_loss, + params_.position_indices, + params_.orientation_indices, + validate, + transaction); + } + } else { + common::processDifferentialPose3DWithCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + params_.independent, + params_.minimum_pose_relative_covariance, + params_.pose_loss, + params_.position_indices, + params_.orientation_indices, + validate, + transaction); + } + previous_pose_ = std::move(transformed_pose); +} + +} // namespace fuse_models diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp new file mode 100644 index 000000000..6fb94e5ee --- /dev/null +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -0,0 +1,618 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// #include "covariance_geometry_ros/covariance_geometry_ros.hpp" +// #include "covariance_geometry_ros/utils.hpp" + +// Register this publisher with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Odometry3DPublisher, fuse_core::Publisher) + +namespace fuse_models +{ + +Odometry3DPublisher::Odometry3DPublisher() +: fuse_core::AsyncPublisher(1), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")), + latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)), + latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) +{ +} + +void Odometry3DPublisher::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) +{ + interfaces_ = interfaces; + fuse_core::AsyncPublisher::initialize(interfaces, name); +} + +void Odometry3DPublisher::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + tf_broadcaster_ = std::make_shared(interfaces_); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { + tf_buffer_ = std::make_unique( + clock_, + params_.tf_cache_time.to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ + ); + + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); + } + + // Advertise the topics + rclcpp::PublisherOptions pub_options; + pub_options.callback_group = cb_group_; + + odom_pub_ = rclcpp::create_publisher( + interfaces_, + params_.topic, + params_.queue_size, + pub_options); + acceleration_pub_ = rclcpp::create_publisher( + interfaces_, + params_.acceleration_topic, + params_.queue_size, + pub_options); +} + +void Odometry3DPublisher::notifyCallback( + fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) +{ + // Find the most recent common timestamp + const auto latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); + if (0u == latest_stamp.nanoseconds()) { + { + std::lock_guard lock(mutex_); + latest_stamp_ = latest_stamp; + } + + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "Failed to find a matching set of state variables with device id '" + << device_id_ << "'."); + return; + } + + // Get the pose values associated with the selected timestamp + fuse_core::UUID position_uuid; + fuse_core::UUID orientation_uuid; + fuse_core::UUID velocity_linear_uuid; + fuse_core::UUID velocity_angular_uuid; + fuse_core::UUID acceleration_linear_uuid; + + nav_msgs::msg::Odometry odom_output; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; + if (!getState( + *graph, + latest_stamp, + device_id_, + position_uuid, + orientation_uuid, + velocity_linear_uuid, + velocity_angular_uuid, + acceleration_linear_uuid, + odom_output, + acceleration_output)) + { + std::lock_guard lock(mutex_); + latest_stamp_ = latest_stamp; + return; + } + + odom_output.header.frame_id = params_.world_frame_id; + odom_output.header.stamp = latest_stamp; + odom_output.child_frame_id = params_.base_link_output_frame_id; + + acceleration_output.header.frame_id = params_.base_link_output_frame_id; + acceleration_output.header.stamp = latest_stamp; + + // Don't waste CPU computing the covariance if nobody is listening + rclcpp::Time latest_covariance_stamp = latest_covariance_stamp_; + bool latest_covariance_valid = latest_covariance_valid_; + if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) { + // Throttle covariance computation + if (params_.covariance_throttle_period.nanoseconds() == 0 || + latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) + { + latest_covariance_stamp = latest_stamp; + + try { + std::vector> covariance_requests; + covariance_requests.emplace_back(position_uuid, position_uuid); + covariance_requests.emplace_back(position_uuid, orientation_uuid); + covariance_requests.emplace_back(orientation_uuid, orientation_uuid); + covariance_requests.emplace_back(velocity_linear_uuid, velocity_linear_uuid); + covariance_requests.emplace_back(velocity_linear_uuid, velocity_angular_uuid); + covariance_requests.emplace_back(velocity_angular_uuid, velocity_angular_uuid); + covariance_requests.emplace_back(acceleration_linear_uuid, acceleration_linear_uuid); + + std::vector> covariance_matrices; + graph->getCovariance(covariance_requests, covariance_matrices, params_.covariance_options, true); + + odom_output.pose.covariance[0] = covariance_matrices[0][0]; // cov(x, x) + odom_output.pose.covariance[1] = covariance_matrices[0][1]; // cov(x, y) + odom_output.pose.covariance[2] = covariance_matrices[0][2]; // cov(x, z) + odom_output.pose.covariance[3] = covariance_matrices[1][0]; // cov(x, roll) + odom_output.pose.covariance[4] = covariance_matrices[1][1]; // cov(x, pitch) + odom_output.pose.covariance[5] = covariance_matrices[1][2]; // cov(x, yaw) + + odom_output.pose.covariance[6] = covariance_matrices[0][3]; // cov(y, x) + odom_output.pose.covariance[7] = covariance_matrices[0][4]; // cov(y, y) + odom_output.pose.covariance[8] = covariance_matrices[0][5]; // cov(y, z) + odom_output.pose.covariance[9] = covariance_matrices[1][3]; // cov(y, roll) + odom_output.pose.covariance[10] = covariance_matrices[1][4]; // cov(y, pitch) + odom_output.pose.covariance[11] = covariance_matrices[1][5]; // cov(y, yaw) + + odom_output.pose.covariance[12] = covariance_matrices[0][6]; // cov(z, x) + odom_output.pose.covariance[13] = covariance_matrices[0][7]; // cov(z, y) + odom_output.pose.covariance[14] = covariance_matrices[0][8]; // cov(z, z) + odom_output.pose.covariance[15] = covariance_matrices[1][6]; // cov(z, roll) + odom_output.pose.covariance[16] = covariance_matrices[1][7]; // cov(z, pitch) + odom_output.pose.covariance[17] = covariance_matrices[1][8]; // cov(z, yaw) + + odom_output.pose.covariance[18] = covariance_matrices[1][0]; // cov(roll, x) + odom_output.pose.covariance[19] = covariance_matrices[1][3]; // cov(roll, y) + odom_output.pose.covariance[20] = covariance_matrices[1][6]; // cov(roll, z) + odom_output.pose.covariance[21] = covariance_matrices[2][0]; // cov(roll, roll) + odom_output.pose.covariance[22] = covariance_matrices[2][1]; // cov(roll, pitch) + odom_output.pose.covariance[23] = covariance_matrices[2][2]; // cov(roll, yaw) + + odom_output.pose.covariance[24] = covariance_matrices[1][1]; // cov(pitch, x) + odom_output.pose.covariance[25] = covariance_matrices[1][4]; // cov(pitch, y) + odom_output.pose.covariance[26] = covariance_matrices[1][7]; // cov(pitch, z) + odom_output.pose.covariance[27] = covariance_matrices[2][1]; // cov(pitch, roll) + odom_output.pose.covariance[28] = covariance_matrices[2][4]; // cov(pitch, pitch) + odom_output.pose.covariance[29] = covariance_matrices[2][5]; // cov(pitch, yaw) + + odom_output.pose.covariance[30] = covariance_matrices[1][2]; // cov(yaw, x) + odom_output.pose.covariance[31] = covariance_matrices[1][5]; // cov(yaw, y) + odom_output.pose.covariance[32] = covariance_matrices[1][8]; // cov(yaw, z) + odom_output.pose.covariance[33] = covariance_matrices[2][2]; // cov(yaw, roll) + odom_output.pose.covariance[34] = covariance_matrices[2][5]; // cov(yaw, pitch) + odom_output.pose.covariance[35] = covariance_matrices[2][8]; // cov(yaw, yaw) + + odom_output.twist.covariance[0] = covariance_matrices[3][0]; + odom_output.twist.covariance[1] = covariance_matrices[3][1]; + odom_output.twist.covariance[2] = covariance_matrices[3][2]; + odom_output.twist.covariance[3] = covariance_matrices[4][0]; + odom_output.twist.covariance[4] = covariance_matrices[4][1]; + odom_output.twist.covariance[5] = covariance_matrices[4][2]; + + odom_output.twist.covariance[6] = covariance_matrices[3][3]; + odom_output.twist.covariance[7] = covariance_matrices[3][4]; + odom_output.twist.covariance[8] = covariance_matrices[3][5]; + odom_output.twist.covariance[9] = covariance_matrices[4][3]; + odom_output.twist.covariance[10] = covariance_matrices[4][4]; + odom_output.twist.covariance[11] = covariance_matrices[4][5]; + + odom_output.twist.covariance[12] = covariance_matrices[3][6]; + odom_output.twist.covariance[13] = covariance_matrices[3][7]; + odom_output.twist.covariance[14] = covariance_matrices[3][8]; + odom_output.twist.covariance[15] = covariance_matrices[4][6]; + odom_output.twist.covariance[16] = covariance_matrices[4][7]; + odom_output.twist.covariance[17] = covariance_matrices[4][8]; + + odom_output.twist.covariance[18] = covariance_matrices[4][0]; + odom_output.twist.covariance[19] = covariance_matrices[4][3]; + odom_output.twist.covariance[20] = covariance_matrices[4][6]; + odom_output.twist.covariance[21] = covariance_matrices[5][0]; + odom_output.twist.covariance[22] = covariance_matrices[5][1]; + odom_output.twist.covariance[23] = covariance_matrices[5][2]; + + odom_output.twist.covariance[24] = covariance_matrices[4][1]; + odom_output.twist.covariance[25] = covariance_matrices[4][4]; + odom_output.twist.covariance[26] = covariance_matrices[4][7]; + odom_output.twist.covariance[27] = covariance_matrices[5][1]; + odom_output.twist.covariance[28] = covariance_matrices[5][4]; + odom_output.twist.covariance[29] = covariance_matrices[5][5]; + + odom_output.twist.covariance[30] = covariance_matrices[4][2]; + odom_output.twist.covariance[31] = covariance_matrices[4][5]; + odom_output.twist.covariance[32] = covariance_matrices[4][8]; + odom_output.twist.covariance[33] = covariance_matrices[5][2]; + odom_output.twist.covariance[34] = covariance_matrices[5][5]; + odom_output.twist.covariance[35] = covariance_matrices[5][8]; + + acceleration_output.accel.covariance[0] = covariance_matrices[6][0]; + acceleration_output.accel.covariance[1] = covariance_matrices[6][1]; + acceleration_output.accel.covariance[2] = covariance_matrices[6][2]; + acceleration_output.accel.covariance[6] = covariance_matrices[6][3]; + acceleration_output.accel.covariance[7] = covariance_matrices[6][4]; + acceleration_output.accel.covariance[8] = covariance_matrices[6][5]; + acceleration_output.accel.covariance[12] = covariance_matrices[6][6]; + acceleration_output.accel.covariance[13] = covariance_matrices[6][7]; + acceleration_output.accel.covariance[14] = covariance_matrices[6][8]; + + // test if covariances are symmetric + Eigen::Map odom_cov_map(odom_output.pose.covariance.data()); + if (!odom_cov_map.isApprox(odom_cov_map.transpose())) { + throw std::runtime_error("Odometry covariance matrix is not symmetric"); + } + + Eigen::Map twist_cov_map(odom_output.twist.covariance.data()); + if (!twist_cov_map.isApprox(twist_cov_map.transpose())) { + throw std::runtime_error("Twist covariance matrix is not symmetric"); + } + + Eigen::Map accel_cov_map(acceleration_output.accel.covariance.data()); + if (!accel_cov_map.isApprox(accel_cov_map.transpose())) { + throw std::runtime_error("Acceleration covariance matrix is not symmetric"); + } + + latest_covariance_valid = true; + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM( + logger_, + "An error occurred computing the covariance information for " + << latest_stamp.nanoseconds() + << ". The covariance will be set to zero.\n" + << e.what()); + std::fill(odom_output.pose.covariance.begin(), odom_output.pose.covariance.end(), 0.0); + std::fill(odom_output.twist.covariance.begin(), odom_output.twist.covariance.end(), 0.0); + std::fill( + acceleration_output.accel.covariance.begin(), + acceleration_output.accel.covariance.end(), 0.0); + + latest_covariance_valid = false; + } + } else { + // This covariance computation cycle has been skipped, so simply take the last covariance + // computed + // + // We do not propagate the latest covariance forward because it would grow unbounded being + // very different from the actual covariance we would have computed if not throttling. + odom_output.pose.covariance = odom_output_.pose.covariance; + odom_output.twist.covariance = odom_output_.twist.covariance; + acceleration_output.accel.covariance = acceleration_output_.accel.covariance; + } + } + + { + std::lock_guard lock(mutex_); + + latest_stamp_ = latest_stamp; + latest_covariance_stamp_ = latest_covariance_stamp; + latest_covariance_valid_ = latest_covariance_valid; + odom_output_ = odom_output; + acceleration_output_ = acceleration_output; + } +} + +void Odometry3DPublisher::onStart() +{ + synchronizer_ = Synchronizer(device_id_); + latest_stamp_ = latest_covariance_stamp_ = rclcpp::Time(0, 0, RCL_ROS_TIME); + latest_covariance_valid_ = false; + odom_output_ = nav_msgs::msg::Odometry(); + acceleration_output_ = geometry_msgs::msg::AccelWithCovarianceStamped(); + + // TODO(CH3): Add this to a separate callback group for async behavior + publish_timer_ = rclcpp::create_timer( + interfaces_, + clock_, + std::chrono::duration(1.0 / params_.publish_frequency), + std::move(std::bind(&Odometry3DPublisher::publishTimerCallback, this)), + cb_group_ + ); + + delayed_throttle_filter_.reset(); +} + +void Odometry3DPublisher::onStop() +{ + publish_timer_->cancel(); +} + +bool Odometry3DPublisher::getState( + const fuse_core::Graph & graph, + const rclcpp::Time & stamp, + const fuse_core::UUID & device_id, + fuse_core::UUID & position_uuid, + fuse_core::UUID & orientation_uuid, + fuse_core::UUID & velocity_linear_uuid, + fuse_core::UUID & velocity_angular_uuid, + fuse_core::UUID & acceleration_linear_uuid, + nav_msgs::msg::Odometry & odometry, + geometry_msgs::msg::AccelWithCovarianceStamped & acceleration) +{ + try { + position_uuid = fuse_variables::Position3DStamped(stamp, device_id).uuid(); + auto position_variable = dynamic_cast( + graph.getVariable(position_uuid)); + + orientation_uuid = fuse_variables::Orientation3DStamped(stamp, device_id).uuid(); + auto orientation_variable = dynamic_cast( + graph.getVariable(orientation_uuid)); + + velocity_linear_uuid = fuse_variables::VelocityLinear3DStamped(stamp, device_id).uuid(); + auto velocity_linear_variable = dynamic_cast( + graph.getVariable(velocity_linear_uuid)); + + velocity_angular_uuid = fuse_variables::VelocityAngular3DStamped(stamp, device_id).uuid(); + auto velocity_angular_variable = dynamic_cast( + graph.getVariable(velocity_angular_uuid)); + + acceleration_linear_uuid = fuse_variables::AccelerationLinear3DStamped(stamp, device_id).uuid(); + auto acceleration_linear_variable = + dynamic_cast( + graph.getVariable(acceleration_linear_uuid)); + + odometry.pose.pose.position.x = position_variable.x(); + odometry.pose.pose.position.y = position_variable.y(); + odometry.pose.pose.position.z = position_variable.z(); + odometry.pose.pose.orientation.w = orientation_variable.w(); + odometry.pose.pose.orientation.x = orientation_variable.x(); + odometry.pose.pose.orientation.y = orientation_variable.y(); + odometry.pose.pose.orientation.z = orientation_variable.z(); + odometry.twist.twist.linear.x = velocity_linear_variable.x(); + odometry.twist.twist.linear.y = velocity_linear_variable.y(); + odometry.twist.twist.linear.z = velocity_linear_variable.z(); + odometry.twist.twist.angular.x = velocity_angular_variable.roll(); + odometry.twist.twist.angular.y = velocity_angular_variable.pitch(); + odometry.twist.twist.angular.z = velocity_angular_variable.yaw(); + + acceleration.accel.accel.linear.x = acceleration_linear_variable.x(); + acceleration.accel.accel.linear.y = acceleration_linear_variable.y(); + acceleration.accel.accel.linear.z = acceleration_linear_variable.z(); + acceleration.accel.accel.angular.x = 0.0; + acceleration.accel.accel.angular.y = 0.0; + acceleration.accel.accel.angular.z = 0.0; + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); + return false; + } catch (...) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); + return false; + } + + return true; +} + +void Odometry3DPublisher::publishTimerCallback() +{ + rclcpp::Time latest_stamp; + rclcpp::Time latest_covariance_stamp; + bool latest_covariance_valid; + nav_msgs::msg::Odometry odom_output; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; + { + std::lock_guard lock(mutex_); + + latest_stamp = latest_stamp_; + latest_covariance_stamp = latest_covariance_stamp_; + latest_covariance_valid = latest_covariance_valid_; + odom_output = odom_output_; + acceleration_output = acceleration_output_; + } + + if (0u == latest_stamp.nanoseconds()) { + RCLCPP_WARN_STREAM_EXPRESSION( + logger_, delayed_throttle_filter_.isEnabled(), + "No valid state data yet. Delaying tf broadcast."); + return; + } + + tf2::Transform pose; + tf2::fromMsg(odom_output.pose.pose, pose); + + // If requested, we need to project our state forward in time using the 3D kinematic model + if (params_.predict_to_current_time) { + rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); + + // Convert pose in Eigen representation + fuse_core::Vector3d position, velocity_linear, velocity_angular; + Eigen::Quaterniond orientation; + position << pose.getOrigin().x(), pose.getOrigin().y(), pose.getOrigin().z(); + orientation.x() = pose.getRotation().x(); + orientation.y() = pose.getRotation().y(); + orientation.z() = pose.getRotation().z(); + orientation.w() = pose.getRotation().w(); + velocity_linear << odom_output.twist.twist.linear.x, + odom_output.twist.twist.linear.y, odom_output.twist.twist.linear.z; + velocity_angular << odom_output.twist.twist.angular.x, + odom_output.twist.twist.angular.y, odom_output.twist.twist.angular.z; + + const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); + + fuse_core::Matrix15d jacobian; + + fuse_core::Vector3d acceleration_linear; + if (params_.predict_with_acceleration) { + acceleration_linear << acceleration_output.accel.accel.linear.x, + acceleration_output.accel.accel.linear.y, acceleration_output.accel.accel.linear.z; + } + + predict( + position, + orientation, + velocity_linear, + velocity_angular, + acceleration_linear, + dt, + position, + orientation, + velocity_linear, + velocity_angular, + acceleration_linear, + jacobian); + + // Convert back to tf2 representation + pose.setOrigin(tf2::Vector3(position.x(), position.y(), position.z())); + pose.setRotation(tf2::Quaternion(orientation.x(), orientation.y(), orientation.z(), orientation.w())); + + odom_output.pose.pose.position.x = position.x(); + odom_output.pose.pose.position.y = position.y(); + odom_output.pose.pose.position.z = position.z(); + odom_output.pose.pose.orientation.x = orientation.x(); + odom_output.pose.pose.orientation.y = orientation.y(); + odom_output.pose.pose.orientation.z = orientation.z(); + odom_output.pose.pose.orientation.w = orientation.w(); + + odom_output.twist.twist.linear.x = velocity_linear.x(); + odom_output.twist.twist.linear.y = velocity_linear.y(); + odom_output.twist.twist.linear.z = velocity_linear.z(); + odom_output.twist.twist.angular.x = velocity_angular.x(); + odom_output.twist.twist.angular.y = velocity_angular.y(); + odom_output.twist.twist.angular.z = velocity_angular.z(); + + if (params_.predict_with_acceleration) { + acceleration_output.accel.accel.linear.x = acceleration_linear.x(); + acceleration_output.accel.accel.linear.y = acceleration_linear.y(); + acceleration_output.accel.accel.linear.z = acceleration_linear.z(); + } + + odom_output.header.stamp = timer_now; + acceleration_output.header.stamp = timer_now; + + // Either the last covariance computation was skipped because there was no subscriber, + // or it failed + if (latest_covariance_valid) { + fuse_core::Matrix15d covariance; + covariance.setZero(); + Eigen::Map pose_covariance(odom_output.pose.covariance.data()); + Eigen::Map twist_covariance(odom_output.twist.covariance.data()); + Eigen::Map acceleration_covariance(acceleration_output.accel.covariance.data()); + + covariance.block<6, 6>(0, 0) = pose_covariance; + covariance.block<6, 6>(6, 6) = twist_covariance; + covariance.block<3, 3>(12, 12) = acceleration_covariance; + + covariance = jacobian * covariance * jacobian.transpose(); + + auto process_noise_covariance = params_.process_noise_covariance; + if (params_.scale_process_noise) { + common::scaleProcessNoiseCovariance( + process_noise_covariance, + velocity_linear, + velocity_angular, + params_.velocity_linear_norm_min_, + params_.velocity_angular_norm_min_); + } + + covariance.noalias() += dt * process_noise_covariance; + + pose_covariance = covariance.block<6, 6>(0, 0); + twist_covariance = covariance.block<6, 6>(6, 6); + acceleration_covariance = covariance.block<3, 3>(12, 12); + } + } + + odom_pub_->publish(odom_output); + acceleration_pub_->publish(acceleration_output); + + if (params_.publish_tf) { + auto frame_id = odom_output.header.frame_id; + auto child_frame_id = odom_output.child_frame_id; + + if (params_.invert_tf) { + pose = pose.inverse(); + std::swap(frame_id, child_frame_id); + } + + geometry_msgs::msg::TransformStamped trans; + trans.header.stamp = odom_output.header.stamp; + trans.header.frame_id = frame_id; + trans.child_frame_id = child_frame_id; + trans.transform = tf2::toMsg(pose); + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { + try { + auto base_to_odom = tf_buffer_->lookupTransform( + params_.base_link_frame_id, + params_.odom_frame_id, + trans.header.stamp, + params_.tf_timeout); + + geometry_msgs::msg::TransformStamped map_to_odom; + tf2::doTransform(base_to_odom, map_to_odom, trans); + map_to_odom.child_frame_id = params_.odom_frame_id; + trans = map_to_odom; + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Could not lookup the " << params_.base_link_frame_id << "->" + << params_.odom_frame_id << " transform. Error: " << e.what()); + + return; + } + } + + tf_broadcaster_->sendTransform(trans); + } +} + +} // namespace fuse_models diff --git a/fuse_models/src/omnidirectional_3d.cpp b/fuse_models/src/omnidirectional_3d.cpp new file mode 100644 index 000000000..92534d25d --- /dev/null +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -0,0 +1,623 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Register this motion model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3D, fuse_core::MotionModel) + +namespace std +{ + +inline bool isfinite(const fuse_core::Vector3d & vector) +{ + return std::isfinite(vector.x()) && std::isfinite(vector.y() && std::isfinite(vector.z())); +} + +inline bool isNormalized(const Eigen::Quaterniond & q) +{ + return std::sqrt(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()) - 1.0 < + Eigen::NumTraits::dummy_precision(); +} + +std::string to_string(const fuse_core::Vector3d & vector) +{ + std::ostringstream oss; + oss << vector; + return oss.str(); +} + +std::string to_string(const Eigen::Quaterniond & quaternion) +{ + std::ostringstream oss; + oss << quaternion; + return oss.str(); +} +} // namespace std + +namespace fuse_core +{ + +template +inline void validateCovariance( + const Eigen::DenseBase & covariance, + const double precision = Eigen::NumTraits::dummy_precision()) +{ + if (!fuse_core::isSymmetric(covariance, precision)) { + throw std::runtime_error( + "Non-symmetric partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } + + if (!fuse_core::isPositiveDefinite(covariance)) { + throw std::runtime_error( + "Non-positive-definite partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } +} + +} // namespace fuse_core + +namespace fuse_models +{ + +Omnidirectional3D::Omnidirectional3D() +: fuse_core::AsyncMotionModel(1), // Thread count = 1 for local callback queue + logger_(rclcpp::get_logger("uninitialized")), + buffer_length_(rclcpp::Duration::max()), + device_id_(fuse_core::uuid::NIL), + timestamp_manager_(&Omnidirectional3D::generateMotionModel, this, rclcpp::Duration::max()) +{ +} + +void Omnidirectional3D::print(std::ostream & stream) const +{ + stream << "state history:\n"; + for (const auto & state : state_history_) { + stream << "- stamp: " << state.first.nanoseconds() << "\n"; + state.second.print(stream); + } +} + +void Omnidirectional3D::StateHistoryElement::print(std::ostream & stream) const +{ + stream << " position uuid: " << position_uuid << "\n" + << " orientation uuid: " << orientation_uuid << "\n" + << " velocity linear uuid: " << vel_linear_uuid << "\n" + << " velocity angular uuid: " << vel_angular_uuid << "\n" + << " acceleration linear uuid: " << acc_linear_uuid << "\n" + << " position: " << position << "\n" + << " orientation: " << orientation << "\n" + << " velocity linear: " << vel_linear << "\n" + << " velocity angular: " << vel_angular << "\n" + << " acceleration linear: " << acc_linear << "\n"; +} + +void Omnidirectional3D::StateHistoryElement::validate() const +{ + if (!std::isfinite(position)) { + throw std::runtime_error("Invalid position " + std::to_string(position)); + } + if (!std::isNormalized(orientation)) { + throw std::runtime_error("Invalid orientation " + std::to_string(orientation)); + } + if (!std::isfinite(vel_linear)) { + throw std::runtime_error("Invalid linear velocity " + std::to_string(vel_linear)); + } + if (!std::isfinite(vel_angular)) { + throw std::runtime_error("Invalid angular velocity " + std::to_string(vel_angular)); + } + if (!std::isfinite(acc_linear)) { + throw std::runtime_error("Invalid linear acceleration " + std::to_string(acc_linear)); + } +} + +bool Omnidirectional3D::applyCallback(fuse_core::Transaction & transaction) +{ + // Use the timestamp manager to generate just the required motion model segments. The timestamp + // manager, in turn, makes calls to the generateMotionModel() function. + try { + // Now actually generate the motion model segments + timestamp_manager_.query(transaction, true); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "An error occurred while completing the motion model query. Error: " << e.what()); + return false; + } + return true; +} + +void Omnidirectional3D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) +{ + updateStateHistoryEstimates(*graph, state_history_, buffer_length_); +} + +void Omnidirectional3D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) +{ + interfaces_ = interfaces; + fuse_core::AsyncMotionModel::initialize(interfaces, name); +} + +void Omnidirectional3D::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + std::vector process_noise_diagonal; + process_noise_diagonal = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "process_noise_diagonal"), + process_noise_diagonal); + + if (process_noise_diagonal.size() != 15) { + throw std::runtime_error("Process noise diagonal must be of length 15!"); + } + + process_noise_covariance_ = fuse_core::Vector15d(process_noise_diagonal.data()).asDiagonal(); + + scale_process_noise_ = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "scale_process_noise"), + scale_process_noise_); + velocity_linear_norm_min_ = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "velocity_linear_norm_min"), + velocity_linear_norm_min_); + + velocity_angular_norm_min_ = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "velocity_angular_norm_min"), + velocity_angular_norm_min_); + + disable_checks_ = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "disable_checks"), + disable_checks_); + + double buffer_length = 3.0; + buffer_length = + fuse_core::getParam( + interfaces_, fuse_core::joinParameterName( + name_, + "buffer_length"), + buffer_length); + + if (buffer_length < 0.0) { + throw std::runtime_error( + "Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + } + + buffer_length_ = + (buffer_length == + 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); + timestamp_manager_.bufferLength(buffer_length_); + + device_id_ = fuse_variables::loadDeviceId(interfaces_); +} + +void Omnidirectional3D::onStart() +{ + timestamp_manager_.clear(); + state_history_.clear(); +} + +void Omnidirectional3D::generateMotionModel( + const rclcpp::Time & beginning_stamp, + const rclcpp::Time & ending_stamp, + std::vector & constraints, + std::vector & variables) +{ + assert( + beginning_stamp < ending_stamp || + (beginning_stamp == ending_stamp && state_history_.empty())); + + StateHistoryElement base_state; + rclcpp::Time base_time{0, 0, RCL_ROS_TIME}; + + // Find an entry that is > beginning_stamp + // The entry that is <= will be the one before it + auto base_state_pair_it = state_history_.upper_bound(beginning_stamp); + if (base_state_pair_it == state_history_.begin()) { + RCLCPP_WARN_STREAM_EXPRESSION( + logger_, !state_history_.empty(), + "Unable to locate a state in this history with stamp <= " + << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); + base_time = beginning_stamp; + } else { + --base_state_pair_it; + base_time = base_state_pair_it->first; + base_state = base_state_pair_it->second; + } + + StateHistoryElement state1; + // If the nearest state we had was before the beginning stamp, we need to project that state to + // the beginning stamp + if (base_time != beginning_stamp) { + predict( + base_state.position, + base_state.orientation, + base_state.vel_linear, + base_state.vel_angular, + base_state.acc_linear, + (beginning_stamp - base_time).seconds(), + state1.position, + state1.orientation, + state1.vel_linear, + state1.vel_angular, + state1.acc_linear); + } else { + state1 = base_state; + } + + // If dt is zero, we only need to update the state history: + const double dt = (ending_stamp - beginning_stamp).seconds(); + + if (dt == 0.0) { + state1.position_uuid = fuse_variables::Position3DStamped(beginning_stamp, device_id_).uuid(); + state1.orientation_uuid = fuse_variables::Orientation3DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_linear_uuid = + fuse_variables::VelocityLinear3DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_angular_uuid = + fuse_variables::VelocityAngular3DStamped(beginning_stamp, device_id_).uuid(); + state1.acc_linear_uuid = + fuse_variables::AccelerationLinear3DStamped(beginning_stamp, device_id_).uuid(); + state_history_.emplace(beginning_stamp, std::move(state1)); + return; + } + + // Now predict to get an initial guess for the state at the ending stamp + StateHistoryElement state2; + predict( + state1.position, + state1.orientation, + state1.vel_linear, + state1.vel_angular, + state1.acc_linear, + dt, + state2.position, + state2.orientation, + state2.vel_linear, + state2.vel_angular, + state2.acc_linear); + + // Define the fuse variables required for this constraint + auto position1 = fuse_variables::Position3DStamped::make_shared(beginning_stamp, device_id_); + auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(beginning_stamp, device_id_); + auto velocity_linear1 = fuse_variables::VelocityLinear3DStamped::make_shared( + beginning_stamp, + device_id_); + auto velocity_angular1 = fuse_variables::VelocityAngular3DStamped::make_shared( + beginning_stamp, + device_id_); + auto acceleration_linear1 = fuse_variables::AccelerationLinear3DStamped::make_shared( + beginning_stamp, device_id_); + auto position2 = fuse_variables::Position3DStamped::make_shared(ending_stamp, device_id_); + auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(ending_stamp, device_id_); + auto velocity_linear2 = fuse_variables::VelocityLinear3DStamped::make_shared( + ending_stamp, + device_id_); + auto velocity_angular2 = fuse_variables::VelocityAngular3DStamped::make_shared( + ending_stamp, + device_id_); + auto acceleration_linear2 = fuse_variables::AccelerationLinear3DStamped::make_shared( + ending_stamp, + device_id_); + + position1->data()[fuse_variables::Position3DStamped::X] = state1.position.x(); + position1->data()[fuse_variables::Position3DStamped::Y] = state1.position.y(); + position1->data()[fuse_variables::Position3DStamped::Z] = state1.position.z(); + + orientation1->data()[fuse_variables::Orientation3DStamped::X] = state1.orientation.x(); + orientation1->data()[fuse_variables::Orientation3DStamped::Y] = state1.orientation.y(); + orientation1->data()[fuse_variables::Orientation3DStamped::Z] = state1.orientation.z(); + orientation1->data()[fuse_variables::Orientation3DStamped::W] = state1.orientation.w(); + + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::X] = state1.vel_linear.x(); + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Y] = state1.vel_linear.y(); + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Z] = state1.vel_linear.z(); + + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state1.vel_angular.x(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state1.vel_angular.y(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state1.vel_angular.z(); + + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::X] = + state1.acc_linear.x(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Y] = + state1.acc_linear.y(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Z] = + state1.acc_linear.z(); + + position2->data()[fuse_variables::Position3DStamped::X] = state2.position.x(); + position2->data()[fuse_variables::Position3DStamped::Y] = state2.position.y(); + position2->data()[fuse_variables::Position3DStamped::Z] = state2.position.z(); + + orientation2->data()[fuse_variables::Orientation3DStamped::X] = state2.orientation.x(); + orientation2->data()[fuse_variables::Orientation3DStamped::Y] = state2.orientation.y(); + orientation2->data()[fuse_variables::Orientation3DStamped::Z] = state2.orientation.z(); + orientation2->data()[fuse_variables::Orientation3DStamped::W] = state2.orientation.w(); + + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::X] = state2.vel_linear.x(); + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Y] = state2.vel_linear.y(); + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Z] = state2.vel_linear.z(); + + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state2.vel_angular.x(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state2.vel_angular.y(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state2.vel_angular.z(); + + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::X] = + state2.acc_linear.x(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Y] = + state2.acc_linear.y(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Z] = + state2.acc_linear.z(); + + state1.position_uuid = position1->uuid(); + state1.orientation_uuid = orientation1->uuid(); + state1.vel_linear_uuid = velocity_linear1->uuid(); + state1.vel_angular_uuid = velocity_angular1->uuid(); + state1.acc_linear_uuid = acceleration_linear1->uuid(); + + state2.position_uuid = position2->uuid(); + state2.orientation_uuid = orientation2->uuid(); + state2.vel_linear_uuid = velocity_linear2->uuid(); + state2.vel_angular_uuid = velocity_angular2->uuid(); + state2.acc_linear_uuid = acceleration_linear2->uuid(); + + state_history_.emplace(beginning_stamp, std::move(state1)); + state_history_.emplace(ending_stamp, std::move(state2)); + + // Scale process noise covariance pose by the norm of the current state twist + auto process_noise_covariance = process_noise_covariance_; + if (scale_process_noise_) { + common::scaleProcessNoiseCovariance( + process_noise_covariance, state1.vel_linear, state1.vel_angular, + velocity_linear_norm_min_, velocity_angular_norm_min_); + } + + // Validate + process_noise_covariance *= dt; + + if (!disable_checks_) { + try { + validateMotionModel(state1, state2, process_noise_covariance); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, *clock_, 10.0 * 1000, + "Invalid '" << name_ << "' motion model: " << ex.what()); + return; + } + } + + // Create the constraints for this motion model segment + auto constraint = fuse_models::Omnidirectional3DStateKinematicConstraint::make_shared( + name(), + *position1, + *orientation1, + *velocity_linear1, + *velocity_angular1, + *acceleration_linear1, + *position2, + *orientation2, + *velocity_linear2, + *velocity_angular2, + *acceleration_linear2, + process_noise_covariance); + + // Update the output variables + constraints.push_back(constraint); + variables.push_back(position1); + variables.push_back(orientation1); + variables.push_back(velocity_linear1); + variables.push_back(velocity_angular1); + variables.push_back(acceleration_linear1); + variables.push_back(position2); + variables.push_back(orientation2); + variables.push_back(velocity_linear2); + variables.push_back(velocity_angular2); + variables.push_back(acceleration_linear2); +} + +void Omnidirectional3D::updateStateHistoryEstimates( + const fuse_core::Graph & graph, + StateHistory & state_history, + const rclcpp::Duration & buffer_length) +{ + if (state_history.empty()) { + return; + } + + // Compute the expiration time carefully, as ROS can't handle negative times + const auto & ending_stamp = state_history.rbegin()->first; + + rclcpp::Time expiration_time; + if (ending_stamp.seconds() > buffer_length.seconds()) { + expiration_time = ending_stamp - buffer_length; + } else { + // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. + expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); + } + + // Remove state history elements before the expiration time. + // Be careful to ensure that: + // - at least one entry remains at all times + // - the history covers *at least* until the expiration time. Longer is acceptable. + auto expiration_iter = state_history.upper_bound(expiration_time); + if (expiration_iter != state_history.begin()) { + // expiration_iter points to the first element > expiration_time. + // Back up one entry, to a point that is <= expiration_time + state_history.erase(state_history.begin(), std::prev(expiration_iter)); + } + + // Update the states in the state history with information from the graph + // If a state is not in the graph yet, predict the state in question from the closest previous + // state + for (auto current_iter = state_history.begin(); current_iter != state_history.end(); + ++current_iter) + { + const auto & current_stamp = current_iter->first; + auto & current_state = current_iter->second; + if (graph.variableExists(current_state.position_uuid) && + graph.variableExists(current_state.orientation_uuid) && + graph.variableExists(current_state.vel_linear_uuid) && + graph.variableExists(current_state.vel_angular_uuid) && + graph.variableExists(current_state.acc_linear_uuid)) + { + // This pose does exist in the graph. Update it directly. + const auto & position = graph.getVariable(current_state.position_uuid); + const auto & orientation = graph.getVariable(current_state.orientation_uuid); + const auto & vel_linear = graph.getVariable(current_state.vel_linear_uuid); + const auto & vel_angular = graph.getVariable(current_state.vel_angular_uuid); + const auto & acc_linear = graph.getVariable(current_state.acc_linear_uuid); + + current_state.position.x() = position.data()[fuse_variables::Position3DStamped::X]; + current_state.position.y() = position.data()[fuse_variables::Position3DStamped::Y]; + current_state.position.z() = position.data()[fuse_variables::Position3DStamped::Z]; + + current_state.orientation.x() = orientation.data()[fuse_variables::Orientation3DStamped::X]; + current_state.orientation.y() = orientation.data()[fuse_variables::Orientation3DStamped::Y]; + current_state.orientation.z() = orientation.data()[fuse_variables::Orientation3DStamped::Z]; + current_state.orientation.w() = orientation.data()[fuse_variables::Orientation3DStamped::W]; + + current_state.vel_linear.x() = + vel_linear.data()[fuse_variables::VelocityLinear3DStamped::X]; + current_state.vel_linear.y() = + vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Y]; + current_state.vel_linear.z() = + vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Z]; + + current_state.vel_angular.x() = + vel_angular.data()[fuse_variables::VelocityAngular3DStamped::ROLL]; + current_state.vel_angular.y() = + vel_angular.data()[fuse_variables::VelocityAngular3DStamped::PITCH]; + current_state.vel_angular.z() = + vel_angular.data()[fuse_variables::VelocityAngular3DStamped::YAW]; + + current_state.acc_linear.x() = + acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::X]; + current_state.acc_linear.y() = + acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Y]; + current_state.acc_linear.z() = + acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Z]; + } else if (current_iter != state_history.begin()) { + auto previous_iter = std::prev(current_iter); + const auto & previous_stamp = previous_iter->first; + const auto & previous_state = previous_iter->second; + + // This state is not in the graph yet, so we can't update/correct the value in our state + // history. However, the state *before* this one may have been corrected (or one of its + // predecessors may have been), so we can use that corrected value, along with our prediction + // logic, to provide a more accurate update to this state. + predict( + previous_state.position, + previous_state.orientation, + previous_state.vel_linear, + previous_state.vel_angular, + previous_state.acc_linear, + (current_stamp - previous_stamp).seconds(), + current_state.position, + current_state.orientation, + current_state.vel_linear, + current_state.vel_angular, + current_state.acc_linear); + } + } +} + +void Omnidirectional3D::validateMotionModel( + const StateHistoryElement & state1, const StateHistoryElement & state2, + const fuse_core::Matrix15d & process_noise_covariance) +{ + try { + state1.validate(); + } catch (const std::runtime_error & ex) { + throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); + } + + try { + state2.validate(); + } catch (const std::runtime_error & ex) { + throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); + } + try { + fuse_core::validateCovariance(process_noise_covariance); + } catch (const std::runtime_error & ex) { + throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); + } +} + +std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & omnidirectional_3d) +{ + omnidirectional_3d.print(stream); + return stream; +} + +} // namespace fuse_models diff --git a/fuse_models/src/omnidirectional_3d_ignition.cpp b/fuse_models/src/omnidirectional_3d_ignition.cpp new file mode 100644 index 000000000..d545007d0 --- /dev/null +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -0,0 +1,459 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Register this motion model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3DIgnition, fuse_core::SensorModel); + +namespace fuse_models +{ + +Omnidirectional3DIgnition::Omnidirectional3DIgnition() +: fuse_core::AsyncSensorModel(1), + started_(false), + initial_transaction_sent_(false), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")) +{ +} + +void Omnidirectional3DIgnition::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + +void Omnidirectional3DIgnition::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + // Connect to the reset service + if (!params_.reset_service.empty()) { + reset_client_ = rclcpp::create_client( + interfaces_.get_node_base_interface(), + interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), + params_.reset_service, + rclcpp::ServicesQoS(), + cb_group_ + ); + } + + // Advertise + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + sub_ = rclcpp::create_subscription( + interfaces_, + params_.topic, + params_.queue_size, + std::bind(&Omnidirectional3DIgnition::subscriberCallback, this, std::placeholders::_1), + sub_options + ); + + set_pose_service_ = rclcpp::create_service( + interfaces_.get_node_base_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName( + interfaces_.get_node_base_interface()->get_name(), + params_.set_pose_service), + std::bind( + &Omnidirectional3DIgnition::setPoseServiceCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + rclcpp::ServicesQoS(), + cb_group_ + ); + set_pose_deprecated_service_ = rclcpp::create_service( + interfaces_.get_node_base_interface(), + interfaces_.get_node_services_interface(), + fuse_core::joinTopicName( + interfaces_.get_node_base_interface()->get_name(), + params_.set_pose_deprecated_service), + std::bind( + &Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + rclcpp::ServicesQoS(), + cb_group_ + ); +} + +void Omnidirectional3DIgnition::start() +{ + started_ = true; + + // TODO(swilliams) Should this be executed every time optimizer.reset() is called, or only once + // ever? I feel like it should be "only once ever". Send an initial state + // transaction immediately, if requested + if (params_.publish_on_startup && !initial_transaction_sent_) { + auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); + tf2::Quaternion q; + // q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); + q.setEuler(params_.initial_state[5], params_.initial_state[4], params_.initial_state[3]); + pose.header.stamp = clock_->now(); + pose.pose.pose.position.x = params_.initial_state[0]; + pose.pose.pose.position.y = params_.initial_state[1]; + pose.pose.pose.position.z = params_.initial_state[2]; + pose.pose.pose.orientation.x = q.x(); + pose.pose.pose.orientation.y = q.y(); + pose.pose.pose.orientation.z = q.z(); + pose.pose.pose.orientation.w = q.w(); + for (size_t i = 0; i < 6; i++) { + pose.pose.covariance[i * 7] = params_.initial_sigma[i] * params_.initial_sigma[i]; + } + sendPrior(pose); + initial_transaction_sent_ = true; + } +} + +void Omnidirectional3DIgnition::stop() +{ + started_ = false; +} + +void Omnidirectional3DIgnition::subscriberCallback( + const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +{ + try { + process(msg); + } catch (const std::exception & e) { + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); + } +} + +bool Omnidirectional3DIgnition::setPoseServiceCallback( + rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPose::Request::SharedPtr req) +{ + try { + process( + req->pose, + [service, request_id]() { + fuse_msgs::srv::SetPose::Response response; + response.success = true; + service->send_response(*request_id, response); + }); + } catch (const std::exception & e) { + fuse_msgs::srv::SetPose::Response response; + response.success = false; + response.message = e.what(); + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); + service->send_response(*request_id, response); + } + return true; +} + +bool Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback( + rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) +{ + try { + process( + req->pose, + [service, request_id]() { + fuse_msgs::srv::SetPoseDeprecated::Response response; + service->send_response(*request_id, response); + }); + } catch (const std::exception & e) { + fuse_msgs::srv::SetPoseDeprecated::Response response; + RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); + service->send_response(*request_id, response); + } + return true; +} + +void Omnidirectional3DIgnition::process( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, std::function post_process) +{ + // Verify we are in the correct state to process set pose requests + if (!started_) { + throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); + } + // Validate the requested pose and covariance before we do anything + if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y) || !std::isfinite(pose.pose.pose.position.z)) { + throw std::invalid_argument( + "Attempting to set the pose to an invalid position (" + + std::to_string(pose.pose.pose.position.x) + ", " + + std::to_string(pose.pose.pose.position.y) + ", " + + std::to_string(pose.pose.pose.position.z) + ")."); + } + auto orientation_norm = std::sqrt( + pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + + pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + + pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + + pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); + if (std::abs(orientation_norm - 1.0) > 1.0e-3) { + throw std::invalid_argument( + "Attempting to set the pose to an invalid orientation (" + + std::to_string(pose.pose.pose.orientation.x) + ", " + + std::to_string(pose.pose.pose.orientation.y) + ", " + + std::to_string(pose.pose.pose.orientation.z) + ", " + + std::to_string(pose.pose.pose.orientation.w) + ")."); + } + auto position_cov = fuse_core::Matrix3d(); + // for (size_t i = 0; i < 3; i++) { + // for (size_t j = 0; j < 3; j++) { + // position_cov(i, j) = pose.pose.covariance[i * 6 + j]; + // } + // } + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], + pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], + pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; + if (!fuse_core::isSymmetric(position_cov)) { + throw std::invalid_argument( + "Attempting to set the pose with a non-symmetric position covariance matrix\n " + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + } + if (!fuse_core::isPositiveDefinite(position_cov)) { + throw std::invalid_argument( + "Attempting to set the pose with a non-positive-definite position covariance matrix\n" + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + } + auto orientation_cov = fuse_core::Matrix3d(); + // for (size_t i = 0; i < 3; i++) { + // for (size_t j = 0; j < 3; j++) { + // position_cov(i, j) = pose.pose.covariance[3+i * 6 + j]; + // } + // } + orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], + pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; + if (!fuse_core::isSymmetric(orientation_cov)) { + throw std::invalid_argument( + "Attempting to set the pose with a non-symmetric orientation covariance matrix\n " + + fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); + } + if (!fuse_core::isPositiveDefinite(orientation_cov)) { + throw std::invalid_argument( + "Attempting to set the pose with a non-positive-definite orientation_cov covariance matrix\n" + + fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); + } + // Tell the optimizer to reset before providing the initial state + if (!params_.reset_service.empty()) { + // Wait for the reset service + while (!reset_client_->wait_for_service(std::chrono::seconds(10)) && + interfaces_.get_node_base_interface()->get_context()->is_valid()) + { + RCLCPP_WARN_STREAM( + logger_, + "Waiting for '" << reset_client_->get_service_name() << "' service to become avaiable."); + } + + auto srv = std::make_shared(); + // Don't block the executor. + // It needs to be free to handle the response to this service call. + // Have a callback do the rest of the work when a response comes. + auto result_future = reset_client_->async_send_request( + srv, + [this, post_process, pose](rclcpp::Client::SharedFuture result) { + (void)result; + // Now that the pose has been validated and the optimizer has been reset, actually send the + // initial state constraints to the optimizer + sendPrior(pose); + if (post_process) { + post_process(); + } + }); + } else { + sendPrior(pose); + if (post_process) { + post_process(); + } + } +} + +void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +{ + const auto & stamp = pose.header.stamp; + + // Create variables for the full state. + // The initial values of the pose are extracted from the provided PoseWithCovarianceStamped + // message. The remaining dimensions are provided as parameters to the parameter server. + auto position = fuse_variables::Position3DStamped::make_shared(stamp, device_id_); + position->x() = pose.pose.pose.position.x; + position->y() = pose.pose.pose.position.y; + position->z() = pose.pose.pose.position.z; + auto orientation = fuse_variables::Orientation3DStamped::make_shared(stamp, device_id_); + orientation->w() = pose.pose.pose.orientation.w; + orientation->x() = pose.pose.pose.orientation.x; + orientation->y() = pose.pose.pose.orientation.y; + orientation->z() = pose.pose.pose.orientation.z; + auto linear_velocity = fuse_variables::VelocityLinear3DStamped::make_shared(stamp, device_id_); + linear_velocity->x() = params_.initial_state[6]; + linear_velocity->y() = params_.initial_state[7]; + linear_velocity->z() = params_.initial_state[8]; + auto angular_velocity = fuse_variables::VelocityAngular3DStamped::make_shared(stamp, device_id_); + angular_velocity->roll() = params_.initial_state[9]; + angular_velocity->pitch() = params_.initial_state[10]; + angular_velocity->yaw() = params_.initial_state[11]; + auto linear_acceleration = fuse_variables::AccelerationLinear3DStamped::make_shared(stamp, device_id_); + linear_acceleration->x() = params_.initial_state[12]; + linear_acceleration->y() = params_.initial_state[13]; + linear_acceleration->z() = params_.initial_state[14]; + + // Create the covariances for each variable + // The pose covariances are extracted from the provided PoseWithCovarianceStamped message. + // The remaining covariances are provided as parameters to the parameter server. + auto position_cov = fuse_core::Matrix3d(); + auto orientation_cov = fuse_core::Matrix3d(); + auto linear_velocity_cov = fuse_core::Matrix3d(); + auto angular_velocity_cov = fuse_core::Matrix3d(); + auto linear_acceleration_cov = fuse_core::Matrix3d(); + + // for (size_t i = 0; i < 3; i++) { + // for (size_t j = 0; j < 3; j++) { + // position_cov(i, j) = pose.pose.covariance[i * 6 + j]; + // orientation_cov(i, j) = pose.pose.covariance[(i + 3) * 6 + j + 3]; + // if (i == j) { + // linear_velocity_cov(i, j) = params_.initial_sigma[6 + i] * params_.initial_sigma[6 + i]; + // angular_velocity_cov(i, j) = params_.initial_sigma[9 + i] * params_.initial_sigma[9 + i]; + // linear_acceleration_cov(i, j) = params_.initial_sigma[12 + i] * params_.initial_sigma[12 + i]; + // } + // } + // } + + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], + pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], + pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; + + orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], + pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; + + linear_velocity_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, + 0.0, params_.initial_sigma[7] * params_.initial_sigma[7], 0.0, + 0.0, 0.0, params_.initial_sigma[8] * params_.initial_sigma[8]; + + angular_velocity_cov << params_.initial_sigma[9] * params_.initial_sigma[9], 0.0, 0.0, + 0.0, params_.initial_sigma[10] * params_.initial_sigma[10], 0.0, + 0.0, 0.0, params_.initial_sigma[11] * params_.initial_sigma[11]; + + linear_acceleration_cov << params_.initial_sigma[12] * params_.initial_sigma[12], 0.0, 0.0, + 0.0, params_.initial_sigma[13] * params_.initial_sigma[13], 0.0, + 0.0, 0.0, params_.initial_sigma[14] * params_.initial_sigma[14]; + // Create absolute constraints for each variable + auto position_constraint = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( + name(), + *position, + fuse_core::Vector3d(position->x(), position->y(), position->z()), + position_cov); + auto orientation_constraint = + fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( + name(), + *orientation, + Eigen::Quaterniond(orientation->w(), orientation->x(), orientation->y(), orientation->z()), + orientation_cov); + auto linear_velocity_constraint = + fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( + name(), + *linear_velocity, + fuse_core::Vector3d(linear_velocity->x(), linear_velocity->y(), linear_velocity->z()), + linear_velocity_cov); + auto angular_velocity_constraint = + fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( + name(), + *angular_velocity, + fuse_core::Vector3d(angular_velocity->roll(), angular_velocity->pitch(), angular_velocity->yaw()), + angular_velocity_cov); + auto linear_acceleration_constraint = + fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( + name(), + *linear_acceleration, + fuse_core::Vector3d(linear_acceleration->x(), linear_acceleration->y(), linear_acceleration->z()), + linear_acceleration_cov); + + // Create the transaction + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(stamp); + transaction->addInvolvedStamp(stamp); + transaction->addVariable(position); + transaction->addVariable(orientation); + transaction->addVariable(linear_velocity); + transaction->addVariable(angular_velocity); + transaction->addVariable(linear_acceleration); + transaction->addConstraint(position_constraint); + transaction->addConstraint(orientation_constraint); + transaction->addConstraint(linear_velocity_constraint); + transaction->addConstraint(angular_velocity_constraint); + transaction->addConstraint(linear_acceleration_constraint); + + // Send the transaction to the optimizer. + sendTransaction(transaction); + + RCLCPP_INFO_STREAM( + logger_, + "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() + << ", x: " << position->x() << ", y: " + << position->y() << ", z: " << position->z() + << ", roll: " << orientation->roll() + << ", pitch: " << orientation->pitch() + << ", yaw: " << orientation->yaw() << ")"); +} +} // namespace fuse_models diff --git a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp new file mode 100644 index 000000000..7caf9dc3b --- /dev/null +++ b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp @@ -0,0 +1,117 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fuse_models +{ + +Omnidirectional3DStateKinematicConstraint::Omnidirectional3DStateKinematicConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::VelocityLinear3DStamped & velocity_linear1, + const fuse_variables::VelocityAngular3DStamped & velocity_angular1, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_variables::VelocityLinear3DStamped & velocity_linear2, + const fuse_variables::VelocityAngular3DStamped & velocity_angular2, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, + const fuse_core::Matrix15d & covariance) +: fuse_core::Constraint( + source, + {position1.uuid(), + orientation1.uuid(), + velocity_linear1.uuid(), + velocity_angular1.uuid(), + acceleration_linear1.uuid(), + position2.uuid(), + orientation2.uuid(), + velocity_linear2.uuid(), + velocity_angular2.uuid(), + acceleration_linear2.uuid()}), // NOLINT + dt_((position2.stamp() - position1.stamp()).seconds()), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +void Omnidirectional3DStateKinematicConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position variable 1: " << variables().at(0) << "\n" + << " orientation variable 1: " << variables().at(1) << "\n" + << " linear velocity variable 1: " << variables().at(2) << "\n" + << " angular velocity variable 1: " << variables().at(3) << "\n" + << " linear acceleration variable 1: " << variables().at(4) << "\n" + << " position variable 2: " << variables().at(5) << "\n" + << " orientation variable 2: " << variables().at(6) << "\n" + << " linear velocity variable 2: " << variables().at(7) << "\n" + << " angular velocity variable 2: " << variables().at(8) << "\n" + << " linear acceleration variable 2: " << variables().at(9) << "\n" + << " dt: " << dt() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; +} + +ceres::CostFunction * Omnidirectional3DStateKinematicConstraint::costFunction() const +{ + return new Omnidirectional3DStateCostFunction(dt_, sqrt_information_); + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could + // use automatic differentiation as follows: + // + // return new ceres::AutoDiffCostFunction( + // new Omnidirectional3DStateCostFunctor(dt_, sqrt_information_)); + + // And including the followings: + // #include + // #include +} + +} // namespace fuse_models + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_models::Omnidirectional3DStateKinematicConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3DStateKinematicConstraint, fuse_core::Constraint); diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index f8e4347f5..30602e78b 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -5,6 +5,9 @@ set(TEST_TARGETS test_unicycle_2d_state_cost_function test_graph_ignition test_unicycle_2d_ignition + test_omnidirectional_3d + test_omnidirectional_3d_predict + test_omnidirectional_3d_state_cost_function ) foreach(test_name ${TEST_TARGETS}) @@ -14,3 +17,6 @@ endforeach() ament_add_gmock(test_sensor_proc "test_sensor_proc.cpp") target_link_libraries(test_sensor_proc ${PROJECT_NAME}) + +ament_add_gmock(test_sensor_proc_3d "test_sensor_proc_3d.cpp") +target_link_libraries(test_sensor_proc_3d ${PROJECT_NAME}) \ No newline at end of file diff --git a/fuse_models/test/test_omnidirectional_3d.cpp b/fuse_models/test/test_omnidirectional_3d.cpp new file mode 100644 index 000000000..f82bb75a2 --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d.cpp @@ -0,0 +1,400 @@ +/*************************************************************************** + * Copyright (C) 2017 Locus Robotics. All rights reserved. + * Unauthorized copying of this file, via any medium, is strictly prohibited + * Proprietary and confidential + ***************************************************************************/ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Derived class used in unit tests to expose protected functions + */ +class Omnidirectional3DModelTest : public fuse_models::Omnidirectional3D +{ +public: + using fuse_models::Omnidirectional3D::updateStateHistoryEstimates; + using fuse_models::Omnidirectional3D::StateHistoryElement; + using fuse_models::Omnidirectional3D::StateHistory; +}; + +TEST(Omnidirectional3D, UpdateStateHistoryEstimates) +{ + // Create some variables + auto position1 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0)); + auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_velocity1 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(1, 0)); + auto angular_velocity1 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_acceleration1 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(1, 0)); + position1->x() = 1.1; + position1->y() = 2.1; + position1->z() = 0.0; + orientation1->w() = 1.0; + orientation1->x() = 0.0; + orientation1->y() = 0.0; + orientation1->z() = 0.0; + linear_velocity1->x() = 1.0; + linear_velocity1->y() = 0.0; + linear_velocity1->z() = 0.0; + angular_velocity1->roll() = 0.0; + angular_velocity1->pitch() = 0.0; + angular_velocity1->yaw() = 0.0; + linear_acceleration1->x() = 1.0; + linear_acceleration1->y() = 0.0; + linear_acceleration1->z() = 0.0; + auto position2 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(2, 0)); + auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_velocity2 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(2, 0)); + auto angular_velocity2 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_acceleration2 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(2, 0)); + position2->x() = 1.2; + position2->y() = 2.2; + position2->z() = 0.0; + orientation2->w() = 1.0; + orientation2->x() = 0.0; + orientation2->y() = 0.0; + orientation2->z() = 0.0; + linear_velocity2->x() = 0.0; + linear_velocity2->y() = 1.0; + linear_velocity2->z() = 0.0; + angular_velocity2->roll() = 0.0; + angular_velocity2->pitch() = 0.0; + angular_velocity2->yaw() = 0.0; + linear_acceleration2->x() = 0.0; + linear_acceleration2->y() = 1.0; + linear_acceleration2->z() = 0.0; + auto position3 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(3, 0)); + auto orientation3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_velocity3 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(3, 0)); + auto angular_velocity3 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_acceleration3 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(3, 0)); + position3->x() = 1.3; + position3->y() = 2.3; + position3->z() = 0.0; + orientation3->w() = 1.0; + orientation3->x() = 0.0; + orientation3->y() = 0.0; + orientation3->z() = 0.0; + linear_velocity3->x() = 4.3; + linear_velocity3->y() = 5.3; + linear_velocity3->z() = 0.0; + angular_velocity3->roll() = 0.0; + angular_velocity3->pitch() = 0.0; + angular_velocity3->yaw() = 0.0; + linear_acceleration3->x() = 7.3; + linear_acceleration3->y() = 8.3; + linear_acceleration3->z() = 0.0; + auto position4 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(4, 0)); + auto orientation4 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_velocity4 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(4, 0)); + auto angular_velocity4 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_acceleration4 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(4, 0)); + position4->x() = 1.4; + position4->y() = 2.4; + position4->z() = 0.0; + orientation4->w() = 1.0; + orientation4->x() = 0.0; + orientation4->y() = 0.0; + orientation4->z() = 0.0; + linear_velocity4->x() = 4.4; + linear_velocity4->y() = 5.4; + linear_velocity4->z() = 0.0; + angular_velocity4->roll() = 0.0; + angular_velocity4->pitch() = 0.0; + angular_velocity4->yaw() = 0.0; + linear_acceleration4->x() = 7.4; + linear_acceleration4->y() = 8.4; + linear_acceleration4->z() = 0.0; + auto position5 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(5, 0)); + auto orientation5 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_velocity5 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(5, 0)); + auto angular_velocity5 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_acceleration5 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(5, 0)); + position5->x() = 1.5; + position5->y() = 2.5; + position5->z() = 0.0; + orientation5->w() = 1.0; + orientation5->x() = 0.0; + orientation5->y() = 0.0; + orientation5->z() = 0.0; + linear_velocity5->x() = 4.5; + linear_velocity5->y() = 5.5; + linear_velocity5->z() = 0.0; + angular_velocity5->roll() = 0.0; + angular_velocity5->pitch() = 0.0; + angular_velocity5->yaw() = 0.0; + linear_acceleration5->x() = 7.5; + linear_acceleration5->y() = 8.5; + linear_acceleration5->z() = 0.0; + + // Add a subset of the variables to a graph + fuse_graphs::HashGraph graph; + graph.addVariable(position2); + graph.addVariable(orientation2); + graph.addVariable(linear_velocity2); + graph.addVariable(angular_velocity2); + graph.addVariable(linear_acceleration2); + + graph.addVariable(position4); + graph.addVariable(orientation4); + graph.addVariable(linear_velocity4); + graph.addVariable(angular_velocity4); + graph.addVariable(linear_acceleration4); + + // Add all of the variables to the state history + Omnidirectional3DModelTest::StateHistory state_history; + state_history.emplace( + position1->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position1->uuid(), + orientation1->uuid(), + linear_velocity1->uuid(), + angular_velocity1->uuid(), + linear_acceleration1->uuid(), + fuse_core::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position2->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position2->uuid(), + orientation2->uuid(), + linear_velocity2->uuid(), + angular_velocity2->uuid(), + linear_acceleration2->uuid(), + fuse_core::Vector3d(2.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position3->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position3->uuid(), + orientation3->uuid(), + linear_velocity3->uuid(), + angular_velocity3->uuid(), + linear_acceleration3->uuid(), + fuse_core::Vector3d(3.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position4->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position4->uuid(), + orientation4->uuid(), + linear_velocity4->uuid(), + angular_velocity4->uuid(), + linear_acceleration4->uuid(), + fuse_core::Vector3d(4.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position5->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position5->uuid(), + orientation5->uuid(), + linear_velocity5->uuid(), + angular_velocity5->uuid(), + linear_acceleration5->uuid(), + fuse_core::Vector3d(5.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + + // Update the state history + Omnidirectional3DModelTest::updateStateHistoryEstimates( + graph, state_history, rclcpp::Duration::from_seconds( + 10.0)); + + // Check the state estimates in the state history + { + // The first entry is missing from the graph. It will not get updated. + auto expected_position = fuse_core::Vector3d(1.0, 0.0, 0.0); + auto expected_orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + auto actual_position = state_history[rclcpp::Time(1, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(1, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(1, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(1, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(1, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The second entry is included in the graph. It will get updated directly. + auto expected_position = fuse_core::Vector3d(1.2, 2.2, 0.0); // <-- value in the Graph + auto expected_orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + auto actual_position = state_history[rclcpp::Time(2, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(2, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 1.0, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(2, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(2, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(0.0, 1.0, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(2, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The third entry is missing from the graph. It will get predicted from previous state. + auto expected_position = fuse_core::Vector3d(1.2, 3.7, 0.0); + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto actual_position = state_history[rclcpp::Time(3, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(3, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 2.0, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(3, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(3, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(0.0, 1.0, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(3, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The forth entry is included in the graph. It will get updated directly. + auto expected_position = fuse_core::Vector3d(1.4, 2.4, 0.0); // <-- value in the Graph + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto actual_position = state_history[rclcpp::Time(4, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(4, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(4.4, 5.4, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(4, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(4, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(7.4, 8.4, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(4, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The fifth entry is missing from the graph. It will get predicted from previous state. + auto expected_position = fuse_core::Vector3d(9.5, 12.0, 0.0); + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto actual_position = state_history[rclcpp::Time(5, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(5, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(11.8, 13.8, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(5, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(5, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(7.4, 8.4, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(5, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } +} diff --git a/fuse_models/test/test_omnidirectional_3d_predict.cpp b/fuse_models/test/test_omnidirectional_3d_predict.cpp new file mode 100644 index 000000000..230c0b8cc --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d_predict.cpp @@ -0,0 +1,651 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include + +#include +#include +#include + +TEST(Predict, predictDirectVals) +{ + fuse_core::Vector3d position1 (0.0, 0.0, 0.0); + Eigen::Quaterniond orientation1 (1.0, 0.0, 0.0, 0.0); + fuse_core::Vector3d vel_linear1 (1.0, 0.0, 0.0); + fuse_core::Vector3d vel_angular1 (0.0, 0.0, 1.570796327); + fuse_core::Vector3d acc_linear1 (1.0, 0.0, 0.0); + double dt = 0.1; + fuse_core::Vector3d position2; + Eigen::Quaterniond orientation2; + fuse_core::Vector3d vel_linear2; + fuse_core::Vector3d vel_angular2; + fuse_core::Vector3d acc_linear2; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.105, position2.x()); + EXPECT_DOUBLE_EQ(0.0, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); + EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); + EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); + EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); + EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // // Carry on with the output state from last time - show in-place update support + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); + EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); + EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); + EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); + EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); + EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // Use non-zero Y values + vel_linear1.y() = -1.0; + vel_angular1.z() = -1.570796327; + acc_linear1.y() = -1.0; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.105, position2.x()); + EXPECT_DOUBLE_EQ(-0.105, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_TRUE(q.isApprox(orientation2)); + EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // Out of plane motion + position1 = {0.0, 0.0, 0.0}; + orientation1 = {1.0, 0.0, 0.0, 0.0}; + vel_linear1 = {0.0, 0.0, 0.1}; + vel_angular1 = {1.570796327, 0.0, 0.0}; + acc_linear1 = {0.0, 0.0, 1.0}; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(0.0, position2.x()); + EXPECT_DOUBLE_EQ(0.0, position2.y()); + EXPECT_DOUBLE_EQ(0.015, position2.z()); + EXPECT_DOUBLE_EQ(0.99691733373232339, orientation2.w()); + EXPECT_DOUBLE_EQ(0.078459095738068516, orientation2.x()); + EXPECT_DOUBLE_EQ(0.0, orientation2.y()); + EXPECT_DOUBLE_EQ(0.0, orientation2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.2, vel_linear2.z()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.z()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.z()); + + // General 3D motion (these value are checked against rl predict() equations) + position1 = {0.0, 0.0, 0.0}; + orientation1 = {0.110, -0.003, -0.943, 0.314}; // RPY {-2.490, -0.206, 3.066} + vel_linear1 = {0.1, 0.2, 0.1}; + vel_angular1 = {1.570796327, 1.570796327, -1.570796327}; + acc_linear1 = {-0.5, 1.0, 1.0}; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(-0.012044123300410431, position2.x()); + EXPECT_DOUBLE_EQ(0.011755776496514461, position2.y()); + EXPECT_DOUBLE_EQ(-0.024959783911094033, position2.z()); + EXPECT_DOUBLE_EQ(0.20388993714859482, orientation2.w()); + EXPECT_DOUBLE_EQ(0.061993007799788086, orientation2.x()); + EXPECT_DOUBLE_EQ(-0.90147820778463239, orientation2.y()); + EXPECT_DOUBLE_EQ(0.3767264277999153, orientation2.z()); + EXPECT_DOUBLE_EQ(0.05, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.3, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.2, vel_linear2.z()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.x()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.y()); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(-0.5, acc_linear2.x()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.z()); +} + +TEST(Predict, predictFromDoublePointers) +{ + double position1[3] {0.0, 0.0, 0.0}; + double orientation1[3] {0.0, 0.0, 0.0}; + double vel_linear1[3] {1.0, 0.0, 0.0}; + double vel_angular1[3] {0.0, 0.0, 1.570796327}; + double acc_linear1[3] {1.0, 0.0, 0.0}; + double dt = 0.1; + double position2[3]; + double orientation2[3]; + double vel_linear2[3]; + double vel_angular2[3]; + double acc_linear2[3]; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(0.105, position2[0]); + EXPECT_DOUBLE_EQ(0.0, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(0.1570796327, orientation2[2]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + + // Carry on with the output state from last time - show in-place update support + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); + EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(0.3141592654, orientation2[2]); + EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + + // Use non-zero Y values + vel_linear1[1] = -1.0; + vel_angular1[2] = -1.570796327; + acc_linear1[1] = -1.0; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(0.105, position2[0]); + EXPECT_DOUBLE_EQ(-0.105,position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(-0.1570796327, orientation2[2]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(-1.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + + // Out of plane motion + position1[0] = 0.0; + position1[1] = 0.0; + position1[2] = 0.0; + orientation1[0] = 0.0; + orientation1[1] = 0.0; + orientation1[2] = 0.0; + vel_linear1[0] = 0.0; + vel_linear1[1] = 0.0; + vel_linear1[2] = 0.1; + vel_angular1[0] = 1.570796327; + vel_angular1[1] = 0.0; + vel_angular1[2] = 0.0; + acc_linear1[0] = 0.0; + acc_linear1[1] = 0.0; + acc_linear1[2] = 1.0; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(0.0, position2[0]); + EXPECT_DOUBLE_EQ(0.0, position2[1]); + EXPECT_DOUBLE_EQ(0.015, position2[2]); + EXPECT_DOUBLE_EQ(0.15707963270000003, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(0.0, orientation2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.2, vel_linear2[2]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[2]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[2]); + + // General 3D motion (these value are checked against rl predict() equations) + position1[0] = 0.0; + position1[1] = 0.0; + position1[2] = 0.0; + orientation1[0] = -2.490; + orientation1[1] = -0.206; + orientation1[2] = 3.066; + vel_linear1[0] = 0.1; + vel_linear1[1] = 0.2; + vel_linear1[2] = 0.1; + vel_angular1[0] = 1.570796327; + vel_angular1[1] = 1.570796327; + vel_angular1[2] = -1.570796327; + acc_linear1[0] = -0.5; + acc_linear1[1] = 1.0; + acc_linear1[2] = 1.0; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(-0.012031207341885572, position2[0]); + EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); + EXPECT_DOUBLE_EQ(-0.024981300126995967, position2[2]); + EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); + EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); + EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); + EXPECT_DOUBLE_EQ(0.05, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.3, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.2, vel_linear2[2]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[0]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[1]); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(-0.5, acc_linear2[0]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[2]); +} + +TEST(Predict, predictFromJetPointers) +{ + using Jet = ceres::Jet; + + Jet position1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; + Jet orientation1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; + Jet vel_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; + Jet vel_angular1[3] = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; + Jet acc_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; + Jet dt = Jet(0.1); + Jet position2[3]; + Jet orientation2[3]; + Jet vel_linear2[3]; + Jet vel_angular2[3]; + Jet acc_linear2[3]; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.1570796327).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + + // Carry on with the output state from last time - show in-place update support + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.3141592654).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + + // // Use non-zero Y values + vel_linear1[1] = Jet(-1.0); + vel_angular1[2] = Jet(-1.570796327); + acc_linear1[1] = Jet(-1.0); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(-0.1570796327).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + +// Out of plane motion + position1[0] = Jet(0.0); + position1[1] = Jet(0.0); + position1[2] = Jet(0.0); + orientation1[0] = Jet(0.0); + orientation1[1] = Jet(0.0); + orientation1[2] = Jet(0.0); + vel_linear1[0] = Jet(0.0); + vel_linear1[1] = Jet(0.0); + vel_linear1[2] = Jet(0.1); + vel_angular1[0] = Jet(1.570796327); + vel_angular1[1] = Jet(0.0); + vel_angular1[2] = Jet(0.0); + acc_linear1[0] = Jet(0.0); + acc_linear1[1] = Jet(0.0); + acc_linear1[2] = Jet(1.0); + dt = Jet(0.1); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.015).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.15707963270000003).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.2).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); + + // General 3D motion (these value are checked against rl predict() equations) + position1[0] = Jet(0.0); + position1[1] = Jet(0.0); + position1[2] = Jet(0.0); + orientation1[0] = Jet(-2.490); + orientation1[1] = Jet(-0.206); + orientation1[2] = Jet(3.066); + vel_linear1[0] = Jet(0.1); + vel_linear1[1] = Jet(0.2); + vel_linear1[2] = Jet(0.1); + vel_angular1[0] = Jet(1.570796327); + vel_angular1[1] = Jet(1.570796327); + vel_angular1[2] = Jet(-1.570796327); + acc_linear1[0] = Jet(-0.5); + acc_linear1[1] = Jet(1.0); + acc_linear1[2] = Jet(1.0); + dt = Jet(0.1); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(Jet(-0.012031207341885572).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(-0.024981300126995967).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.05).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.3).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.2).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(-0.5).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); +} \ No newline at end of file diff --git a/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp new file mode 100644 index 000000000..bea153f97 --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp @@ -0,0 +1,147 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +TEST(CostFunction, evaluateCostFunction) +{ + // Create cost function + const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3, 1e-3, 1e-3}; + const fuse_core::Matrix15d covariance = fuse_core::Vector15d(process_noise_diagonal).asDiagonal(); + + const double dt{0.1}; + const fuse_core::Matrix15d sqrt_information{covariance.inverse().llt().matrixU()}; + + const fuse_models::Omnidirectional3DStateCostFunction cost_function{dt, sqrt_information}; + + // Evaluate cost function + const double position1[3] = {0.0, 0.0, 0.0}; + const double orientation1[4] = {1.0, 0.0, 0.0, 0.0}; + const double vel_linear1[3] = {1.0, 1.0, 1.0}; + const double vel_angular1[3] = {1.570796327, 1.570796327, 1.570796327}; + const double acc_linear1[3] = {1.0, 1.0, 1.0}; + + const double position2[3] = {0.105, 0.105, 0.105}; + Eigen::Quaterniond q2 = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitX()); + const double orientation2[4] = {q2.w(), q2.x(), q2.y(), q2.z()}; + const double vel_linear2[3] = {1.1, 1.1, 1.1}; + const double vel_angular2[3] = {1.570796327, 1.570796327, 1.570796327}; + const double acc_linear2[3] = {1.0, 1.0, 1.0}; + + const double * parameters[10] = + { + position1, orientation1, vel_linear1, vel_angular1, acc_linear1, + position2, orientation2, vel_linear2, vel_angular2, acc_linear2 + }; + + fuse_core::Vector15d residuals; + + const auto & block_sizes = cost_function.parameter_block_sizes(); + const auto num_parameter_blocks = block_sizes.size(); + + const auto num_residuals = cost_function.num_residuals(); + + std::vector J(num_parameter_blocks); + std::vector jacobians(num_parameter_blocks); + + for (size_t i = 0; i < num_parameter_blocks; ++i) { + J[i].resize(num_residuals, block_sizes[i]); + jacobians[i] = J[i].data(); + } + + EXPECT_TRUE(cost_function.Evaluate(parameters, residuals.data(), jacobians.data())); + + // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 + // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 + EXPECT_MATRIX_NEAR(fuse_core::Vector15d::Zero(), residuals, 1e-15); + +// // Check jacobians are correct using a gradient checker + ceres::NumericDiffOptions numeric_diff_options; + +#if CERES_VERSION_AT_LEAST(2, 1, 0) + std::vector parameterizations; + ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); +#else + ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); +#endif + + // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error + // is 5.26356e-10 + ceres::GradientChecker::ProbeResults probe_results; + // TODO(efernandez) probe_results segfaults when it's destroyed at the end of this TEST function, + // but Probe actually returns true and the jacobians are correct according to the + // gradient checker numeric differentiation + // EXPECT_TRUE(gradient_checker.Probe(parameters, 1e-9, &probe_results)) << + // probe_results.error_log; + + // Create cost function using automatic differentiation on the cost functor + ceres::AutoDiffCostFunctioncost_function_autodiff(new fuse_models::Omnidirectional3DStateCostFunctor(dt, sqrt_information)); + // Evaluate cost function that uses automatic differentiation + std::vector J_autodiff(num_parameter_blocks); + std::vector jacobians_autodiff(num_parameter_blocks); + + for (size_t i = 0; i < num_parameter_blocks; ++i) { + J_autodiff[i].resize(num_residuals, cost_function_autodiff.parameter_block_sizes()[i]); + jacobians_autodiff[i] = J_autodiff[i].data(); + } + + EXPECT_TRUE( + cost_function_autodiff.Evaluate( + parameters, residuals.data(), + jacobians_autodiff.data())); + + const Eigen::IOFormat HeavyFmt( + Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + + for (size_t i = 0; i < num_parameter_blocks; ++i) { + EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-4) + << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) + << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + } +} diff --git a/fuse_models/test/test_sensor_proc_3d.cpp b/fuse_models/test/test_sensor_proc_3d.cpp new file mode 100644 index 000000000..c71d24bf6 --- /dev/null +++ b/fuse_models/test/test_sensor_proc_3d.cpp @@ -0,0 +1,150 @@ +/*************************************************************************** + * Copyright (C) 2024 Giacomo Franchini. All rights reserved. + * Unauthorized copying of this file, via any medium, is strictly prohibited + * Proprietary and confidential + ***************************************************************************/ +#include +#include + +#include + +#include + +namespace fm_common = fuse_models::common; +TEST(TestSuite, mergeFullPositionAndFullOrientationIndices) +{ + const std::vector position_indices{0, 1, 2}; + const std::vector orientation_indices{0, 1, 2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); + EXPECT_THAT( + position_indices, + testing::ElementsAreArray( + merged_indices.begin(), + merged_indices.begin() + position_indices.size())); + EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); +} + +TEST(TestSuite, mergeXYPositionAndRollYawOrientationIndices) +{ + const std::vector position_indices{0, 1}; + const std::vector orientation_indices{0, 2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); + EXPECT_THAT( + position_indices, + testing::ElementsAreArray( + merged_indices.begin(), + merged_indices.begin() + position_indices.size())); + EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); +} + +TEST(TestSuite, mergeFullPositionAndEmptyOrientationIndices) +{ + const std::vector position_indices{0, 1, 2}; + const std::vector orientation_indices; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(position_indices.size(), merged_indices.size()); + EXPECT_THAT(position_indices, testing::ElementsAreArray(merged_indices)); +} + +TEST(TestSuite, mergeEmptyPositionAndFullOrientationIndices) +{ + const std::vector position_indices; + const std::vector orientation_indices{0, 1, 2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(orientation_indices.size(), merged_indices.size()); + EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); +} + +TEST(TestSuite, mergeEmptyPositionAndEmptyOrientationIndices) +{ + const std::vector position_indices; + const std::vector orientation_indices; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_TRUE(merged_indices.empty()); +} + +TEST(TestSuite, populatePartialMeasurements) +{ + // Test both conversion from quaternion to RPY and partial measurement population + // This one is just to generate a random unit quaternion and have the reference in RPY + fuse_core::Vector3d rpy = fuse_core::Vector3d::Random(); + Eigen::Quaterniond q = + Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); + + tf2::Transform tf2_pose; + tf2_pose.setOrigin(tf2::Vector3(1.0, 2.0, 3.0)); + tf2_pose.setRotation(tf2::Quaternion(q.x(), q.y(), q.z(), q.w())); + fuse_core::Vector6d pose_mean_partial; + pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean_partial(3), pose_mean_partial(4), pose_mean_partial(5)); + + fuse_core::Matrix6d pose_covariance = fuse_core::Matrix6d::Random(); + + const std::vector position_indices{0, 1}; + const std::vector orientation_indices{2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + std::replace_if( + pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), + [&merged_indices, &pose_mean_partial](const double & value) { + return std::find( + merged_indices.begin(), + merged_indices.end(), + &value - pose_mean_partial.data()) == merged_indices.end(); + }, 0.0); + + fuse_core::MatrixXd pose_covariance_partial(3, 3); + + fm_common::populatePartialMeasurement( + pose_covariance, + merged_indices, + pose_covariance_partial); + + fuse_core::Vector6d expected_pose; + expected_pose << 1.0, 2.0, 0.0, 0.0, 0.0, rpy(2); + fuse_core::Matrix3d expected_covariance; + expected_covariance.col(0) << pose_covariance(0, 0), pose_covariance(1, 0), pose_covariance(5, 0); + expected_covariance.col(1) << pose_covariance(0, 1), pose_covariance(1, 1), pose_covariance(5, 1); + expected_covariance.col(2) << pose_covariance(0, 5), pose_covariance(1, 5), pose_covariance(5, 5); + EXPECT_TRUE(expected_pose.isApprox(pose_mean_partial)); + EXPECT_TRUE(expected_covariance.isApprox(pose_covariance_partial)); +} diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 87de2e8fe..0955e5ec0 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -252,6 +252,8 @@ void FixedLagSmoother::optimizationLoop() // Optimize the entire graph summary_ = graph_->optimize(params_.solver_options); + // std::cout << summary_.FullReport() << std::endl; + // Optimization is complete. Notify all the things about the graph changes. const auto new_transaction_stamp = new_transaction->stamp(); notify(std::move(new_transaction), graph_->clone()); @@ -399,7 +401,6 @@ void FixedLagSmoother::processQueue( return; } } - // Use the most recent transaction time as the current time const auto current_time = pending_transactions_.front().stamp(); From 07732073f4e9b5ea4dc2703c0287f26969c7148b Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 24 Sep 2024 18:31:07 +0000 Subject: [PATCH 05/46] apply pre-commit --- fuse.repos | 2 +- ...olute_pose_3d_stamped_euler_constraint.hpp | 53 +- ...ormal_delta_pose_3d_euler_cost_functor.hpp | 62 +-- .../normal_prior_orientation_3d.hpp | 8 +- .../fuse_constraints/normal_prior_pose_3d.hpp | 10 +- .../normal_prior_pose_3d_euler.hpp | 10 +- ...ormal_prior_pose_3d_euler_cost_functor.hpp | 30 +- ...ative_pose_3d_stamped_euler_constraint.hpp | 60 +-- fuse_constraints/src/absolute_constraint.cpp | 44 +- ...olute_pose_3d_stamped_euler_constraint.cpp | 50 +- .../src/normal_prior_orientation_3d.cpp | 31 +- fuse_constraints/src/normal_prior_pose_3d.cpp | 38 +- .../src/normal_prior_pose_3d_euler.cpp | 24 +- ...ative_pose_3d_stamped_euler_constraint.cpp | 48 +- ...olute_pose_3d_stamped_euler_constraint.cpp | 298 +++++------- .../test/test_normal_prior_pose_3d.cpp | 27 +- .../test/test_normal_prior_pose_3d_euler.cpp | 69 ++- ...ative_pose_3d_stamped_euler_constraint.cpp | 405 ++++++---------- fuse_core/test/test_util.cpp | 123 +++-- fuse_models/include/fuse_models/imu_3d.hpp | 34 +- .../include/fuse_models/odometry_3d.hpp | 34 +- .../fuse_models/odometry_3d_publisher.hpp | 66 +-- .../fuse_models/omnidirectional_3d.hpp | 92 ++-- .../omnidirectional_3d_ignition.hpp | 72 ++- .../omnidirectional_3d_predict.hpp | 449 ++++++----------- ...omnidirectional_3d_state_cost_function.hpp | 113 ++--- .../omnidirectional_3d_state_cost_functor.hpp | 95 ++-- ...ectional_3d_state_kinematic_constraint.hpp | 58 +-- .../fuse_models/parameters/imu_3d_params.hpp | 188 +++----- .../parameters/odometry_3d_params.hpp | 169 ++----- .../odometry_3d_publisher_params.hpp | 224 +++------ .../omnidirectional_3d_ignition_params.hpp | 130 ++--- fuse_models/src/imu_3d.cpp | 210 +++----- fuse_models/src/odometry_3d.cpp | 189 +++----- fuse_models/src/odometry_3d_publisher.cpp | 356 +++++++------- fuse_models/src/omnidirectional_3d.cpp | 456 ++++++++---------- .../src/omnidirectional_3d_ignition.cpp | 339 ++++++------- ...ectional_3d_state_kinematic_constraint.cpp | 46 +- fuse_models/test/test_omnidirectional_3d.cpp | 157 +++--- .../test/test_omnidirectional_3d_predict.cpp | 403 +++++----------- ...omnidirectional_3d_state_cost_function.cpp | 74 ++- fuse_models/test/test_sensor_proc_3d.cpp | 85 ++-- 42 files changed, 2117 insertions(+), 3314 deletions(-) diff --git a/fuse.repos b/fuse.repos index 8ad27e25b..d569f4ec3 100644 --- a/fuse.repos +++ b/fuse.repos @@ -6,4 +6,4 @@ repositories: covariance_geometry_ros: type: git url: https://github.com/giafranchini/covariance_geometry_ros.git - version: iron \ No newline at end of file + version: iron diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp index 63c2d8502..e367e1742 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -32,7 +32,6 @@ * POSSIBILITY OF SUCH DAMAGE. */ - #ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ #define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ @@ -54,13 +53,12 @@ #include #include - namespace fuse_constraints { /** * @brief A constraint that represents either prior information about a 3D pose, or a direct - * measurement of the 3D pose. + * measurement of the 3D pose. * * A 3D pose is the combination of a 3D position and a 3D orientation variable. As a convenience, * this class applies an absolute constraint on both variables at once. This type of constraint @@ -92,13 +90,10 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * (6x1 vector: x, y, z, roll, pitch, yaw) * @param[in] covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) */ - AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector6d & mean, - const fuse_core::Matrix6d & covariance); - + AbsolutePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector6d& mean, const fuse_core::Matrix6d& covariance); + /** * @brief Create a constraint using a partial measurement/prior of the 3D pose * @@ -110,13 +105,11 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * @param[in] partial_covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) * @param[in] variable_indices The indices of the measured variables */ - AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector6d & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices); + AbsolutePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::Vector6d& partial_mean, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& variable_indices); /** * @brief Destructor @@ -128,14 +121,20 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * * Order is (x, y, z, roll, pitch, yaw) */ - const fuse_core::Vector6d & mean() const {return mean_;} + const fuse_core::Vector6d& mean() const + { + return mean_; + } /** * @brief Read-only access to the square root information matrix. * * Order is (x, y, z, roll, pitch, yaw) */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -149,7 +148,7 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -161,10 +160,10 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector6d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Vector6d mean_; //!< The measured/prior mean vector for this variable fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix private: @@ -178,12 +177,12 @@ class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & mean_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& mean_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp index afb16217d..4c1201650 100644 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -41,7 +41,6 @@ #include #include - namespace fuse_constraints { @@ -76,62 +75,42 @@ class NormalDeltaPose3DEulerCostFunctor * order (dx, dy, dz, droll, dpitch, dyaw) * @param[in] b The exposed pose difference in order (dx, dy, dz, droll, dpitch, dyaw) */ - NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector6d& b); /** * @brief Compute the cost values/residuals using the provided variable/parameter values */ - template - bool operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const; + template + bool operator()(const T* const position1, const T* const orientation1, const T* const position2, + const T* const orientation2, T* residual) const; private: fuse_core::MatrixXd A_; fuse_core::Vector6d b_; }; -NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector6d & b) -: A_(A), - b_(b) +NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, + const fuse_core::Vector6d& b) + : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 6); } -template -bool NormalDeltaPose3DEulerCostFunctor::operator()( - const T * const position1, - const T * const orientation1, - const T * const position2, - const T * const orientation2, - T * residual) const +template +bool NormalDeltaPose3DEulerCostFunctor::operator()(const T* const position1, const T* const orientation1, + const T* const position2, const T* const orientation2, + T* residual) const { T full_residuals[6]; T position_delta_rotated[3]; T orientation_delta[4]; T orientation_delta_rpy[3]; - T orientation1_inverse[4] - { - orientation1[0], - -orientation1[1], - -orientation1[2], - -orientation1[3] - }; - - T position_delta[3] - { - position2[0] - position1[0], - position2[1] - position1[1], - position2[2] - position1[2] - }; - + T orientation1_inverse[4]{ orientation1[0], -orientation1[1], -orientation1[2], -orientation1[3] }; + + T position_delta[3]{ position2[0] - position1[0], position2[1] - position1[1], position2[2] - position1[2] }; + // Compute the position residual ceres::QuaternionRotatePoint(orientation1_inverse, position_delta, position_delta_rotated); full_residuals[0] = position_delta_rotated[0] - T(b_(0)); @@ -140,9 +119,12 @@ bool NormalDeltaPose3DEulerCostFunctor::operator()( // Compute the orientation residual ceres::QuaternionProduct(orientation1_inverse, orientation2, orientation_delta); - orientation_delta_rpy[0] = fuse_core::getRoll(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); - orientation_delta_rpy[1] = fuse_core::getPitch(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); - orientation_delta_rpy[2] = fuse_core::getYaw(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[0] = + fuse_core::getRoll(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[1] = + fuse_core::getPitch(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); + orientation_delta_rpy[2] = + fuse_core::getYaw(orientation_delta[0], orientation_delta[1], orientation_delta[2], orientation_delta[3]); full_residuals[3] = orientation_delta_rpy[0] - T(b_(3)); full_residuals[4] = orientation_delta_rpy[1] - T(b_(4)); full_residuals[5] = orientation_delta_rpy[2] - T(b_(5)); @@ -158,4 +140,4 @@ bool NormalDeltaPose3DEulerCostFunctor::operator()( } // namespace fuse_constraints -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ \ No newline at end of file +#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp index 4e6c4b60e..bcb2ddb8f 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -73,7 +72,7 @@ class NormalPriorOrientation3D : public ceres::SizedCostFunction<3, 4> * order (qx, qy, qz) * @param[in] b The orientation measurement or prior in order (qw, qx, qy, qz) */ - NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b); + NormalPriorOrientation3D(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b); /** * @brief Destructor @@ -84,10 +83,7 @@ class NormalPriorOrientation3D : public ceres::SizedCostFunction<3, 4> * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp index 7ec76458c..8088539d7 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -39,7 +39,6 @@ #include - namespace fuse_constraints { @@ -55,7 +54,7 @@ namespace fuse_constraints * cost(x) = || A * [ y - b(1)] || * || [ z - b(2)] || * || [ quat2angleaxis(b(3-6)^-1 * q)] || - * + * * In case the user is interested in implementing a cost function of the form: * * cost(X) = (X - mu)^T S^{-1} (X - mu) @@ -73,7 +72,7 @@ class NormalPriorPose3D : public ceres::SizedCostFunction<6, 3, 4> * order (x, y, z, roll, pitch, yaw) * @param[in] b The pose measurement or prior in order (x, y, z, qw, qx, qy, qz) */ - NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + NormalPriorPose3D(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b); /** * @brief Destructor @@ -84,10 +83,7 @@ class NormalPriorPose3D : public ceres::SizedCostFunction<6, 3, 4> * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided * variable/parameter values */ - virtual bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const; + virtual bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const; private: fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp index 0579b30c7..723fc3fbd 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -38,7 +38,6 @@ #include - namespace fuse_constraints { @@ -54,7 +53,7 @@ namespace fuse_constraints * cost(x) = || A * [ y - b(1)] || * || [ z - b(2)] || * || [ quat2eul(q) - b(3:5) ] || - * + * * Here, the matrix A can be of variable size, thereby permitting the computation of errors for * partial measurements. The vector b is a fixed-size 6x1. In case the user is interested in * implementing a cost function of the form: @@ -79,7 +78,7 @@ class NormalPriorPose3DEuler : public ceres::SizedCostFunction #include - namespace fuse_constraints { @@ -76,40 +75,35 @@ class NormalPriorPose3DEulerCostFunctor * order (x, y, z, roll, pitch, yaw) * @param[in] b The 3D pose measurement or prior in order (x, y, z, roll, pitch, yaw) */ - NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector6d& b); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. */ - template - bool operator()(const T * const position, const T * const orientation, T * residual) const; + template + bool operator()(const T* const position, const T* const orientation, T* residual) const; private: fuse_core::MatrixXd A_; fuse_core::Vector6d b_; }; -NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor( - const fuse_core::MatrixXd & A, - const fuse_core::Vector6d & b) -: A_(A), - b_(b) +NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd& A, + const fuse_core::Vector6d& b) + : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 6); } -template -bool NormalPriorPose3DEulerCostFunctor::operator()( - const T * const position, const T * const orientation, - T * residual) const +template +bool NormalPriorPose3DEulerCostFunctor::operator()(const T* const position, const T* const orientation, + T* residual) const { T full_residuals[6]; - T orientation_rpy[3] = { - fuse_core::getRoll(orientation[0], orientation[1], orientation[2], orientation[3]), - fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]), - fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]) - }; + T orientation_rpy[3] = { fuse_core::getRoll(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]) }; // Compute the position residual full_residuals[0] = position[0] - T(b_(0)); diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp index a5f2fdf9d..ab59f7915 100644 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp @@ -53,7 +53,6 @@ #include #include - namespace fuse_constraints { @@ -86,15 +85,12 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, droll, dpitch, dyaw) */ - RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector6d & delta, - const fuse_core::Matrix6d & covariance); - + RelativePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector6d& delta, const fuse_core::Matrix6d& covariance); + /** * @brief Create a constraint using a measurement/prior of the relative 3D pose * @@ -107,16 +103,14 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, droll, dpitch, dyaw) * @param[in] variable_indices The indices of the measured variables - */ - RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector6d & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices); + */ + RelativePose3DStampedEulerConstraint(const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_core::Vector6d& partial_delta, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& variable_indices); /** * @brief Destructor @@ -126,12 +120,18 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint /** * @brief Read-only access to the measured pose change. */ - const fuse_core::Vector6d & delta() const {return delta_;} + const fuse_core::Vector6d& delta() const + { + return delta_; + } /** * @brief Read-only access to the square root information matrix. */ - const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + const fuse_core::MatrixXd& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. @@ -143,7 +143,7 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Access the cost function for this constraint @@ -155,10 +155,10 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - fuse_core::Vector6d delta_; //!< The measured pose change (dx, dy, dz, droll, dpitch, dyaw) + fuse_core::Vector6d delta_; //!< The measured pose change (dx, dy, dz, droll, dpitch, dyaw) fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the //!< covariance matrix) @@ -173,12 +173,12 @@ class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & delta_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& delta_; + archive& sqrt_information_; } }; diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index bf4ad6519..26c223a36 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -47,36 +47,14 @@ BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular3DStampedC BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteOrientation2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsolutePosition2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsolutePosition3DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, - fuse_core::Constraint); -PLUGINLIB_EXPORT_CLASS( - fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint, - fuse_core::Constraint); \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteOrientation2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp index 56206e301..e3bec0ff4 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -45,27 +45,24 @@ namespace fuse_constraints { AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector6d & mean, - const fuse_core::Matrix6d & covariance) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT - mean_(mean), - sqrt_information_(covariance.inverse().llt().matrixU()) -{ + const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, const fuse_core::Vector6d& mean, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) + , // NOLINT + mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) +{ } AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position, - const fuse_variables::Orientation3DStamped & orientation, - const fuse_core::Vector6d & partial_mean, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices) -: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT + const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, const fuse_core::Vector6d& partial_mean, + const fuse_core::MatrixXd& partial_covariance, const std::vector& variable_indices) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid() }) + , // NOLINT mean_(partial_mean) -{ +{ // Compute the partial sqrt information matrix of the provided cov matrix fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); @@ -76,8 +73,8 @@ AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( // dimensions. But the variable vectors will be full sized. We can make this all work out by // creating a non-square A, where each row computes a cost for one measured dimensions, // and the columns are in the order defined by the variable. - - // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); for (size_t i = 0; i < variable_indices.size(); ++i) { @@ -103,7 +100,7 @@ fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const return pinv * pinv.transpose(); } -void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const +void AbsolutePose3DStampedEulerConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -113,23 +110,24 @@ void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const << " mean: " << mean().transpose() << "\n" << " sqrt_info: " << sqrtInformation() << "\n"; - if (loss()) { + if (loss()) + { stream << " loss: "; loss()->print(stream); } } -ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const +ceres::CostFunction* AbsolutePose3DStampedEulerConstraint::costFunction() const { return new NormalPriorPose3DEuler(sqrt_information_, mean_); - + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could // use automatic differentiation as follows: // return new ceres::AutoDiffCostFunction( - // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), - // sqrt_information_.rows()); - + // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), + // sqrt_information_.rows()); + // And including the followings: // #include // #include diff --git a/fuse_constraints/src/normal_prior_orientation_3d.cpp b/fuse_constraints/src/normal_prior_orientation_3d.cpp index 34269cedc..c0537dc24 100644 --- a/fuse_constraints/src/normal_prior_orientation_3d.cpp +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -40,32 +40,21 @@ namespace fuse_constraints { -NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b) -: A_(A), - b_(b) +NormalPriorOrientation3D::NormalPriorOrientation3D(const fuse_core::Matrix3d& A, const fuse_core::Vector4d& b) + : A_(A), b_(b) { } -bool NormalPriorOrientation3D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorOrientation3D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { - double variable[4] = - { + double variable[4] = { parameters[0][0], parameters[0][1], parameters[0][2], parameters[0][3], }; - double observation_inverse[4] = - { - b_(0), - -b_(1), - -b_(2), - -b_(3) - }; + double observation_inverse[4] = { b_(0), -b_(1), -b_(2), -b_(3) }; double difference[4]; double j_product[16]; @@ -73,15 +62,17 @@ bool NormalPriorOrientation3D::Evaluate( // TODO(giafranchini): these jacobians should be populated only if jacobians[1] != nullptr fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); - fuse_core::quaternionToAngleAxis(difference, residuals, j_quat2angle); // orientation angle-axis - + fuse_core::quaternionToAngleAxis(difference, residuals, j_quat2angle); // orientation angle-axis + // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. Eigen::Map residuals_map(residuals); residuals_map.applyOnTheLeft(A_); - if (jacobians != nullptr) { - if (jacobians[0] != nullptr) { + if (jacobians != nullptr) + { + if (jacobians[0] != nullptr) + { Eigen::Map> j_map(jacobians[0]); Eigen::Map j_product_map(j_product); Eigen::Map> j_quat2angle_map(j_quat2angle); diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp index 581e455dd..4fa0864a3 100644 --- a/fuse_constraints/src/normal_prior_pose_3d.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -40,33 +40,20 @@ namespace fuse_constraints { -NormalPriorPose3D::NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b) -: A_(A), - b_(b) +NormalPriorPose3D::NormalPriorPose3D(const fuse_core::Matrix6d& A, const fuse_core::Vector7d& b) : A_(A), b_(b) { } -bool NormalPriorPose3D::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorPose3D::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { - - double variable[4] = - { + double variable[4] = { parameters[1][0], parameters[1][1], parameters[1][2], parameters[1][3], }; - double observation_inverse[4] = - { - b_(3), - -b_(4), - -b_(5), - -b_(6) - }; + double observation_inverse[4] = { b_(3), -b_(4), -b_(5), -b_(6) }; double difference[4]; double j_product[16]; @@ -78,23 +65,26 @@ bool NormalPriorPose3D::Evaluate( residuals[2] = parameters[0][2] - b_[2]; // Compute orientation residuals - // TODO(giafranchini): this is repeated code, it should be better to include normal_prior_orientation_3d.hpp - // and use that to compute residuals and jacobians. + // TODO(giafranchini): this is repeated code, it should be better to include normal_prior_orientation_3d.hpp + // and use that to compute residuals and jacobians. fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); - fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis - + fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis + // Scale the residuals by the square root information matrix to account for the measurement // uncertainty. Eigen::Map residuals_map(residuals); residuals_map.applyOnTheLeft(A_); - if (jacobians != nullptr) { + if (jacobians != nullptr) + { // Jacobian of the residuals wrt position parameter block - if (jacobians[0] != nullptr) { + if (jacobians[0] != nullptr) + { Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); } // Jacobian of the residuals wrt orientation parameter block - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { Eigen::Map j_product_map(j_product); Eigen::Map> j_quat2angle_map(j_quat2angle); Eigen::Map j1_map(jacobians[1], num_residuals(), 4); diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp index c4871aa3b..098168027 100644 --- a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -42,19 +42,15 @@ namespace fuse_constraints { -NormalPriorPose3DEuler::NormalPriorPose3DEuler(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b) -: A_(A), - b_(b) +NormalPriorPose3DEuler::NormalPriorPose3DEuler(const fuse_core::MatrixXd& A, const fuse_core::Vector6d& b) + : A_(A), b_(b) { CHECK_GT(A_.rows(), 0); CHECK_EQ(A_.cols(), 6); set_num_residuals(A_.rows()); } -bool NormalPriorPose3DEuler::Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const +bool NormalPriorPose3DEuler::Evaluate(double const* const* parameters, double* residuals, double** jacobians) const { fuse_core::Vector6d full_residuals; double orientation_rpy[3]; @@ -74,19 +70,21 @@ bool NormalPriorPose3DEuler::Evaluate( // Scale the residuals by the square root information matrix to account for // the measurement uncertainty. - Eigen::Map> residuals_map(residuals, A_.rows()); + Eigen::Map> residuals_map(residuals, A_.rows()); residuals_map = A_ * full_residuals; - if (jacobians != nullptr) { + if (jacobians != nullptr) + { // Jacobian wrt position - if (jacobians[0] != nullptr) { + if (jacobians[0] != nullptr) + { Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); } // Jacobian wrt orientation - if (jacobians[1] != nullptr) { + if (jacobians[1] != nullptr) + { Eigen::Map> j_quat2angle_map(j_quat2rpy); - Eigen::Map(jacobians[1], num_residuals(), 4) = - A_.rightCols<3>() * j_quat2angle_map; + Eigen::Map(jacobians[1], num_residuals(), 4) = A_.rightCols<3>() * j_quat2angle_map; } } return true; diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp index 0c4e7eee3..91de1542d 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -44,33 +44,24 @@ namespace fuse_constraints { RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector6d & delta, - const fuse_core::Matrix6d & covariance) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT - delta_(delta), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector6d& delta, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) + , // NOLINT + delta_(delta) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_core::Vector6d & partial_delta, - const fuse_core::MatrixXd & partial_covariance, - const std::vector & variable_indices) -: fuse_core::Constraint( - source, - {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT + const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, const fuse_core::Vector6d& partial_delta, + const fuse_core::MatrixXd& partial_covariance, const std::vector& variable_indices) + : fuse_core::Constraint(source, { position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid() }) + , // NOLINT delta_(partial_delta) { // Compute the partial sqrt information matrix of the provided cov matrix @@ -83,8 +74,8 @@ RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( // dimensions. But the variable vectors will be full sized. We can make this all work out by // creating a non-square A, where each row computes a cost for one measured dimensions, // and the columns are in the order defined by the variable. - - // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); for (size_t i = 0; i < variable_indices.size(); ++i) { @@ -106,7 +97,7 @@ fuse_core::Matrix6d RelativePose3DStampedEulerConstraint::covariance() const return pinv * pinv.transpose(); } -void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const +void RelativePose3DStampedEulerConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -119,12 +110,11 @@ void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * RelativePose3DStampedEulerConstraint::costFunction() const +ceres::CostFunction* RelativePose3DStampedEulerConstraint::costFunction() const { // TODO(giafranchini): implement cost function with analytical Jacobians return new ceres::AutoDiffCostFunction( - new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_), - sqrt_information_.rows()); + new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_), sqrt_information_.rows()); } } // namespace fuse_constraints diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp index 4e0b0457a..43b92de91 100644 --- a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -47,17 +47,15 @@ #include #include +using fuse_constraints::AbsolutePose3DStampedEulerConstraint; using fuse_variables::Orientation3DStamped; using fuse_variables::Position3DStamped; -using fuse_constraints::AbsolutePose3DStampedEulerConstraint; - TEST(AbsolutePose3DStampedEulerConstraint, Constructor) { // Construct a constraint just to make sure it compiles. Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector6d mean; mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; @@ -66,53 +64,53 @@ TEST(AbsolutePose3DStampedEulerConstraint, Constructor) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ EXPECT_NO_THROW( - AbsolutePose3DStampedEulerConstraint constraint( - "test", position_variable, orientation_variable, - mean, cov)); + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, cov)); } TEST(AbsolutePose3DStampedEulerConstraint, ConstructorPartial) { // Construct a constraint just to make sure it compiles. Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - std::vector variable_indices {0, 2, 3, 4, 5}; + std::vector variable_indices{ 0, 2, 3, 4, 5 }; fuse_core::Vector6d mean_partial; mean_partial << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; fuse_core::Matrix5d cov_partial; /* *INDENT-OFF* */ - cov_partial << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov_partial << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - EXPECT_NO_THROW( - AbsolutePose3DStampedEulerConstraint constraint( - "test", position_variable, orientation_variable, - mean_partial, cov_partial, variable_indices)); + EXPECT_NO_THROW(AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, + mean_partial, cov_partial, variable_indices)); } TEST(AbsolutePose3DStampedEulerConstraint, Covariance) { // Verify the covariance <--> sqrt information conversions are correct Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); fuse_core::Vector6d mean; mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; @@ -121,26 +119,32 @@ TEST(AbsolutePose3DStampedEulerConstraint, Covariance) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix6d expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT - 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT - 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT - 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, + -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov = cov; @@ -153,10 +157,9 @@ TEST(AbsolutePose3DStampedEulerConstraint, CovariancePartial) { // Verify the covariance <--> sqrt information conversions are correct Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( - "mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); - std::vector variable_indices {0, 2, 3, 4, 5}; + std::vector variable_indices{ 0, 2, 3, 4, 5 }; fuse_core::Vector6d mean; mean << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; @@ -165,34 +168,35 @@ TEST(AbsolutePose3DStampedEulerConstraint, CovariancePartial) // required precision) fuse_core::Matrix5d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, - mean, cov, variable_indices); + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, mean, cov, + variable_indices); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, // NOLINT - 0. , 0.0, 3.82674687, 2.80341172 , -2.68168479, -2.88943844, // NOLINT - 0. , 0.0, 0. , 1.83006791 , -0.69691741, -1.17412835, // NOLINT - 0. , 0.0, 0. , 0. , 0.95330283, -0.76965441, // NOLINT - 0. , 0.0, 0. , 0. , 0. , 0.68147774; // NOLINT + expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, // NOLINT + 0., 0.0, 3.82674687, 2.80341172, -2.68168479, -2.88943844, // NOLINT + 0., 0.0, 0., 1.83006791, -0.69691741, -1.17412835, // NOLINT + 0., 0.0, 0., 0., 0.95330283, -0.76965441, // NOLINT + 0., 0.0, 0., 0., 0., 0.68147774; // NOLINT /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov; /* *INDENT-OFF* */ - expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT - 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ // Compare @@ -204,14 +208,12 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables - auto position_variable = Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = -3.0; position_variable->z() = 10.0; - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; @@ -223,51 +225,36 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, - 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, - 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, 0.3, 0.5, 0.2, 4.0, + 0.3, 0.4, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; /* *INDENT-ON* */ - auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean, - cov); + auto constraint = + AbsolutePose3DStampedEulerConstraint::make_shared("test", *position_variable, *orientation_variable, mean, cov); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - position_variable->data(), - position_variable->size(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -284,7 +271,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); @@ -293,18 +280,15 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - position_variable->data(), cov_pos_pos.data()); + covariance.GetCovarianceBlock(position_variable->data(), position_variable->data(), cov_pos_pos.data()); - fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position_variable->data(), orientation_variable->data(), + cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -313,13 +297,8 @@ TEST(AbsolutePose3DStampedEulerConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, - 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, - 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + expected_covariance << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, 0.3, + 0.5, 0.2, 4.0, 0.3, 0.4, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; /* *INDENT-ON* */ // High tolerance here, but also high values of covariance @@ -330,72 +309,55 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) { // Optimize a single pose and single constraint, verify the expected value and covariance are // generated. Create the variables. Version for partial measurements - auto position_variable = Position3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position_variable = Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = 1.0; position_variable->z() = 10.0; - auto orientation_variable = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation_variable = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation_variable->w() = 0.952; orientation_variable->x() = 0.038; orientation_variable->y() = -0.189; orientation_variable->z() = 0.239; // Create an absolute pose constraint - std::vector variable_indices {0, 2, 3, 4, 5}; + std::vector variable_indices{ 0, 2, 3, 4, 5 }; fuse_core::Vector6d mean; mean << 1.0, 0.0, 3.0, 0.0, 0.0, 0.0; fuse_core::Matrix5d cov; /* *INDENT-OFF* */ - cov << 1.0, 0.2, 0.3, 0.4, 0.5, - 0.2, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.2, 0.4, 0.5, 6.0; + cov << 1.0, 0.2, 0.3, 0.4, 0.5, 0.2, 3.0, 0.2, 0.1, 0.2, 0.3, 0.2, 4.0, 0.3, 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, 0.5, 0.2, + 0.4, 0.5, 6.0; /* *INDENT-ON* */ - auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( - "test", - *position_variable, - *orientation_variable, - mean, - cov, - variable_indices); + auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared("test", *position_variable, *orientation_variable, + mean, cov, variable_indices); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - position_variable->data(), - position_variable->size(), + problem.AddParameterBlock(position_variable->data(), position_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - position_variable->localParameterization()); + position_variable->localParameterization()); #else - position_variable->manifold()); + position_variable->manifold()); #endif - problem.AddParameterBlock( - orientation_variable->data(), - orientation_variable->size(), + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation_variable->localParameterization()); + orientation_variable->localParameterization()); #else - orientation_variable->manifold()); + orientation_variable->manifold()); #endif - std::vector parameter_blocks; + std::vector parameter_blocks; parameter_blocks.push_back(position_variable->data()); parameter_blocks.push_back(orientation_variable->data()); - problem.AddResidualBlock( - constraint->costFunction(), - constraint->lossFunction(), - parameter_blocks); + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -404,7 +366,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) // Check EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); - EXPECT_NEAR(1.0, position_variable->y(), 1.0e-5); // This is not measured so it will not change + EXPECT_NEAR(1.0, position_variable->y(), 1.0e-5); // This is not measured so it will not change EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); @@ -412,7 +374,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); // Compute the covariance - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); @@ -422,18 +384,15 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) ceres::Covariance covariance(cov_options); covariance.Compute(covariance_blocks, &problem); fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); - covariance.GetCovarianceBlock( - position_variable->data(), - position_variable->data(), cov_pos_pos.data()); + covariance.GetCovarianceBlock(position_variable->data(), position_variable->data(), cov_pos_pos.data()); - fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), - orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace(orientation_variable->data(), orientation_variable->data(), + cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position_variable->data(), orientation_variable->data(), + cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -442,13 +401,8 @@ TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.0, 0.2, 0.3, 0.4, 0.5, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.2, 0.0, 3.0, 0.2, 0.1, 0.2, - 0.3, 0.0, 0.2, 4.0, 0.3, 0.4, - 0.4, 0.0, 0.1, 0.3, 5.0, 0.5, - 0.5, 0.0, 0.2, 0.4, 0.5, 6.0; + expected_covariance << 1.0, 0.0, 0.2, 0.3, 0.4, 0.5, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, 0.0, 3.0, 0.2, 0.1, 0.2, 0.3, + 0.0, 0.2, 4.0, 0.3, 0.4, 0.4, 0.0, 0.1, 0.3, 5.0, 0.5, 0.5, 0.0, 0.2, 0.4, 0.5, 6.0; /* *INDENT-ON* */ // High tolerance here, but also high values of covariance @@ -459,8 +413,7 @@ TEST(AbsolutePose3DStampedEulerConstraint, Serialization) { // Construct a constraint Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); - Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), - fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); fuse_core::Vector6d mean; mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; @@ -469,16 +422,21 @@ TEST(AbsolutePose3DStampedEulerConstraint, Serialization) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - AbsolutePose3DStampedEulerConstraint expected("test", position_variable, orientation_variable, mean, - cov); + AbsolutePose3DStampedEulerConstraint expected("test", position_variable, orientation_variable, mean, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp index 8a778c092..43f17f94c 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -51,8 +51,7 @@ class NormalPriorPose3DTestFixture : public ::testing::Test public: //!< The automatic differentiation cost function type for the pose 3d cost functor using AutoDiffNormalPriorPose3D = - ceres::AutoDiffCostFunction; + ceres::AutoDiffCostFunction; /** * @brief Constructor @@ -63,14 +62,13 @@ class NormalPriorPose3DTestFixture : public ::testing::Test } const fuse_core::Matrix6d covariance = - fuse_core::Vector6d(1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, - //!< y, z, roll, pitch and yaw components + fuse_core::Vector6d(1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y - //!< z, roll, pitch, and yaw components - Eigen::Vector full_mean{1.0, 2.0, 1.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean - //!< components: x, y z, - //!< qw, qx, qy, qz + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{ 1.0, 2.0, 1.0, 1.0, 0.0, 0.0, 0.0 }; //!< The full pose 3d mean + //!< components: x, y z, + //!< qw, qx, qy, qz }; TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) @@ -78,15 +76,14 @@ TEST_F(NormalPriorPose3DTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqual) // Create cost function auto q = Eigen::Quaterniond::UnitRandom(); full_mean << 1.0, 2.0, 1.0, q.w(), q.x(), q.y(), q.z(); // Create automatic differentiation cost function - const fuse_constraints::NormalPriorPose3D cost_function{full_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3D cost_function{ full_sqrt_information, full_mean }; const auto num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose3D autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DCostFunctor(full_sqrt_information, full_mean), - num_residuals); - + new fuse_constraints::NormalPriorPose3DCostFunctor(full_sqrt_information, full_mean), num_residuals); + // Compare the expected, automatic differentiation, cost function and the actual one // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function - // and the second argument is the actual cost function + // and the second argument is the actual cost function ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); -} \ No newline at end of file +} diff --git a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp index 5b9f08f64..59150f134 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -51,8 +51,7 @@ class NormalPriorPose3DEulerTestFixture : public ::testing::Test public: //!< The automatic differentiation cost function type for the pose 3d cost functor using AutoDiffNormalPriorPose3DEuler = - ceres::AutoDiffCostFunction; + ceres::AutoDiffCostFunction; /** * @brief Constructor @@ -63,31 +62,29 @@ class NormalPriorPose3DEulerTestFixture : public ::testing::Test } const fuse_core::Matrix6d covariance = - fuse_core::Vector6d(1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, - //!< y, z, roll, pitch and yaw components + fuse_core::Vector6d(1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3).asDiagonal(); //!< The full pose 3d covariance for the x, + //!< y, z, roll, pitch and yaw components Eigen::Matrix full_sqrt_information; //!< The full pose 3d sqrt information matrix for the x, y - //!< z, roll, pitch, and yaw components - Eigen::Vector full_mean{1.0, 2.0, 1.0, 0.0, 0.0, 0.0}; //!< The full pose 3d mean - //!< components: x, y z, - //!< roll, pitch, and yaw + //!< z, roll, pitch, and yaw components + Eigen::Vector full_mean{ 1.0, 2.0, 1.0, 0.0, 0.0, 0.0 }; //!< The full pose 3d mean + //!< components: x, y z, + //!< roll, pitch, and yaw }; TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqualForFullResiduals) { // Create cost function auto rpy = Eigen::Vector3d::Random(); - full_mean << 1.0, 2.0, 1.0, rpy.x(), rpy.y(), rpy.z(); - const fuse_constraints::NormalPriorPose3DEuler cost_function{full_sqrt_information, full_mean}; + full_mean << 1.0, 2.0, 1.0, rpy.x(), rpy.y(), rpy.z(); + const fuse_constraints::NormalPriorPose3DEuler cost_function{ full_sqrt_information, full_mean }; const auto num_residuals = full_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(full_sqrt_information, full_mean), - num_residuals); - + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(full_sqrt_information, full_mean), num_residuals); + // Compare the expected, automatic differentiation, cost function and the actual one // N.B. in ExpectCostFunctionsAreEqual constructor, the first argument is the expected cost function - // and the second argument is the actual cost function + // and the second argument is the actual cost function ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } @@ -95,25 +92,25 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu { // Create cost function for a subset of residuals // Version with y position = 0 - std::vector indices = {0, 2, 3, 4, 5}; + std::vector indices = { 0, 2, 3, 4, 5 }; auto rpy = Eigen::Vector3d::Random(); full_mean << 1.0, 0.0, 1.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); } std::cout << "full_mean: " << full_mean << std::endl; std::cout << "partial_sqrt_information: " << partial_sqrt_information << std::endl; - const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } @@ -122,25 +119,25 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu { // Create cost function for a subset of residuals // Version with roll, pitch = 0 - std::vector indices = {0, 1, 2, 5}; + std::vector indices = { 0, 1, 2, 5 }; Eigen::Vector3d rpy = Eigen::Vector3d::Random(); rpy(0) = 0.0; rpy(1) = 0.0; full_mean << 1.0, 0.0, 1.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); } - const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } @@ -149,23 +146,23 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu { // Create cost function for a subset of residuals // Version with z = 0, orientation = 0 - std::vector indices = {0, 1}; - Eigen::Vector3d rpy {0.0, 0.0, 0.0}; + std::vector indices = { 0, 1 }; + Eigen::Vector3d rpy{ 0.0, 0.0, 0.0 }; full_mean << 0.1, 0.5, 0.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); } - const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } @@ -174,24 +171,24 @@ TEST_F(NormalPriorPose3DEulerTestFixture, AnalyticAndAutoDiffCostFunctionsAreEqu { // Create cost function for a subset of residuals // Version with position = 0, roll = 0 - std::vector indices = {4, 5}; + std::vector indices = { 4, 5 }; Eigen::Vector3d rpy = Eigen::Vector3d::Random(); rpy(0) = 0.0; full_mean << 0.0, 0.0, 0.0, rpy.x(), rpy.y(), rpy.z(); fuse_core::Matrix partial_sqrt_information; - for (size_t i = 0; i < indices.size(); ++i) { + for (size_t i = 0; i < indices.size(); ++i) + { partial_sqrt_information.row(i) = full_sqrt_information.row(indices[i]); } - const fuse_constraints::NormalPriorPose3DEuler cost_function{partial_sqrt_information, full_mean}; + const fuse_constraints::NormalPriorPose3DEuler cost_function{ partial_sqrt_information, full_mean }; // Create automatic differentiation cost function const auto num_residuals = partial_sqrt_information.rows(); AutoDiffNormalPriorPose3DEuler autodiff_cost_function( - new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), - num_residuals); + new fuse_constraints::NormalPriorPose3DEulerCostFunctor(partial_sqrt_information, full_mean), num_residuals); ExpectCostFunctionsAreEqual(cost_function, autodiff_cost_function); } diff --git a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp index 8f266107a..283b7535a 100644 --- a/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -49,12 +49,11 @@ #include #include -using fuse_variables::Orientation3DStamped; -using fuse_variables::Position3DStamped; using fuse_constraints::AbsolutePose3DStampedConstraint; using fuse_constraints::AbsolutePose3DStampedEulerConstraint; using fuse_constraints::RelativePose3DStampedEulerConstraint; - +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; TEST(RelativePose3DStampedEulerConstraint, Constructor) { @@ -71,18 +70,22 @@ TEST(RelativePose3DStampedEulerConstraint, Constructor) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - EXPECT_NO_THROW( - RelativePose3DStampedEulerConstraint constraint( - "test", position1, orientation1, position2, - orientation2, delta, cov)); + EXPECT_NO_THROW(RelativePose3DStampedEulerConstraint constraint("test", position1, orientation1, position2, + orientation2, delta, cov)); } TEST(RelativePose3DStampedEulerConstraint, ConstructorPartial) @@ -93,7 +96,7 @@ TEST(RelativePose3DStampedEulerConstraint, ConstructorPartial) Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); - std::vector indices {0, 1, 3, 4, 5}; + std::vector indices{ 0, 1, 3, 4, 5 }; fuse_core::Vector6d delta; delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; @@ -101,17 +104,15 @@ TEST(RelativePose3DStampedEulerConstraint, ConstructorPartial) // required precision) fuse_core::Matrix5d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - EXPECT_NO_THROW( - RelativePose3DStampedEulerConstraint constraint( - "test", position1, orientation1, position2, - orientation2, delta, cov, indices)); + EXPECT_NO_THROW(RelativePose3DStampedEulerConstraint constraint("test", position1, orientation1, position2, + orientation2, delta, cov, indices)); } TEST(RelativePose3DStampedEulerConstraint, Covariance) @@ -128,32 +129,32 @@ TEST(RelativePose3DStampedEulerConstraint, Covariance) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - RelativePose3DStampedEulerConstraint constraint( - "test", - position1, - orientation1, - position2, - orientation2, - delta, - cov); + RelativePose3DStampedEulerConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix6d expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT - 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT - 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT - 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, + -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov = cov; @@ -170,7 +171,7 @@ TEST(RelativePose3DStampedEulerConstraint, CovariancePartial) Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); - std::vector indices {0, 1, 3, 4, 5}; + std::vector indices{ 0, 1, 3, 4, 5 }; fuse_core::Vector6d delta; delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; @@ -178,45 +179,39 @@ TEST(RelativePose3DStampedEulerConstraint, CovariancePartial) // required precision) fuse_core::Matrix5d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT /* *INDENT-ON* */ - RelativePose3DStampedEulerConstraint constraint( - "test", - position1, - orientation1, - position2, - orientation2, - delta, - cov, - indices); + RelativePose3DStampedEulerConstraint constraint("test", position1, orientation1, position2, orientation2, delta, cov, + indices); // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') fuse_core::Matrix expected_sqrt_info; /* *INDENT-OFF* */ - expected_sqrt_info << 1.7687, -0.1286, 0.0, -1.3877, -0.6508, 0.6747, // NOLINT - 0.0, 1.3117, 0.0, -0.3361, -0.0415, -0.4332, // NOLINT - 0.0, 0.0, 0.0, 1.8301, -0.6969, -1.1741, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.9533, -0.7697, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.6815; // NOLINT + expected_sqrt_info << 1.7687, -0.1286, 0.0, -1.3877, -0.6508, 0.6747, // NOLINT + 0.0, 1.3117, 0.0, -0.3361, -0.0415, -0.4332, // NOLINT + 0.0, 0.0, 0.0, 1.8301, -0.6969, -1.1741, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.9533, -0.7697, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.6815; // NOLINT /* *INDENT-ON* */ /* *INDENT-ON* */ fuse_core::Matrix6d expected_cov; - expected_cov << 2.0847236144069, 1.10752598122138, 0.0, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.0, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT - 1.96120532313878, 1.35471905449013, 0.0, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 0.0, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 0.0, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + expected_cov << 2.0847236144069, 1.10752598122138, 0.0, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.0, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.96120532313878, 1.35471905449013, 0.0, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 0.0, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 0.0, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT // Compare EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); - EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-4); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-4); } TEST(RelativePose3DStampedEulerConstraint, Optimization) @@ -224,27 +219,23 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Optimize a two-pose system with a pose prior and a relative pose constraint // Verify the expected poses and covariances are generated. // Create two poses - auto position1 = - Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position1 = Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position1->x() = 1.5; position1->y() = -3.0; position1->z() = 10.0; - auto orientation1 = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation1 = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation1->w() = 0.952; orientation1->x() = 0.038; orientation1->y() = -0.189; orientation1->z() = 0.239; - auto position2 = - Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto position2 = Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); position2->x() = -1.5; position2->y() = 3.0; position2->z() = -10.0; - auto orientation2 = Orientation3DStamped::make_shared( - rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto orientation2 = Orientation3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); orientation2->w() = 0.944; orientation2->x() = -0.128; orientation2->y() = 0.145; @@ -254,78 +245,53 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) fuse_core::Vector7d mean_origin; mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); - auto prior = AbsolutePose3DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - mean_origin, - cov_origin); + auto prior = AbsolutePose3DStampedConstraint::make_shared("test", *position1, *orientation1, mean_origin, cov_origin); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector6d mean_delta; mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; fuse_core::Matrix6d cov_delta = fuse_core::Matrix6d::Identity(); - auto relative = RelativePose3DStampedEulerConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - mean_delta, - cov_delta); + auto relative = RelativePose3DStampedEulerConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, mean_delta, cov_delta); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_parameter_blocks; + problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(position1->data()); relative_parameter_blocks.push_back(orientation1->data()); relative_parameter_blocks.push_back(position2->data()); relative_parameter_blocks.push_back(orientation2->data()); - problem.AddResidualBlock( - relative->costFunction(), - relative->lossFunction(), - relative_parameter_blocks); + problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); // Run the solver ceres::Solver::Options options; @@ -350,7 +316,7 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); @@ -363,14 +329,10 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(3, 3); - covariance.GetCovarianceBlockInTangentSpace( - orientation1->data(), - orientation1->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation1->data(), orientation1->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->size(), 3); - covariance.GetCovarianceBlockInTangentSpace( - position1->data(), - orientation1->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position1->data(), orientation1->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -379,13 +341,8 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + expected_covariance << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); @@ -393,7 +350,7 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -406,14 +363,10 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(3, 3); - covariance.GetCovarianceBlockInTangentSpace( - orientation2->data(), - orientation2->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation2->data(), orientation2->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->size(), 3); - covariance.GetCovarianceBlockInTangentSpace( - position2->data(), - orientation2->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position2->data(), orientation2->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -422,13 +375,8 @@ TEST(RelativePose3DStampedEulerConstraint, Optimization) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, - 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, - 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; + expected_covariance << 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; /* *INDENT-ON* */ // High tolerance here, but also high values of covariance @@ -441,135 +389,95 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Optimize a two-pose system with a pose prior and a relative pose constraint // Verify the expected poses and covariances are generated. // Create two poses - auto position1 = - Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto position1 = Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); position1->x() = 1.5; position1->y() = -3.0; position1->z() = 10.0; - auto orientation1 = Orientation3DStamped::make_shared( - rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + auto orientation1 = Orientation3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); orientation1->w() = 0.952; orientation1->x() = 0.038; orientation1->y() = -0.189; orientation1->z() = 0.239; - auto position2 = - Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto position2 = Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); position2->x() = -1.5; position2->y() = 3.0; position2->z() = -10.0; - auto orientation2 = Orientation3DStamped::make_shared( - rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + auto orientation2 = Orientation3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); orientation2->w() = 0.944; orientation2->x() = -0.128; orientation2->y() = 0.145; orientation2->z() = -0.269; - std::vector indices {0, 1, 3, 4, 5}; + std::vector indices{ 0, 1, 3, 4, 5 }; // Create an absolute pose constraint at the origin fuse_core::Vector7d mean_origin; mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); - auto prior = AbsolutePose3DStampedConstraint::make_shared( - "test", - *position1, - *orientation1, - mean_origin, - cov_origin); + auto prior = AbsolutePose3DStampedConstraint::make_shared("test", *position1, *orientation1, mean_origin, cov_origin); // Create a relative pose constraint for 1m in the x direction fuse_core::Vector6d mean_delta; mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; fuse_core::Matrix5d cov_delta = fuse_core::Matrix5d::Identity(); - auto relative = RelativePose3DStampedEulerConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - mean_delta, - cov_delta, - indices); - + auto relative = RelativePose3DStampedEulerConstraint::make_shared("test", *position1, *orientation1, *position2, + *orientation2, mean_delta, cov_delta, indices); + // Create a relative pose constraint for 1m in the y direction - std::vector indices_y {1, 2, 3, 4, 5}; + std::vector indices_y{ 1, 2, 3, 4, 5 }; fuse_core::Vector6d mean_delta_y; mean_delta_y << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0; fuse_core::Matrix5d cov_delta_y = fuse_core::Matrix5d::Identity(); auto relative_y = RelativePose3DStampedEulerConstraint::make_shared( - "test", - *position1, - *orientation1, - *position2, - *orientation2, - mean_delta_y, - cov_delta_y, - indices_y); + "test", *position1, *orientation1, *position2, *orientation2, mean_delta_y, cov_delta_y, indices_y); // Build the problem ceres::Problem::Options problem_options; problem_options.loss_function_ownership = fuse_core::Loss::Ownership; ceres::Problem problem(problem_options); - problem.AddParameterBlock( - orientation1->data(), - orientation1->size(), + problem.AddParameterBlock(orientation1->data(), orientation1->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation1->localParameterization()); + orientation1->localParameterization()); #else - orientation1->manifold()); + orientation1->manifold()); #endif - problem.AddParameterBlock( - position1->data(), - position1->size(), + problem.AddParameterBlock(position1->data(), position1->size(), #if !CERES_SUPPORTS_MANIFOLDS - position1->localParameterization()); + position1->localParameterization()); #else - position1->manifold()); + position1->manifold()); #endif - problem.AddParameterBlock( - orientation2->data(), - orientation2->size(), + problem.AddParameterBlock(orientation2->data(), orientation2->size(), #if !CERES_SUPPORTS_MANIFOLDS - orientation2->localParameterization()); + orientation2->localParameterization()); #else - orientation2->manifold()); + orientation2->manifold()); #endif - problem.AddParameterBlock( - position2->data(), - position2->size(), + problem.AddParameterBlock(position2->data(), position2->size(), #if !CERES_SUPPORTS_MANIFOLDS - position2->localParameterization()); + position2->localParameterization()); #else - position2->manifold()); + position2->manifold()); #endif - std::vector prior_parameter_blocks; + std::vector prior_parameter_blocks; prior_parameter_blocks.push_back(position1->data()); prior_parameter_blocks.push_back(orientation1->data()); - problem.AddResidualBlock( - prior->costFunction(), - prior->lossFunction(), - prior_parameter_blocks); - std::vector relative_parameter_blocks; + problem.AddResidualBlock(prior->costFunction(), prior->lossFunction(), prior_parameter_blocks); + std::vector relative_parameter_blocks; relative_parameter_blocks.push_back(position1->data()); relative_parameter_blocks.push_back(orientation1->data()); relative_parameter_blocks.push_back(position2->data()); relative_parameter_blocks.push_back(orientation2->data()); - problem.AddResidualBlock( - relative->costFunction(), - relative->lossFunction(), - relative_parameter_blocks); - std::vector relative_parameter_blocks_y; + problem.AddResidualBlock(relative->costFunction(), relative->lossFunction(), relative_parameter_blocks); + std::vector relative_parameter_blocks_y; relative_parameter_blocks_y.push_back(position1->data()); relative_parameter_blocks_y.push_back(orientation1->data()); relative_parameter_blocks_y.push_back(position2->data()); relative_parameter_blocks_y.push_back(orientation2->data()); - problem.AddResidualBlock( - relative_y->costFunction(), - relative_y->lossFunction(), - relative_parameter_blocks_y); + problem.AddResidualBlock(relative_y->costFunction(), relative_y->lossFunction(), relative_parameter_blocks_y); // Run the solver ceres::Solver::Options options; @@ -594,7 +502,7 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Compute the marginal covariance for pose1 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position1->data(), position1->data()); covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); covariance_blocks.emplace_back(position1->data(), orientation1->data()); @@ -608,14 +516,10 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation1->data(), - orientation1->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation1->data(), orientation1->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - position1->data(), - orientation1->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position1->data(), orientation1->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -624,13 +528,8 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + expected_covariance << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); @@ -638,7 +537,7 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Compute the marginal covariance for pose2 { - std::vector> covariance_blocks; + std::vector> covariance_blocks; covariance_blocks.emplace_back(position2->data(), position2->data()); covariance_blocks.emplace_back(position2->data(), orientation2->data()); covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); @@ -652,14 +551,10 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - orientation2->data(), - orientation2->data(), cov_or_or.data()); + covariance.GetCovarianceBlockInTangentSpace(orientation2->data(), orientation2->data(), cov_or_or.data()); fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); - covariance.GetCovarianceBlockInTangentSpace( - position2->data(), - orientation2->data(), cov_pos_or.data()); + covariance.GetCovarianceBlockInTangentSpace(position2->data(), orientation2->data(), cov_pos_or.data()); // Assemble the full covariance from the covariance blocks fuse_core::Matrix6d actual_covariance; @@ -668,13 +563,8 @@ TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) // Define the expected covariance fuse_core::Matrix6d expected_covariance; /* *INDENT-OFF* */ - expected_covariance << - 2.25, -0.5, 0.0, 0.0, 0.0, -0.5, - -0.5, 2.5, 0.0, 0.0, 0.0, 1.0, - 0.0, 0.0, 3.25, 0.5, -1.0, 0.0, - 0.0, 0.0, 0.5, 1.5, 0.0, 0.0, - 0.0, 0.0, -1.0, 0.0, 1.5, 0.0, - -0.5, 1.0, 0.0, 0.0, 0.0, 1.5; + expected_covariance << 2.25, -0.5, 0.0, 0.0, 0.0, -0.5, -0.5, 2.5, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 3.25, 0.5, -1.0, + 0.0, 0.0, 0.0, 0.5, 1.5, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0, 1.5, 0.0, -0.5, 1.0, 0.0, 0.0, 0.0, 1.5; /* *INDENT-ON* */ EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); @@ -696,16 +586,21 @@ TEST(RelativePose3DStampedEulerConstraint, Serialization) // required precision) fuse_core::Matrix6d cov; /* *INDENT-OFF* */ - cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT - 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT - 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT - 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT - 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT - 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, + 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, + 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, + 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, + 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, + 2.15326088526198; // NOLINT /* *INDENT-ON* */ - RelativePose3DStampedEulerConstraint expected("test", position1, orientation1, position2, orientation2, - delta, cov); + RelativePose3DStampedEulerConstraint expected("test", position1, orientation1, position2, orientation2, delta, cov); // Serialize the constraint into an archive std::stringstream stream; diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 1ce51eb79..eeab5dbcd 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -40,43 +40,49 @@ #include #include -struct Quat2RPY { +struct Quat2RPY +{ template - bool operator()(const T * const q, T * rpy) const { + bool operator()(const T* const q, T* rpy) const + { rpy[0] = fuse_core::getRoll(q[0], q[1], q[2], q[3]); rpy[1] = fuse_core::getPitch(q[0], q[1], q[2], q[3]); rpy[2] = fuse_core::getYaw(q[0], q[1], q[2], q[3]); return true; } - static ceres::CostFunction* Create() { + static ceres::CostFunction* Create() + { return (new ceres::AutoDiffCostFunction(new Quat2RPY())); } }; -struct QuatProd { +struct QuatProd +{ template - bool operator()( - const T * const q1, - const T * const q2, - T * q_out) const { - ceres::QuaternionProduct(q1, q2, q_out); - return true; - } - - static ceres::CostFunction* Create() { + bool operator()(const T* const q1, const T* const q2, T* q_out) const + { + ceres::QuaternionProduct(q1, q2, q_out); + return true; + } + + static ceres::CostFunction* Create() + { return (new ceres::AutoDiffCostFunction(new QuatProd())); } }; -struct Quat2AngleAxis { +struct Quat2AngleAxis +{ template - bool operator()(const T * const q, T * aa) const { - ceres::QuaternionToAngleAxis(q, aa); - return true; - } + bool operator()(const T* const q, T* aa) const + { + ceres::QuaternionToAngleAxis(q, aa); + return true; + } - static ceres::CostFunction* Create() { + static ceres::CostFunction* Create() + { return (new ceres::AutoDiffCostFunction(new Quat2AngleAxis())); } }; @@ -128,7 +134,7 @@ TEST(Util, wrapAngle2D) TEST(Util, quaternion2rpy) { // Test correct conversion from quaternion to roll-pitch-yaw - double q[4] = {1.0, 0.0, 0.0, 0.0}; + double q[4] = { 1.0, 0.0, 0.0, 0.0 }; double rpy[3]; fuse_core::quaternion2rpy(q, rpy); EXPECT_EQ(0.0, rpy[0]); @@ -152,19 +158,19 @@ TEST(Util, quaternion2rpy) q[1] = q_eigen.x(); q[2] = q_eigen.y(); q[3] = q_eigen.z(); - + fuse_core::quaternion2rpy(q, rpy, J_analytic); - double * jacobians[1] = {J_autodiff}; - const double * parameters[1] = {q}; + double* jacobians[1] = { J_autodiff }; + const double* parameters[1] = { q }; - ceres::CostFunction * quat2rpy_cf = Quat2RPY::Create(); + ceres::CostFunction* quat2rpy_cf = Quat2RPY::Create(); double rpy_autodiff[3]; quat2rpy_cf->Evaluate(parameters, rpy_autodiff, jacobians); - + Eigen::Map> J_autodiff_map(jacobians[0]); Eigen::Map> J_analytic_map(J_analytic); - + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); } @@ -174,47 +180,35 @@ TEST(Util, quaternionProduct) const Eigen::Quaterniond q1_eigen = Eigen::Quaterniond::UnitRandom(); const Eigen::Quaterniond q2_eigen = Eigen::Quaterniond::UnitRandom(); double q_out[4]; - double q1[4] - { - q1_eigen.w(), - q1_eigen.x(), - q1_eigen.y(), - q1_eigen.z() - }; + double q1[4]{ q1_eigen.w(), q1_eigen.x(), q1_eigen.y(), q1_eigen.z() }; + + double q2[4]{ q2_eigen.w(), q2_eigen.x(), q2_eigen.y(), q2_eigen.z() }; - double q2[4] - { - q2_eigen.w(), - q2_eigen.x(), - q2_eigen.y(), - q2_eigen.z() - }; - // Atm only the jacobian wrt the second quaternion is implemented. If the computation will be - // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. - double J_analytic_q1[16], J_analytic_q2[16]; // Analytical Jacobians wrt first and second quaternion - double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion - - fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); + // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. + double J_analytic_q1[16], J_analytic_q2[16]; // Analytical Jacobians wrt first and second quaternion + double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion + + fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); - double * jacobians[2]; + double* jacobians[2]; jacobians[0] = J_autodiff_q1; jacobians[1] = J_autodiff_q2; - const double * parameters[2]; + const double* parameters[2]; parameters[0] = q1; parameters[1] = q2; - ceres::CostFunction * quat_prod_cf = QuatProd::Create(); + ceres::CostFunction* quat_prod_cf = QuatProd::Create(); double q_out_autodiff[4]; quat_prod_cf->Evaluate(parameters, q_out_autodiff, jacobians); - + Eigen::Map> J_autodiff_q1_map(jacobians[0]); Eigen::Map> J_autodiff_q2_map(jacobians[1]); // Eigen::Map> J_analytic_q1_map(J_analytic_q1); Eigen::Map> J_analytic_q2_map(J_analytic_q2); - + EXPECT_TRUE(J_analytic_q2_map.isApprox(J_autodiff_q2_map)); } @@ -223,29 +217,22 @@ TEST(Util, quaternionToAngleAxis) // Test correct quaternion to angle-axis function jacobian const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); double angle_axis[3]; - double q[4] - { - q_eigen.w(), - q_eigen.x(), - q_eigen.y(), - q_eigen.z() - }; - - double J_analytic[12]; + double q[4]{ q_eigen.w(), q_eigen.x(), q_eigen.y(), q_eigen.z() }; + + double J_analytic[12]; double J_autodiff[12]; - - fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); - double * jacobians[1] = {J_autodiff}; - const double * parameters[1] = {q}; + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + + double* jacobians[1] = { J_autodiff }; + const double* parameters[1] = { q }; - ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); + ceres::CostFunction* quat2angle_axis_cf = Quat2AngleAxis::Create(); double angle_axis_autodiff[3]; quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); - + Eigen::Map> J_autodiff_map(jacobians[0]); Eigen::Map> J_analytic_map(J_analytic); - + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); } - diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp index 8dc570000..2ff2fc0f6 100644 --- a/fuse_models/include/fuse_models/imu_3d.hpp +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -115,16 +114,14 @@ class Imu3D : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The IMU message to process */ - void process(const sensor_msgs::msg::Imu & msg); + void process(const sensor_msgs::msg::Imu& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -158,23 +155,18 @@ class Imu3D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const bool validate, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp index 8a888f3a8..0ba4df86c 100644 --- a/fuse_models/include/fuse_models/odometry_3d.hpp +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -50,7 +50,6 @@ #include #include - namespace fuse_models { @@ -106,16 +105,14 @@ class Odometry3D : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Callback for pose messages * @param[in] msg - The pose message to process */ - void process(const nav_msgs::msg::Odometry & msg); + void process(const nav_msgs::msg::Odometry& msg); protected: fuse_core::UUID device_id_; //!< The UUID of this device @@ -149,23 +146,18 @@ class Odometry3D : public fuse_core::AsyncSensorModel * @param[in] validate - Whether to validate the pose and twist coavriance or not * @param[out] transaction - The generated variables and constraints are added to this transaction */ - void processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const bool validate, - fuse_core::Transaction & transaction); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + void processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp index f1c70f441..dc623ad99 100644 --- a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -56,7 +56,6 @@ #include #include - namespace fuse_models { @@ -123,9 +122,8 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher /** * @brief Shadowing extension to the AsyncPublisher::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; protected: /** @@ -142,9 +140,8 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher * @param[in] graph A read-only pointer to the graph object, allowing queries to be * performed whenever needed */ - void notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) override; + void notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; /** * @brief Perform any required operations before the first call to notify() occurs @@ -176,17 +173,11 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher * structure * @return true if the checks pass, false otherwise */ - bool getState( - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & position_uuid, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & velocity_linear_uuid, - fuse_core::UUID & velocity_angular_uuid, - fuse_core::UUID & acceleration_linear_uuid, - nav_msgs::msg::Odometry & odometry, - geometry_msgs::msg::AccelWithCovarianceStamped & acceleration); + bool getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, const fuse_core::UUID& device_id, + fuse_core::UUID& position_uuid, fuse_core::UUID& orientation_uuid, + fuse_core::UUID& velocity_linear_uuid, fuse_core::UUID& velocity_angular_uuid, + fuse_core::UUID& acceleration_linear_uuid, nav_msgs::msg::Odometry& odometry, + geometry_msgs::msg::AccelWithCovarianceStamped& acceleration); /** * @brief Timer callback method for the filtered state publication and tf broadcasting @@ -198,31 +189,24 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher * @brief Object that searches for the most recent common timestamp for a set of variables */ using Synchronizer = fuse_publishers::StampedVariableSynchronizer< - fuse_variables::Orientation3DStamped, - fuse_variables::Position3DStamped, - fuse_variables::VelocityLinear3DStamped, - fuse_variables::VelocityAngular3DStamped, - fuse_variables::AccelerationLinear3DStamped>; - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Timers, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncPublisher interfaces_ - - fuse_core::UUID device_id_; //!< The UUID of this device + fuse_variables::Orientation3DStamped, fuse_variables::Position3DStamped, fuse_variables::VelocityLinear3DStamped, + fuse_variables::VelocityAngular3DStamped, fuse_variables::AccelerationLinear3DStamped>; + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncPublisher interfaces_ + + fuse_core::UUID device_id_; //!< The UUID of this device rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The publisher's logger + rclcpp::Logger logger_; //!< The publisher's logger ParameterType params_; rclcpp::Time latest_stamp_; rclcpp::Time latest_covariance_stamp_; - bool latest_covariance_valid_{false}; //!< Whether the latest covariance computed is valid or not + bool latest_covariance_valid_{ false }; //!< Whether the latest covariance computed is valid or not nav_msgs::msg::Odometry odom_output_; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output_; @@ -237,10 +221,10 @@ class Odometry3DPublisher : public fuse_core::AsyncPublisher std::shared_ptr tf_broadcaster_ = nullptr; std::unique_ptr tf_listener_; - fuse_core::DelayedThrottleFilter delayed_throttle_filter_{10.0}; //!< A ros::console filter to - //!< print delayed throttle - //!< messages, that can be reset - //!< on start + fuse_core::DelayedThrottleFilter delayed_throttle_filter_{ 10.0 }; //!< A ros::console filter to + //!< print delayed throttle + //!< messages, that can be reset + //!< on start rclcpp::TimerBase::SharedPtr publish_timer_; diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index f8e4ccab3..cdb016689 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -66,8 +66,8 @@ namespace fuse_models * - ~buffer_length (double) The length of the graph state buffer and state history, in seconds * - ~process_noise_diagonal (vector of doubles) An 15-dimensional vector containing the diagonal * values for the process noise covariance matrix. - * Variable order is (x, y, z, roll, pitch, yaw, - * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * Variable order is (x, y, z, roll, pitch, yaw, + * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc). */ class Omnidirectional3D : public fuse_core::AsyncMotionModel @@ -90,11 +90,10 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel /** * @brief Shadowing extension to the AsyncMotionModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) override; - void print(std::ostream & stream = std::cout) const; + void print(std::ostream& stream = std::cout) const; protected: /** @@ -102,19 +101,20 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel */ struct StateHistoryElement { - fuse_core::UUID position_uuid; //!< The uuid of the associated position variable - fuse_core::UUID orientation_uuid; //!< The uuid of the associated orientation variable - fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable - fuse_core::UUID vel_angular_uuid; //!< The uuid of the associated angular velocity variable - fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration - //!< variable - fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position - Eigen::Quaterniond orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); //!< Map-frame orientation (quaternion) - fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities - fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities - fuse_core::Vector3d acc_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations - - void print(std::ostream & stream = std::cout) const; + fuse_core::UUID position_uuid; //!< The uuid of the associated position variable + fuse_core::UUID orientation_uuid; //!< The uuid of the associated orientation variable + fuse_core::UUID vel_linear_uuid; //!< The uuid of the associated linear velocity variable + fuse_core::UUID vel_angular_uuid; //!< The uuid of the associated angular velocity variable + fuse_core::UUID acc_linear_uuid; //!< The uuid of the associated linear acceleration + //!< variable + fuse_core::Vector3d position = fuse_core::Vector3d::Zero(); //!< Map-frame position + Eigen::Quaterniond orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); //!< Map-frame orientation (quaternion) + fuse_core::Vector3d vel_linear = fuse_core::Vector3d::Zero(); //!< Body-frame linear velocities + fuse_core::Vector3d vel_angular = fuse_core::Vector3d::Zero(); //!< Body-frame angular velocities + fuse_core::Vector3d acc_linear = + fuse_core::Vector3d::Zero(); //!< Body-frame linear (angular not needed) accelerations + + void print(std::ostream& stream = std::cout) const; /** * @brief Validate the state components: pose, linear velocities, angular velocities and linear @@ -136,7 +136,7 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * constraints * @return True if the motion models were generated successfully, false otherwise */ - bool applyCallback(fuse_core::Transaction & transaction) override; + bool applyCallback(fuse_core::Transaction& transaction) override; /** * @brief Generate a single motion model segment between the specified timestamps. @@ -156,11 +156,9 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * ending_stamp. The variables should include initial values for the * optimizer. */ - void generateMotionModel( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables); + void generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables); /** * @brief Callback fired in the local callback queue thread(s) whenever a new Graph is received @@ -187,10 +185,8 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * @param[in] state_history The state history object to be updated * @param[in] buffer_length States older than this in the history will be pruned */ - static void updateStateHistoryEstimates( - const fuse_core::Graph & graph, - StateHistory & state_history, - const rclcpp::Duration & buffer_length); + static void updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, + const rclcpp::Duration& buffer_length); /** * @brief Validate the motion model state #1, state #2 and process noise covariance @@ -203,41 +199,37 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel * @param[in] process_noise_covariance The process noise covariance, after it is scaled and * multiplied by dt */ - static void validateMotionModel( - const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix15d & process_noise_covariance); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + static void validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, + const fuse_core::Matrix15d& process_noise_covariance); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger rclcpp::Duration buffer_length_; //!< The length of the state history fuse_core::UUID device_id_; //!< The UUID of the device to be published fuse_core::TimestampManager timestamp_manager_; //!< Tracks timestamps and previously created //!< motion model segments fuse_core::Matrix15d process_noise_covariance_; //!< Process noise covariance matrix - bool scale_process_noise_{false}; //!< Whether to scale the process noise + bool scale_process_noise_{ false }; //!< Whether to scale the process noise //!< covariance pose by the norm of the current //!< state twist - double velocity_linear_norm_min_{1e-3}; //!< The minimum linear velocity norm allowed when + double velocity_linear_norm_min_{ 1e-3 }; //!< The minimum linear velocity norm allowed when //!< scaling the process noise covariance - double velocity_angular_norm_min_{1e-3}; //!< The minimum twist norm allowed when + double velocity_angular_norm_min_{ 1e-3 }; //!< The minimum twist norm allowed when //!< scaling the process noise covariance - bool disable_checks_{false}; //!< Whether to disable the validation checks for the current and - //!< predicted state, including the process noise covariance after - //!< it is scaled and multiplied by dt - StateHistory state_history_; //!< History of optimized graph pose estimates + bool disable_checks_{ false }; //!< Whether to disable the validation checks for the current and + //!< predicted state, including the process noise covariance after + //!< it is scaled and multiplied by dt + StateHistory state_history_; //!< History of optimized graph pose estimates }; -std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & unicycle_2d); +std::ostream& operator<<(std::ostream& stream, const Omnidirectional3D& unicycle_2d); } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp index ecbf39028..a9757f1cb 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -49,7 +49,6 @@ #include #include - namespace fuse_models { @@ -58,8 +57,8 @@ namespace fuse_models * motion model. * * This class publishes a transaction that contains a prior on each state subvariable used in the - * Omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, - * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction + * Omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction * with the configured initial state and covariance. * Additionally, whenever a pose is received, either on the set_pose service or the topic, this * ignition sensor resets the optimizer then publishes a new transaction with a prior at the @@ -73,12 +72,12 @@ namespace fuse_models * - ~initial_sigma (vector of doubles) A 15-dimensional vector containing the standard deviations * for the initial state values. The covariance matrix is * created placing the squared standard deviations along the - * diagonal of an 15x15 matrix. Variable order is (x, y, z, - * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * diagonal of an 15x15 matrix. Variable order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). * - ~initial_state (vector of doubles) A 15-dimensional vector containing the initial values for - * the state. Variable order is (x, y, z, - * qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * the state. Variable order is (x, y, z, + * qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). * - ~queue_size (int, default: 10) The subscriber queue size for the pose messages * - ~reset_service (string, default: "~/reset") The name of the reset service to call before @@ -111,10 +110,8 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel /** * @brief Shadowing extension to the AsyncSensorModel::initialize call */ - void initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) override; + void initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) override; /** * @brief Subscribe to the input topic to start sending transactions to the optimizer @@ -141,23 +138,20 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped & msg); + void subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr, - const fuse_msgs::srv::SetPose::Request::SharedPtr req); + bool setPoseServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr, const fuse_msgs::srv::SetPose::Request::SharedPtr req); /** * @brief Triggers the publication of a new prior transaction at the supplied pose */ - bool setPoseDeprecatedServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); + bool setPoseDeprecatedServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req); protected: /** @@ -173,37 +167,31 @@ class Omnidirectional3DIgnition : public fuse_core::AsyncSensorModel * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) */ - void process( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - std::function post_process = nullptr); + void process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, std::function post_process = nullptr); /** * @brief Create and send a prior transaction based on the supplied pose * - * The omnidirectional 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, - * yaw_vel, x_acc, y_acc, z_acc) will use the initial state values and standard deviations configured on the + * The omnidirectional 3d state members not included in the pose message (x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * yaw_vel, x_acc, y_acc, z_acc) will use the initial state values and standard deviations configured on the * parameter server. * * @param[in] pose - The pose and covariance to use for the prior constraints on (x, y, z, roll, pitch, yaw) */ - void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose); - - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Clock, - fuse_core::node_interfaces::Graph, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters, - fuse_core::node_interfaces::Services, - fuse_core::node_interfaces::Topics, - fuse_core::node_interfaces::Waitables - > interfaces_; //!< Shadows AsyncSensorModel interfaces_ - - std::atomic_bool started_; //!< Flag indicating the sensor has been started - bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already - fuse_core::UUID device_id_; //!< The UUID of this device + void sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose); + + fuse_core::node_interfaces::NodeInterfaces + interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + std::atomic_bool started_; //!< Flag indicating the sensor has been started + bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already + fuse_core::UUID device_id_; //!< The UUID of this device rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping - rclcpp::Logger logger_; //!< The sensor model's logger + rclcpp::Logger logger_; //!< The sensor model's logger ParameterType params_; //!< Object containing all of the configuration parameters diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp index 1a9a3501d..167554624 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -75,39 +75,14 @@ namespace fuse_models * @param[out] acc_linear2_y - Second Y acceleration * @param[out] acc_linear2_z - Second Z acceleration */ -template -inline void predict( - const T position1_x, - const T position1_y, - const T position1_z, - const T orientation1_r, - const T orientation1_p, - const T orientation1_y, - const T vel_linear1_x, - const T vel_linear1_y, - const T vel_linear1_z, - const T vel_angular1_r, - const T vel_angular1_p, - const T vel_angular1_y, - const T acc_linear1_x, - const T acc_linear1_y, - const T acc_linear1_z, - const T dt, - T & position2_x, - T & position2_y, - T & position2_z, - T & orientation2_r, - T & orientation2_p, - T & orientation2_y, - T & vel_linear2_x, - T & vel_linear2_y, - T & vel_linear2_z, - T & vel_angular2_r, - T & vel_angular2_p, - T & vel_angular2_y, - T & acc_linear2_x, - T & acc_linear2_y, - T & acc_linear2_z) +template +inline void predict(const T position1_x, const T position1_y, const T position1_z, const T orientation1_r, + const T orientation1_p, const T orientation1_y, const T vel_linear1_x, const T vel_linear1_y, + const T vel_linear1_z, const T vel_angular1_r, const T vel_angular1_p, const T vel_angular1_y, + const T acc_linear1_x, const T acc_linear1_y, const T acc_linear1_z, const T dt, T& position2_x, + T& position2_y, T& position2_z, T& orientation2_r, T& orientation2_p, T& orientation2_y, + T& vel_linear2_x, T& vel_linear2_y, T& vel_linear2_z, T& vel_angular2_r, T& vel_angular2_p, + T& vel_angular2_y, T& acc_linear2_x, T& acc_linear2_y, T& acc_linear2_z) { // 3D material point projection model which matches the one used by r_l. const T sr = ceres::sin(orientation1_r); @@ -120,23 +95,28 @@ inline void predict( const T dt2 = T(0.5) * dt * dt; // Project the state - position2_x = position1_x + - ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + (cy * sp * cr + sy * sr) * vel_linear1_z) * dt + - ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + (cy * sp * cr + sy * sr) * acc_linear1_z) * dt2; + position2_x = position1_x + + ((cy * cp) * vel_linear1_x + (cy * sp * sr - sy * cr) * vel_linear1_y + + (cy * sp * cr + sy * sr) * vel_linear1_z) * + dt + + ((cy * cp) * acc_linear1_x + (cy * sp * sr - sy * cr) * acc_linear1_y + + (cy * sp * cr + sy * sr) * acc_linear1_z) * + dt2; position2_y = position1_y + - ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + (sy * sp * cr - cy * sr) * vel_linear1_z) * dt + - ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + (sy * sp * cr - cy * sr) * acc_linear1_z) * dt2; - position2_z = position1_z + - ((-sp) * vel_linear1_x + (cp * sr) * vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + - ((-sp) * acc_linear1_x + (cp * sr) * acc_linear1_y + (cp * cr) * acc_linear1_z) * dt2; - - orientation2_r = orientation1_r + - (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; - orientation2_p = orientation1_p + - (cr * vel_angular1_p - sr * vel_angular1_y) * dt; - orientation2_y = orientation1_y + - (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; - + ((sy * cp) * vel_linear1_x + (sy * sp * sr + cy * cr) * vel_linear1_y + + (sy * sp * cr - cy * sr) * vel_linear1_z) * + dt + + ((sy * cp) * acc_linear1_x + (sy * sp * sr + cy * cr) * acc_linear1_y + + (sy * sp * cr - cy * sr) * acc_linear1_z) * + dt2; + position2_z = position1_z + ((-sp) * vel_linear1_x + (cp * sr) * vel_linear1_y + (cp * cr) * vel_linear1_z) * dt + + ((-sp) * acc_linear1_x + (cp * sr) * acc_linear1_y + (cp * cr) * acc_linear1_z) * dt2; + + orientation2_r = + orientation1_r + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; + orientation2_p = orientation1_p + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; + orientation2_y = orientation1_y + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; @@ -189,40 +169,16 @@ inline void predict( * @param[out] acc_linear2_z - Second Z acceleration * @param[out] jacobians - Jacobians wrt the state */ -inline void predict( - const double position1_x, - const double position1_y, - const double position1_z, - const double orientation1_r, - const double orientation1_p, - const double orientation1_y, - const double vel_linear1_x, - const double vel_linear1_y, - const double vel_linear1_z, - const double vel_angular1_r, - const double vel_angular1_p, - const double vel_angular1_y, - const double acc_linear1_x, - const double acc_linear1_y, - const double acc_linear1_z, - const double dt, - double & position2_x, - double & position2_y, - double & position2_z, - double & orientation2_r, - double & orientation2_p, - double & orientation2_y, - double & vel_linear2_x, - double & vel_linear2_y, - double & vel_linear2_z, - double & vel_angular2_r, - double & vel_angular2_p, - double & vel_angular2_y, - double & acc_linear2_x, - double & acc_linear2_y, - double & acc_linear2_z, - double ** jacobians, - double * jacobian_quat2rpy) +inline void predict(const double position1_x, const double position1_y, const double position1_z, + const double orientation1_r, const double orientation1_p, const double orientation1_y, + const double vel_linear1_x, const double vel_linear1_y, const double vel_linear1_z, + const double vel_angular1_r, const double vel_angular1_p, const double vel_angular1_y, + const double acc_linear1_x, const double acc_linear1_y, const double acc_linear1_z, const double dt, + double& position2_x, double& position2_y, double& position2_z, double& orientation2_r, + double& orientation2_p, double& orientation2_y, double& vel_linear2_x, double& vel_linear2_y, + double& vel_linear2_z, double& vel_angular2_r, double& vel_angular2_p, double& vel_angular2_y, + double& acc_linear2_x, double& acc_linear2_y, double& acc_linear2_z, double** jacobians, + double* jacobian_quat2rpy) { // 3D material point projection model which matches the one used by r_l. const double sr = ceres::sin(orientation1_r); @@ -235,23 +191,28 @@ inline void predict( const double dt2 = 0.5 * dt * dt; // Project the state - position2_x = position1_x + - ((cp * cy) * vel_linear1_x + (sr * sp * cy - cr * sy) * vel_linear1_y + (cr * sp * cy + sr * sy) * vel_linear1_z) * dt + - ((cp * cy) * acc_linear1_x + (sr * sp * cy - cr * sy) * acc_linear1_y + (cr * sp * cy + sr * sy) * acc_linear1_z) * dt2; + position2_x = position1_x + + ((cp * cy) * vel_linear1_x + (sr * sp * cy - cr * sy) * vel_linear1_y + + (cr * sp * cy + sr * sy) * vel_linear1_z) * + dt + + ((cp * cy) * acc_linear1_x + (sr * sp * cy - cr * sy) * acc_linear1_y + + (cr * sp * cy + sr * sy) * acc_linear1_z) * + dt2; position2_y = position1_y + - ((cp * sy) * vel_linear1_x + (sr * sp * sy + cr * cy) * vel_linear1_y + (cr * sp * sy - sr * cy) * vel_linear1_z) * dt + - ((cp * sy) * acc_linear1_x + (sr * sp * sy + cr * cy) * acc_linear1_y + (cr * sp * sy - sr * cy) * acc_linear1_z) * dt2; - position2_z = position1_z + - ((-sp) * vel_linear1_x + (sr * cp) * vel_linear1_y + (cr * cp) * vel_linear1_z) * dt + - ((-sp) * acc_linear1_x + (sr * cp) * acc_linear1_y + (cr * cp) * acc_linear1_z) * dt2; - - orientation2_r = orientation1_r + - (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; - orientation2_p = orientation1_p + - (cr * vel_angular1_p - sr * vel_angular1_y) * dt; - orientation2_y = orientation1_y + - (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; - + ((cp * sy) * vel_linear1_x + (sr * sp * sy + cr * cy) * vel_linear1_y + + (cr * sp * sy - sr * cy) * vel_linear1_z) * + dt + + ((cp * sy) * acc_linear1_x + (sr * sp * sy + cr * cy) * acc_linear1_y + + (cr * sp * sy - sr * cy) * acc_linear1_z) * + dt2; + position2_z = position1_z + ((-sp) * vel_linear1_x + (sr * cp) * vel_linear1_y + (cr * cp) * vel_linear1_z) * dt + + ((-sp) * acc_linear1_x + (sr * cp) * acc_linear1_y + (cr * cp) * acc_linear1_z) * dt2; + + orientation2_r = + orientation1_r + (vel_angular1_r + sr * sp * cpi * vel_angular1_p + cr * sp * cpi * vel_angular1_y) * dt; + orientation2_p = orientation1_p + (cr * vel_angular1_p - sr * vel_angular1_y) * dt; + orientation2_y = orientation1_y + (sr * cpi * vel_angular1_p + cr * cpi * vel_angular1_y) * dt; + vel_linear2_x = vel_linear1_x + acc_linear1_x * dt; vel_linear2_y = vel_linear1_y + acc_linear1_y * dt; vel_linear2_z = vel_linear1_z + acc_linear1_z * dt; @@ -268,9 +229,11 @@ inline void predict( fuse_core::wrapAngle2D(orientation2_p); fuse_core::wrapAngle2D(orientation2_y); - if (jacobians) { + if (jacobians) + { // Jacobian wrt position1 - if (jacobians[0]) { + if (jacobians[0]) + { Eigen::Map> jacobian(jacobians[0]); jacobian.setZero(); // partial derivatives of position2 wrt orientation1 @@ -278,49 +241,51 @@ inline void predict( jacobian(1, 1) = 1.0; jacobian(2, 2) = 1.0; } - // Jacobian wrt orientation1 - if (jacobians[1]) { + // Jacobian wrt orientation1 + if (jacobians[1]) + { Eigen::Map> jacobian(jacobians[1]); Eigen::Map> j_quat2rpy_map(jacobian_quat2rpy); fuse_core::Matrix j_tmp; jacobian.setZero(); j_tmp.setZero(); - + // partial derivatives of position2_x wrt orientation1 - j_tmp(0, 0) = - ((cr * sp * cy + sr * sy) * vel_linear1_y + (- sr * sp * cy + cr * sy) * vel_linear1_z) * dt + - ((cr * sp * cy + sr * sy) * acc_linear1_y + (- sr * sp * cy + cr * sy) * acc_linear1_z) * dt2; - j_tmp(0, 1) = - ((-sp * cy) * vel_linear1_x + (sr * cp * cy) * vel_linear1_y + (cr * cp * cy) * vel_linear1_z) * dt + - ((-sp * cy) * acc_linear1_x + (sr * cp * cy) * acc_linear1_y + (cr * cp * cy) * acc_linear1_z) * dt2; - j_tmp(0, 2) = - ((-cp * sy) * vel_linear1_x + (- sr * sp * sy - cr * cy) * vel_linear1_y + (- cr * sp * sy + sr * cy) * vel_linear1_z) * dt + - ((-cp * sy) * acc_linear1_x + (- sr * sp * sy - cr * cy) * acc_linear1_y + (- cr * sp * sy + sr * cy) * acc_linear1_z) * dt2; - - // partial derivatives of position2_y wrt orientation1 - j_tmp(1, 0) = - ((- sr * cy + cr * sp * sy) * vel_linear1_y + (- cr * cy - sr * sp * sy) * vel_linear1_z) * dt + - ((- sr * cy + cr * sp * sy) * acc_linear1_y + (- cr * cy - sr * sp * sy) * acc_linear1_z) * dt2; - j_tmp(1, 1) = - ((-sp * sy) * vel_linear1_x + (sr * cp * sy) * vel_linear1_y + (cr * cp * sy) * vel_linear1_z) * dt + - ((-sp * sy) * acc_linear1_x + (sr * cp * sy) * acc_linear1_y + (cr * cp * sy) * acc_linear1_z) * dt2; - j_tmp(1, 2) = - ((cp * cy) * vel_linear1_x + (- cr * sy + sr * sp * cy) * vel_linear1_y + (sr * sy + cr * sp * cy) * vel_linear1_z) * dt + - ((cp * cy) * acc_linear1_x + (- cr * sy + sr * sp * cy) * acc_linear1_y + (sr * sy + cr * sp * cy) * acc_linear1_z) * dt2; - + j_tmp(0, 0) = ((cr * sp * cy + sr * sy) * vel_linear1_y + (-sr * sp * cy + cr * sy) * vel_linear1_z) * dt + + ((cr * sp * cy + sr * sy) * acc_linear1_y + (-sr * sp * cy + cr * sy) * acc_linear1_z) * dt2; + j_tmp(0, 1) = + ((-sp * cy) * vel_linear1_x + (sr * cp * cy) * vel_linear1_y + (cr * cp * cy) * vel_linear1_z) * dt + + ((-sp * cy) * acc_linear1_x + (sr * cp * cy) * acc_linear1_y + (cr * cp * cy) * acc_linear1_z) * dt2; + j_tmp(0, 2) = ((-cp * sy) * vel_linear1_x + (-sr * sp * sy - cr * cy) * vel_linear1_y + + (-cr * sp * sy + sr * cy) * vel_linear1_z) * + dt + + ((-cp * sy) * acc_linear1_x + (-sr * sp * sy - cr * cy) * acc_linear1_y + + (-cr * sp * sy + sr * cy) * acc_linear1_z) * + dt2; + + // partial derivatives of position2_y wrt orientation1 + j_tmp(1, 0) = ((-sr * cy + cr * sp * sy) * vel_linear1_y + (-cr * cy - sr * sp * sy) * vel_linear1_z) * dt + + ((-sr * cy + cr * sp * sy) * acc_linear1_y + (-cr * cy - sr * sp * sy) * acc_linear1_z) * dt2; + j_tmp(1, 1) = + ((-sp * sy) * vel_linear1_x + (sr * cp * sy) * vel_linear1_y + (cr * cp * sy) * vel_linear1_z) * dt + + ((-sp * sy) * acc_linear1_x + (sr * cp * sy) * acc_linear1_y + (cr * cp * sy) * acc_linear1_z) * dt2; + j_tmp(1, 2) = ((cp * cy) * vel_linear1_x + (-cr * sy + sr * sp * cy) * vel_linear1_y + + (sr * sy + cr * sp * cy) * vel_linear1_z) * + dt + + ((cp * cy) * acc_linear1_x + (-cr * sy + sr * sp * cy) * acc_linear1_y + + (sr * sy + cr * sp * cy) * acc_linear1_z) * + dt2; + // partial derivatives of position2_z wrt orientation1 - j_tmp(2, 0) = - ((cr * cp) * vel_linear1_y - (sr * cp) * vel_linear1_z) * dt + - ((cr * cp) * acc_linear1_y - (sr * cp) * acc_linear1_z) * dt2; - j_tmp(2, 1) = - (-cp * vel_linear1_x - (sr * sp) * vel_linear1_y - (cr * sp) * vel_linear1_z) * dt + - (-cp * acc_linear1_x - (sr * sp) * acc_linear1_y - (cr * sp) * acc_linear1_z) * dt2; + j_tmp(2, 0) = ((cr * cp) * vel_linear1_y - (sr * cp) * vel_linear1_z) * dt + + ((cr * cp) * acc_linear1_y - (sr * cp) * acc_linear1_z) * dt2; + j_tmp(2, 1) = (-cp * vel_linear1_x - (sr * sp) * vel_linear1_y - (cr * sp) * vel_linear1_z) * dt + + (-cp * acc_linear1_x - (sr * sp) * acc_linear1_y - (cr * sp) * acc_linear1_z) * dt2; // partial derivatives of orientation2_r wrt orientation1 - j_tmp(3, 0) = 1.0 + - ((cr * sp * cpi) * vel_angular1_p - (sr * sp * cpi) * vel_angular1_y) * dt; - j_tmp(3, 1) = - ((sr + sr * sp * sp * cpi * cpi) * vel_angular1_p + (cr + cr *sp * sp * cpi * cpi) * vel_angular1_y) * dt; + j_tmp(3, 0) = 1.0 + ((cr * sp * cpi) * vel_angular1_p - (sr * sp * cpi) * vel_angular1_y) * dt; + j_tmp(3, 1) = + ((sr + sr * sp * sp * cpi * cpi) * vel_angular1_p + (cr + cr * sp * sp * cpi * cpi) * vel_angular1_y) * dt; // partial derivatives of orientation2_p wrt orientation1 j_tmp(4, 0) = (-sr * vel_angular1_p - cr * vel_angular1_y) * dt; @@ -332,16 +297,17 @@ inline void predict( j_tmp(5, 2) = 1.0; jacobian.block<6, 4>(0, 0) = j_tmp * j_quat2rpy_map; - } + } // Jacobian wrt vel_linear1 - if (jacobians[2]) { + if (jacobians[2]) + { Eigen::Map> jacobian(jacobians[2]); jacobian.setZero(); // partial derivatives of position1_x wrt vel_linear1 jacobian(0, 0) = cp * cy * dt; jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt; - jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt; // partial derivatives of position1_y wrt vel_linear1 jacobian(1, 0) = cp * sy * dt; jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt; @@ -359,7 +325,8 @@ inline void predict( } // Jacobian wrt vel_angular1 - if (jacobians[3]) { + if (jacobians[3]) + { Eigen::Map> jacobian(jacobians[3]); jacobian.setZero(); @@ -382,13 +349,14 @@ inline void predict( } // Jacobian wrt acc_linear1 - if (jacobians[4]) { + if (jacobians[4]) + { Eigen::Map> jacobian(jacobians[4]); jacobian.setZero(); // partial derivatives of position1_x wrt acc_linear1 jacobian(0, 0) = cp * cy * dt2; jacobian(0, 1) = (sr * sp * cy - cr * sy) * dt2; - jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt2; + jacobian(0, 2) = (cr * sp * cy + sr * sy) * dt2; // partial derivatives of position1_y wrt acc_linear1 jacobian(1, 0) = cp * sy * dt2; jacobian(1, 1) = (sr * sp * sy + cr * cy) * dt2; @@ -427,52 +395,16 @@ inline void predict( * @param[out] vel_angular2 - Second yaw velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) */ -template -inline void predict( - const T * const position1, - const T * const orientation1, - const T * const vel_linear1, - const T * const vel_angular1, - const T * const acc_linear1, - const T dt, - T * const position2, - T * const orientation2, - T * const vel_linear2, - T * const vel_angular2, - T * const acc_linear2) +template +inline void predict(const T* const position1, const T* const orientation1, const T* const vel_linear1, + const T* const vel_angular1, const T* const acc_linear1, const T dt, T* const position2, + T* const orientation2, T* const vel_linear2, T* const vel_angular2, T* const acc_linear2) { - predict( - position1[0], - position1[1], - position1[2], - orientation1[0], - orientation1[1], - orientation1[2], - vel_linear1[0], - vel_linear1[1], - vel_linear1[2], - vel_angular1[0], - vel_angular1[1], - vel_angular1[2], - acc_linear1[0], - acc_linear1[1], - acc_linear1[2], - dt, - position2[0], - position2[1], - position2[2], - orientation2[0], - orientation2[1], - orientation2[2], - vel_linear2[0], - vel_linear2[1], - vel_linear2[2], - vel_angular2[0], - vel_angular2[1], - vel_angular2[2], - acc_linear2[0], - acc_linear2[1], - acc_linear2[2]); + predict(position1[0], position1[1], position1[2], orientation1[0], orientation1[1], orientation1[2], vel_linear1[0], + vel_linear1[1], vel_linear1[2], vel_angular1[0], vel_angular1[1], vel_angular1[2], acc_linear1[0], + acc_linear1[1], acc_linear1[2], dt, position2[0], position2[1], position2[2], orientation2[0], + orientation2[1], orientation2[2], vel_linear2[0], vel_linear2[1], vel_linear2[2], vel_angular2[0], + vel_angular2[1], vel_angular2[2], acc_linear2[0], acc_linear2[1], acc_linear2[2]); } /** * @brief Given a state and time delta, predicts a new state @@ -488,62 +420,26 @@ inline void predict( * @param[out] vel_angular2 - Second angular velocity * @param[out] acc_linear2 - Second linear acceleration */ -inline void predict( - const fuse_core::Vector3d & position1, - const Eigen::Quaterniond & orientation1, - const fuse_core::Vector3d & vel_linear1, - const fuse_core::Vector3d & vel_angular1, - const fuse_core::Vector3d & acc_linear1, - const double dt, - fuse_core::Vector3d & position2, - Eigen::Quaterniond & orientation2, - fuse_core::Vector3d & vel_linear2, - fuse_core::Vector3d & vel_angular2, - fuse_core::Vector3d & acc_linear2) +inline void predict(const fuse_core::Vector3d& position1, const Eigen::Quaterniond& orientation1, + const fuse_core::Vector3d& vel_linear1, const fuse_core::Vector3d& vel_angular1, + const fuse_core::Vector3d& acc_linear1, const double dt, fuse_core::Vector3d& position2, + Eigen::Quaterniond& orientation2, fuse_core::Vector3d& vel_linear2, + fuse_core::Vector3d& vel_angular2, fuse_core::Vector3d& acc_linear2) { - fuse_core::Vector3d rpy( - fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), - fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()) - ); - - predict( - position1.x(), - position1.y(), - position1.z(), - rpy.x(), - rpy.y(), - rpy.z(), - vel_linear1.x(), - vel_linear1.y(), - vel_linear1.z(), - vel_angular1.x(), - vel_angular1.y(), - vel_angular1.z(), - acc_linear1.x(), - acc_linear1.y(), - acc_linear1.z(), - dt, - position2.x(), - position2.y(), - position2.z(), - rpy.x(), - rpy.y(), - rpy.z(), - vel_linear2.x(), - vel_linear2.y(), - vel_linear2.z(), - vel_angular2.x(), - vel_angular2.y(), - vel_angular2.z(), - acc_linear2.x(), - acc_linear2.y(), - acc_linear2.z()); + fuse_core::Vector3d rpy(fuse_core::getRoll(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getPitch(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()), + fuse_core::getYaw(orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z())); + + predict(position1.x(), position1.y(), position1.z(), rpy.x(), rpy.y(), rpy.z(), vel_linear1.x(), vel_linear1.y(), + vel_linear1.z(), vel_angular1.x(), vel_angular1.y(), vel_angular1.z(), acc_linear1.x(), acc_linear1.y(), + acc_linear1.z(), dt, position2.x(), position2.y(), position2.z(), rpy.x(), rpy.y(), rpy.z(), vel_linear2.x(), + vel_linear2.y(), vel_linear2.z(), vel_angular2.x(), vel_angular2.y(), vel_angular2.z(), acc_linear2.x(), + acc_linear2.y(), acc_linear2.z()); // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy.z(), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(rpy.y(), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy.x(), Eigen::Vector3d::UnitX()); } /** @@ -561,22 +457,13 @@ inline void predict( * @param[out] acc_linear2 - Second linear acceleration * @param[out] jacobian - Jacobian wrt the state */ -inline void predict( - const fuse_core::Vector3d & position1, - const Eigen::Quaterniond & orientation1, - const fuse_core::Vector3d & vel_linear1, - const fuse_core::Vector3d & vel_angular1, - const fuse_core::Vector3d & acc_linear1, - const double dt, - fuse_core::Vector3d & position2, - Eigen::Quaterniond & orientation2, - fuse_core::Vector3d & vel_linear2, - fuse_core::Vector3d & vel_angular2, - fuse_core::Vector3d & acc_linear2, - fuse_core::Matrix15d & jacobian) +inline void predict(const fuse_core::Vector3d& position1, const Eigen::Quaterniond& orientation1, + const fuse_core::Vector3d& vel_linear1, const fuse_core::Vector3d& vel_angular1, + const fuse_core::Vector3d& acc_linear1, const double dt, fuse_core::Vector3d& position2, + Eigen::Quaterniond& orientation2, fuse_core::Vector3d& vel_linear2, + fuse_core::Vector3d& vel_angular2, fuse_core::Vector3d& acc_linear2, fuse_core::Matrix15d& jacobian) { - - double quat[4] = {orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z()}; + double quat[4] = { orientation1.w(), orientation1.x(), orientation1.y(), orientation1.z() }; double rpy[3]; double jacobian_quat2rpy[12]; fuse_core::quaternion2rpy(quat, rpy, jacobian_quat2rpy); @@ -584,62 +471,34 @@ inline void predict( // fuse_core::Matrix15d is Eigen::RowMajor, so we cannot use pointers to the columns where each // parameter block starts. Instead, we need to create a vector of Eigen::RowMajor matrices per // parameter block and later reconstruct the fuse_core::Matrix15d with the full jacobian. The - // parameter blocks have the following sizes: {position1: 3, orientation1: 4, vel_linear1: 3, + // parameter blocks have the following sizes: {position1: 3, orientation1: 4, vel_linear1: 3, // vel_angular1: 3, acc_linear1: 3} - static constexpr size_t num_residuals{15}; - static constexpr size_t num_parameter_blocks{5}; - static const std::array block_sizes = {3, 4, 3, 3, 3}; + static constexpr size_t num_residuals{ 15 }; + static constexpr size_t num_parameter_blocks{ 5 }; + static const std::array block_sizes = { 3, 4, 3, 3, 3 }; std::array J; - std::array jacobians; + std::array jacobians; - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } - predict( - position1.x(), - position1.y(), - position1.z(), - rpy[0], - rpy[1], - rpy[2], - vel_linear1.x(), - vel_linear1.y(), - vel_linear1.z(), - vel_angular1.x(), - vel_angular1.y(), - vel_angular1.z(), - acc_linear1.x(), - acc_linear1.y(), - acc_linear1.z(), - dt, - position2.x(), - position2.y(), - position2.z(), - rpy[0], - rpy[1], - rpy[2], - vel_linear2.x(), - vel_linear2.y(), - vel_linear2.z(), - vel_angular2.x(), - vel_angular2.y(), - vel_angular2.z(), - acc_linear2.x(), - acc_linear2.y(), - acc_linear2.z(), - jacobians.data(), - jacobian_quat2rpy); + predict(position1.x(), position1.y(), position1.z(), rpy[0], rpy[1], rpy[2], vel_linear1.x(), vel_linear1.y(), + vel_linear1.z(), vel_angular1.x(), vel_angular1.y(), vel_angular1.z(), acc_linear1.x(), acc_linear1.y(), + acc_linear1.z(), dt, position2.x(), position2.y(), position2.z(), rpy[0], rpy[1], rpy[2], vel_linear2.x(), + vel_linear2.y(), vel_linear2.z(), vel_angular2.x(), vel_angular2.y(), vel_angular2.z(), acc_linear2.x(), + acc_linear2.y(), acc_linear2.z(), jacobians.data(), jacobian_quat2rpy); jacobian << J[0], J[1], J[2], J[3], J[4]; // Convert back to quaternion orientation2 = Eigen::AngleAxisd(rpy[2], Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()); + Eigen::AngleAxisd(rpy[1], Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy[0], Eigen::Vector3d::UnitX()); } } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp index 730d48def..0fcaf9063 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp @@ -42,7 +42,6 @@ #include #include - namespace fuse_models { @@ -74,7 +73,7 @@ namespace fuse_models * || [ x_acc_t2 - proj(x_acc_t1) ] || * || [ y_acc_t2 - proj(y_acc_t1) ] || * || [ z_acc_t2 - proj(z_acc_t1) ] || - * + * * where, the matrix A is fixed, the state variables are provided at two discrete time steps, and * proj is a function that projects the state variables from time t1 to time t2. In case the user is * interested in implementing a cost function of the form @@ -94,22 +93,22 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * * @param[in] dt The time delta across which to generate the kinematic model cost * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * order (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Omnidirectional3DStateCostFunction(const double dt, const fuse_core::Matrix15d & A); + Omnidirectional3DStateCostFunction(const double dt, const fuse_core::Matrix15d& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. * * @param[in] parameters - Parameter blocks: - * 0 : position1 - First position (array with x at index 0, y at index 1, + * 0 : position1 - First position (array with x at index 0, y at index 1, * z at index 2) * 1 : orientation1 - First orientation (array with w at index 0, x at index 1, * y at index 2, z at index 3) * 2 : vel_linear1 - First linear velocity (array with x at index 0, y at * index 1, z at index 2) - * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, + * 3 : vel_angular1 - First angular velocity (array with vroll at index 0, * vpitch at index 1, vyaw at index 2) * 4 : acc_linear1 - First linear acceleration (array with x at index 0, y * at index 1, z at index 2) @@ -121,7 +120,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * index 1, z at index 2) * 8 : vel_angular2 - Second angular velocity (array with vroll at index 0, * vpitch at index 1, vyaw at index 2) - * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at + * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at * index 1) * @param[out] residual - The computed residual (error) * @param[out] jacobians - Jacobians of the residuals wrt the parameters. Only computed if not @@ -130,10 +129,7 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 * @return The return value indicates whether the computation of the residuals and/or jacobians * was successful or not. */ - bool Evaluate( - double const * const * parameters, - double * residuals, - double ** jacobians) const override + bool Evaluate(double const* const* parameters, double* residuals, double** jacobians) const override { double position_pred[3]; double orientation_pred_rpy[3]; @@ -147,40 +143,25 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 fuse_core::quaternion2rpy(parameters[1], orientation1_rpy, j1_quat2rpy); fuse_core::quaternion2rpy(parameters[6], orientation2_rpy, j2_quat2rpy); - predict( - parameters[0][0], // position1_x - parameters[0][1], // position1_y - parameters[0][2], // position1_z - orientation1_rpy[0], // orientation1_roll - orientation1_rpy[1], // orientation1_pitch - orientation1_rpy[2], // orientation1_yaw - parameters[2][0], // vel_linear1_x - parameters[2][1], // vel_linear1_y - parameters[2][2], // vel_linear1_z - parameters[3][0], // vel_angular1_roll - parameters[3][1], // vel_angular1_pitch - parameters[3][2], // vel_angular1_yaw - parameters[4][0], // acc_linear1_x - parameters[4][1], // acc_linear1_y - parameters[4][2], // acc_linear1_z - dt_, - position_pred[0], - position_pred[1], - position_pred[2], - orientation_pred_rpy[0], - orientation_pred_rpy[1], - orientation_pred_rpy[2], - vel_linear_pred[0], - vel_linear_pred[1], - vel_linear_pred[2], - vel_angular_pred[0], - vel_angular_pred[1], - vel_angular_pred[2], - acc_linear_pred[0], - acc_linear_pred[1], - acc_linear_pred[2], - jacobians, - j1_quat2rpy); + predict(parameters[0][0], // position1_x + parameters[0][1], // position1_y + parameters[0][2], // position1_z + orientation1_rpy[0], // orientation1_roll + orientation1_rpy[1], // orientation1_pitch + orientation1_rpy[2], // orientation1_yaw + parameters[2][0], // vel_linear1_x + parameters[2][1], // vel_linear1_y + parameters[2][2], // vel_linear1_z + parameters[3][0], // vel_angular1_roll + parameters[3][1], // vel_angular1_pitch + parameters[3][2], // vel_angular1_yaw + parameters[4][0], // acc_linear1_x + parameters[4][1], // acc_linear1_y + parameters[4][2], // acc_linear1_z + dt_, position_pred[0], position_pred[1], position_pred[2], orientation_pred_rpy[0], orientation_pred_rpy[1], + orientation_pred_rpy[2], vel_linear_pred[0], vel_linear_pred[1], vel_linear_pred[2], vel_angular_pred[0], + vel_angular_pred[1], vel_angular_pred[2], acc_linear_pred[0], acc_linear_pred[1], acc_linear_pred[2], + jacobians, j1_quat2rpy); residuals[0] = parameters[5][0] - position_pred[0]; residuals[1] = parameters[5][1] - position_pred[1]; @@ -207,64 +188,75 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 Eigen::Map residuals_map(residuals); residuals_map.applyOnTheLeft(A_); - if (jacobians) { + if (jacobians) + { // Update jacobian wrt position1 - if (jacobians[0]) { + if (jacobians[0]) + { Eigen::Map> jacobian(jacobians[0]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt orientation1 - if (jacobians[1]) { + if (jacobians[1]) + { Eigen::Map> jacobian(jacobians[1]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_linear1 - if (jacobians[2]) { + if (jacobians[2]) + { Eigen::Map> jacobian(jacobians[2]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt vel_yaw1 - if (jacobians[3]) { + if (jacobians[3]) + { Eigen::Map> jacobian(jacobians[3]); jacobian.applyOnTheLeft(-A_); } // Update jacobian wrt acc_linear1 - if (jacobians[4]) { + if (jacobians[4]) + { Eigen::Map> jacobian(jacobians[4]); jacobian.applyOnTheLeft(-A_); } - + // Jacobian wrt position2 - if (jacobians[5]) { + if (jacobians[5]) + { Eigen::Map> jacobian(jacobians[5]); jacobian = A_.block<15, 3>(0, 0); } // Jacobian wrt orientation2 - if (jacobians[6]) { + if (jacobians[6]) + { Eigen::Map> jacobian(jacobians[6]); Eigen::Map> j2_quat2rpy_map(j2_quat2rpy); jacobian = A_.block<15, 3>(0, 3) * j2_quat2rpy_map; } // Jacobian wrt vel_linear2 - if (jacobians[7]) { + if (jacobians[7]) + { Eigen::Map> jacobian(jacobians[7]); jacobian = A_.block<15, 3>(0, 6); } // Jacobian wrt vel_angular2 - if (jacobians[8]) { + if (jacobians[8]) + { Eigen::Map> jacobian(jacobians[8]); jacobian = A_.block<15, 3>(0, 9); } // Jacobian wrt acc_linear2 - if (jacobians[9]) { + if (jacobians[9]) + { Eigen::Map> jacobian(jacobians[9]); jacobian = A_.block<15, 3>(0, 12); } @@ -279,11 +271,8 @@ class Omnidirectional3DStateCostFunction : public ceres::SizedCostFunction<15, 3 //!< information matrix }; -Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction( - const double dt, - const fuse_core::Matrix15d & A) -: dt_(dt), - A_(A) +Omnidirectional3DStateCostFunction::Omnidirectional3DStateCostFunction(const double dt, const fuse_core::Matrix15d& A) + : dt_(dt), A_(A) { } diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp index d95523a01..0ccafd8fc 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp @@ -40,7 +40,6 @@ #include #include - namespace fuse_models { @@ -92,66 +91,52 @@ class Omnidirectional3DStateCostFunctor * * @param[in] dt The time delta across which to generate the kinematic model cost * @param[in] A The residual weighting matrix, most likely the square root information matrix in - * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * order (x, y, z, qx, qy, qz, qw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Omnidirectional3DStateCostFunctor(const double dt, const fuse_core::Matrix15d & A); + Omnidirectional3DStateCostFunctor(const double dt, const fuse_core::Matrix15d& A); /** * @brief Evaluate the cost function. Used by the Ceres optimization engine. * @param[in] position1 - First position (array with x at index 0, y at index 1, z at index 2) - * @param[in] orientation1 - First orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[in] orientation1 - First orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) + * check this order * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1, z at index 2) * @param[in] vel_angular1 - First angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1, z at index 2) * @param[in] dt - The time delta across which to predict the state * @param[out] position2 - Second position (array with x at index 0, y at index 1, z at index 2) - * @param[out] orientation2 - Second orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) check this order + * @param[out] orientation2 - Second orientation (array with w at index 0, x at index 1, y at index 2, z at index 3) + * check this order * @param[out] vel_linear2 - Second velocity (array with x at index 0, y at index 1, z at index 2) - * @param[out] vel_angular2 - Second angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index 2) + * @param[out] vel_angular2 - Second angular velocity (array with vroll at index 0, vpitch at index 1, vyaw at index + * 2) * @param[out] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) */ - template - bool operator()( - const T * const position1, - const T * const orientation1, - const T * const vel_linear1, - const T * const vel_angular1, - const T * const acc_linear1, - const T * const position2, - const T * const orientation2, - const T * const vel_linear2, - const T * const vel_angular2, - const T * const acc_linear2, - T * residual) const; + template + bool operator()(const T* const position1, const T* const orientation1, const T* const vel_linear1, + const T* const vel_angular1, const T* const acc_linear1, const T* const position2, + const T* const orientation2, const T* const vel_linear2, const T* const vel_angular2, + const T* const acc_linear2, T* residual) const; private: double dt_; fuse_core::Matrix15d A_; //!< The residual weighting matrix, most likely the square root - //!< information matrix + //!< information matrix }; -Omnidirectional3DStateCostFunctor::Omnidirectional3DStateCostFunctor( - const double dt, - const fuse_core::Matrix15d & A) -: dt_(dt), - A_(A) +Omnidirectional3DStateCostFunctor::Omnidirectional3DStateCostFunctor(const double dt, const fuse_core::Matrix15d& A) + : dt_(dt), A_(A) { } -template -bool Omnidirectional3DStateCostFunctor::operator()( - const T * const position1, - const T * const orientation1, - const T * const vel_linear1, - const T * const vel_angular1, - const T * const acc_linear1, - const T * const position2, - const T * const orientation2, - const T * const vel_linear2, - const T * const vel_angular2, - const T * const acc_linear2, - T * residual) const +template +bool Omnidirectional3DStateCostFunctor::operator()(const T* const position1, const T* const orientation1, + const T* const vel_linear1, const T* const vel_angular1, + const T* const acc_linear1, const T* const position2, + const T* const orientation2, const T* const vel_linear2, + const T* const vel_angular2, const T* const acc_linear2, + T* residual) const { T position_pred[3]; T orientation_pred[3]; @@ -160,30 +145,16 @@ bool Omnidirectional3DStateCostFunctor::operator()( T acc_linear_pred[3]; // Convert orientation variables from quaternion to roll-pitch-yaw - const T orientation1_rpy[3] { - fuse_core::getRoll(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), - fuse_core::getPitch(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), - fuse_core::getYaw(orientation1[0], orientation1[1], orientation1[2], orientation1[3]) - }; - const T orientation2_rpy[3] { - fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), - fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), - fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]) - }; - - predict( - position1, - orientation1_rpy, - vel_linear1, - vel_angular1, - acc_linear1, - T(dt_), - position_pred, - orientation_pred, - vel_linear_pred, - vel_angular_pred, - acc_linear_pred); - + const T orientation1_rpy[3]{ fuse_core::getRoll(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), + fuse_core::getPitch(orientation1[0], orientation1[1], orientation1[2], orientation1[3]), + fuse_core::getYaw(orientation1[0], orientation1[1], orientation1[2], orientation1[3]) }; + const T orientation2_rpy[3]{ fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), + fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]), + fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]) }; + + predict(position1, orientation1_rpy, vel_linear1, vel_angular1, acc_linear1, T(dt_), position_pred, orientation_pred, + vel_linear_pred, vel_angular_pred, acc_linear_pred); + Eigen::Map> residuals_map(residual); residuals_map(0) = position2[0] - position_pred[0]; residuals_map(1) = position2[1] - position_pred[1]; diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp index a61de7bab..18273e190 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp @@ -53,7 +53,6 @@ #include #include - namespace fuse_models { @@ -91,22 +90,21 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * @param[in] velocity_angular2 Angular velocity component variable of the second state * @param[in] acceleration_linear2 Linear acceleration component variable of the second state * @param[in] covariance The covariance matrix used to weight the constraint. Order is (x, y, z, - * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - Omnidirectional3DStateKinematicConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::VelocityLinear3DStamped & velocity_linear1, - const fuse_variables::VelocityAngular3DStamped & velocity_angular1, - const fuse_variables::AccelerationLinear3DStamped & acceleration_linear1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_variables::VelocityLinear3DStamped & velocity_linear2, - const fuse_variables::VelocityAngular3DStamped & velocity_angular2, - const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, - const fuse_core::Matrix15d & covariance); + Omnidirectional3DStateKinematicConstraint(const std::string& source, + const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::VelocityLinear3DStamped& velocity_linear1, + const fuse_variables::VelocityAngular3DStamped& velocity_angular1, + const fuse_variables::AccelerationLinear3DStamped& acceleration_linear1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_variables::VelocityLinear3DStamped& velocity_linear2, + const fuse_variables::VelocityAngular3DStamped& velocity_angular2, + const fuse_variables::AccelerationLinear3DStamped& acceleration_linear2, + const fuse_core::Matrix15d& covariance); /** * @brief Destructor @@ -117,20 +115,26 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * @brief Read-only access to the time delta between the first and second state (really, between * the position1 and position2 variables in the constructor) */ - double dt() const {return dt_;} + double dt() const + { + return dt_; + } /** * @brief Read-only access to the square root information matrix. * - * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ - const fuse_core::Matrix15d & sqrtInformation() const {return sqrt_information_;} + const fuse_core::Matrix15d& sqrtInformation() const + { + return sqrt_information_; + } /** * @brief Compute the measurement covariance matrix. * - * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * Order is (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, * x_acc, y_acc, z_acc) */ fuse_core::Matrix15d covariance() const @@ -143,7 +147,7 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * * @param[out] stream The stream to write to. Defaults to stdout. */ - void print(std::ostream & stream = std::cout) const override; + void print(std::ostream& stream = std::cout) const override; /** * @brief Construct an instance of this constraint's cost function @@ -155,10 +159,10 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * * @return A base pointer to an instance of a derived CostFunction. */ - ceres::CostFunction * costFunction() const override; + ceres::CostFunction* costFunction() const override; protected: - double dt_; //!< The time delta for the constraint + double dt_; //!< The time delta for the constraint fuse_core::Matrix15d sqrt_information_; //!< The square root information matrix private: @@ -172,12 +176,12 @@ class Omnidirectional3DStateKinematicConstraint : public fuse_core::Constraint * @param[in/out] archive - The archive object that holds the serialized class members * @param[in] version - The version of the archive being read/written. Generally unused. */ - template - void serialize(Archive & archive, const unsigned int /* version */) + template + void serialize(Archive& archive, const unsigned int /* version */) { - archive & boost::serialization::base_object(*this); - archive & dt_; - archive & sqrt_information_; + archive& boost::serialization::base_object(*this); + archive& dt_; + archive& sqrt_information_; } }; diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp index 1c0dda5c5..c0df539a7 100644 --- a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -45,7 +45,6 @@ #include #include - namespace fuse_models { @@ -65,147 +64,80 @@ struct Imu3DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - angular_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_dimensions")); - linear_acceleration_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_acceleration_dimensions")); + angular_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); + linear_acceleration_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "linear_acceleration_dimensions")); orientation_indices = loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); disable_checks = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "disable_checks"), - disable_checks); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_period"), throttle_period, - false); - throttle_use_wall_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_use_wall_time"), - throttle_use_wall_time); - - remove_gravitational_acceleration = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, "remove_gravitational_acceleration"), remove_gravitational_acceleration); - gravitational_acceleration = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "gravitational_acceleration"), - gravitational_acceleration); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); + + remove_gravitational_acceleration = + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "remove_gravitational_acceleration"), + remove_gravitational_acceleration); + gravitational_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "gravitational_acceleration"), gravitational_acceleration); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - use_twist_covariance = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "use_twist_covariance"), - use_twist_covariance); - - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<6>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); - twist_covariance_offset = - fuse_core::getCovarianceDiagonalParam<6>( - interfaces, - fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + use_twist_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<6>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = fuse_core::getCovarianceDiagonalParam<6>( + interfaces, fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); } - acceleration_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "acceleration_target_frame"), - acceleration_target_frame); - orientation_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_target_frame"), - orientation_target_frame); + acceleration_target_frame = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "acceleration_target_frame"), acceleration_target_frame); + orientation_target_frame = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "orientation_target_frame"), orientation_target_frame); twist_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "twist_target_frame"), - twist_target_frame); - - pose_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "twist_target_frame"), twist_target_frame); + + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); angular_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_velocity_loss")); linear_acceleration_loss = - fuse_core::loadLossConfig( - interfaces, - fuse_core::joinParameterName(ns, "linear_acceleration_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_acceleration_loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; - bool use_twist_covariance {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; + bool use_twist_covariance{ true }; fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance - //!< matrix, that will be substracted in order to - //!< recover the raw values - bool remove_gravitational_acceleration {false}; - int queue_size {10}; - rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become - //!< available - rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds - bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not - double gravitational_acceleration {9.80665}; - std::string acceleration_target_frame {}; - std::string orientation_target_frame {}; - std::string topic {}; - std::string twist_target_frame {}; + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + bool remove_gravitational_acceleration{ false }; + int queue_size{ 10 }; + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + double gravitational_acceleration{ 9.80665 }; + std::string acceleration_target_frame{}; + std::string orientation_target_frame{}; + std::string topic{}; + std::string twist_target_frame{}; std::vector angular_velocity_indices; std::vector linear_acceleration_indices; std::vector orientation_indices; diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp index 1e01a073c..734e75c98 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -46,7 +46,6 @@ #include #include - namespace fuse_models { @@ -66,137 +65,73 @@ struct Odometry3DParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - position_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "position_dimensions")); - orientation_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "orientation_dimensions")); - linear_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_velocity_dimensions")); - angular_velocity_indices = - loadSensorConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_dimensions")); - - differential = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "differential"), - differential); + position_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "position_dimensions")); + orientation_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "orientation_dimensions")); + linear_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "linear_velocity_dimensions")); + angular_velocity_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName(ns, "angular_velocity_dimensions")); + + differential = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "differential"), differential); disable_checks = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "disable_checks"), - disable_checks); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_period"), throttle_period, - false); - throttle_use_wall_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "throttle_use_wall_time"), - throttle_use_wall_time); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "disable_checks"), disable_checks); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "throttle_period"), throttle_period, false); + throttle_use_wall_time = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "throttle_use_wall_time"), + throttle_use_wall_time); fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); twist_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "twist_target_frame"), - twist_target_frame); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "twist_target_frame"), twist_target_frame); pose_target_frame = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "pose_target_frame"), - pose_target_frame); - - if (differential) { - independent = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "independent"), - independent); - use_twist_covariance = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "use_twist_covariance"), - use_twist_covariance); - - minimum_pose_relative_covariance = - fuse_core::getCovarianceDiagonalParam<6>( - interfaces, - fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); - twist_covariance_offset = - fuse_core::getCovarianceDiagonalParam<6>( - interfaces, - fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "pose_target_frame"), pose_target_frame); + + if (differential) + { + independent = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "independent"), independent); + use_twist_covariance = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = fuse_core::getCovarianceDiagonalParam<6>( + interfaces, fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = fuse_core::getCovarianceDiagonalParam<6>( + interfaces, fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); } - pose_loss = - fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + pose_loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); linear_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "linear_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "linear_velocity_loss")); angular_velocity_loss = - fuse_core::loadLossConfig( - interfaces, fuse_core::joinParameterName( - ns, - "angular_velocity_loss")); + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "angular_velocity_loss")); } - bool differential {false}; - bool disable_checks {false}; - bool independent {true}; - bool use_twist_covariance {true}; + bool differential{ false }; + bool disable_checks{ false }; + bool independent{ true }; + bool use_twist_covariance{ true }; fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance //!< matrix - fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance - //!< matrix, that will be substracted in order to - //!< recover the raw values - int queue_size {10}; - rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become - //!< available - rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds - bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not - std::string topic {}; - std::string pose_target_frame {}; - std::string twist_target_frame {}; + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + int queue_size{ 10 }; + rclcpp::Duration tf_timeout{ 0, 0 }; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + bool throttle_use_wall_time{ false }; //!< Whether to throttle using ros::WallTime or not + std::string topic{}; + std::string pose_target_frame{}; + std::string twist_target_frame{}; std::vector position_indices; std::vector orientation_indices; std::vector linear_velocity_indices; diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp index 64d2d1380..21cb0a29e 100644 --- a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -49,7 +49,6 @@ #include - namespace fuse_models { @@ -71,174 +70,95 @@ struct Odometry3DPublisherParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { - publish_tf = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_tf"), - publish_tf); - invert_tf = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "invert_tf"), - invert_tf); - predict_to_current_time = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "predict_to_current_time"), - predict_to_current_time); - predict_with_acceleration = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "predict_with_acceleration"), - predict_with_acceleration); + publish_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_tf"), publish_tf); + invert_tf = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "invert_tf"), invert_tf); + predict_to_current_time = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "predict_to_current_time"), predict_to_current_time); + predict_with_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "predict_with_acceleration"), predict_with_acceleration); publish_frequency = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_frequency"), - publish_frequency); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_frequency"), publish_frequency); process_noise_covariance = fuse_core::getCovarianceDiagonalParam<15>( - interfaces, fuse_core::joinParameterName( - ns, - "process_noise_diagonal"), 0.0); + interfaces, fuse_core::joinParameterName(ns, "process_noise_diagonal"), 0.0); scale_process_noise = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "scale_process_noise"), - scale_process_noise); - velocity_linear_norm_min_ = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "velocity_linear_norm_min"), - velocity_linear_norm_min_); - velocity_angular_norm_min_ = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "velocity_angular_norm_min"), - velocity_angular_norm_min_); - fuse_core::getPositiveParam( - interfaces, - fuse_core::joinParameterName( - ns, - "covariance_throttle_period"), covariance_throttle_period, - false); - - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_cache_time"), tf_cache_time, - false); - fuse_core::getPositiveParam( - interfaces, fuse_core::joinParameterName( - ns, - "tf_timeout"), tf_timeout, - false); - - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - - map_frame_id = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "map_frame_id"), - map_frame_id); - odom_frame_id = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "odom_frame_id"), - odom_frame_id); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "scale_process_noise"), scale_process_noise); + velocity_linear_norm_min_ = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "velocity_linear_norm_min"), velocity_linear_norm_min_); + velocity_angular_norm_min_ = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "velocity_angular_norm_min"), velocity_angular_norm_min_); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "covariance_throttle_period"), + covariance_throttle_period, false); + + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_cache_time"), tf_cache_time, false); + fuse_core::getPositiveParam(interfaces, fuse_core::joinParameterName(ns, "tf_timeout"), tf_timeout, false); + + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + + map_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "map_frame_id"), map_frame_id); + odom_frame_id = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "odom_frame_id"), odom_frame_id); base_link_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "base_link_frame_id"), - base_link_frame_id); - base_link_output_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "base_link_output_frame_id"), - base_link_output_frame_id); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "base_link_frame_id"), base_link_frame_id); + base_link_output_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "base_link_output_frame_id"), base_link_output_frame_id); world_frame_id = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "world_frame_id"), - world_frame_id); - - const bool frames_valid = - map_frame_id != odom_frame_id && - map_frame_id != base_link_frame_id && - map_frame_id != base_link_output_frame_id && - odom_frame_id != base_link_frame_id && - odom_frame_id != base_link_output_frame_id && - (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); - - if (!frames_valid) { - RCLCPP_FATAL_STREAM( - interfaces.get_node_logging_interface()->get_logger(), - "Invalid frame configuration! Please note:\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " - << "unique\n" - << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " - << "unique\n" - << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "world_frame_id"), world_frame_id); + + const bool frames_valid = map_frame_id != odom_frame_id && map_frame_id != base_link_frame_id && + map_frame_id != base_link_output_frame_id && odom_frame_id != base_link_frame_id && + odom_frame_id != base_link_output_frame_id && + (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); + + if (!frames_valid) + { + RCLCPP_FATAL_STREAM(interfaces.get_node_logging_interface()->get_logger(), + "Invalid frame configuration! Please note:\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " + << "unique\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " + << "unique\n" + << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); assert(frames_valid); } topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); acceleration_topic = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "acceleration_topic"), - acceleration_topic); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "acceleration_topic"), acceleration_topic); fuse_core::loadCovarianceOptionsFromROS(interfaces, covariance_options, "covariance_options"); } - bool publish_tf {true}; //!< Whether to publish/broadcast the TF transform or not - bool invert_tf{false}; //!< Whether to broadcast the inverse of the TF transform or not. When - //!< the inverse is broadcasted, the transform is inverted and the - //!< header.frame_id and child_frame_id are swapped, i.e. the odometry - //!< output header.frame_id is set to the base_link_output_frame_id and - //!< the child_frame_id to the world_frame_id - bool predict_to_current_time {false}; - bool predict_with_acceleration {false}; - double publish_frequency {10.0}; - fuse_core::Matrix15d process_noise_covariance; //!< Process noise covariance matrix - bool scale_process_noise{false}; - double velocity_linear_norm_min_{1e-3}; - double velocity_angular_norm_min_{1e-3}; - rclcpp::Duration covariance_throttle_period {0, 0}; //!< The throttle period duration in seconds - //!< to compute the covariance - rclcpp::Duration tf_cache_time {10, 0}; - rclcpp::Duration tf_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; - int queue_size {1}; - std::string map_frame_id {"map"}; - std::string odom_frame_id {"odom"}; - std::string base_link_frame_id {"base_link"}; - std::string base_link_output_frame_id {base_link_frame_id}; - std::string world_frame_id {odom_frame_id}; - std::string topic {"odometry/filtered"}; - std::string acceleration_topic {"acceleration/filtered"}; + bool publish_tf{ true }; //!< Whether to publish/broadcast the TF transform or not + bool invert_tf{ false }; //!< Whether to broadcast the inverse of the TF transform or not. When + //!< the inverse is broadcasted, the transform is inverted and the + //!< header.frame_id and child_frame_id are swapped, i.e. the odometry + //!< output header.frame_id is set to the base_link_output_frame_id and + //!< the child_frame_id to the world_frame_id + bool predict_to_current_time{ false }; + bool predict_with_acceleration{ false }; + double publish_frequency{ 10.0 }; + fuse_core::Matrix15d process_noise_covariance; //!< Process noise covariance matrix + bool scale_process_noise{ false }; + double velocity_linear_norm_min_{ 1e-3 }; + double velocity_angular_norm_min_{ 1e-3 }; + rclcpp::Duration covariance_throttle_period{ 0, 0 }; //!< The throttle period duration in seconds + //!< to compute the covariance + rclcpp::Duration tf_cache_time{ 10, 0 }; + rclcpp::Duration tf_timeout{ 0, static_cast(RCUTILS_S_TO_NS(0.1)) }; + int queue_size{ 1 }; + std::string map_frame_id{ "map" }; + std::string odom_frame_id{ "odom" }; + std::string base_link_frame_id{ "base_link" }; + std::string base_link_output_frame_id{ base_link_frame_id }; + std::string world_frame_id{ odom_frame_id }; + std::string topic{ "odometry/filtered" }; + std::string acceleration_topic{ "acceleration/filtered" }; ceres::Covariance::Options covariance_options; }; diff --git a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp index 053ac5572..b84d1c253 100644 --- a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp @@ -44,7 +44,6 @@ #include #include - namespace fuse_models { @@ -64,88 +63,54 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase * @param[in] ns - The parameter namespace to use */ void loadFromROS( - fuse_core::node_interfaces::NodeInterfaces< - fuse_core::node_interfaces::Base, - fuse_core::node_interfaces::Logging, - fuse_core::node_interfaces::Parameters - > interfaces, - const std::string & ns) + fuse_core::node_interfaces::NodeInterfaces + interfaces, + const std::string& ns) { publish_on_startup = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "publish_on_startup"), - publish_on_startup); - queue_size = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "queue_size"), - queue_size); - reset_service = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "reset_service"), - reset_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "publish_on_startup"), publish_on_startup); + queue_size = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "queue_size"), queue_size); + reset_service = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "reset_service"), reset_service); set_pose_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_pose_service"), - set_pose_service); - set_pose_deprecated_service = - fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "set_pose_deprecated_service"), - set_pose_deprecated_service); + fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "set_pose_service"), set_pose_service); + set_pose_deprecated_service = fuse_core::getParam( + interfaces, fuse_core::joinParameterName(ns, "set_pose_deprecated_service"), set_pose_deprecated_service); topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); std::vector sigma_vector; - sigma_vector = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "initial_sigma"), - sigma_vector); - if (!sigma_vector.empty()) { - if (sigma_vector.size() != 15) { - throw std::invalid_argument( - "The supplied initial_sigma parameter must be length 15, but " - "is actually length " + - std::to_string(sigma_vector.size())); + sigma_vector = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_sigma"), sigma_vector); + if (!sigma_vector.empty()) + { + if (sigma_vector.size() != 15) + { + throw std::invalid_argument("The supplied initial_sigma parameter must be length 15, but " + "is actually length " + + std::to_string(sigma_vector.size())); } - auto is_sigma_valid = [](const double sigma) - { - return std::isfinite(sigma) && (sigma > 0); - }; - if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { - throw std::invalid_argument( - "The supplied initial_sigma parameter must contain valid floating point values. " - "NaN, Inf, and values <= 0 are not acceptable."); + auto is_sigma_valid = [](const double sigma) { return std::isfinite(sigma) && (sigma > 0); }; + if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) + { + throw std::invalid_argument("The supplied initial_sigma parameter must contain valid floating point values. " + "NaN, Inf, and values <= 0 are not acceptable."); } initial_sigma.swap(sigma_vector); } std::vector state_vector; - state_vector = fuse_core::getParam( - interfaces, fuse_core::joinParameterName( - ns, - "initial_state"), - state_vector); - if (!state_vector.empty()) { - if (state_vector.size() != 15) { - throw std::invalid_argument( - "The supplied initial_state parameter must be length 15, but is actually length " + - std::to_string(state_vector.size())); + state_vector = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "initial_state"), state_vector); + if (!state_vector.empty()) + { + if (state_vector.size() != 15) + { + throw std::invalid_argument("The supplied initial_state parameter must be length 15, but is actually length " + + std::to_string(state_vector.size())); } - auto is_state_valid = [](const double state) - { - return std::isfinite(state); - }; - if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { - throw std::invalid_argument( - "The supplied initial_state parameter must contain valid floating point values. " - "NaN, Inf, etc are not acceptable."); + auto is_state_valid = [](const double state) { return std::isfinite(state); }; + if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) + { + throw std::invalid_argument("The supplied initial_state parameter must contain valid floating point values. " + "NaN, Inf, etc are not acceptable."); } initial_state.swap(state_vector); } @@ -153,37 +118,36 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); } - /** * @brief Flag indicating if an initial state transaction should be sent on startup, or only in * response to a set_pose service call or topic message. */ - bool publish_on_startup {true}; + bool publish_on_startup{ true }; /** * @brief The size of the subscriber queue for the set_pose topic */ - int queue_size {10}; + int queue_size{ 10 }; /** * @brief The name of the reset service to call before sending transactions to the optimizer */ - std::string reset_service {"~/reset"}; + std::string reset_service{ "~/reset" }; /** * @brief The name of the set_pose service to advertise */ - std::string set_pose_service {"set_pose"}; + std::string set_pose_service{ "set_pose" }; /** * @brief The name of the deprecated set_pose service without return codes */ - std::string set_pose_deprecated_service {"set_pose_deprecated"}; + std::string set_pose_deprecated_service{ "set_pose_deprecated" }; /** * @brief The topic name for received PoseWithCovarianceStamped messages */ - std::string topic {"set_pose"}; + std::string topic{ "set_pose" }; /** * @brief The uncertainty of the initial state value @@ -193,18 +157,14 @@ struct Omnidirectional3DIgnitionParams : public ParameterBase * The covariance matrix is created placing the squared standard deviations along the diagonal of * an 15x15 matrix. */ - std::vector initial_sigma {1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, - 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, - 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 - }; + std::vector initial_sigma{ 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 }; /** - * @brief The initial value of the 15-dimension state vector (x, y, z, roll, pitch, yaw, + * @brief The initial value of the 15-dimension state vector (x, y, z, roll, pitch, yaw, * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) */ - std::vector initial_state {0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0}; + std::vector initial_state{ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 }; /** * @brief Loss function */ diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp index 9fdc11d4f..1a12217cf 100644 --- a/fuse_models/src/imu_3d.cpp +++ b/fuse_models/src/imu_3d.cpp @@ -45,7 +45,6 @@ #include #include - // Register this sensor model with ROS as a plugin. PLUGINLIB_EXPORT_CLASS(fuse_models::Imu3D, fuse_core::SensorModel) @@ -53,17 +52,15 @@ namespace fuse_models { Imu3D::Imu3D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Imu3D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Imu3D::process, this, std::placeholders::_1)) { } -void Imu3D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Imu3D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -81,35 +78,29 @@ void Imu3D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } - if (params_.orientation_indices.empty() && - params_.linear_acceleration_indices.empty() && - params_.angular_velocity_indices.empty()) + if (params_.orientation_indices.empty() && params_.linear_acceleration_indices.empty() && + params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will be ignored."); } tf_buffer_ = std::make_unique(clock_); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() - ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } void Imu3D::onStart() { - if (!params_.orientation_indices.empty() || - !params_.linear_acceleration_indices.empty() || - !params_.angular_velocity_indices.empty()) + if (!params_.orientation_indices.empty() || !params_.linear_acceleration_indices.empty() || + !params_.angular_velocity_indices.empty()) { previous_pose_.reset(); @@ -117,16 +108,10 @@ void Imu3D::onStart() sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &ImuThrottledCallback::callback, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&ImuThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1), + sub_options); } } @@ -135,7 +120,7 @@ void Imu3D::onStop() sub_.reset(); } -void Imu3D::process(const sensor_msgs::msg::Imu & msg) +void Imu3D::process(const sensor_msgs::msg::Imu& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -167,40 +152,24 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) twist.twist.covariance[33] = msg.angular_velocity_covariance[6]; twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; - + const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(*pose, twist, validate, *transaction); - } else { - common::processAbsolutePose3DWithCovariance( - name(), - device_id_, - *pose, - params_.pose_loss, - params_.orientation_target_frame, - {}, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePose3DWithCovariance(name(), device_id_, *pose, params_.pose_loss, + params_.orientation_target_frame, {}, params_.orientation_indices, + *tf_buffer_, validate, *transaction, params_.tf_timeout); } // Handle the twist data (only include indices for angular velocity) - common::processTwist3DWithCovariance( - name(), - device_id_, - twist, - nullptr, - params_.angular_velocity_loss, - params_.twist_target_frame, - {}, - params_.angular_velocity_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processTwist3DWithCovariance(name(), device_id_, twist, nullptr, params_.angular_velocity_loss, + params_.twist_target_frame, {}, params_.angular_velocity_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); // Handle the acceleration data geometry_msgs::msg::AccelWithCovarianceStamped accel; @@ -217,7 +186,8 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) accel.accel.covariance[14] = msg.linear_acceleration_covariance[8]; // Optionally remove the acceleration due to gravity - if (params_.remove_gravitational_acceleration) { + if (params_.remove_gravitational_acceleration) + { geometry_msgs::msg::Vector3 accel_gravity; accel_gravity.z = params_.gravitational_acceleration; geometry_msgs::msg::TransformStamped orientation_trans; @@ -230,88 +200,64 @@ void Imu3D::process(const sensor_msgs::msg::Imu & msg) accel.accel.accel.linear.z -= accel_gravity.z; } - common::processAccel3DWithCovariance( - name(), - device_id_, - accel, - params_.linear_acceleration_loss, - params_.acceleration_target_frame, - params_.linear_acceleration_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processAccel3DWithCovariance(name(), device_id_, accel, params_.linear_acceleration_loss, + params_.acceleration_target_frame, params_.linear_acceleration_indices, + *tf_buffer_, validate, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); } -void Imu3D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const bool validate, - fuse_core::Transaction & transaction) +void Imu3D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = - params_.orientation_target_frame.empty() ? pose.header.frame_id : params_. - orientation_target_frame; - - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " << rclcpp::Time( - pose.header.stamp).nanoseconds() - << " to orientation target frame " << - params_.orientation_target_frame); + params_.orientation_target_frame.empty() ? pose.header.frame_id : params_.orientation_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to orientation target frame " + << params_.orientation_target_frame); return; } - if (!previous_pose_) { + if (!previous_pose_) + { previous_pose_ = std::move(transformed_pose); - return; + return; } - if (params_.use_twist_covariance) { + if (params_.use_twist_covariance) + { geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = - params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() - << " to twist target frame " << - params_.twist_target_frame); - } else { - common::processDifferentialPose3DWithTwistCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - twist, - params_.minimum_pose_relative_covariance, - params_.twist_covariance_offset, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " + << params_.twist_target_frame); } - } else { - common::processDifferentialPose3DWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - {}, - params_.orientation_indices, - validate, - transaction); + else + { + common::processDifferentialPose3DWithTwistCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + twist, params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, params_.pose_loss, {}, + params_.orientation_indices, validate, transaction); + } + } + else + { + common::processDifferentialPose3DWithCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.pose_loss, {}, params_.orientation_indices, validate, + transaction); } previous_pose_ = std::move(transformed_pose); diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp index 85cb1cc7f..cd05a3c22 100644 --- a/fuse_models/src/odometry_3d.cpp +++ b/fuse_models/src/odometry_3d.cpp @@ -51,17 +51,15 @@ namespace fuse_models { Odometry3D::Odometry3D() -: fuse_core::AsyncSensorModel(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - throttled_callback_(std::bind(&Odometry3D::process, this, std::placeholders::_1)) + : fuse_core::AsyncSensorModel(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , throttled_callback_(std::bind(&Odometry3D::process, this, std::placeholders::_1)) { } -void Odometry3D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) +void Odometry3D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name, fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -79,51 +77,40 @@ void Odometry3D::onInit() throttled_callback_.setThrottlePeriod(params_.throttle_period); - if (!params_.throttle_use_wall_time) { + if (!params_.throttle_use_wall_time) + { throttled_callback_.setClock(clock_); } if (params_.position_indices.empty() && params_.orientation_indices.empty() && - params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) + params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) { - RCLCPP_WARN_STREAM( - logger_, - "No dimensions were specified. Data from topic " << params_.topic - << " will be ignored."); + RCLCPP_WARN_STREAM(logger_, + "No dimensions were specified. Data from topic " << params_.topic << " will be ignored."); } tf_buffer_ = std::make_unique(clock_); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface() - ); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } void Odometry3D::onStart() { if (!params_.position_indices.empty() || !params_.orientation_indices.empty() || - !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) + !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) { - previous_pose_.reset(); + previous_pose_.reset(); - rclcpp::SubscriptionOptions sub_options; - sub_options.callback_group = cb_group_; + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; - sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind( - &OdometryThrottledCallback::callback< - const nav_msgs::msg::Odometry &>, - &throttled_callback_, - std::placeholders::_1 - ), - sub_options - ); + sub_ = rclcpp::create_subscription( + interfaces_, params_.topic, params_.queue_size, + std::bind(&OdometryThrottledCallback::callback, &throttled_callback_, + std::placeholders::_1), + sub_options); } } @@ -132,7 +119,7 @@ void Odometry3D::onStop() sub_.reset(); } -void Odometry3D::process(const nav_msgs::msg::Odometry & msg) +void Odometry3D::process(const nav_msgs::msg::Odometry& msg) { // Create a transaction object auto transaction = fuse_core::Transaction::make_shared(); @@ -150,106 +137,78 @@ void Odometry3D::process(const nav_msgs::msg::Odometry & msg) const bool validate = !params_.disable_checks; - if (params_.differential) { + if (params_.differential) + { processDifferential(*pose, twist, validate, *transaction); - } else { - common::processAbsolutePose3DWithCovariance( - name(), - device_id_, - *pose, - params_.pose_loss, - params_.pose_target_frame, - params_.position_indices, - params_.orientation_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + } + else + { + common::processAbsolutePose3DWithCovariance(name(), device_id_, *pose, params_.pose_loss, params_.pose_target_frame, + params_.position_indices, params_.orientation_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); } // Handle the twist data - common::processTwist3DWithCovariance( - name(), - device_id_, - twist, - params_.linear_velocity_loss, - params_.angular_velocity_loss, - params_.twist_target_frame, - params_.linear_velocity_indices, - params_.angular_velocity_indices, - *tf_buffer_, - validate, - *transaction, - params_.tf_timeout); + common::processTwist3DWithCovariance(name(), device_id_, twist, params_.linear_velocity_loss, + params_.angular_velocity_loss, params_.twist_target_frame, + params_.linear_velocity_indices, params_.angular_velocity_indices, *tf_buffer_, + validate, *transaction, params_.tf_timeout); // Send the transaction object to the plugin's parent sendTransaction(transaction); } -void Odometry3D::processDifferential( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::TwistWithCovarianceStamped & twist, - const bool validate, - fuse_core::Transaction & transaction) +void Odometry3D::processDifferential(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + const geometry_msgs::msg::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction) { auto transformed_pose = std::make_unique(); transformed_pose->header.frame_id = - params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; + params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; - if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform pose message with stamp " - << rclcpp::Time( - pose.header.stamp).nanoseconds() << " to pose target frame " << params_.pose_target_frame); + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time(pose.header.stamp).nanoseconds() << " to pose target frame " + << params_.pose_target_frame); return; } - if (!previous_pose_) { + if (!previous_pose_) + { previous_pose_ = std::move(transformed_pose); return; } - if (params_.use_twist_covariance) { + if (params_.use_twist_covariance) + { geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; transformed_twist.header.frame_id = - params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; - - if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Cannot transform twist message with stamp " << rclcpp::Time( - twist.header.stamp).nanoseconds() - << " to twist target frame " << - params_.twist_target_frame); - } else { - common::processDifferentialPose3DWithTwistCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - transformed_twist, - params_.minimum_pose_relative_covariance, - params_.twist_covariance_offset, - params_.pose_loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " + << rclcpp::Time(twist.header.stamp).nanoseconds() << " to twist target frame " + << params_.twist_target_frame); + } + else + { + common::processDifferentialPose3DWithTwistCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + transformed_twist, params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, params_.pose_loss, + params_.position_indices, params_.orientation_indices, + validate, transaction); } - } else { - common::processDifferentialPose3DWithCovariance( - name(), - device_id_, - *previous_pose_, - *transformed_pose, - params_.independent, - params_.minimum_pose_relative_covariance, - params_.pose_loss, - params_.position_indices, - params_.orientation_indices, - validate, - transaction); + } + else + { + common::processDifferentialPose3DWithCovariance(name(), device_id_, *previous_pose_, *transformed_pose, + params_.independent, params_.minimum_pose_relative_covariance, + params_.pose_loss, params_.position_indices, + params_.orientation_indices, validate, transaction); } previous_pose_ = std::move(transformed_pose); } diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp index 6fb94e5ee..ec2649f91 100644 --- a/fuse_models/src/odometry_3d_publisher.cpp +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -65,17 +65,16 @@ namespace fuse_models { Odometry3DPublisher::Odometry3DPublisher() -: fuse_core::AsyncPublisher(1), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")), - latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)), - latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) + : fuse_core::AsyncPublisher(1) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) + , latest_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) + , latest_covariance_stamp_(rclcpp::Time(0, 0, RCL_ROS_TIME)) { } void Odometry3DPublisher::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) + fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncPublisher::initialize(interfaces, name); @@ -93,54 +92,45 @@ void Odometry3DPublisher::onInit() params_.loadFromROS(interfaces_, name_); - if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { tf_buffer_ = std::make_unique( - clock_, - params_.tf_cache_time.to_chrono() - // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h - // TODO(methylDragon): See above ^ + clock_, params_.tf_cache_time.to_chrono() + // , interfaces_ // NOTE(methylDragon): This one is pending a change on tf2_ros/buffer.h + // TODO(methylDragon): See above ^ ); - tf_listener_ = std::make_unique( - *tf_buffer_, - interfaces_.get_node_base_interface(), - interfaces_.get_node_logging_interface(), - interfaces_.get_node_parameters_interface(), - interfaces_.get_node_topics_interface()); + tf_listener_ = std::make_unique(*tf_buffer_, interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface()); } // Advertise the topics rclcpp::PublisherOptions pub_options; pub_options.callback_group = cb_group_; - odom_pub_ = rclcpp::create_publisher( - interfaces_, - params_.topic, - params_.queue_size, - pub_options); + odom_pub_ = + rclcpp::create_publisher(interfaces_, params_.topic, params_.queue_size, pub_options); acceleration_pub_ = rclcpp::create_publisher( - interfaces_, - params_.acceleration_topic, - params_.queue_size, - pub_options); + interfaces_, params_.acceleration_topic, params_.queue_size, pub_options); } -void Odometry3DPublisher::notifyCallback( - fuse_core::Transaction::ConstSharedPtr transaction, - fuse_core::Graph::ConstSharedPtr graph) +void Odometry3DPublisher::notifyCallback(fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) { // Find the most recent common timestamp const auto latest_stamp = synchronizer_.findLatestCommonStamp(*transaction, *graph); - if (0u == latest_stamp.nanoseconds()) { + if (0u == latest_stamp.nanoseconds()) + { { std::lock_guard lock(mutex_); latest_stamp_ = latest_stamp; } - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a matching set of state variables with device id '" - << device_id_ << "'."); + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a matching set of state variables with device id '" << device_id_ + << "'."); return; } @@ -153,17 +143,8 @@ void Odometry3DPublisher::notifyCallback( nav_msgs::msg::Odometry odom_output; geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output; - if (!getState( - *graph, - latest_stamp, - device_id_, - position_uuid, - orientation_uuid, - velocity_linear_uuid, - velocity_angular_uuid, - acceleration_linear_uuid, - odom_output, - acceleration_output)) + if (!getState(*graph, latest_stamp, device_id_, position_uuid, orientation_uuid, velocity_linear_uuid, + velocity_angular_uuid, acceleration_linear_uuid, odom_output, acceleration_output)) { std::lock_guard lock(mutex_); latest_stamp_ = latest_stamp; @@ -180,14 +161,16 @@ void Odometry3DPublisher::notifyCallback( // Don't waste CPU computing the covariance if nobody is listening rclcpp::Time latest_covariance_stamp = latest_covariance_stamp_; bool latest_covariance_valid = latest_covariance_valid_; - if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) { + if (odom_pub_->get_subscription_count() > 0 || acceleration_pub_->get_subscription_count() > 0) + { // Throttle covariance computation if (params_.covariance_throttle_period.nanoseconds() == 0 || - latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) + latest_stamp - latest_covariance_stamp > params_.covariance_throttle_period) { latest_covariance_stamp = latest_stamp; - try { + try + { std::vector> covariance_requests; covariance_requests.emplace_back(position_uuid, position_uuid); covariance_requests.emplace_back(position_uuid, orientation_uuid); @@ -200,47 +183,47 @@ void Odometry3DPublisher::notifyCallback( std::vector> covariance_matrices; graph->getCovariance(covariance_requests, covariance_matrices, params_.covariance_options, true); - odom_output.pose.covariance[0] = covariance_matrices[0][0]; // cov(x, x) - odom_output.pose.covariance[1] = covariance_matrices[0][1]; // cov(x, y) - odom_output.pose.covariance[2] = covariance_matrices[0][2]; // cov(x, z) - odom_output.pose.covariance[3] = covariance_matrices[1][0]; // cov(x, roll) - odom_output.pose.covariance[4] = covariance_matrices[1][1]; // cov(x, pitch) - odom_output.pose.covariance[5] = covariance_matrices[1][2]; // cov(x, yaw) - - odom_output.pose.covariance[6] = covariance_matrices[0][3]; // cov(y, x) - odom_output.pose.covariance[7] = covariance_matrices[0][4]; // cov(y, y) - odom_output.pose.covariance[8] = covariance_matrices[0][5]; // cov(y, z) - odom_output.pose.covariance[9] = covariance_matrices[1][3]; // cov(y, roll) - odom_output.pose.covariance[10] = covariance_matrices[1][4]; // cov(y, pitch) - odom_output.pose.covariance[11] = covariance_matrices[1][5]; // cov(y, yaw) - - odom_output.pose.covariance[12] = covariance_matrices[0][6]; // cov(z, x) - odom_output.pose.covariance[13] = covariance_matrices[0][7]; // cov(z, y) - odom_output.pose.covariance[14] = covariance_matrices[0][8]; // cov(z, z) - odom_output.pose.covariance[15] = covariance_matrices[1][6]; // cov(z, roll) - odom_output.pose.covariance[16] = covariance_matrices[1][7]; // cov(z, pitch) - odom_output.pose.covariance[17] = covariance_matrices[1][8]; // cov(z, yaw) - - odom_output.pose.covariance[18] = covariance_matrices[1][0]; // cov(roll, x) - odom_output.pose.covariance[19] = covariance_matrices[1][3]; // cov(roll, y) - odom_output.pose.covariance[20] = covariance_matrices[1][6]; // cov(roll, z) - odom_output.pose.covariance[21] = covariance_matrices[2][0]; // cov(roll, roll) - odom_output.pose.covariance[22] = covariance_matrices[2][1]; // cov(roll, pitch) - odom_output.pose.covariance[23] = covariance_matrices[2][2]; // cov(roll, yaw) - - odom_output.pose.covariance[24] = covariance_matrices[1][1]; // cov(pitch, x) - odom_output.pose.covariance[25] = covariance_matrices[1][4]; // cov(pitch, y) - odom_output.pose.covariance[26] = covariance_matrices[1][7]; // cov(pitch, z) - odom_output.pose.covariance[27] = covariance_matrices[2][1]; // cov(pitch, roll) - odom_output.pose.covariance[28] = covariance_matrices[2][4]; // cov(pitch, pitch) - odom_output.pose.covariance[29] = covariance_matrices[2][5]; // cov(pitch, yaw) - - odom_output.pose.covariance[30] = covariance_matrices[1][2]; // cov(yaw, x) - odom_output.pose.covariance[31] = covariance_matrices[1][5]; // cov(yaw, y) - odom_output.pose.covariance[32] = covariance_matrices[1][8]; // cov(yaw, z) - odom_output.pose.covariance[33] = covariance_matrices[2][2]; // cov(yaw, roll) - odom_output.pose.covariance[34] = covariance_matrices[2][5]; // cov(yaw, pitch) - odom_output.pose.covariance[35] = covariance_matrices[2][8]; // cov(yaw, yaw) + odom_output.pose.covariance[0] = covariance_matrices[0][0]; // cov(x, x) + odom_output.pose.covariance[1] = covariance_matrices[0][1]; // cov(x, y) + odom_output.pose.covariance[2] = covariance_matrices[0][2]; // cov(x, z) + odom_output.pose.covariance[3] = covariance_matrices[1][0]; // cov(x, roll) + odom_output.pose.covariance[4] = covariance_matrices[1][1]; // cov(x, pitch) + odom_output.pose.covariance[5] = covariance_matrices[1][2]; // cov(x, yaw) + + odom_output.pose.covariance[6] = covariance_matrices[0][3]; // cov(y, x) + odom_output.pose.covariance[7] = covariance_matrices[0][4]; // cov(y, y) + odom_output.pose.covariance[8] = covariance_matrices[0][5]; // cov(y, z) + odom_output.pose.covariance[9] = covariance_matrices[1][3]; // cov(y, roll) + odom_output.pose.covariance[10] = covariance_matrices[1][4]; // cov(y, pitch) + odom_output.pose.covariance[11] = covariance_matrices[1][5]; // cov(y, yaw) + + odom_output.pose.covariance[12] = covariance_matrices[0][6]; // cov(z, x) + odom_output.pose.covariance[13] = covariance_matrices[0][7]; // cov(z, y) + odom_output.pose.covariance[14] = covariance_matrices[0][8]; // cov(z, z) + odom_output.pose.covariance[15] = covariance_matrices[1][6]; // cov(z, roll) + odom_output.pose.covariance[16] = covariance_matrices[1][7]; // cov(z, pitch) + odom_output.pose.covariance[17] = covariance_matrices[1][8]; // cov(z, yaw) + + odom_output.pose.covariance[18] = covariance_matrices[1][0]; // cov(roll, x) + odom_output.pose.covariance[19] = covariance_matrices[1][3]; // cov(roll, y) + odom_output.pose.covariance[20] = covariance_matrices[1][6]; // cov(roll, z) + odom_output.pose.covariance[21] = covariance_matrices[2][0]; // cov(roll, roll) + odom_output.pose.covariance[22] = covariance_matrices[2][1]; // cov(roll, pitch) + odom_output.pose.covariance[23] = covariance_matrices[2][2]; // cov(roll, yaw) + + odom_output.pose.covariance[24] = covariance_matrices[1][1]; // cov(pitch, x) + odom_output.pose.covariance[25] = covariance_matrices[1][4]; // cov(pitch, y) + odom_output.pose.covariance[26] = covariance_matrices[1][7]; // cov(pitch, z) + odom_output.pose.covariance[27] = covariance_matrices[2][1]; // cov(pitch, roll) + odom_output.pose.covariance[28] = covariance_matrices[2][4]; // cov(pitch, pitch) + odom_output.pose.covariance[29] = covariance_matrices[2][5]; // cov(pitch, yaw) + + odom_output.pose.covariance[30] = covariance_matrices[1][2]; // cov(yaw, x) + odom_output.pose.covariance[31] = covariance_matrices[1][5]; // cov(yaw, y) + odom_output.pose.covariance[32] = covariance_matrices[1][8]; // cov(yaw, z) + odom_output.pose.covariance[33] = covariance_matrices[2][2]; // cov(yaw, roll) + odom_output.pose.covariance[34] = covariance_matrices[2][5]; // cov(yaw, pitch) + odom_output.pose.covariance[35] = covariance_matrices[2][8]; // cov(yaw, yaw) odom_output.twist.covariance[0] = covariance_matrices[3][0]; odom_output.twist.covariance[1] = covariance_matrices[3][1]; @@ -262,7 +245,7 @@ void Odometry3DPublisher::notifyCallback( odom_output.twist.covariance[15] = covariance_matrices[4][6]; odom_output.twist.covariance[16] = covariance_matrices[4][7]; odom_output.twist.covariance[17] = covariance_matrices[4][8]; - + odom_output.twist.covariance[18] = covariance_matrices[4][0]; odom_output.twist.covariance[19] = covariance_matrices[4][3]; odom_output.twist.covariance[20] = covariance_matrices[4][6]; @@ -283,7 +266,7 @@ void Odometry3DPublisher::notifyCallback( odom_output.twist.covariance[33] = covariance_matrices[5][2]; odom_output.twist.covariance[34] = covariance_matrices[5][5]; odom_output.twist.covariance[35] = covariance_matrices[5][8]; - + acceleration_output.accel.covariance[0] = covariance_matrices[6][0]; acceleration_output.accel.covariance[1] = covariance_matrices[6][1]; acceleration_output.accel.covariance[2] = covariance_matrices[6][2]; @@ -296,37 +279,39 @@ void Odometry3DPublisher::notifyCallback( // test if covariances are symmetric Eigen::Map odom_cov_map(odom_output.pose.covariance.data()); - if (!odom_cov_map.isApprox(odom_cov_map.transpose())) { + if (!odom_cov_map.isApprox(odom_cov_map.transpose())) + { throw std::runtime_error("Odometry covariance matrix is not symmetric"); } Eigen::Map twist_cov_map(odom_output.twist.covariance.data()); - if (!twist_cov_map.isApprox(twist_cov_map.transpose())) { + if (!twist_cov_map.isApprox(twist_cov_map.transpose())) + { throw std::runtime_error("Twist covariance matrix is not symmetric"); } Eigen::Map accel_cov_map(acceleration_output.accel.covariance.data()); - if (!accel_cov_map.isApprox(accel_cov_map.transpose())) { + if (!accel_cov_map.isApprox(accel_cov_map.transpose())) + { throw std::runtime_error("Acceleration covariance matrix is not symmetric"); } latest_covariance_valid = true; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM( - logger_, - "An error occurred computing the covariance information for " - << latest_stamp.nanoseconds() - << ". The covariance will be set to zero.\n" - << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM(logger_, "An error occurred computing the covariance information for " + << latest_stamp.nanoseconds() << ". The covariance will be set to zero.\n" + << e.what()); std::fill(odom_output.pose.covariance.begin(), odom_output.pose.covariance.end(), 0.0); std::fill(odom_output.twist.covariance.begin(), odom_output.twist.covariance.end(), 0.0); - std::fill( - acceleration_output.accel.covariance.begin(), - acceleration_output.accel.covariance.end(), 0.0); + std::fill(acceleration_output.accel.covariance.begin(), acceleration_output.accel.covariance.end(), 0.0); latest_covariance_valid = false; } - } else { + } + else + { // This covariance computation cycle has been skipped, so simply take the last covariance // computed // @@ -358,13 +343,9 @@ void Odometry3DPublisher::onStart() acceleration_output_ = geometry_msgs::msg::AccelWithCovarianceStamped(); // TODO(CH3): Add this to a separate callback group for async behavior - publish_timer_ = rclcpp::create_timer( - interfaces_, - clock_, - std::chrono::duration(1.0 / params_.publish_frequency), - std::move(std::bind(&Odometry3DPublisher::publishTimerCallback, this)), - cb_group_ - ); + publish_timer_ = + rclcpp::create_timer(interfaces_, clock_, std::chrono::duration(1.0 / params_.publish_frequency), + std::move(std::bind(&Odometry3DPublisher::publishTimerCallback, this)), cb_group_); delayed_throttle_filter_.reset(); } @@ -374,40 +355,34 @@ void Odometry3DPublisher::onStop() publish_timer_->cancel(); } -bool Odometry3DPublisher::getState( - const fuse_core::Graph & graph, - const rclcpp::Time & stamp, - const fuse_core::UUID & device_id, - fuse_core::UUID & position_uuid, - fuse_core::UUID & orientation_uuid, - fuse_core::UUID & velocity_linear_uuid, - fuse_core::UUID & velocity_angular_uuid, - fuse_core::UUID & acceleration_linear_uuid, - nav_msgs::msg::Odometry & odometry, - geometry_msgs::msg::AccelWithCovarianceStamped & acceleration) +bool Odometry3DPublisher::getState(const fuse_core::Graph& graph, const rclcpp::Time& stamp, + const fuse_core::UUID& device_id, fuse_core::UUID& position_uuid, + fuse_core::UUID& orientation_uuid, fuse_core::UUID& velocity_linear_uuid, + fuse_core::UUID& velocity_angular_uuid, fuse_core::UUID& acceleration_linear_uuid, + nav_msgs::msg::Odometry& odometry, + geometry_msgs::msg::AccelWithCovarianceStamped& acceleration) { - try { + try + { position_uuid = fuse_variables::Position3DStamped(stamp, device_id).uuid(); - auto position_variable = dynamic_cast( - graph.getVariable(position_uuid)); + auto position_variable = dynamic_cast(graph.getVariable(position_uuid)); orientation_uuid = fuse_variables::Orientation3DStamped(stamp, device_id).uuid(); - auto orientation_variable = dynamic_cast( - graph.getVariable(orientation_uuid)); + auto orientation_variable = + dynamic_cast(graph.getVariable(orientation_uuid)); velocity_linear_uuid = fuse_variables::VelocityLinear3DStamped(stamp, device_id).uuid(); - auto velocity_linear_variable = dynamic_cast( - graph.getVariable(velocity_linear_uuid)); + auto velocity_linear_variable = + dynamic_cast(graph.getVariable(velocity_linear_uuid)); velocity_angular_uuid = fuse_variables::VelocityAngular3DStamped(stamp, device_id).uuid(); - auto velocity_angular_variable = dynamic_cast( - graph.getVariable(velocity_angular_uuid)); + auto velocity_angular_variable = + dynamic_cast(graph.getVariable(velocity_angular_uuid)); acceleration_linear_uuid = fuse_variables::AccelerationLinear3DStamped(stamp, device_id).uuid(); auto acceleration_linear_variable = - dynamic_cast( - graph.getVariable(acceleration_linear_uuid)); - + dynamic_cast(graph.getVariable(acceleration_linear_uuid)); + odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); odometry.pose.pose.position.z = position_variable.z(); @@ -428,15 +403,17 @@ bool Odometry3DPublisher::getState( acceleration.accel.accel.angular.x = 0.0; acceleration.accel.accel.angular.y = 0.0; acceleration.accel.accel.angular.z = 0.0; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: " << e.what()); return false; - } catch (...) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); + } + catch (...) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Failed to find a state at time " << stamp.nanoseconds() << ". Error: unknown"); return false; } @@ -460,10 +437,10 @@ void Odometry3DPublisher::publishTimerCallback() acceleration_output = acceleration_output_; } - if (0u == latest_stamp.nanoseconds()) { - RCLCPP_WARN_STREAM_EXPRESSION( - logger_, delayed_throttle_filter_.isEnabled(), - "No valid state data yet. Delaying tf broadcast."); + if (0u == latest_stamp.nanoseconds()) + { + RCLCPP_WARN_STREAM_EXPRESSION(logger_, delayed_throttle_filter_.isEnabled(), + "No valid state data yet. Delaying tf broadcast."); return; } @@ -471,7 +448,8 @@ void Odometry3DPublisher::publishTimerCallback() tf2::fromMsg(odom_output.pose.pose, pose); // If requested, we need to project our state forward in time using the 3D kinematic model - if (params_.predict_to_current_time) { + if (params_.predict_to_current_time) + { rclcpp::Time timer_now = interfaces_.get_node_clock_interface()->get_clock()->now(); // Convert pose in Eigen representation @@ -482,34 +460,24 @@ void Odometry3DPublisher::publishTimerCallback() orientation.y() = pose.getRotation().y(); orientation.z() = pose.getRotation().z(); orientation.w() = pose.getRotation().w(); - velocity_linear << odom_output.twist.twist.linear.x, - odom_output.twist.twist.linear.y, odom_output.twist.twist.linear.z; - velocity_angular << odom_output.twist.twist.angular.x, - odom_output.twist.twist.angular.y, odom_output.twist.twist.angular.z; + velocity_linear << odom_output.twist.twist.linear.x, odom_output.twist.twist.linear.y, + odom_output.twist.twist.linear.z; + velocity_angular << odom_output.twist.twist.angular.x, odom_output.twist.twist.angular.y, + odom_output.twist.twist.angular.z; const double dt = timer_now.seconds() - rclcpp::Time(odom_output.header.stamp).seconds(); fuse_core::Matrix15d jacobian; fuse_core::Vector3d acceleration_linear; - if (params_.predict_with_acceleration) { - acceleration_linear << acceleration_output.accel.accel.linear.x, - acceleration_output.accel.accel.linear.y, acceleration_output.accel.accel.linear.z; + if (params_.predict_with_acceleration) + { + acceleration_linear << acceleration_output.accel.accel.linear.x, acceleration_output.accel.accel.linear.y, + acceleration_output.accel.accel.linear.z; } - predict( - position, - orientation, - velocity_linear, - velocity_angular, - acceleration_linear, - dt, - position, - orientation, - velocity_linear, - velocity_angular, - acceleration_linear, - jacobian); + predict(position, orientation, velocity_linear, velocity_angular, acceleration_linear, dt, position, orientation, + velocity_linear, velocity_angular, acceleration_linear, jacobian); // Convert back to tf2 representation pose.setOrigin(tf2::Vector3(position.x(), position.y(), position.z())); @@ -530,7 +498,8 @@ void Odometry3DPublisher::publishTimerCallback() odom_output.twist.twist.angular.y = velocity_angular.y(); odom_output.twist.twist.angular.z = velocity_angular.z(); - if (params_.predict_with_acceleration) { + if (params_.predict_with_acceleration) + { acceleration_output.accel.accel.linear.x = acceleration_linear.x(); acceleration_output.accel.accel.linear.y = acceleration_linear.y(); acceleration_output.accel.accel.linear.z = acceleration_linear.z(); @@ -541,7 +510,8 @@ void Odometry3DPublisher::publishTimerCallback() // Either the last covariance computation was skipped because there was no subscriber, // or it failed - if (latest_covariance_valid) { + if (latest_covariance_valid) + { fuse_core::Matrix15d covariance; covariance.setZero(); Eigen::Map pose_covariance(odom_output.pose.covariance.data()); @@ -555,13 +525,10 @@ void Odometry3DPublisher::publishTimerCallback() covariance = jacobian * covariance * jacobian.transpose(); auto process_noise_covariance = params_.process_noise_covariance; - if (params_.scale_process_noise) { - common::scaleProcessNoiseCovariance( - process_noise_covariance, - velocity_linear, - velocity_angular, - params_.velocity_linear_norm_min_, - params_.velocity_angular_norm_min_); + if (params_.scale_process_noise) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, velocity_linear, velocity_angular, + params_.velocity_linear_norm_min_, params_.velocity_angular_norm_min_); } covariance.noalias() += dt * process_noise_covariance; @@ -575,11 +542,13 @@ void Odometry3DPublisher::publishTimerCallback() odom_pub_->publish(odom_output); acceleration_pub_->publish(acceleration_output); - if (params_.publish_tf) { + if (params_.publish_tf) + { auto frame_id = odom_output.header.frame_id; auto child_frame_id = odom_output.child_frame_id; - if (params_.invert_tf) { + if (params_.invert_tf) + { pose = pose.inverse(); std::swap(frame_id, child_frame_id); } @@ -588,24 +557,25 @@ void Odometry3DPublisher::publishTimerCallback() trans.header.stamp = odom_output.header.stamp; trans.header.frame_id = frame_id; trans.child_frame_id = child_frame_id; - trans.transform = tf2::toMsg(pose); - if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) { - try { - auto base_to_odom = tf_buffer_->lookupTransform( - params_.base_link_frame_id, - params_.odom_frame_id, - trans.header.stamp, - params_.tf_timeout); + trans.transform = tf2::toMsg(pose); + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { + try + { + auto base_to_odom = tf_buffer_->lookupTransform(params_.base_link_frame_id, params_.odom_frame_id, + trans.header.stamp, params_.tf_timeout); geometry_msgs::msg::TransformStamped map_to_odom; tf2::doTransform(base_to_odom, map_to_odom, trans); map_to_odom.child_frame_id = params_.odom_frame_id; trans = map_to_odom; - } catch (const std::exception & e) { - RCLCPP_WARN_STREAM_THROTTLE( - logger_, *clock_, 5.0 * 1000, - "Could not lookup the " << params_.base_link_frame_id << "->" - << params_.odom_frame_id << " transform. Error: " << e.what()); + } + catch (const std::exception& e) + { + RCLCPP_WARN_STREAM_THROTTLE(logger_, *clock_, 5.0 * 1000, + "Could not lookup the " << params_.base_link_frame_id << "->" + << params_.odom_frame_id + << " transform. Error: " << e.what()); return; } diff --git a/fuse_models/src/omnidirectional_3d.cpp b/fuse_models/src/omnidirectional_3d.cpp index 92534d25d..5def0f0c3 100644 --- a/fuse_models/src/omnidirectional_3d.cpp +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -65,25 +65,25 @@ PLUGINLIB_EXPORT_CLASS(fuse_models::Omnidirectional3D, fuse_core::MotionModel) namespace std { -inline bool isfinite(const fuse_core::Vector3d & vector) +inline bool isfinite(const fuse_core::Vector3d& vector) { return std::isfinite(vector.x()) && std::isfinite(vector.y() && std::isfinite(vector.z())); } -inline bool isNormalized(const Eigen::Quaterniond & q) +inline bool isNormalized(const Eigen::Quaterniond& q) { return std::sqrt(q.w() * q.w() + q.x() * q.x() + q.y() * q.y() + q.z() * q.z()) - 1.0 < Eigen::NumTraits::dummy_precision(); } -std::string to_string(const fuse_core::Vector3d & vector) +std::string to_string(const fuse_core::Vector3d& vector) { std::ostringstream oss; oss << vector; return oss.str(); } -std::string to_string(const Eigen::Quaterniond & quaternion) +std::string to_string(const Eigen::Quaterniond& quaternion) { std::ostringstream oss; oss << quaternion; @@ -94,21 +94,20 @@ std::string to_string(const Eigen::Quaterniond & quaternion) namespace fuse_core { -template -inline void validateCovariance( - const Eigen::DenseBase & covariance, - const double precision = Eigen::NumTraits::dummy_precision()) +template +inline void validateCovariance(const Eigen::DenseBase& covariance, + const double precision = Eigen::NumTraits::dummy_precision()) { - if (!fuse_core::isSymmetric(covariance, precision)) { - throw std::runtime_error( - "Non-symmetric partial covariance matrix\n" + - fuse_core::to_string(covariance, Eigen::FullPrecision)); + if (!fuse_core::isSymmetric(covariance, precision)) + { + throw std::runtime_error("Non-symmetric partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); } - if (!fuse_core::isPositiveDefinite(covariance)) { - throw std::runtime_error( - "Non-positive-definite partial covariance matrix\n" + - fuse_core::to_string(covariance, Eigen::FullPrecision)); + if (!fuse_core::isPositiveDefinite(covariance)) + { + throw std::runtime_error("Non-positive-definite partial covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); } } @@ -118,24 +117,26 @@ namespace fuse_models { Omnidirectional3D::Omnidirectional3D() -: fuse_core::AsyncMotionModel(1), // Thread count = 1 for local callback queue - logger_(rclcpp::get_logger("uninitialized")), - buffer_length_(rclcpp::Duration::max()), - device_id_(fuse_core::uuid::NIL), - timestamp_manager_(&Omnidirectional3D::generateMotionModel, this, rclcpp::Duration::max()) + : fuse_core::AsyncMotionModel(1) + , // Thread count = 1 for local callback queue + logger_(rclcpp::get_logger("uninitialized")) + , buffer_length_(rclcpp::Duration::max()) + , device_id_(fuse_core::uuid::NIL) + , timestamp_manager_(&Omnidirectional3D::generateMotionModel, this, rclcpp::Duration::max()) { } -void Omnidirectional3D::print(std::ostream & stream) const +void Omnidirectional3D::print(std::ostream& stream) const { stream << "state history:\n"; - for (const auto & state : state_history_) { + for (const auto& state : state_history_) + { stream << "- stamp: " << state.first.nanoseconds() << "\n"; state.second.print(stream); } } -void Omnidirectional3D::StateHistoryElement::print(std::ostream & stream) const +void Omnidirectional3D::StateHistoryElement::print(std::ostream& stream) const { stream << " position uuid: " << position_uuid << "\n" << " orientation uuid: " << orientation_uuid << "\n" @@ -151,34 +152,41 @@ void Omnidirectional3D::StateHistoryElement::print(std::ostream & stream) const void Omnidirectional3D::StateHistoryElement::validate() const { - if (!std::isfinite(position)) { + if (!std::isfinite(position)) + { throw std::runtime_error("Invalid position " + std::to_string(position)); } - if (!std::isNormalized(orientation)) { + if (!std::isNormalized(orientation)) + { throw std::runtime_error("Invalid orientation " + std::to_string(orientation)); } - if (!std::isfinite(vel_linear)) { + if (!std::isfinite(vel_linear)) + { throw std::runtime_error("Invalid linear velocity " + std::to_string(vel_linear)); } - if (!std::isfinite(vel_angular)) { + if (!std::isfinite(vel_angular)) + { throw std::runtime_error("Invalid angular velocity " + std::to_string(vel_angular)); } - if (!std::isfinite(acc_linear)) { + if (!std::isfinite(acc_linear)) + { throw std::runtime_error("Invalid linear acceleration " + std::to_string(acc_linear)); } } -bool Omnidirectional3D::applyCallback(fuse_core::Transaction & transaction) +bool Omnidirectional3D::applyCallback(fuse_core::Transaction& transaction) { // Use the timestamp manager to generate just the required motion model segments. The timestamp // manager, in turn, makes calls to the generateMotionModel() function. - try { + try + { // Now actually generate the motion model segments - timestamp_manager_.query(transaction, true); - } catch (const std::exception & e) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "An error occurred while completing the motion model query. Error: " << e.what()); + timestamp_manager_.query(transaction, true); + } + catch (const std::exception& e) + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "An error occurred while completing the motion model query. Error: " << e.what()); return false; } return true; @@ -189,9 +197,8 @@ void Omnidirectional3D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) updateStateHistoryEstimates(*graph, state_history_, buffer_length_); } -void Omnidirectional3D::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name) +void Omnidirectional3D::initialize(fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string& name) { interfaces_ = interfaces; fuse_core::AsyncMotionModel::initialize(interfaces, name); @@ -203,62 +210,36 @@ void Omnidirectional3D::onInit() clock_ = interfaces_.get_node_clock_interface()->get_clock(); std::vector process_noise_diagonal; - process_noise_diagonal = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "process_noise_diagonal"), - process_noise_diagonal); - - if (process_noise_diagonal.size() != 15) { + process_noise_diagonal = fuse_core::getParam( + interfaces_, fuse_core::joinParameterName(name_, "process_noise_diagonal"), process_noise_diagonal); + + if (process_noise_diagonal.size() != 15) + { throw std::runtime_error("Process noise diagonal must be of length 15!"); } process_noise_covariance_ = fuse_core::Vector15d(process_noise_diagonal.data()).asDiagonal(); - scale_process_noise_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "scale_process_noise"), - scale_process_noise_); - velocity_linear_norm_min_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "velocity_linear_norm_min"), - velocity_linear_norm_min_); - - velocity_angular_norm_min_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "velocity_angular_norm_min"), - velocity_angular_norm_min_); + scale_process_noise_ = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "scale_process_noise"), + scale_process_noise_); + velocity_linear_norm_min_ = fuse_core::getParam( + interfaces_, fuse_core::joinParameterName(name_, "velocity_linear_norm_min"), velocity_linear_norm_min_); + + velocity_angular_norm_min_ = fuse_core::getParam( + interfaces_, fuse_core::joinParameterName(name_, "velocity_angular_norm_min"), velocity_angular_norm_min_); disable_checks_ = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "disable_checks"), - disable_checks_); + fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "disable_checks"), disable_checks_); double buffer_length = 3.0; - buffer_length = - fuse_core::getParam( - interfaces_, fuse_core::joinParameterName( - name_, - "buffer_length"), - buffer_length); - - if (buffer_length < 0.0) { - throw std::runtime_error( - "Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); + buffer_length = fuse_core::getParam(interfaces_, fuse_core::joinParameterName(name_, "buffer_length"), buffer_length); + + if (buffer_length < 0.0) + { + throw std::runtime_error("Invalid negative buffer length of " + std::to_string(buffer_length) + " specified."); } - buffer_length_ = - (buffer_length == - 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); + buffer_length_ = (buffer_length == 0.0) ? rclcpp::Duration::max() : rclcpp::Duration::from_seconds(buffer_length); timestamp_manager_.bufferLength(buffer_length_); device_id_ = fuse_variables::loadDeviceId(interfaces_); @@ -270,29 +251,27 @@ void Omnidirectional3D::onStart() state_history_.clear(); } -void Omnidirectional3D::generateMotionModel( - const rclcpp::Time & beginning_stamp, - const rclcpp::Time & ending_stamp, - std::vector & constraints, - std::vector & variables) +void Omnidirectional3D::generateMotionModel(const rclcpp::Time& beginning_stamp, const rclcpp::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) { - assert( - beginning_stamp < ending_stamp || - (beginning_stamp == ending_stamp && state_history_.empty())); + assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history_.empty())); StateHistoryElement base_state; - rclcpp::Time base_time{0, 0, RCL_ROS_TIME}; + rclcpp::Time base_time{ 0, 0, RCL_ROS_TIME }; // Find an entry that is > beginning_stamp // The entry that is <= will be the one before it auto base_state_pair_it = state_history_.upper_bound(beginning_stamp); - if (base_state_pair_it == state_history_.begin()) { - RCLCPP_WARN_STREAM_EXPRESSION( - logger_, !state_history_.empty(), - "Unable to locate a state in this history with stamp <= " - << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); + if (base_state_pair_it == state_history_.begin()) + { + RCLCPP_WARN_STREAM_EXPRESSION(logger_, !state_history_.empty(), + "Unable to locate a state in this history with stamp <= " + << beginning_stamp.nanoseconds() << ". Variables will all be initialized to 0."); base_time = beginning_stamp; - } else { + } + else + { --base_state_pair_it; base_time = base_state_pair_it->first; base_state = base_state_pair_it->second; @@ -301,76 +280,47 @@ void Omnidirectional3D::generateMotionModel( StateHistoryElement state1; // If the nearest state we had was before the beginning stamp, we need to project that state to // the beginning stamp - if (base_time != beginning_stamp) { - predict( - base_state.position, - base_state.orientation, - base_state.vel_linear, - base_state.vel_angular, - base_state.acc_linear, - (beginning_stamp - base_time).seconds(), - state1.position, - state1.orientation, - state1.vel_linear, - state1.vel_angular, - state1.acc_linear); - } else { + if (base_time != beginning_stamp) + { + predict(base_state.position, base_state.orientation, base_state.vel_linear, base_state.vel_angular, + base_state.acc_linear, (beginning_stamp - base_time).seconds(), state1.position, state1.orientation, + state1.vel_linear, state1.vel_angular, state1.acc_linear); + } + else + { state1 = base_state; } - + // If dt is zero, we only need to update the state history: const double dt = (ending_stamp - beginning_stamp).seconds(); - if (dt == 0.0) { + if (dt == 0.0) + { state1.position_uuid = fuse_variables::Position3DStamped(beginning_stamp, device_id_).uuid(); state1.orientation_uuid = fuse_variables::Orientation3DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_linear_uuid = - fuse_variables::VelocityLinear3DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_angular_uuid = - fuse_variables::VelocityAngular3DStamped(beginning_stamp, device_id_).uuid(); - state1.acc_linear_uuid = - fuse_variables::AccelerationLinear3DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_linear_uuid = fuse_variables::VelocityLinear3DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_angular_uuid = fuse_variables::VelocityAngular3DStamped(beginning_stamp, device_id_).uuid(); + state1.acc_linear_uuid = fuse_variables::AccelerationLinear3DStamped(beginning_stamp, device_id_).uuid(); state_history_.emplace(beginning_stamp, std::move(state1)); return; } // Now predict to get an initial guess for the state at the ending stamp StateHistoryElement state2; - predict( - state1.position, - state1.orientation, - state1.vel_linear, - state1.vel_angular, - state1.acc_linear, - dt, - state2.position, - state2.orientation, - state2.vel_linear, - state2.vel_angular, - state2.acc_linear); - - // Define the fuse variables required for this constraint + predict(state1.position, state1.orientation, state1.vel_linear, state1.vel_angular, state1.acc_linear, dt, + state2.position, state2.orientation, state2.vel_linear, state2.vel_angular, state2.acc_linear); + + // Define the fuse variables required for this constraint auto position1 = fuse_variables::Position3DStamped::make_shared(beginning_stamp, device_id_); auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(beginning_stamp, device_id_); - auto velocity_linear1 = fuse_variables::VelocityLinear3DStamped::make_shared( - beginning_stamp, - device_id_); - auto velocity_angular1 = fuse_variables::VelocityAngular3DStamped::make_shared( - beginning_stamp, - device_id_); - auto acceleration_linear1 = fuse_variables::AccelerationLinear3DStamped::make_shared( - beginning_stamp, device_id_); + auto velocity_linear1 = fuse_variables::VelocityLinear3DStamped::make_shared(beginning_stamp, device_id_); + auto velocity_angular1 = fuse_variables::VelocityAngular3DStamped::make_shared(beginning_stamp, device_id_); + auto acceleration_linear1 = fuse_variables::AccelerationLinear3DStamped::make_shared(beginning_stamp, device_id_); auto position2 = fuse_variables::Position3DStamped::make_shared(ending_stamp, device_id_); auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(ending_stamp, device_id_); - auto velocity_linear2 = fuse_variables::VelocityLinear3DStamped::make_shared( - ending_stamp, - device_id_); - auto velocity_angular2 = fuse_variables::VelocityAngular3DStamped::make_shared( - ending_stamp, - device_id_); - auto acceleration_linear2 = fuse_variables::AccelerationLinear3DStamped::make_shared( - ending_stamp, - device_id_); + auto velocity_linear2 = fuse_variables::VelocityLinear3DStamped::make_shared(ending_stamp, device_id_); + auto velocity_angular2 = fuse_variables::VelocityAngular3DStamped::make_shared(ending_stamp, device_id_); + auto acceleration_linear2 = fuse_variables::AccelerationLinear3DStamped::make_shared(ending_stamp, device_id_); position1->data()[fuse_variables::Position3DStamped::X] = state1.position.x(); position1->data()[fuse_variables::Position3DStamped::Y] = state1.position.y(); @@ -380,22 +330,19 @@ void Omnidirectional3D::generateMotionModel( orientation1->data()[fuse_variables::Orientation3DStamped::Y] = state1.orientation.y(); orientation1->data()[fuse_variables::Orientation3DStamped::Z] = state1.orientation.z(); orientation1->data()[fuse_variables::Orientation3DStamped::W] = state1.orientation.w(); - + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::X] = state1.vel_linear.x(); velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Y] = state1.vel_linear.y(); velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Z] = state1.vel_linear.z(); - velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state1.vel_angular.x(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state1.vel_angular.x(); velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state1.vel_angular.y(); - velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state1.vel_angular.z(); - - acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::X] = - state1.acc_linear.x(); - acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Y] = - state1.acc_linear.y(); - acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Z] = - state1.acc_linear.z(); - + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state1.vel_angular.z(); + + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::X] = state1.acc_linear.x(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Y] = state1.acc_linear.y(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Z] = state1.acc_linear.z(); + position2->data()[fuse_variables::Position3DStamped::X] = state2.position.x(); position2->data()[fuse_variables::Position3DStamped::Y] = state2.position.y(); position2->data()[fuse_variables::Position3DStamped::Z] = state2.position.z(); @@ -404,21 +351,18 @@ void Omnidirectional3D::generateMotionModel( orientation2->data()[fuse_variables::Orientation3DStamped::Y] = state2.orientation.y(); orientation2->data()[fuse_variables::Orientation3DStamped::Z] = state2.orientation.z(); orientation2->data()[fuse_variables::Orientation3DStamped::W] = state2.orientation.w(); - + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::X] = state2.vel_linear.x(); velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Y] = state2.vel_linear.y(); velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Z] = state2.vel_linear.z(); - velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state2.vel_angular.x(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state2.vel_angular.x(); velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state2.vel_angular.y(); - velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state2.vel_angular.z(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state2.vel_angular.z(); - acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::X] = - state2.acc_linear.x(); - acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Y] = - state2.acc_linear.y(); - acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Z] = - state2.acc_linear.z(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::X] = state2.acc_linear.x(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Y] = state2.acc_linear.y(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Z] = state2.acc_linear.z(); state1.position_uuid = position1->uuid(); state1.orientation_uuid = orientation1->uuid(); @@ -437,40 +381,33 @@ void Omnidirectional3D::generateMotionModel( // Scale process noise covariance pose by the norm of the current state twist auto process_noise_covariance = process_noise_covariance_; - if (scale_process_noise_) { - common::scaleProcessNoiseCovariance( - process_noise_covariance, state1.vel_linear, state1.vel_angular, - velocity_linear_norm_min_, velocity_angular_norm_min_); + if (scale_process_noise_) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, state1.vel_linear, state1.vel_angular, + velocity_linear_norm_min_, velocity_angular_norm_min_); } // Validate process_noise_covariance *= dt; - if (!disable_checks_) { - try { + if (!disable_checks_) + { + try + { validateMotionModel(state1, state2, process_noise_covariance); - } catch (const std::runtime_error & ex) { - RCLCPP_ERROR_STREAM_THROTTLE( - logger_, *clock_, 10.0 * 1000, - "Invalid '" << name_ << "' motion model: " << ex.what()); + } + catch (const std::runtime_error& ex) + { + RCLCPP_ERROR_STREAM_THROTTLE(logger_, *clock_, 10.0 * 1000, + "Invalid '" << name_ << "' motion model: " << ex.what()); return; } } // Create the constraints for this motion model segment auto constraint = fuse_models::Omnidirectional3DStateKinematicConstraint::make_shared( - name(), - *position1, - *orientation1, - *velocity_linear1, - *velocity_angular1, - *acceleration_linear1, - *position2, - *orientation2, - *velocity_linear2, - *velocity_angular2, - *acceleration_linear2, - process_noise_covariance); + name(), *position1, *orientation1, *velocity_linear1, *velocity_angular1, *acceleration_linear1, *position2, + *orientation2, *velocity_linear2, *velocity_angular2, *acceleration_linear2, process_noise_covariance); // Update the output variables constraints.push_back(constraint); @@ -486,22 +423,24 @@ void Omnidirectional3D::generateMotionModel( variables.push_back(acceleration_linear2); } -void Omnidirectional3D::updateStateHistoryEstimates( - const fuse_core::Graph & graph, - StateHistory & state_history, - const rclcpp::Duration & buffer_length) +void Omnidirectional3D::updateStateHistoryEstimates(const fuse_core::Graph& graph, StateHistory& state_history, + const rclcpp::Duration& buffer_length) { - if (state_history.empty()) { + if (state_history.empty()) + { return; } // Compute the expiration time carefully, as ROS can't handle negative times - const auto & ending_stamp = state_history.rbegin()->first; + const auto& ending_stamp = state_history.rbegin()->first; rclcpp::Time expiration_time; - if (ending_stamp.seconds() > buffer_length.seconds()) { + if (ending_stamp.seconds() > buffer_length.seconds()) + { expiration_time = ending_stamp - buffer_length; - } else { + } + else + { // NOTE(CH3): Uninitialized. But okay because it's just used for comparison. expiration_time = rclcpp::Time(0, 0, ending_stamp.get_clock_type()); } @@ -511,7 +450,8 @@ void Omnidirectional3D::updateStateHistoryEstimates( // - at least one entry remains at all times // - the history covers *at least* until the expiration time. Longer is acceptable. auto expiration_iter = state_history.upper_bound(expiration_time); - if (expiration_iter != state_history.begin()) { + if (expiration_iter != state_history.begin()) + { // expiration_iter points to the first element > expiration_time. // Back up one entry, to a point that is <= expiration_time state_history.erase(state_history.begin(), std::prev(expiration_iter)); @@ -520,23 +460,20 @@ void Omnidirectional3D::updateStateHistoryEstimates( // Update the states in the state history with information from the graph // If a state is not in the graph yet, predict the state in question from the closest previous // state - for (auto current_iter = state_history.begin(); current_iter != state_history.end(); - ++current_iter) + for (auto current_iter = state_history.begin(); current_iter != state_history.end(); ++current_iter) { - const auto & current_stamp = current_iter->first; - auto & current_state = current_iter->second; - if (graph.variableExists(current_state.position_uuid) && - graph.variableExists(current_state.orientation_uuid) && - graph.variableExists(current_state.vel_linear_uuid) && - graph.variableExists(current_state.vel_angular_uuid) && - graph.variableExists(current_state.acc_linear_uuid)) + const auto& current_stamp = current_iter->first; + auto& current_state = current_iter->second; + if (graph.variableExists(current_state.position_uuid) && graph.variableExists(current_state.orientation_uuid) && + graph.variableExists(current_state.vel_linear_uuid) && graph.variableExists(current_state.vel_angular_uuid) && + graph.variableExists(current_state.acc_linear_uuid)) { // This pose does exist in the graph. Update it directly. - const auto & position = graph.getVariable(current_state.position_uuid); - const auto & orientation = graph.getVariable(current_state.orientation_uuid); - const auto & vel_linear = graph.getVariable(current_state.vel_linear_uuid); - const auto & vel_angular = graph.getVariable(current_state.vel_angular_uuid); - const auto & acc_linear = graph.getVariable(current_state.acc_linear_uuid); + const auto& position = graph.getVariable(current_state.position_uuid); + const auto& orientation = graph.getVariable(current_state.orientation_uuid); + const auto& vel_linear = graph.getVariable(current_state.vel_linear_uuid); + const auto& vel_angular = graph.getVariable(current_state.vel_angular_uuid); + const auto& acc_linear = graph.getVariable(current_state.acc_linear_uuid); current_state.position.x() = position.data()[fuse_variables::Position3DStamped::X]; current_state.position.y() = position.data()[fuse_variables::Position3DStamped::Y]; @@ -547,74 +484,67 @@ void Omnidirectional3D::updateStateHistoryEstimates( current_state.orientation.z() = orientation.data()[fuse_variables::Orientation3DStamped::Z]; current_state.orientation.w() = orientation.data()[fuse_variables::Orientation3DStamped::W]; - current_state.vel_linear.x() = - vel_linear.data()[fuse_variables::VelocityLinear3DStamped::X]; - current_state.vel_linear.y() = - vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Y]; - current_state.vel_linear.z() = - vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Z]; - - current_state.vel_angular.x() = - vel_angular.data()[fuse_variables::VelocityAngular3DStamped::ROLL]; - current_state.vel_angular.y() = - vel_angular.data()[fuse_variables::VelocityAngular3DStamped::PITCH]; - current_state.vel_angular.z() = - vel_angular.data()[fuse_variables::VelocityAngular3DStamped::YAW]; - - current_state.acc_linear.x() = - acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::X]; - current_state.acc_linear.y() = - acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Y]; - current_state.acc_linear.z() = - acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Z]; - } else if (current_iter != state_history.begin()) { + current_state.vel_linear.x() = vel_linear.data()[fuse_variables::VelocityLinear3DStamped::X]; + current_state.vel_linear.y() = vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Y]; + current_state.vel_linear.z() = vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Z]; + + current_state.vel_angular.x() = vel_angular.data()[fuse_variables::VelocityAngular3DStamped::ROLL]; + current_state.vel_angular.y() = vel_angular.data()[fuse_variables::VelocityAngular3DStamped::PITCH]; + current_state.vel_angular.z() = vel_angular.data()[fuse_variables::VelocityAngular3DStamped::YAW]; + + current_state.acc_linear.x() = acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::X]; + current_state.acc_linear.y() = acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Y]; + current_state.acc_linear.z() = acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Z]; + } + else if (current_iter != state_history.begin()) + { auto previous_iter = std::prev(current_iter); - const auto & previous_stamp = previous_iter->first; - const auto & previous_state = previous_iter->second; - + const auto& previous_stamp = previous_iter->first; + const auto& previous_state = previous_iter->second; + // This state is not in the graph yet, so we can't update/correct the value in our state // history. However, the state *before* this one may have been corrected (or one of its // predecessors may have been), so we can use that corrected value, along with our prediction // logic, to provide a more accurate update to this state. - predict( - previous_state.position, - previous_state.orientation, - previous_state.vel_linear, - previous_state.vel_angular, - previous_state.acc_linear, - (current_stamp - previous_stamp).seconds(), - current_state.position, - current_state.orientation, - current_state.vel_linear, - current_state.vel_angular, - current_state.acc_linear); + predict(previous_state.position, previous_state.orientation, previous_state.vel_linear, + previous_state.vel_angular, previous_state.acc_linear, (current_stamp - previous_stamp).seconds(), + current_state.position, current_state.orientation, current_state.vel_linear, current_state.vel_angular, + current_state.acc_linear); } } } -void Omnidirectional3D::validateMotionModel( - const StateHistoryElement & state1, const StateHistoryElement & state2, - const fuse_core::Matrix15d & process_noise_covariance) +void Omnidirectional3D::validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, + const fuse_core::Matrix15d& process_noise_covariance) { - try { + try + { state1.validate(); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid state #1: " + std::string(ex.what())); } - try { + try + { state2.validate(); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid state #2: " + std::string(ex.what())); } - try { + try + { fuse_core::validateCovariance(process_noise_covariance); - } catch (const std::runtime_error & ex) { + } + catch (const std::runtime_error& ex) + { throw std::runtime_error("Invalid process noise covariance: " + std::string(ex.what())); } } -std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & omnidirectional_3d) +std::ostream& operator<<(std::ostream& stream, const Omnidirectional3D& omnidirectional_3d) { omnidirectional_3d.print(stream); return stream; diff --git a/fuse_models/src/omnidirectional_3d_ignition.cpp b/fuse_models/src/omnidirectional_3d_ignition.cpp index d545007d0..55c64a60f 100644 --- a/fuse_models/src/omnidirectional_3d_ignition.cpp +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -67,18 +67,17 @@ namespace fuse_models { Omnidirectional3DIgnition::Omnidirectional3DIgnition() -: fuse_core::AsyncSensorModel(1), - started_(false), - initial_transaction_sent_(false), - device_id_(fuse_core::uuid::NIL), - logger_(rclcpp::get_logger("uninitialized")) + : fuse_core::AsyncSensorModel(1) + , started_(false) + , initial_transaction_sent_(false) + , device_id_(fuse_core::uuid::NIL) + , logger_(rclcpp::get_logger("uninitialized")) { } void Omnidirectional3DIgnition::initialize( - fuse_core::node_interfaces::NodeInterfaces interfaces, - const std::string & name, - fuse_core::TransactionCallback transaction_callback) + fuse_core::node_interfaces::NodeInterfaces interfaces, const std::string& name, + fuse_core::TransactionCallback transaction_callback) { interfaces_ = interfaces; fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); @@ -95,52 +94,32 @@ void Omnidirectional3DIgnition::onInit() params_.loadFromROS(interfaces_, name_); // Connect to the reset service - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { reset_client_ = rclcpp::create_client( - interfaces_.get_node_base_interface(), - interfaces_.get_node_graph_interface(), - interfaces_.get_node_services_interface(), - params_.reset_service, - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_graph_interface(), + interfaces_.get_node_services_interface(), params_.reset_service, rclcpp::ServicesQoS(), cb_group_); } // Advertise rclcpp::SubscriptionOptions sub_options; sub_options.callback_group = cb_group_; sub_ = rclcpp::create_subscription( - interfaces_, - params_.topic, - params_.queue_size, - std::bind(&Omnidirectional3DIgnition::subscriberCallback, this, std::placeholders::_1), - sub_options - ); + interfaces_, params_.topic, params_.queue_size, + std::bind(&Omnidirectional3DIgnition::subscriberCallback, this, std::placeholders::_1), sub_options); set_pose_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_pose_service), - std::bind( - &Omnidirectional3DIgnition::setPoseServiceCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_service), + std::bind(&Omnidirectional3DIgnition::setPoseServiceCallback, this, std::placeholders::_1, std::placeholders::_2, + std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); set_pose_deprecated_service_ = rclcpp::create_service( - interfaces_.get_node_base_interface(), - interfaces_.get_node_services_interface(), - fuse_core::joinTopicName( - interfaces_.get_node_base_interface()->get_name(), - params_.set_pose_deprecated_service), - std::bind( - &Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, - std::placeholders::_2, std::placeholders::_3), - rclcpp::ServicesQoS(), - cb_group_ - ); + interfaces_.get_node_base_interface(), interfaces_.get_node_services_interface(), + fuse_core::joinTopicName(interfaces_.get_node_base_interface()->get_name(), params_.set_pose_deprecated_service), + std::bind(&Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback, this, std::placeholders::_1, + std::placeholders::_2, std::placeholders::_3), + rclcpp::ServicesQoS(), cb_group_); } void Omnidirectional3DIgnition::start() @@ -150,7 +129,8 @@ void Omnidirectional3DIgnition::start() // TODO(swilliams) Should this be executed every time optimizer.reset() is called, or only once // ever? I feel like it should be "only once ever". Send an initial state // transaction immediately, if requested - if (params_.publish_on_startup && !initial_transaction_sent_) { + if (params_.publish_on_startup && !initial_transaction_sent_) + { auto pose = geometry_msgs::msg::PoseWithCovarianceStamped(); tf2::Quaternion q; // q.setRPY(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); @@ -163,7 +143,8 @@ void Omnidirectional3DIgnition::start() pose.pose.pose.orientation.y = q.y(); pose.pose.pose.orientation.z = q.z(); pose.pose.pose.orientation.w = q.w(); - for (size_t i = 0; i < 6; i++) { + for (size_t i = 0; i < 6; i++) + { pose.pose.covariance[i * 7] = params_.initial_sigma[i] * params_.initial_sigma[i]; } sendPrior(pose); @@ -176,30 +157,32 @@ void Omnidirectional3DIgnition::stop() started_ = false; } -void Omnidirectional3DIgnition::subscriberCallback( - const geometry_msgs::msg::PoseWithCovarianceStamped & msg) +void Omnidirectional3DIgnition::subscriberCallback(const geometry_msgs::msg::PoseWithCovarianceStamped& msg) { - try { + try + { process(msg); - } catch (const std::exception & e) { + } + catch (const std::exception& e) + { RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring message."); } } -bool Omnidirectional3DIgnition::setPoseServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPose::Request::SharedPtr req) +bool Omnidirectional3DIgnition::setPoseServiceCallback(rclcpp::Service::SharedPtr service, + std::shared_ptr request_id, + const fuse_msgs::srv::SetPose::Request::SharedPtr req) { - try { - process( - req->pose, - [service, request_id]() { - fuse_msgs::srv::SetPose::Response response; - response.success = true; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->pose, [service, request_id]() { + fuse_msgs::srv::SetPose::Response response; + response.success = true; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetPose::Response response; response.success = false; response.message = e.what(); @@ -210,18 +193,18 @@ bool Omnidirectional3DIgnition::setPoseServiceCallback( } bool Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback( - rclcpp::Service::SharedPtr service, - std::shared_ptr request_id, - const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) + rclcpp::Service::SharedPtr service, std::shared_ptr request_id, + const fuse_msgs::srv::SetPoseDeprecated::Request::SharedPtr req) { - try { - process( - req->pose, - [service, request_id]() { - fuse_msgs::srv::SetPoseDeprecated::Response response; - service->send_response(*request_id, response); - }); - } catch (const std::exception & e) { + try + { + process(req->pose, [service, request_id]() { + fuse_msgs::srv::SetPoseDeprecated::Response response; + service->send_response(*request_id, response); + }); + } + catch (const std::exception& e) + { fuse_msgs::srv::SetPoseDeprecated::Response response; RCLCPP_ERROR_STREAM(logger_, e.what() << " Ignoring request."); service->send_response(*request_id, response); @@ -229,33 +212,32 @@ bool Omnidirectional3DIgnition::setPoseDeprecatedServiceCallback( return true; } -void Omnidirectional3DIgnition::process( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, std::function post_process) +void Omnidirectional3DIgnition::process(const geometry_msgs::msg::PoseWithCovarianceStamped& pose, + std::function post_process) { // Verify we are in the correct state to process set pose requests - if (!started_) { + if (!started_) + { throw std::runtime_error("Attempting to set the pose while the sensor is stopped."); } // Validate the requested pose and covariance before we do anything - if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y) || !std::isfinite(pose.pose.pose.position.z)) { + if (!std::isfinite(pose.pose.pose.position.x) || !std::isfinite(pose.pose.pose.position.y) || + !std::isfinite(pose.pose.pose.position.z)) + { throw std::invalid_argument( - "Attempting to set the pose to an invalid position (" + - std::to_string(pose.pose.pose.position.x) + ", " + - std::to_string(pose.pose.pose.position.y) + ", " + - std::to_string(pose.pose.pose.position.z) + ")."); + "Attempting to set the pose to an invalid position (" + std::to_string(pose.pose.pose.position.x) + ", " + + std::to_string(pose.pose.pose.position.y) + ", " + std::to_string(pose.pose.pose.position.z) + ")."); } - auto orientation_norm = std::sqrt( - pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + - pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + - pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + - pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); - if (std::abs(orientation_norm - 1.0) > 1.0e-3) { + auto orientation_norm = std::sqrt(pose.pose.pose.orientation.x * pose.pose.pose.orientation.x + + pose.pose.pose.orientation.y * pose.pose.pose.orientation.y + + pose.pose.pose.orientation.z * pose.pose.pose.orientation.z + + pose.pose.pose.orientation.w * pose.pose.pose.orientation.w); + if (std::abs(orientation_norm - 1.0) > 1.0e-3) + { throw std::invalid_argument( - "Attempting to set the pose to an invalid orientation (" + - std::to_string(pose.pose.pose.orientation.x) + ", " + - std::to_string(pose.pose.pose.orientation.y) + ", " + - std::to_string(pose.pose.pose.orientation.z) + ", " + - std::to_string(pose.pose.pose.orientation.w) + ")."); + "Attempting to set the pose to an invalid orientation (" + std::to_string(pose.pose.pose.orientation.x) + ", " + + std::to_string(pose.pose.pose.orientation.y) + ", " + std::to_string(pose.pose.pose.orientation.z) + ", " + + std::to_string(pose.pose.pose.orientation.w) + ")."); } auto position_cov = fuse_core::Matrix3d(); // for (size_t i = 0; i < 3; i++) { @@ -263,18 +245,18 @@ void Omnidirectional3DIgnition::process( // position_cov(i, j) = pose.pose.covariance[i * 6 + j]; // } // } - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], - pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], - pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; - if (!fuse_core::isSymmetric(position_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-symmetric position covariance matrix\n " + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], pose.pose.covariance[6], + pose.pose.covariance[7], pose.pose.covariance[8], pose.pose.covariance[12], pose.pose.covariance[13], + pose.pose.covariance[14]; + if (!fuse_core::isSymmetric(position_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-symmetric position covariance matrix\n " + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } - if (!fuse_core::isPositiveDefinite(position_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-positive-definite position covariance matrix\n" + - fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); + if (!fuse_core::isPositiveDefinite(position_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-positive-definite position covariance matrix\n" + + fuse_core::to_string(position_cov, Eigen::FullPrecision) + "."); } auto orientation_cov = fuse_core::Matrix3d(); // for (size_t i = 0; i < 3; i++) { @@ -283,27 +265,28 @@ void Omnidirectional3DIgnition::process( // } // } orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], - pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], - pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; - if (!fuse_core::isSymmetric(orientation_cov)) { - throw std::invalid_argument( - "Attempting to set the pose with a non-symmetric orientation covariance matrix\n " + - fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], pose.pose.covariance[33], + pose.pose.covariance[34], pose.pose.covariance[35]; + if (!fuse_core::isSymmetric(orientation_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-symmetric orientation covariance matrix\n " + + fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); } - if (!fuse_core::isPositiveDefinite(orientation_cov)) { + if (!fuse_core::isPositiveDefinite(orientation_cov)) + { throw std::invalid_argument( - "Attempting to set the pose with a non-positive-definite orientation_cov covariance matrix\n" + - fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); + "Attempting to set the pose with a non-positive-definite orientation_cov covariance matrix\n" + + fuse_core::to_string(orientation_cov, Eigen::FullPrecision) + "."); } // Tell the optimizer to reset before providing the initial state - if (!params_.reset_service.empty()) { + if (!params_.reset_service.empty()) + { // Wait for the reset service while (!reset_client_->wait_for_service(std::chrono::seconds(10)) && - interfaces_.get_node_base_interface()->get_context()->is_valid()) + interfaces_.get_node_base_interface()->get_context()->is_valid()) { - RCLCPP_WARN_STREAM( - logger_, - "Waiting for '" << reset_client_->get_service_name() << "' service to become avaiable."); + RCLCPP_WARN_STREAM(logger_, + "Waiting for '" << reset_client_->get_service_name() << "' service to become avaiable."); } auto srv = std::make_shared(); @@ -311,27 +294,30 @@ void Omnidirectional3DIgnition::process( // It needs to be free to handle the response to this service call. // Have a callback do the rest of the work when a response comes. auto result_future = reset_client_->async_send_request( - srv, - [this, post_process, pose](rclcpp::Client::SharedFuture result) { - (void)result; - // Now that the pose has been validated and the optimizer has been reset, actually send the - // initial state constraints to the optimizer - sendPrior(pose); - if (post_process) { - post_process(); - } - }); - } else { + srv, [this, post_process, pose](rclcpp::Client::SharedFuture result) { + (void)result; + // Now that the pose has been validated and the optimizer has been reset, actually send the + // initial state constraints to the optimizer + sendPrior(pose); + if (post_process) + { + post_process(); + } + }); + } + else + { sendPrior(pose); - if (post_process) { + if (post_process) + { post_process(); } } } -void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped & pose) +void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCovarianceStamped& pose) { - const auto & stamp = pose.header.stamp; + const auto& stamp = pose.header.stamp; // Create variables for the full state. // The initial values of the pose are extracted from the provided PoseWithCovarianceStamped @@ -379,55 +365,42 @@ void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCova // } // } - position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], - pose.pose.covariance[6], pose.pose.covariance[7], pose.pose.covariance[8], - pose.pose.covariance[12], pose.pose.covariance[13], pose.pose.covariance[14]; + position_cov << pose.pose.covariance[0], pose.pose.covariance[1], pose.pose.covariance[2], pose.pose.covariance[6], + pose.pose.covariance[7], pose.pose.covariance[8], pose.pose.covariance[12], pose.pose.covariance[13], + pose.pose.covariance[14]; orientation_cov << pose.pose.covariance[21], pose.pose.covariance[22], pose.pose.covariance[23], - pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], - pose.pose.covariance[33], pose.pose.covariance[34], pose.pose.covariance[35]; - - linear_velocity_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, - 0.0, params_.initial_sigma[7] * params_.initial_sigma[7], 0.0, - 0.0, 0.0, params_.initial_sigma[8] * params_.initial_sigma[8]; - - angular_velocity_cov << params_.initial_sigma[9] * params_.initial_sigma[9], 0.0, 0.0, - 0.0, params_.initial_sigma[10] * params_.initial_sigma[10], 0.0, - 0.0, 0.0, params_.initial_sigma[11] * params_.initial_sigma[11]; - - linear_acceleration_cov << params_.initial_sigma[12] * params_.initial_sigma[12], 0.0, 0.0, - 0.0, params_.initial_sigma[13] * params_.initial_sigma[13], 0.0, - 0.0, 0.0, params_.initial_sigma[14] * params_.initial_sigma[14]; + pose.pose.covariance[27], pose.pose.covariance[28], pose.pose.covariance[29], pose.pose.covariance[33], + pose.pose.covariance[34], pose.pose.covariance[35]; + + linear_velocity_cov << params_.initial_sigma[6] * params_.initial_sigma[6], 0.0, 0.0, 0.0, + params_.initial_sigma[7] * params_.initial_sigma[7], 0.0, 0.0, 0.0, + params_.initial_sigma[8] * params_.initial_sigma[8]; + + angular_velocity_cov << params_.initial_sigma[9] * params_.initial_sigma[9], 0.0, 0.0, 0.0, + params_.initial_sigma[10] * params_.initial_sigma[10], 0.0, 0.0, 0.0, + params_.initial_sigma[11] * params_.initial_sigma[11]; + + linear_acceleration_cov << params_.initial_sigma[12] * params_.initial_sigma[12], 0.0, 0.0, 0.0, + params_.initial_sigma[13] * params_.initial_sigma[13], 0.0, 0.0, 0.0, + params_.initial_sigma[14] * params_.initial_sigma[14]; // Create absolute constraints for each variable auto position_constraint = fuse_constraints::AbsolutePosition3DStampedConstraint::make_shared( - name(), - *position, - fuse_core::Vector3d(position->x(), position->y(), position->z()), - position_cov); - auto orientation_constraint = - fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( - name(), - *orientation, - Eigen::Quaterniond(orientation->w(), orientation->x(), orientation->y(), orientation->z()), - orientation_cov); - auto linear_velocity_constraint = - fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( - name(), - *linear_velocity, - fuse_core::Vector3d(linear_velocity->x(), linear_velocity->y(), linear_velocity->z()), - linear_velocity_cov); - auto angular_velocity_constraint = - fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( - name(), - *angular_velocity, - fuse_core::Vector3d(angular_velocity->roll(), angular_velocity->pitch(), angular_velocity->yaw()), - angular_velocity_cov); - auto linear_acceleration_constraint = - fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( - name(), - *linear_acceleration, - fuse_core::Vector3d(linear_acceleration->x(), linear_acceleration->y(), linear_acceleration->z()), - linear_acceleration_cov); + name(), *position, fuse_core::Vector3d(position->x(), position->y(), position->z()), position_cov); + auto orientation_constraint = fuse_constraints::AbsoluteOrientation3DStampedConstraint::make_shared( + name(), *orientation, Eigen::Quaterniond(orientation->w(), orientation->x(), orientation->y(), orientation->z()), + orientation_cov); + auto linear_velocity_constraint = fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( + name(), *linear_velocity, fuse_core::Vector3d(linear_velocity->x(), linear_velocity->y(), linear_velocity->z()), + linear_velocity_cov); + auto angular_velocity_constraint = fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( + name(), *angular_velocity, + fuse_core::Vector3d(angular_velocity->roll(), angular_velocity->pitch(), angular_velocity->yaw()), + angular_velocity_cov); + auto linear_acceleration_constraint = fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( + name(), *linear_acceleration, + fuse_core::Vector3d(linear_acceleration->x(), linear_acceleration->y(), linear_acceleration->z()), + linear_acceleration_cov); // Create the transaction auto transaction = fuse_core::Transaction::make_shared(); @@ -447,13 +420,9 @@ void Omnidirectional3DIgnition::sendPrior(const geometry_msgs::msg::PoseWithCova // Send the transaction to the optimizer. sendTransaction(transaction); - RCLCPP_INFO_STREAM( - logger_, - "Received a set_pose request (stamp: " << rclcpp::Time(stamp).nanoseconds() - << ", x: " << position->x() << ", y: " - << position->y() << ", z: " << position->z() - << ", roll: " << orientation->roll() - << ", pitch: " << orientation->pitch() - << ", yaw: " << orientation->yaw() << ")"); + RCLCPP_INFO_STREAM(logger_, "Received a set_pose request (stamp: " + << rclcpp::Time(stamp).nanoseconds() << ", x: " << position->x() << ", y: " + << position->y() << ", z: " << position->z() << ", roll: " << orientation->roll() + << ", pitch: " << orientation->pitch() << ", yaw: " << orientation->yaw() << ")"); } } // namespace fuse_models diff --git a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp index 7caf9dc3b..7b222f762 100644 --- a/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp +++ b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp @@ -49,36 +49,26 @@ namespace fuse_models { Omnidirectional3DStateKinematicConstraint::Omnidirectional3DStateKinematicConstraint( - const std::string & source, - const fuse_variables::Position3DStamped & position1, - const fuse_variables::Orientation3DStamped & orientation1, - const fuse_variables::VelocityLinear3DStamped & velocity_linear1, - const fuse_variables::VelocityAngular3DStamped & velocity_angular1, - const fuse_variables::AccelerationLinear3DStamped & acceleration_linear1, - const fuse_variables::Position3DStamped & position2, - const fuse_variables::Orientation3DStamped & orientation2, - const fuse_variables::VelocityLinear3DStamped & velocity_linear2, - const fuse_variables::VelocityAngular3DStamped & velocity_angular2, - const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, - const fuse_core::Matrix15d & covariance) -: fuse_core::Constraint( - source, - {position1.uuid(), - orientation1.uuid(), - velocity_linear1.uuid(), - velocity_angular1.uuid(), - acceleration_linear1.uuid(), - position2.uuid(), - orientation2.uuid(), - velocity_linear2.uuid(), - velocity_angular2.uuid(), - acceleration_linear2.uuid()}), // NOLINT - dt_((position2.stamp() - position1.stamp()).seconds()), - sqrt_information_(covariance.inverse().llt().matrixU()) + const std::string& source, const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::VelocityLinear3DStamped& velocity_linear1, + const fuse_variables::VelocityAngular3DStamped& velocity_angular1, + const fuse_variables::AccelerationLinear3DStamped& acceleration_linear1, + const fuse_variables::Position3DStamped& position2, const fuse_variables::Orientation3DStamped& orientation2, + const fuse_variables::VelocityLinear3DStamped& velocity_linear2, + const fuse_variables::VelocityAngular3DStamped& velocity_angular2, + const fuse_variables::AccelerationLinear3DStamped& acceleration_linear2, const fuse_core::Matrix15d& covariance) + : fuse_core::Constraint(source, + { position1.uuid(), orientation1.uuid(), velocity_linear1.uuid(), velocity_angular1.uuid(), + acceleration_linear1.uuid(), position2.uuid(), orientation2.uuid(), velocity_linear2.uuid(), + velocity_angular2.uuid(), acceleration_linear2.uuid() }) + , // NOLINT + dt_((position2.stamp() - position1.stamp()).seconds()) + , sqrt_information_(covariance.inverse().llt().matrixU()) { } -void Omnidirectional3DStateKinematicConstraint::print(std::ostream & stream) const +void Omnidirectional3DStateKinematicConstraint::print(std::ostream& stream) const { stream << type() << "\n" << " source: " << source() << "\n" @@ -97,7 +87,7 @@ void Omnidirectional3DStateKinematicConstraint::print(std::ostream & stream) con << " sqrt_info: " << sqrtInformation() << "\n"; } -ceres::CostFunction * Omnidirectional3DStateKinematicConstraint::costFunction() const +ceres::CostFunction* Omnidirectional3DStateKinematicConstraint::costFunction() const { return new Omnidirectional3DStateCostFunction(dt_, sqrt_information_); // Here we return a cost function that computes the analytic derivatives/jacobians, but we could diff --git a/fuse_models/test/test_omnidirectional_3d.cpp b/fuse_models/test/test_omnidirectional_3d.cpp index f82bb75a2..01fbf7f6b 100644 --- a/fuse_models/test/test_omnidirectional_3d.cpp +++ b/fuse_models/test/test_omnidirectional_3d.cpp @@ -21,9 +21,9 @@ class Omnidirectional3DModelTest : public fuse_models::Omnidirectional3D { public: - using fuse_models::Omnidirectional3D::updateStateHistoryEstimates; - using fuse_models::Omnidirectional3D::StateHistoryElement; using fuse_models::Omnidirectional3D::StateHistory; + using fuse_models::Omnidirectional3D::StateHistoryElement; + using fuse_models::Omnidirectional3D::updateStateHistoryEstimates; }; TEST(Omnidirectional3D, UpdateStateHistoryEstimates) @@ -33,8 +33,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1, 0)); auto linear_velocity1 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(1, 0)); auto angular_velocity1 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(1, 0)); - auto linear_acceleration1 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_acceleration1 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(1, 0)); position1->x() = 1.1; position1->y() = 2.1; position1->z() = 0.0; @@ -55,8 +54,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2, 0)); auto linear_velocity2 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(2, 0)); auto angular_velocity2 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(2, 0)); - auto linear_acceleration2 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_acceleration2 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(2, 0)); position2->x() = 1.2; position2->y() = 2.2; position2->z() = 0.0; @@ -77,8 +75,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 0)); auto linear_velocity3 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(3, 0)); auto angular_velocity3 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(3, 0)); - auto linear_acceleration3 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_acceleration3 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(3, 0)); position3->x() = 1.3; position3->y() = 2.3; position3->z() = 0.0; @@ -99,8 +96,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation4 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(4, 0)); auto linear_velocity4 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(4, 0)); auto angular_velocity4 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(4, 0)); - auto linear_acceleration4 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_acceleration4 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(4, 0)); position4->x() = 1.4; position4->y() = 2.4; position4->z() = 0.0; @@ -121,8 +117,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto orientation5 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(5, 0)); auto linear_velocity5 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(5, 0)); auto angular_velocity5 = fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(5, 0)); - auto linear_acceleration5 = - fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_acceleration5 = fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(5, 0)); position5->x() = 1.5; position5->y() = 2.5; position5->z() = 0.0; @@ -156,76 +151,49 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) // Add all of the variables to the state history Omnidirectional3DModelTest::StateHistory state_history; - state_history.emplace( - position1->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position1->uuid(), - orientation1->uuid(), - linear_velocity1->uuid(), - angular_velocity1->uuid(), - linear_acceleration1->uuid(), - fuse_core::Vector3d(1.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position2->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position2->uuid(), - orientation2->uuid(), - linear_velocity2->uuid(), - angular_velocity2->uuid(), - linear_acceleration2->uuid(), - fuse_core::Vector3d(2.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position3->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position3->uuid(), - orientation3->uuid(), - linear_velocity3->uuid(), - angular_velocity3->uuid(), - linear_acceleration3->uuid(), - fuse_core::Vector3d(3.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position4->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position4->uuid(), - orientation4->uuid(), - linear_velocity4->uuid(), - angular_velocity4->uuid(), - linear_acceleration4->uuid(), - fuse_core::Vector3d(4.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) - state_history.emplace( - position5->stamp(), - Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) - position5->uuid(), - orientation5->uuid(), - linear_velocity5->uuid(), - angular_velocity5->uuid(), - linear_acceleration5->uuid(), - fuse_core::Vector3d(5.0, 0.0, 0.0), - Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0), - fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace(position1->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position1->uuid(), orientation1->uuid(), linear_velocity1->uuid(), + angular_velocity1->uuid(), linear_acceleration1->uuid(), fuse_core::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position2->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position2->uuid(), orientation2->uuid(), linear_velocity2->uuid(), + angular_velocity2->uuid(), linear_acceleration2->uuid(), fuse_core::Vector3d(2.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position3->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position3->uuid(), orientation3->uuid(), linear_velocity3->uuid(), + angular_velocity3->uuid(), linear_acceleration3->uuid(), fuse_core::Vector3d(3.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position4->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position4->uuid(), orientation4->uuid(), linear_velocity4->uuid(), + angular_velocity4->uuid(), linear_acceleration4->uuid(), fuse_core::Vector3d(4.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) + state_history.emplace(position5->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ + // NOLINT(whitespace/braces) + position5->uuid(), orientation5->uuid(), linear_velocity5->uuid(), + angular_velocity5->uuid(), linear_acceleration5->uuid(), fuse_core::Vector3d(5.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0) }); // NOLINT(whitespace/braces) // Update the state history - Omnidirectional3DModelTest::updateStateHistoryEstimates( - graph, state_history, rclcpp::Duration::from_seconds( - 10.0)); + Omnidirectional3DModelTest::updateStateHistoryEstimates(graph, state_history, rclcpp::Duration::from_seconds(10.0)); // Check the state estimates in the state history { @@ -262,7 +230,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) } { // The second entry is included in the graph. It will get updated directly. - auto expected_position = fuse_core::Vector3d(1.2, 2.2, 0.0); // <-- value in the Graph + auto expected_position = fuse_core::Vector3d(1.2, 2.2, 0.0); // <-- value in the Graph auto expected_orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); auto actual_position = state_history[rclcpp::Time(2, 0)].position; auto actual_orientation = state_history[rclcpp::Time(2, 0)].orientation; @@ -295,10 +263,9 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) { // The third entry is missing from the graph. It will get predicted from previous state. auto expected_position = fuse_core::Vector3d(1.2, 3.7, 0.0); - auto expected_orientation = - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto expected_orientation = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); auto actual_position = state_history[rclcpp::Time(3, 0)].position; auto actual_orientation = state_history[rclcpp::Time(3, 0)].orientation; EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); @@ -309,7 +276,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); - auto expected_linear_velocity = fuse_core::Vector3d(0.0, 2.0, 0.0); + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 2.0, 0.0); auto actual_linear_velocity = state_history[rclcpp::Time(3, 0)].vel_linear; EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); @@ -329,11 +296,10 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) } { // The forth entry is included in the graph. It will get updated directly. - auto expected_position = fuse_core::Vector3d(1.4, 2.4, 0.0); // <-- value in the Graph - auto expected_orientation = - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto expected_position = fuse_core::Vector3d(1.4, 2.4, 0.0); // <-- value in the Graph + auto expected_orientation = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); auto actual_position = state_history[rclcpp::Time(4, 0)].position; auto actual_orientation = state_history[rclcpp::Time(4, 0)].orientation; EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); @@ -365,10 +331,9 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) { // The fifth entry is missing from the graph. It will get predicted from previous state. auto expected_position = fuse_core::Vector3d(9.5, 12.0, 0.0); - auto expected_orientation = - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto expected_orientation = Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); auto actual_position = state_history[rclcpp::Time(5, 0)].position; auto actual_orientation = state_history[rclcpp::Time(5, 0)].orientation; EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); @@ -389,7 +354,7 @@ TEST(Omnidirectional3D, UpdateStateHistoryEstimates) auto actual_angular_velocity = state_history[rclcpp::Time(5, 0)].vel_angular; EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); - EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); auto expected_linear_acceleration = fuse_core::Vector3d(7.4, 8.4, 0.0); auto actual_linear_acceleration = state_history[rclcpp::Time(5, 0)].acc_linear; diff --git a/fuse_models/test/test_omnidirectional_3d_predict.cpp b/fuse_models/test/test_omnidirectional_3d_predict.cpp index 230c0b8cc..26e97be53 100644 --- a/fuse_models/test/test_omnidirectional_3d_predict.cpp +++ b/fuse_models/test/test_omnidirectional_3d_predict.cpp @@ -43,11 +43,11 @@ TEST(Predict, predictDirectVals) { - fuse_core::Vector3d position1 (0.0, 0.0, 0.0); - Eigen::Quaterniond orientation1 (1.0, 0.0, 0.0, 0.0); - fuse_core::Vector3d vel_linear1 (1.0, 0.0, 0.0); - fuse_core::Vector3d vel_angular1 (0.0, 0.0, 1.570796327); - fuse_core::Vector3d acc_linear1 (1.0, 0.0, 0.0); + fuse_core::Vector3d position1(0.0, 0.0, 0.0); + Eigen::Quaterniond orientation1(1.0, 0.0, 0.0, 0.0); + fuse_core::Vector3d vel_linear1(1.0, 0.0, 0.0); + fuse_core::Vector3d vel_angular1(0.0, 0.0, 1.570796327); + fuse_core::Vector3d acc_linear1(1.0, 0.0, 0.0); double dt = 0.1; fuse_core::Vector3d position2; Eigen::Quaterniond orientation2; @@ -55,22 +55,11 @@ TEST(Predict, predictDirectVals) fuse_core::Vector3d vel_angular2; fuse_core::Vector3d acc_linear2; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); Eigen::Quaterniond q; - q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); EXPECT_DOUBLE_EQ(0.105, position2.x()); @@ -91,23 +80,12 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); // // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + fuse_models::predict(position2, orientation2, vel_linear2, vel_angular2, acc_linear2, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + + q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); - + EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); EXPECT_DOUBLE_EQ(0.0, position2.z()); @@ -130,21 +108,10 @@ TEST(Predict, predictDirectVals) vel_angular1.z() = -1.570796327; acc_linear1.y() = -1.0; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + + q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); EXPECT_DOUBLE_EQ(0.105, position2.x()); @@ -162,26 +129,15 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); // Out of plane motion - position1 = {0.0, 0.0, 0.0}; - orientation1 = {1.0, 0.0, 0.0, 0.0}; - vel_linear1 = {0.0, 0.0, 0.1}; - vel_angular1 = {1.570796327, 0.0, 0.0}; - acc_linear1 = {0.0, 0.0, 1.0}; + position1 = { 0.0, 0.0, 0.0 }; + orientation1 = { 1.0, 0.0, 0.0, 0.0 }; + vel_linear1 = { 0.0, 0.0, 0.1 }; + vel_angular1 = { 1.570796327, 0.0, 0.0 }; + acc_linear1 = { 0.0, 0.0, 1.0 }; dt = 0.1; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(0.0, position2.x()); EXPECT_DOUBLE_EQ(0.0, position2.y()); @@ -201,26 +157,15 @@ TEST(Predict, predictDirectVals) EXPECT_DOUBLE_EQ(1.0, acc_linear2.z()); // General 3D motion (these value are checked against rl predict() equations) - position1 = {0.0, 0.0, 0.0}; - orientation1 = {0.110, -0.003, -0.943, 0.314}; // RPY {-2.490, -0.206, 3.066} - vel_linear1 = {0.1, 0.2, 0.1}; - vel_angular1 = {1.570796327, 1.570796327, -1.570796327}; - acc_linear1 = {-0.5, 1.0, 1.0}; + position1 = { 0.0, 0.0, 0.0 }; + orientation1 = { 0.110, -0.003, -0.943, 0.314 }; // RPY {-2.490, -0.206, 3.066} + vel_linear1 = { 0.1, 0.2, 0.1 }; + vel_angular1 = { 1.570796327, 1.570796327, -1.570796327 }; + acc_linear1 = { -0.5, 1.0, 1.0 }; dt = 0.1; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(-0.012044123300410431, position2.x()); EXPECT_DOUBLE_EQ(0.011755776496514461, position2.y()); @@ -242,11 +187,11 @@ TEST(Predict, predictDirectVals) TEST(Predict, predictFromDoublePointers) { - double position1[3] {0.0, 0.0, 0.0}; - double orientation1[3] {0.0, 0.0, 0.0}; - double vel_linear1[3] {1.0, 0.0, 0.0}; - double vel_angular1[3] {0.0, 0.0, 1.570796327}; - double acc_linear1[3] {1.0, 0.0, 0.0}; + double position1[3]{ 0.0, 0.0, 0.0 }; + double orientation1[3]{ 0.0, 0.0, 0.0 }; + double vel_linear1[3]{ 1.0, 0.0, 0.0 }; + double vel_angular1[3]{ 0.0, 0.0, 1.570796327 }; + double acc_linear1[3]{ 1.0, 0.0, 0.0 }; double dt = 0.1; double position2[3]; double orientation2[3]; @@ -254,60 +199,40 @@ TEST(Predict, predictFromDoublePointers) double vel_angular2[3]; double acc_linear2[3]; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(0.105, position2[0]); - EXPECT_DOUBLE_EQ(0.0, position2[1]); - EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(0.1570796327, orientation2[2]); - EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); + fuse_models::predict(position2, orientation2, vel_linear2, vel_angular2, acc_linear2, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + + EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(0.3141592654, orientation2[2]); - EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); @@ -318,35 +243,25 @@ TEST(Predict, predictFromDoublePointers) vel_angular1[2] = -1.570796327; acc_linear1[1] = -1.0; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + EXPECT_DOUBLE_EQ(0.105, position2[0]); - EXPECT_DOUBLE_EQ(-0.105,position2[1]); - EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(-0.105, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); EXPECT_DOUBLE_EQ(0.0, orientation2[0]); EXPECT_DOUBLE_EQ(0.0, orientation2[1]); EXPECT_DOUBLE_EQ(-0.1570796327, orientation2[2]); - EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); - EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); - EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); - EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); - EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); EXPECT_DOUBLE_EQ(-1.0, acc_linear2[1]); EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); - + // Out of plane motion position1[0] = 0.0; position1[1] = 0.0; @@ -365,19 +280,8 @@ TEST(Predict, predictFromDoublePointers) acc_linear1[2] = 1.0; dt = 0.1; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(0.0, position2[0]); EXPECT_DOUBLE_EQ(0.0, position2[1]); @@ -413,26 +317,15 @@ TEST(Predict, predictFromDoublePointers) acc_linear1[2] = 1.0; dt = 0.1; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(-0.012031207341885572, position2[0]); - EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); + EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); EXPECT_DOUBLE_EQ(-0.024981300126995967, position2[2]); - EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); - EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); - EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); + EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); + EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); + EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); EXPECT_DOUBLE_EQ(0.05, vel_linear2[0]); EXPECT_DOUBLE_EQ(0.3, vel_linear2[1]); EXPECT_DOUBLE_EQ(0.2, vel_linear2[2]); @@ -448,11 +341,11 @@ TEST(Predict, predictFromJetPointers) { using Jet = ceres::Jet; - Jet position1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; - Jet orientation1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; - Jet vel_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; - Jet vel_angular1[3] = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; - Jet acc_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; + Jet position1[3] = { Jet(0.0), Jet(0.0), Jet(0.0) }; + Jet orientation1[3] = { Jet(0.0), Jet(0.0), Jet(0.0) }; + Jet vel_linear1[3] = { Jet(1.0), Jet(0.0), Jet(0.0) }; + Jet vel_angular1[3] = { Jet(0.0), Jet(0.0), Jet(1.570796327) }; + Jet acc_linear1[3] = { Jet(1.0), Jet(0.0), Jet(0.0) }; Jet dt = Jet(0.1); Jet position2[3]; Jet orientation2[3]; @@ -460,100 +353,70 @@ TEST(Predict, predictFromJetPointers) Jet vel_angular2[3]; Jet acc_linear2[3]; - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(0.1570796327).a, orientation2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); // Carry on with the output state from last time - show in-place update support - fuse_models::predict( - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - - EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); + fuse_models::predict(position2, orientation2, vel_linear2, vel_angular2, acc_linear2, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + + EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(0.3141592654).a, orientation2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); // // Use non-zero Y values - vel_linear1[1] = Jet(-1.0); + vel_linear1[1] = Jet(-1.0); vel_angular1[2] = Jet(-1.570796327); - acc_linear1[1] = Jet(-1.0); - - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2); - + acc_linear1[1] = Jet(-1.0); + + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(-0.105).a,position2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(-0.105).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); EXPECT_DOUBLE_EQ(Jet(-0.1570796327).a, orientation2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); - EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); - EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); - EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); -// Out of plane motion + // Out of plane motion position1[0] = Jet(0.0); position1[1] = Jet(0.0); position1[2] = Jet(0.0); @@ -571,19 +434,8 @@ TEST(Predict, predictFromJetPointers) acc_linear1[2] = Jet(1.0); dt = Jet(0.1); - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[0].a); EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); @@ -619,26 +471,15 @@ TEST(Predict, predictFromJetPointers) acc_linear1[2] = Jet(1.0); dt = Jet(0.1); - fuse_models::predict( - position1, - orientation1, - vel_linear1, - vel_angular1, - acc_linear1, - dt, - position2, - orientation2, - vel_linear2, - vel_angular2, - acc_linear2 - ); + fuse_models::predict(position1, orientation1, vel_linear1, vel_angular1, acc_linear1, dt, position2, orientation2, + vel_linear2, vel_angular2, acc_linear2); EXPECT_DOUBLE_EQ(Jet(-0.012031207341885572).a, position2[0].a); - EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); EXPECT_DOUBLE_EQ(Jet(-0.024981300126995967).a, position2[2].a); - EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); - EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); - EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); EXPECT_DOUBLE_EQ(Jet(0.05).a, vel_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(0.3).a, vel_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(0.2).a, vel_linear2[2].a); @@ -648,4 +489,4 @@ TEST(Predict, predictFromJetPointers) EXPECT_DOUBLE_EQ(Jet(-0.5).a, acc_linear2[0].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[1].a); EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); -} \ No newline at end of file +} diff --git a/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp index bea153f97..db0b93b63 100644 --- a/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp +++ b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp @@ -47,49 +47,46 @@ TEST(CostFunction, evaluateCostFunction) { // Create cost function - const double process_noise_diagonal[] = {1e-3, 1e-3, 1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, - 1e-3, 1e-3, 1e-3, 1e-3, 1e-3}; + const double process_noise_diagonal[] = { 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, + 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3 }; const fuse_core::Matrix15d covariance = fuse_core::Vector15d(process_noise_diagonal).asDiagonal(); - const double dt{0.1}; - const fuse_core::Matrix15d sqrt_information{covariance.inverse().llt().matrixU()}; + const double dt{ 0.1 }; + const fuse_core::Matrix15d sqrt_information{ covariance.inverse().llt().matrixU() }; - const fuse_models::Omnidirectional3DStateCostFunction cost_function{dt, sqrt_information}; + const fuse_models::Omnidirectional3DStateCostFunction cost_function{ dt, sqrt_information }; // Evaluate cost function - const double position1[3] = {0.0, 0.0, 0.0}; - const double orientation1[4] = {1.0, 0.0, 0.0, 0.0}; - const double vel_linear1[3] = {1.0, 1.0, 1.0}; - const double vel_angular1[3] = {1.570796327, 1.570796327, 1.570796327}; - const double acc_linear1[3] = {1.0, 1.0, 1.0}; + const double position1[3] = { 0.0, 0.0, 0.0 }; + const double orientation1[4] = { 1.0, 0.0, 0.0, 0.0 }; + const double vel_linear1[3] = { 1.0, 1.0, 1.0 }; + const double vel_angular1[3] = { 1.570796327, 1.570796327, 1.570796327 }; + const double acc_linear1[3] = { 1.0, 1.0, 1.0 }; - const double position2[3] = {0.105, 0.105, 0.105}; + const double position2[3] = { 0.105, 0.105, 0.105 }; Eigen::Quaterniond q2 = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitX()); - const double orientation2[4] = {q2.w(), q2.x(), q2.y(), q2.z()}; - const double vel_linear2[3] = {1.1, 1.1, 1.1}; - const double vel_angular2[3] = {1.570796327, 1.570796327, 1.570796327}; - const double acc_linear2[3] = {1.0, 1.0, 1.0}; + const double orientation2[4] = { q2.w(), q2.x(), q2.y(), q2.z() }; + const double vel_linear2[3] = { 1.1, 1.1, 1.1 }; + const double vel_angular2[3] = { 1.570796327, 1.570796327, 1.570796327 }; + const double acc_linear2[3] = { 1.0, 1.0, 1.0 }; - const double * parameters[10] = - { - position1, orientation1, vel_linear1, vel_angular1, acc_linear1, - position2, orientation2, vel_linear2, vel_angular2, acc_linear2 - }; + const double* parameters[10] = { position1, orientation1, vel_linear1, vel_angular1, acc_linear1, + position2, orientation2, vel_linear2, vel_angular2, acc_linear2 }; fuse_core::Vector15d residuals; - const auto & block_sizes = cost_function.parameter_block_sizes(); + const auto& block_sizes = cost_function.parameter_block_sizes(); const auto num_parameter_blocks = block_sizes.size(); const auto num_residuals = cost_function.num_residuals(); std::vector J(num_parameter_blocks); - std::vector jacobians(num_parameter_blocks); + std::vector jacobians(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J[i].resize(num_residuals, block_sizes[i]); jacobians[i] = J[i].data(); } @@ -100,11 +97,11 @@ TEST(CostFunction, evaluateCostFunction) // above the residuals are not zero for position2.x = -4.389e-16 and yaw2 = -2.776e-16 EXPECT_MATRIX_NEAR(fuse_core::Vector15d::Zero(), residuals, 1e-15); -// // Check jacobians are correct using a gradient checker + // // Check jacobians are correct using a gradient checker ceres::NumericDiffOptions numeric_diff_options; #if CERES_VERSION_AT_LEAST(2, 1, 0) - std::vector parameterizations; + std::vector parameterizations; ceres::GradientChecker gradient_checker(&cost_function, ¶meterizations, numeric_diff_options); #else ceres::GradientChecker gradient_checker(&cost_function, nullptr, numeric_diff_options); @@ -120,28 +117,27 @@ TEST(CostFunction, evaluateCostFunction) // probe_results.error_log; // Create cost function using automatic differentiation on the cost functor - ceres::AutoDiffCostFunctioncost_function_autodiff(new fuse_models::Omnidirectional3DStateCostFunctor(dt, sqrt_information)); + ceres::AutoDiffCostFunction + cost_function_autodiff(new fuse_models::Omnidirectional3DStateCostFunctor(dt, sqrt_information)); // Evaluate cost function that uses automatic differentiation std::vector J_autodiff(num_parameter_blocks); - std::vector jacobians_autodiff(num_parameter_blocks); + std::vector jacobians_autodiff(num_parameter_blocks); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { J_autodiff[i].resize(num_residuals, cost_function_autodiff.parameter_block_sizes()[i]); jacobians_autodiff[i] = J_autodiff[i].data(); } - EXPECT_TRUE( - cost_function_autodiff.Evaluate( - parameters, residuals.data(), - jacobians_autodiff.data())); + EXPECT_TRUE(cost_function_autodiff.Evaluate(parameters, residuals.data(), jacobians_autodiff.data())); - const Eigen::IOFormat HeavyFmt( - Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); + const Eigen::IOFormat HeavyFmt(Eigen::FullPrecision, 0, ", ", ";\n", "[", "]", "[", "]"); - for (size_t i = 0; i < num_parameter_blocks; ++i) { + for (size_t i = 0; i < num_parameter_blocks; ++i) + { EXPECT_MATRIX_NEAR(J_autodiff[i], J[i], 1e-4) - << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) - << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + << "Autodiff Jacobian[" << i << "] =\n" + << J_autodiff[i].format(HeavyFmt) << "\nAnalytic Jacobian[" << i << "] =\n" + << J[i].format(HeavyFmt); } } diff --git a/fuse_models/test/test_sensor_proc_3d.cpp b/fuse_models/test/test_sensor_proc_3d.cpp index c71d24bf6..5c1caaa88 100644 --- a/fuse_models/test/test_sensor_proc_3d.cpp +++ b/fuse_models/test/test_sensor_proc_3d.cpp @@ -13,54 +13,42 @@ namespace fm_common = fuse_models::common; TEST(TestSuite, mergeFullPositionAndFullOrientationIndices) { - const std::vector position_indices{0, 1, 2}; - const std::vector orientation_indices{0, 1, 2}; + const std::vector position_indices{ 0, 1, 2 }; + const std::vector orientation_indices{ 0, 1, 2 }; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); - EXPECT_THAT( - position_indices, - testing::ElementsAreArray( - merged_indices.begin(), - merged_indices.begin() + position_indices.size())); + EXPECT_THAT(position_indices, + testing::ElementsAreArray(merged_indices.begin(), merged_indices.begin() + position_indices.size())); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); } TEST(TestSuite, mergeXYPositionAndRollYawOrientationIndices) { - const std::vector position_indices{0, 1}; - const std::vector orientation_indices{0, 2}; + const std::vector position_indices{ 0, 1 }; + const std::vector orientation_indices{ 0, 2 }; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); - EXPECT_THAT( - position_indices, - testing::ElementsAreArray( - merged_indices.begin(), - merged_indices.begin() + position_indices.size())); + EXPECT_THAT(position_indices, + testing::ElementsAreArray(merged_indices.begin(), merged_indices.begin() + position_indices.size())); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); } TEST(TestSuite, mergeFullPositionAndEmptyOrientationIndices) { - const std::vector position_indices{0, 1, 2}; + const std::vector position_indices{ 0, 1, 2 }; const std::vector orientation_indices; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(position_indices.size(), merged_indices.size()); EXPECT_THAT(position_indices, testing::ElementsAreArray(merged_indices)); @@ -69,13 +57,11 @@ TEST(TestSuite, mergeFullPositionAndEmptyOrientationIndices) TEST(TestSuite, mergeEmptyPositionAndFullOrientationIndices) { const std::vector position_indices; - const std::vector orientation_indices{0, 1, 2}; + const std::vector orientation_indices{ 0, 1, 2 }; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_EQ(orientation_indices.size(), merged_indices.size()); EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); @@ -88,9 +74,7 @@ TEST(TestSuite, mergeEmptyPositionAndEmptyOrientationIndices) const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); EXPECT_TRUE(merged_indices.empty()); } @@ -100,44 +84,37 @@ TEST(TestSuite, populatePartialMeasurements) // Test both conversion from quaternion to RPY and partial measurement population // This one is just to generate a random unit quaternion and have the reference in RPY fuse_core::Vector3d rpy = fuse_core::Vector3d::Random(); - Eigen::Quaterniond q = - Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * - Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * - Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); - + Eigen::Quaterniond q = Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); + tf2::Transform tf2_pose; tf2_pose.setOrigin(tf2::Vector3(1.0, 2.0, 3.0)); tf2_pose.setRotation(tf2::Quaternion(q.x(), q.y(), q.z(), q.w())); fuse_core::Vector6d pose_mean_partial; pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), tf2_pose.getOrigin().z(); tf2::Matrix3x3(tf2_pose.getRotation()).getRPY(pose_mean_partial(3), pose_mean_partial(4), pose_mean_partial(5)); - + fuse_core::Matrix6d pose_covariance = fuse_core::Matrix6d::Random(); - const std::vector position_indices{0, 1}; - const std::vector orientation_indices{2}; + const std::vector position_indices{ 0, 1 }; + const std::vector orientation_indices{ 2 }; const size_t orientation_offset = 3; - const auto merged_indices = fm_common::mergeIndices( - position_indices, orientation_indices, - orientation_offset); - + const auto merged_indices = fm_common::mergeIndices(position_indices, orientation_indices, orientation_offset); + std::replace_if( - pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), - [&merged_indices, &pose_mean_partial](const double & value) { - return std::find( - merged_indices.begin(), - merged_indices.end(), - &value - pose_mean_partial.data()) == merged_indices.end(); - }, 0.0); + pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), + [&merged_indices, &pose_mean_partial](const double& value) { + return std::find(merged_indices.begin(), merged_indices.end(), &value - pose_mean_partial.data()) == + merged_indices.end(); + }, + 0.0); fuse_core::MatrixXd pose_covariance_partial(3, 3); - fm_common::populatePartialMeasurement( - pose_covariance, - merged_indices, - pose_covariance_partial); + fm_common::populatePartialMeasurement(pose_covariance, merged_indices, pose_covariance_partial); fuse_core::Vector6d expected_pose; expected_pose << 1.0, 2.0, 0.0, 0.0, 0.0, rpy(2); From 2b01c9086078711062fb9245d67f2d459cf57bfd Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 24 Sep 2024 18:40:42 +0000 Subject: [PATCH 06/46] do vcs in dockerfile instead --- .github/workflows/ci.yaml | 3 --- Dockerfile | 2 ++ 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 6730b0b11..03bdea8c8 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -24,9 +24,6 @@ jobs: - name: Checkout source uses: actions/checkout@v4 - - name: VCS import - run: vcs import src --input src/fuse/fuse.repos; rosdep install --ignore-src --from-paths src -y -r - - name: Setup Docker Buildx uses: docker/setup-buildx-action@v2 diff --git a/Dockerfile b/Dockerfile index d82059ae6..c815a1148 100644 --- a/Dockerfile +++ b/Dockerfile @@ -9,6 +9,7 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ apt-get update && apt-get upgrade -y && \ apt-get install -y --no-install-recommends \ clang-tidy \ + python3-vcstool \ # use cyclonedds instead of fastdds ros-rolling-rmw-cyclonedds-cpp @@ -22,6 +23,7 @@ RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ --mount=type=cache,target=/var/lib/apt,sharing=locked \ apt-get update && apt-get upgrade -y && \ . /opt/ros/rolling/setup.sh && \ + vcs import src --input src/fuse/fuse.repos && \ rosdep install --from-paths src -y --ignore-src && \ colcon build --mixin compile-commands coverage-gcc coverage-pytest From 098cb7eb5bcd4ec10710867ff394f8e957db7fc6 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 24 Sep 2024 18:46:48 +0000 Subject: [PATCH 07/46] let industrial ci find our repos --- .github/workflows/ros2.yml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml index 02f231083..b29898118 100644 --- a/.github/workflows/ros2.yml +++ b/.github/workflows/ros2.yml @@ -13,3 +13,5 @@ jobs: - uses: actions/checkout@v1 - uses: 'ros-industrial/industrial_ci@master' env: ${{matrix.env}} + with: + upstream_workspace: fuse.repos From d62f6508f0a3d946fb4a54970b8e63637b705585 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 24 Sep 2024 18:52:34 +0000 Subject: [PATCH 08/46] remove industrial CI --- .github/workflows/ros2.yml | 17 ----------------- 1 file changed, 17 deletions(-) delete mode 100644 .github/workflows/ros2.yml diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml deleted file mode 100644 index b29898118..000000000 --- a/.github/workflows/ros2.yml +++ /dev/null @@ -1,17 +0,0 @@ -name: ros2 - -on: [push, pull_request] - -jobs: - industrial_ci: - strategy: - matrix: - env: - - {ROS_DISTRO: rolling, ROS_REPO: main} - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v1 - - uses: 'ros-industrial/industrial_ci@master' - env: ${{matrix.env}} - with: - upstream_workspace: fuse.repos From a558ab1dd38d2a633cee4e4411398454090359c7 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 24 Sep 2024 19:56:18 +0000 Subject: [PATCH 09/46] remove deprecated .h headers --- .../fuse_constraints/absolute_constraint.h | 42 ------------------ ...solute_orientation_3d_stamped_constraint.h | 42 ------------------ ..._orientation_3d_stamped_euler_constraint.h | 42 ------------------ .../absolute_pose_2d_stamped_constraint.h | 43 ------------------- .../absolute_pose_3d_stamped_constraint.h | 43 ------------------- .../fuse_constraints/marginal_constraint.h | 42 ------------------ .../fuse_constraints/marginal_cost_function.h | 42 ------------------ .../fuse_constraints/marginalize_variables.h | 42 ------------------ .../include/fuse_constraints/normal_delta.h | 42 ------------------ .../normal_delta_orientation_2d.h | 42 ------------------ ...normal_delta_orientation_3d_cost_functor.h | 42 ------------------ .../fuse_constraints/normal_delta_pose_2d.h | 42 ------------------ .../normal_delta_pose_2d_cost_functor.h | 43 ------------------- .../normal_delta_pose_3d_cost_functor.h | 43 ------------------- .../normal_prior_orientation_2d.h | 42 ------------------ ...normal_prior_orientation_3d_cost_functor.h | 42 ------------------ ..._prior_orientation_3d_euler_cost_functor.h | 42 ------------------ .../fuse_constraints/normal_prior_pose_2d.h | 42 ------------------ .../normal_prior_pose_2d_cost_functor.h | 43 ------------------- .../normal_prior_pose_3d_cost_functor.h | 43 ------------------- .../fuse_constraints/relative_constraint.h | 42 ------------------ ...lative_orientation_3d_stamped_constraint.h | 42 ------------------ .../relative_pose_2d_stamped_constraint.h | 43 ------------------- .../relative_pose_3d_stamped_constraint.h | 43 ------------------- .../include/fuse_constraints/uuid_ordering.h | 42 ------------------ .../fuse_constraints/variable_constraints.h | 42 ------------------ fuse_constraints/test/cost_function_gtest.hpp | 2 + .../test/test_normal_prior_pose_3d.cpp | 3 -- .../include/fuse_core/async_motion_model.h | 42 ------------------ fuse_core/include/fuse_core/async_publisher.h | 42 ------------------ .../include/fuse_core/async_sensor_model.h | 42 ------------------ .../autodiff_local_parameterization.h | 42 ------------------ .../include/fuse_core/callback_wrapper.h | 42 ------------------ fuse_core/include/fuse_core/ceres_macros.h | 42 ------------------ fuse_core/include/fuse_core/ceres_options.h | 42 ------------------ fuse_core/include/fuse_core/console.h | 42 ------------------ fuse_core/include/fuse_core/constraint.h | 42 ------------------ fuse_core/include/fuse_core/eigen.h | 42 ------------------ fuse_core/include/fuse_core/eigen_gtest.h | 42 ------------------ fuse_core/include/fuse_core/fuse_macros.h | 42 ------------------ fuse_core/include/fuse_core/graph.h | 42 ------------------ .../include/fuse_core/graph_deserializer.h | 42 ------------------ .../fuse_core/local_parameterization.h | 42 ------------------ fuse_core/include/fuse_core/loss.h | 42 ------------------ fuse_core/include/fuse_core/loss_loader.h | 42 ------------------ fuse_core/include/fuse_core/macros.h | 42 ------------------ fuse_core/include/fuse_core/manifold.h | 41 ------------------ .../include/fuse_core/manifold_adapter.h | 42 ------------------ fuse_core/include/fuse_core/message_buffer.h | 42 ------------------ fuse_core/include/fuse_core/motion_model.h | 42 ------------------ fuse_core/include/fuse_core/parameter.h | 42 ------------------ fuse_core/include/fuse_core/publisher.h | 42 ------------------ fuse_core/include/fuse_core/sensor_model.h | 42 ------------------ fuse_core/include/fuse_core/serialization.h | 42 ------------------ .../include/fuse_core/throttled_callback.h | 42 ------------------ .../include/fuse_core/timestamp_manager.h | 42 ------------------ fuse_core/include/fuse_core/transaction.h | 42 ------------------ .../fuse_core/transaction_deserializer.h | 42 ------------------ fuse_core/include/fuse_core/util.h | 42 ------------------ fuse_core/include/fuse_core/uuid.h | 42 ------------------ fuse_core/include/fuse_core/variable.h | 42 ------------------ fuse_core/src/uuid.cpp | 1 - .../test/launch_tests/test_parameters.cpp | 1 - fuse_core/test/test_async_motion_model.cpp | 2 +- fuse_core/test/test_async_publisher.cpp | 5 +-- fuse_core/test/test_parameter.cpp | 1 - fuse_core/test/test_timestamp_manager.cpp | 1 - fuse_core/test/test_util.cpp | 4 +- fuse_graphs/include/fuse_graphs/hash_graph.h | 42 ------------------ .../include/fuse_graphs/hash_graph_params.h | 42 ------------------ fuse_loss/include/fuse_loss/arctan_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/cauchy_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/composed_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/dcs_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/fair_loss.h | 42 ------------------ .../include/fuse_loss/geman_mcclure_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/huber_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/loss_function.h | 42 ------------------ fuse_loss/include/fuse_loss/qwt_loss_plot.h | 42 ------------------ fuse_loss/include/fuse_loss/scaled_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/softlone_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/tolerant_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/trivial_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/tukey_loss.h | 42 ------------------ fuse_loss/include/fuse_loss/welsch_loss.h | 42 ------------------ .../include/fuse_models/acceleration_2d.h | 42 ------------------ .../fuse_models/common/sensor_config.h | 42 ------------------ .../include/fuse_models/common/sensor_proc.h | 42 ------------------ .../fuse_models/common/variable_traits.h | 42 ------------------ .../include/fuse_models/graph_ignition.h | 42 ------------------ fuse_models/include/fuse_models/imu_2d.h | 42 ------------------ fuse_models/include/fuse_models/odometry_2d.h | 42 ------------------ .../fuse_models/odometry_2d_publisher.h | 42 ------------------ .../parameters/acceleration_2d_params.h | 42 ------------------ .../parameters/graph_ignition_params.h | 42 ------------------ .../fuse_models/parameters/imu_2d_params.h | 42 ------------------ .../parameters/odometry_2d_params.h | 42 ------------------ .../parameters/odometry_2d_publisher_params.h | 43 ------------------- .../fuse_models/parameters/parameter_base.h | 42 ------------------ .../fuse_models/parameters/pose_2d_params.h | 42 ------------------ .../parameters/transaction_params.h | 42 ------------------ .../fuse_models/parameters/twist_2d_params.h | 42 ------------------ .../parameters/unicycle_2d_ignition_params.h | 43 ------------------- fuse_models/include/fuse_models/pose_2d.h | 42 ------------------ fuse_models/include/fuse_models/transaction.h | 42 ------------------ fuse_models/include/fuse_models/twist_2d.h | 42 ------------------ fuse_models/include/fuse_models/unicycle_2d.h | 42 ------------------ .../fuse_models/unicycle_2d_ignition.h | 42 ------------------ .../include/fuse_models/unicycle_2d_predict.h | 42 ------------------ .../unicycle_2d_state_cost_function.h | 42 ------------------ .../unicycle_2d_state_cost_functor.h | 42 ------------------ .../unicycle_2d_state_kinematic_constraint.h | 43 ------------------- .../include/fuse_optimizers/batch_optimizer.h | 42 ------------------ .../fuse_optimizers/batch_optimizer_params.h | 42 ------------------ .../fuse_optimizers/fixed_lag_smoother.h | 42 ------------------ .../fixed_lag_smoother_params.h | 42 ------------------ .../include/fuse_optimizers/optimizer.h | 42 ------------------ .../fuse_optimizers/variable_stamp_index.h | 42 ------------------ .../fuse_publishers/path_2d_publisher.h | 42 ------------------ .../fuse_publishers/pose_2d_publisher.h | 42 ------------------ .../fuse_publishers/serialized_publisher.h | 42 ------------------ .../stamped_variable_synchronizer.h | 42 ------------------ .../include/fuse_tutorials/beacon_publisher.h | 42 ------------------ .../include/fuse_tutorials/range_constraint.h | 42 ------------------ .../fuse_tutorials/range_cost_functor.h | 42 ------------------ .../fuse_tutorials/range_sensor_model.h | 42 ------------------ .../acceleration_angular_2d_stamped.h | 42 ------------------ .../acceleration_angular_3d_stamped.h | 42 ------------------ .../acceleration_linear_2d_stamped.h | 42 ------------------ .../acceleration_linear_3d_stamped.h | 42 ------------------ .../fuse_variables/fixed_size_variable.h | 42 ------------------ .../fuse_variables/orientation_2d_stamped.h | 42 ------------------ .../fuse_variables/orientation_3d_stamped.h | 42 ------------------ .../fuse_variables/point_2d_fixed_landmark.h | 42 ------------------ .../fuse_variables/point_2d_landmark.h | 42 ------------------ .../fuse_variables/point_3d_fixed_landmark.h | 42 ------------------ .../fuse_variables/point_3d_landmark.h | 42 ------------------ .../fuse_variables/position_2d_stamped.h | 42 ------------------ .../fuse_variables/position_3d_stamped.h | 42 ------------------ .../include/fuse_variables/stamped.h | 42 ------------------ .../velocity_angular_2d_stamped.h | 42 ------------------ .../velocity_angular_3d_stamped.h | 42 ------------------ .../velocity_linear_2d_stamped.h | 42 ------------------ .../velocity_linear_3d_stamped.h | 42 ------------------ fuse_viz/include/fuse_viz/conversions.h | 42 ------------------ .../fuse_viz/mapped_covariance_property.h | 42 ------------------ .../fuse_viz/mapped_covariance_visual.h | 42 ------------------ .../fuse_viz/pose_2d_stamped_property.h | 42 ------------------ .../include/fuse_viz/pose_2d_stamped_visual.h | 42 ------------------ ...tive_pose_2d_stamped_constraint_property.h | 42 ------------------ ...lative_pose_2d_stamped_constraint_visual.h | 43 ------------------- .../fuse_viz/serialized_graph_display.h | 42 ------------------ 152 files changed, 7 insertions(+), 6030 deletions(-) delete mode 100644 fuse_constraints/include/fuse_constraints/absolute_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/marginal_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/marginal_cost_function.h delete mode 100644 fuse_constraints/include/fuse_constraints/marginalize_variables.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_delta.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h delete mode 100644 fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h delete mode 100644 fuse_constraints/include/fuse_constraints/relative_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h delete mode 100644 fuse_constraints/include/fuse_constraints/uuid_ordering.h delete mode 100644 fuse_constraints/include/fuse_constraints/variable_constraints.h delete mode 100644 fuse_core/include/fuse_core/async_motion_model.h delete mode 100644 fuse_core/include/fuse_core/async_publisher.h delete mode 100644 fuse_core/include/fuse_core/async_sensor_model.h delete mode 100644 fuse_core/include/fuse_core/autodiff_local_parameterization.h delete mode 100644 fuse_core/include/fuse_core/callback_wrapper.h delete mode 100644 fuse_core/include/fuse_core/ceres_macros.h delete mode 100644 fuse_core/include/fuse_core/ceres_options.h delete mode 100644 fuse_core/include/fuse_core/console.h delete mode 100644 fuse_core/include/fuse_core/constraint.h delete mode 100644 fuse_core/include/fuse_core/eigen.h delete mode 100644 fuse_core/include/fuse_core/eigen_gtest.h delete mode 100644 fuse_core/include/fuse_core/fuse_macros.h delete mode 100644 fuse_core/include/fuse_core/graph.h delete mode 100644 fuse_core/include/fuse_core/graph_deserializer.h delete mode 100644 fuse_core/include/fuse_core/local_parameterization.h delete mode 100644 fuse_core/include/fuse_core/loss.h delete mode 100644 fuse_core/include/fuse_core/loss_loader.h delete mode 100644 fuse_core/include/fuse_core/macros.h delete mode 100644 fuse_core/include/fuse_core/manifold.h delete mode 100644 fuse_core/include/fuse_core/manifold_adapter.h delete mode 100644 fuse_core/include/fuse_core/message_buffer.h delete mode 100644 fuse_core/include/fuse_core/motion_model.h delete mode 100644 fuse_core/include/fuse_core/parameter.h delete mode 100644 fuse_core/include/fuse_core/publisher.h delete mode 100644 fuse_core/include/fuse_core/sensor_model.h delete mode 100644 fuse_core/include/fuse_core/serialization.h delete mode 100644 fuse_core/include/fuse_core/throttled_callback.h delete mode 100644 fuse_core/include/fuse_core/timestamp_manager.h delete mode 100644 fuse_core/include/fuse_core/transaction.h delete mode 100644 fuse_core/include/fuse_core/transaction_deserializer.h delete mode 100644 fuse_core/include/fuse_core/util.h delete mode 100644 fuse_core/include/fuse_core/uuid.h delete mode 100644 fuse_core/include/fuse_core/variable.h delete mode 100644 fuse_graphs/include/fuse_graphs/hash_graph.h delete mode 100644 fuse_graphs/include/fuse_graphs/hash_graph_params.h delete mode 100644 fuse_loss/include/fuse_loss/arctan_loss.h delete mode 100644 fuse_loss/include/fuse_loss/cauchy_loss.h delete mode 100644 fuse_loss/include/fuse_loss/composed_loss.h delete mode 100644 fuse_loss/include/fuse_loss/dcs_loss.h delete mode 100644 fuse_loss/include/fuse_loss/fair_loss.h delete mode 100644 fuse_loss/include/fuse_loss/geman_mcclure_loss.h delete mode 100644 fuse_loss/include/fuse_loss/huber_loss.h delete mode 100644 fuse_loss/include/fuse_loss/loss_function.h delete mode 100644 fuse_loss/include/fuse_loss/qwt_loss_plot.h delete mode 100644 fuse_loss/include/fuse_loss/scaled_loss.h delete mode 100644 fuse_loss/include/fuse_loss/softlone_loss.h delete mode 100644 fuse_loss/include/fuse_loss/tolerant_loss.h delete mode 100644 fuse_loss/include/fuse_loss/trivial_loss.h delete mode 100644 fuse_loss/include/fuse_loss/tukey_loss.h delete mode 100644 fuse_loss/include/fuse_loss/welsch_loss.h delete mode 100644 fuse_models/include/fuse_models/acceleration_2d.h delete mode 100644 fuse_models/include/fuse_models/common/sensor_config.h delete mode 100644 fuse_models/include/fuse_models/common/sensor_proc.h delete mode 100644 fuse_models/include/fuse_models/common/variable_traits.h delete mode 100644 fuse_models/include/fuse_models/graph_ignition.h delete mode 100644 fuse_models/include/fuse_models/imu_2d.h delete mode 100644 fuse_models/include/fuse_models/odometry_2d.h delete mode 100644 fuse_models/include/fuse_models/odometry_2d_publisher.h delete mode 100644 fuse_models/include/fuse_models/parameters/acceleration_2d_params.h delete mode 100644 fuse_models/include/fuse_models/parameters/graph_ignition_params.h delete mode 100644 fuse_models/include/fuse_models/parameters/imu_2d_params.h delete mode 100644 fuse_models/include/fuse_models/parameters/odometry_2d_params.h delete mode 100644 fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h delete mode 100644 fuse_models/include/fuse_models/parameters/parameter_base.h delete mode 100644 fuse_models/include/fuse_models/parameters/pose_2d_params.h delete mode 100644 fuse_models/include/fuse_models/parameters/transaction_params.h delete mode 100644 fuse_models/include/fuse_models/parameters/twist_2d_params.h delete mode 100644 fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h delete mode 100644 fuse_models/include/fuse_models/pose_2d.h delete mode 100644 fuse_models/include/fuse_models/transaction.h delete mode 100644 fuse_models/include/fuse_models/twist_2d.h delete mode 100644 fuse_models/include/fuse_models/unicycle_2d.h delete mode 100644 fuse_models/include/fuse_models/unicycle_2d_ignition.h delete mode 100644 fuse_models/include/fuse_models/unicycle_2d_predict.h delete mode 100644 fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h delete mode 100644 fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h delete mode 100644 fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h delete mode 100644 fuse_optimizers/include/fuse_optimizers/batch_optimizer.h delete mode 100644 fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h delete mode 100644 fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h delete mode 100644 fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h delete mode 100644 fuse_optimizers/include/fuse_optimizers/optimizer.h delete mode 100644 fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h delete mode 100644 fuse_publishers/include/fuse_publishers/path_2d_publisher.h delete mode 100644 fuse_publishers/include/fuse_publishers/pose_2d_publisher.h delete mode 100644 fuse_publishers/include/fuse_publishers/serialized_publisher.h delete mode 100644 fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h delete mode 100644 fuse_tutorials/include/fuse_tutorials/beacon_publisher.h delete mode 100644 fuse_tutorials/include/fuse_tutorials/range_constraint.h delete mode 100644 fuse_tutorials/include/fuse_tutorials/range_cost_functor.h delete mode 100644 fuse_tutorials/include/fuse_tutorials/range_sensor_model.h delete mode 100644 fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/fixed_size_variable.h delete mode 100644 fuse_variables/include/fuse_variables/orientation_2d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/orientation_3d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h delete mode 100644 fuse_variables/include/fuse_variables/point_2d_landmark.h delete mode 100644 fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h delete mode 100644 fuse_variables/include/fuse_variables/point_3d_landmark.h delete mode 100644 fuse_variables/include/fuse_variables/position_2d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/position_3d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/stamped.h delete mode 100644 fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h delete mode 100644 fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h delete mode 100644 fuse_viz/include/fuse_viz/conversions.h delete mode 100644 fuse_viz/include/fuse_viz/mapped_covariance_property.h delete mode 100644 fuse_viz/include/fuse_viz/mapped_covariance_visual.h delete mode 100644 fuse_viz/include/fuse_viz/pose_2d_stamped_property.h delete mode 100644 fuse_viz/include/fuse_viz/pose_2d_stamped_visual.h delete mode 100644 fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h delete mode 100644 fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h delete mode 100644 fuse_viz/include/fuse_viz/serialized_graph_display.h diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_constraint.h deleted file mode 100644 index bade588a5..000000000 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/absolute_constraint.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h deleted file mode 100644 index 92eb503a2..000000000 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_constraint.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/absolute_orientation_3d_stamped_constraint.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h deleted file mode 100644 index 0511e627f..000000000 --- a/fuse_constraints/include/fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_EULER_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_EULER_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/absolute_orientation_3d_stamped_euler_constraint.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__ABSOLUTE_ORIENTATION_3D_STAMPED_EULER_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h deleted file mode 100644 index 5be4cba8e..000000000 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_2d_stamped_constraint.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_2D_STAMPED_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__ABSOLUTE_POSE_2D_STAMPED_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/absolute_pose_2d_stamped_constraint.hpp \ - instead - -#include - -#endif // FUSE_CONSTRAINTS__ABSOLUTE_POSE_2D_STAMPED_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h deleted file mode 100644 index 2f501a6f7..000000000 --- a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_constraint.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/absolute_pose_3d_stamped_constraint.hpp \ - instead - -#include - -#endif // FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/marginal_constraint.h b/fuse_constraints/include/fuse_constraints/marginal_constraint.h deleted file mode 100644 index 129192d0c..000000000 --- a/fuse_constraints/include/fuse_constraints/marginal_constraint.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__MARGINAL_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__MARGINAL_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/marginal_constraint.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__MARGINAL_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/marginal_cost_function.h b/fuse_constraints/include/fuse_constraints/marginal_cost_function.h deleted file mode 100644 index 130427af8..000000000 --- a/fuse_constraints/include/fuse_constraints/marginal_cost_function.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__MARGINAL_COST_FUNCTION_H_ -#define FUSE_CONSTRAINTS__MARGINAL_COST_FUNCTION_H_ - -#warning This header is obsolete, please include fuse_constraints/marginal_cost_function.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__MARGINAL_COST_FUNCTION_H_ diff --git a/fuse_constraints/include/fuse_constraints/marginalize_variables.h b/fuse_constraints/include/fuse_constraints/marginalize_variables.h deleted file mode 100644 index ad31cc8a3..000000000 --- a/fuse_constraints/include/fuse_constraints/marginalize_variables.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__MARGINALIZE_VARIABLES_H_ -#define FUSE_CONSTRAINTS__MARGINALIZE_VARIABLES_H_ - -#warning This header is obsolete, please include fuse_constraints/marginalize_variables.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__MARGINALIZE_VARIABLES_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta.h b/fuse_constraints/include/fuse_constraints/normal_delta.h deleted file mode 100644 index d72e1134f..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_delta.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_H_ -#define FUSE_CONSTRAINTS__NORMAL_DELTA_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_delta.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h deleted file mode 100644 index b443ece34..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_2D_H_ -#define FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_2D_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_delta_orientation_2d.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_2D_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h deleted file mode 100644 index ae0a55424..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_cost_functor.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_COST_FUNCTOR_H_ -#define FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_COST_FUNCTOR_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_delta_orientation_3d_cost_functor.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_ORIENTATION_3D_COST_FUNCTOR_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.h b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.h deleted file mode 100644 index 7dc2ddca6..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_2D_H_ -#define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_2D_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_delta_pose_2d.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_2D_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.h deleted file mode 100644 index 3b4f6a096..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_2d_cost_functor.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_2D_COST_FUNCTOR_H_ -#define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_2D_COST_FUNCTOR_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_delta_pose_2d_cost_functor.hpp \ - instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_2D_COST_FUNCTOR_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h deleted file mode 100644 index cad6467d2..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_cost_functor.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H_ -#define FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_delta_pose_3d_cost_functor.hpp \ - instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_DELTA_POSE_3D_COST_FUNCTOR_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h deleted file mode 100644 index d06011234..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_2D_H_ -#define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_2D_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_prior_orientation_2d.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_2D_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h deleted file mode 100644 index 6ac0a9810..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_cost_functor.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_COST_FUNCTOR_H_ -#define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_COST_FUNCTOR_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_prior_orientation_3d_cost_functor.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_COST_FUNCTOR_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h deleted file mode 100644 index 5abe413fe..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_EULER_COST_FUNCTOR_H_ -#define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_EULER_COST_FUNCTOR_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_EULER_COST_FUNCTOR_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.h b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.h deleted file mode 100644 index f1097cb45..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_H_ -#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_prior_pose_2d.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h deleted file mode 100644 index 83fd97112..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_2d_cost_functor.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_COST_FUNCTOR_H_ -#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_COST_FUNCTOR_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_prior_pose_2d_cost_functor.hpp \ - instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_2D_COST_FUNCTOR_H_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h deleted file mode 100644 index a302c592b..000000000 --- a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_cost_functor.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_H_ -#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_H_ - -#warning This header is obsolete, please include fuse_constraints/normal_prior_pose_3d_cost_functor.hpp \ - instead - -#include - -#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_H_ diff --git a/fuse_constraints/include/fuse_constraints/relative_constraint.h b/fuse_constraints/include/fuse_constraints/relative_constraint.h deleted file mode 100644 index ba329a6bb..000000000 --- a/fuse_constraints/include/fuse_constraints/relative_constraint.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__RELATIVE_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__RELATIVE_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/relative_constraint.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__RELATIVE_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h deleted file mode 100644 index 58e3d3e9b..000000000 --- a/fuse_constraints/include/fuse_constraints/relative_orientation_3d_stamped_constraint.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__RELATIVE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__RELATIVE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/relative_orientation_3d_stamped_constraint.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__RELATIVE_ORIENTATION_3D_STAMPED_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h deleted file mode 100644 index 267ed2a15..000000000 --- a/fuse_constraints/include/fuse_constraints/relative_pose_2d_stamped_constraint.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/relative_pose_2d_stamped_constraint.hpp \ - instead - -#include - -#endif // FUSE_CONSTRAINTS__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h deleted file mode 100644 index a2dee13d6..000000000 --- a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_constraint.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H_ -#define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_constraints/relative_pose_3d_stamped_constraint.hpp \ - instead - -#include - -#endif // FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_CONSTRAINT_H_ diff --git a/fuse_constraints/include/fuse_constraints/uuid_ordering.h b/fuse_constraints/include/fuse_constraints/uuid_ordering.h deleted file mode 100644 index 1310b9301..000000000 --- a/fuse_constraints/include/fuse_constraints/uuid_ordering.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__UUID_ORDERING_H_ -#define FUSE_CONSTRAINTS__UUID_ORDERING_H_ - -#warning This header is obsolete, please include fuse_constraints/uuid_ordering.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__UUID_ORDERING_H_ diff --git a/fuse_constraints/include/fuse_constraints/variable_constraints.h b/fuse_constraints/include/fuse_constraints/variable_constraints.h deleted file mode 100644 index d7c8cd7b1..000000000 --- a/fuse_constraints/include/fuse_constraints/variable_constraints.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CONSTRAINTS__VARIABLE_CONSTRAINTS_H_ -#define FUSE_CONSTRAINTS__VARIABLE_CONSTRAINTS_H_ - -#warning This header is obsolete, please include fuse_constraints/variable_constraints.hpp instead - -#include - -#endif // FUSE_CONSTRAINTS__VARIABLE_CONSTRAINTS_H_ diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index d207b4a6f..9b1c9da16 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -38,7 +38,9 @@ #include #include +#include #include +#include /** * @brief A helper function to compare a expected and actual cost function. diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp index 43f17f94c..57a4d18d0 100644 --- a/fuse_constraints/test/test_normal_prior_pose_3d.cpp +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -35,9 +35,6 @@ #include #include -#include -#include - #include "cost_function_gtest.hpp" #include #include diff --git a/fuse_core/include/fuse_core/async_motion_model.h b/fuse_core/include/fuse_core/async_motion_model.h deleted file mode 100644 index 17ff955d4..000000000 --- a/fuse_core/include/fuse_core/async_motion_model.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__ASYNC_MOTION_MODEL_H_ -#define FUSE_CORE__ASYNC_MOTION_MODEL_H_ - -#warning This header is obsolete, please include fuse_core/async_motion_model.hpp instead - -#include - -#endif // FUSE_CORE__ASYNC_MOTION_MODEL_H_ diff --git a/fuse_core/include/fuse_core/async_publisher.h b/fuse_core/include/fuse_core/async_publisher.h deleted file mode 100644 index 4f3719ba3..000000000 --- a/fuse_core/include/fuse_core/async_publisher.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__ASYNC_PUBLISHER_H_ -#define FUSE_CORE__ASYNC_PUBLISHER_H_ - -#warning This header is obsolete, please include fuse_core/async_publisher.hpp instead - -#include - -#endif // FUSE_CORE__ASYNC_PUBLISHER_H_ diff --git a/fuse_core/include/fuse_core/async_sensor_model.h b/fuse_core/include/fuse_core/async_sensor_model.h deleted file mode 100644 index e7861ec4d..000000000 --- a/fuse_core/include/fuse_core/async_sensor_model.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__ASYNC_SENSOR_MODEL_H_ -#define FUSE_CORE__ASYNC_SENSOR_MODEL_H_ - -#warning This header is obsolete, please include fuse_core/async_sensor_model.hpp instead - -#include - -#endif // FUSE_CORE__ASYNC_SENSOR_MODEL_H_ diff --git a/fuse_core/include/fuse_core/autodiff_local_parameterization.h b/fuse_core/include/fuse_core/autodiff_local_parameterization.h deleted file mode 100644 index f3135b8c2..000000000 --- a/fuse_core/include/fuse_core/autodiff_local_parameterization.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__AUTODIFF_LOCAL_PARAMETERIZATION_H_ -#define FUSE_CORE__AUTODIFF_LOCAL_PARAMETERIZATION_H_ - -#warning This header is obsolete, please include fuse_core/autodiff_local_parameterization.hpp instead - -#include - -#endif // FUSE_CORE__AUTODIFF_LOCAL_PARAMETERIZATION_H_ diff --git a/fuse_core/include/fuse_core/callback_wrapper.h b/fuse_core/include/fuse_core/callback_wrapper.h deleted file mode 100644 index b2585f4f6..000000000 --- a/fuse_core/include/fuse_core/callback_wrapper.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__CALLBACK_WRAPPER_H_ -#define FUSE_CORE__CALLBACK_WRAPPER_H_ - -#warning This header is obsolete, please include fuse_core/callback_wrapper.hpp instead - -#include - -#endif // FUSE_CORE__CALLBACK_WRAPPER_H_ diff --git a/fuse_core/include/fuse_core/ceres_macros.h b/fuse_core/include/fuse_core/ceres_macros.h deleted file mode 100644 index 318ddacbd..000000000 --- a/fuse_core/include/fuse_core/ceres_macros.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__CERES_MACROS_H_ -#define FUSE_CORE__CERES_MACROS_H_ - -#warning This header is obsolete, please include fuse_core/ceres_macros.hpp instead - -#include - -#endif // FUSE_CORE__CERES_MACROS_H_ diff --git a/fuse_core/include/fuse_core/ceres_options.h b/fuse_core/include/fuse_core/ceres_options.h deleted file mode 100644 index ea467a913..000000000 --- a/fuse_core/include/fuse_core/ceres_options.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__CERES_OPTIONS_H_ -#define FUSE_CORE__CERES_OPTIONS_H_ - -#warning This header is obsolete, please include fuse_core/ceres_options.hpp instead - -#include - -#endif // FUSE_CORE__CERES_OPTIONS_H_ diff --git a/fuse_core/include/fuse_core/console.h b/fuse_core/include/fuse_core/console.h deleted file mode 100644 index e9569f056..000000000 --- a/fuse_core/include/fuse_core/console.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__CONSOLE_H_ -#define FUSE_CORE__CONSOLE_H_ - -#warning This header is obsolete, please include fuse_core/console.hpp instead - -#include - -#endif // FUSE_CORE__CONSOLE_H_ diff --git a/fuse_core/include/fuse_core/constraint.h b/fuse_core/include/fuse_core/constraint.h deleted file mode 100644 index f58ab9724..000000000 --- a/fuse_core/include/fuse_core/constraint.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__CONSTRAINT_H_ -#define FUSE_CORE__CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_core/constraint.hpp instead - -#include - -#endif // FUSE_CORE__CONSTRAINT_H_ diff --git a/fuse_core/include/fuse_core/eigen.h b/fuse_core/include/fuse_core/eigen.h deleted file mode 100644 index 65ce7ecb2..000000000 --- a/fuse_core/include/fuse_core/eigen.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__EIGEN_H_ -#define FUSE_CORE__EIGEN_H_ - -#warning This header is obsolete, please include fuse_core/eigen.hpp instead - -#include - -#endif // FUSE_CORE__EIGEN_H_ diff --git a/fuse_core/include/fuse_core/eigen_gtest.h b/fuse_core/include/fuse_core/eigen_gtest.h deleted file mode 100644 index 99915b601..000000000 --- a/fuse_core/include/fuse_core/eigen_gtest.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__EIGEN_GTEST_H_ -#define FUSE_CORE__EIGEN_GTEST_H_ - -#warning This header is obsolete, please include fuse_core/eigen_gtest.hpp instead - -#include - -#endif // FUSE_CORE__EIGEN_GTEST_H_ diff --git a/fuse_core/include/fuse_core/fuse_macros.h b/fuse_core/include/fuse_core/fuse_macros.h deleted file mode 100644 index b3bd27fcd..000000000 --- a/fuse_core/include/fuse_core/fuse_macros.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__FUSE_MACROS_H_ -#define FUSE_CORE__FUSE_MACROS_H_ - -#warning This header is obsolete, please include fuse_core/fuse_macros.hpp instead - -#include - -#endif // FUSE_CORE__FUSE_MACROS_H_ diff --git a/fuse_core/include/fuse_core/graph.h b/fuse_core/include/fuse_core/graph.h deleted file mode 100644 index 0f2ee08ab..000000000 --- a/fuse_core/include/fuse_core/graph.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__GRAPH_H_ -#define FUSE_CORE__GRAPH_H_ - -#warning This header is obsolete, please include fuse_core/graph.hpp instead - -#include - -#endif // FUSE_CORE__GRAPH_H_ diff --git a/fuse_core/include/fuse_core/graph_deserializer.h b/fuse_core/include/fuse_core/graph_deserializer.h deleted file mode 100644 index baad77d4b..000000000 --- a/fuse_core/include/fuse_core/graph_deserializer.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__GRAPH_DESERIALIZER_H_ -#define FUSE_CORE__GRAPH_DESERIALIZER_H_ - -#warning This header is obsolete, please include fuse_core/graph_deserializer.hpp instead - -#include - -#endif // FUSE_CORE__GRAPH_DESERIALIZER_H_ diff --git a/fuse_core/include/fuse_core/local_parameterization.h b/fuse_core/include/fuse_core/local_parameterization.h deleted file mode 100644 index 9e2b2d41c..000000000 --- a/fuse_core/include/fuse_core/local_parameterization.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__LOCAL_PARAMETERIZATION_H_ -#define FUSE_CORE__LOCAL_PARAMETERIZATION_H_ - -#warning This header is obsolete, please include fuse_core/local_parameterization.hpp instead - -#include - -#endif // FUSE_CORE__LOCAL_PARAMETERIZATION_H_ diff --git a/fuse_core/include/fuse_core/loss.h b/fuse_core/include/fuse_core/loss.h deleted file mode 100644 index 2d2b80224..000000000 --- a/fuse_core/include/fuse_core/loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__LOSS_H_ -#define FUSE_CORE__LOSS_H_ - -#warning This header is obsolete, please include fuse_core/loss.hpp instead - -#include - -#endif // FUSE_CORE__LOSS_H_ diff --git a/fuse_core/include/fuse_core/loss_loader.h b/fuse_core/include/fuse_core/loss_loader.h deleted file mode 100644 index 0e23a38d9..000000000 --- a/fuse_core/include/fuse_core/loss_loader.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__LOSS_LOADER_H_ -#define FUSE_CORE__LOSS_LOADER_H_ - -#warning This header is obsolete, please include fuse_core/loss_loader.hpp instead - -#include - -#endif // FUSE_CORE__LOSS_LOADER_H_ diff --git a/fuse_core/include/fuse_core/macros.h b/fuse_core/include/fuse_core/macros.h deleted file mode 100644 index 6e9faca23..000000000 --- a/fuse_core/include/fuse_core/macros.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__MACROS_H_ -#define FUSE_CORE__MACROS_H_ - -#warning This header is obsolete, please include fuse_core/fuse_macros.hpp instead - -#include - -#endif // FUSE_CORE__MACROS_H_ diff --git a/fuse_core/include/fuse_core/manifold.h b/fuse_core/include/fuse_core/manifold.h deleted file mode 100644 index be76e2b9c..000000000 --- a/fuse_core/include/fuse_core/manifold.h +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2024, Clearpath Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ -#ifndef FUSE_CORE__MANIFOLD_H_ -#define FUSE_CORE__MANIFOLD_H_ - -#warning This header is obsolete, please include fuse_core/manifold.hpp instead - -#include - -#endif // FUSE_CORE__MANIFOLD_H_ diff --git a/fuse_core/include/fuse_core/manifold_adapter.h b/fuse_core/include/fuse_core/manifold_adapter.h deleted file mode 100644 index e6055bca7..000000000 --- a/fuse_core/include/fuse_core/manifold_adapter.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__MANIFOLD_ADAPTER_H_ -#define FUSE_CORE__MANIFOLD_ADAPTER_H_ - -#warning This header is obsolete, please include fuse_core/manifold_adapter.hpp instead - -#include - -#endif // FUSE_CORE__MANIFOLD_ADAPTER_H_ diff --git a/fuse_core/include/fuse_core/message_buffer.h b/fuse_core/include/fuse_core/message_buffer.h deleted file mode 100644 index 85c2a6fd8..000000000 --- a/fuse_core/include/fuse_core/message_buffer.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__MESSAGE_BUFFER_H_ -#define FUSE_CORE__MESSAGE_BUFFER_H_ - -#warning This header is obsolete, please include fuse_core/message_buffer.hpp instead - -#include - -#endif // FUSE_CORE__MESSAGE_BUFFER_H_ diff --git a/fuse_core/include/fuse_core/motion_model.h b/fuse_core/include/fuse_core/motion_model.h deleted file mode 100644 index 44e3126e6..000000000 --- a/fuse_core/include/fuse_core/motion_model.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__MOTION_MODEL_H_ -#define FUSE_CORE__MOTION_MODEL_H_ - -#warning This header is obsolete, please include fuse_core/motion_model.hpp instead - -#include - -#endif // FUSE_CORE__MOTION_MODEL_H_ diff --git a/fuse_core/include/fuse_core/parameter.h b/fuse_core/include/fuse_core/parameter.h deleted file mode 100644 index 99deafceb..000000000 --- a/fuse_core/include/fuse_core/parameter.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__PARAMETER_H_ -#define FUSE_CORE__PARAMETER_H_ - -#warning This header is obsolete, please include fuse_core/parameter.hpp instead - -#include - -#endif // FUSE_CORE__PARAMETER_H_ diff --git a/fuse_core/include/fuse_core/publisher.h b/fuse_core/include/fuse_core/publisher.h deleted file mode 100644 index 956011974..000000000 --- a/fuse_core/include/fuse_core/publisher.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__PUBLISHER_H_ -#define FUSE_CORE__PUBLISHER_H_ - -#warning This header is obsolete, please include fuse_core/publisher.hpp instead - -#include - -#endif // FUSE_CORE__PUBLISHER_H_ diff --git a/fuse_core/include/fuse_core/sensor_model.h b/fuse_core/include/fuse_core/sensor_model.h deleted file mode 100644 index 0994242c1..000000000 --- a/fuse_core/include/fuse_core/sensor_model.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__SENSOR_MODEL_H_ -#define FUSE_CORE__SENSOR_MODEL_H_ - -#warning This header is obsolete, please include fuse_core/sensor_model.hpp instead - -#include - -#endif // FUSE_CORE__SENSOR_MODEL_H_ diff --git a/fuse_core/include/fuse_core/serialization.h b/fuse_core/include/fuse_core/serialization.h deleted file mode 100644 index 7350d5b2f..000000000 --- a/fuse_core/include/fuse_core/serialization.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__SERIALIZATION_H_ -#define FUSE_CORE__SERIALIZATION_H_ - -#warning This header is obsolete, please include fuse_core/serialization.hpp instead - -#include - -#endif // FUSE_CORE__SERIALIZATION_H_ diff --git a/fuse_core/include/fuse_core/throttled_callback.h b/fuse_core/include/fuse_core/throttled_callback.h deleted file mode 100644 index 4bef1cccd..000000000 --- a/fuse_core/include/fuse_core/throttled_callback.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__THROTTLED_CALLBACK_H_ -#define FUSE_CORE__THROTTLED_CALLBACK_H_ - -#warning This header is obsolete, please include fuse_core/throttled_callback.hpp instead - -#include - -#endif // FUSE_CORE__THROTTLED_CALLBACK_H_ diff --git a/fuse_core/include/fuse_core/timestamp_manager.h b/fuse_core/include/fuse_core/timestamp_manager.h deleted file mode 100644 index 5a3c4b5f6..000000000 --- a/fuse_core/include/fuse_core/timestamp_manager.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__TIMESTAMP_MANAGER_H_ -#define FUSE_CORE__TIMESTAMP_MANAGER_H_ - -#warning This header is obsolete, please include fuse_core/timestamp_manager.hpp instead - -#include - -#endif // FUSE_CORE__TIMESTAMP_MANAGER_H_ diff --git a/fuse_core/include/fuse_core/transaction.h b/fuse_core/include/fuse_core/transaction.h deleted file mode 100644 index df6f8ed35..000000000 --- a/fuse_core/include/fuse_core/transaction.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__TRANSACTION_H_ -#define FUSE_CORE__TRANSACTION_H_ - -#warning This header is obsolete, please include fuse_core/transaction.hpp instead - -#include - -#endif // FUSE_CORE__TRANSACTION_H_ diff --git a/fuse_core/include/fuse_core/transaction_deserializer.h b/fuse_core/include/fuse_core/transaction_deserializer.h deleted file mode 100644 index 3eb002ef1..000000000 --- a/fuse_core/include/fuse_core/transaction_deserializer.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__TRANSACTION_DESERIALIZER_H_ -#define FUSE_CORE__TRANSACTION_DESERIALIZER_H_ - -#warning This header is obsolete, please include fuse_core/transaction_deserializer.hpp instead - -#include - -#endif // FUSE_CORE__TRANSACTION_DESERIALIZER_H_ diff --git a/fuse_core/include/fuse_core/util.h b/fuse_core/include/fuse_core/util.h deleted file mode 100644 index 2daf0d29c..000000000 --- a/fuse_core/include/fuse_core/util.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__UTIL_H_ -#define FUSE_CORE__UTIL_H_ - -#warning This header is obsolete, please include fuse_core/util.hpp instead - -#include - -#endif // FUSE_CORE__UTIL_H_ diff --git a/fuse_core/include/fuse_core/uuid.h b/fuse_core/include/fuse_core/uuid.h deleted file mode 100644 index 7e9f77282..000000000 --- a/fuse_core/include/fuse_core/uuid.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__UUID_H_ -#define FUSE_CORE__UUID_H_ - -#warning This header is obsolete, please include fuse_core/uuid.hpp instead - -#include - -#endif // FUSE_CORE__UUID_H_ diff --git a/fuse_core/include/fuse_core/variable.h b/fuse_core/include/fuse_core/variable.h deleted file mode 100644 index b13fc9bc1..000000000 --- a/fuse_core/include/fuse_core/variable.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_CORE__VARIABLE_H_ -#define FUSE_CORE__VARIABLE_H_ - -#warning This header is obsolete, please include fuse_core/variable.hpp instead - -#include - -#endif // FUSE_CORE__VARIABLE_H_ diff --git a/fuse_core/src/uuid.cpp b/fuse_core/src/uuid.cpp index fe139576a..b06fb0908 100644 --- a/fuse_core/src/uuid.cpp +++ b/fuse_core/src/uuid.cpp @@ -34,7 +34,6 @@ #include #include #include -#include #include #include diff --git a/fuse_core/test/launch_tests/test_parameters.cpp b/fuse_core/test/launch_tests/test_parameters.cpp index 17197b019..5cfc6b8c4 100644 --- a/fuse_core/test/launch_tests/test_parameters.cpp +++ b/fuse_core/test/launch_tests/test_parameters.cpp @@ -33,7 +33,6 @@ */ #include -#include #include #include diff --git a/fuse_core/test/test_async_motion_model.cpp b/fuse_core/test/test_async_motion_model.cpp index 3fb601983..ad508782c 100644 --- a/fuse_core/test/test_async_motion_model.cpp +++ b/fuse_core/test/test_async_motion_model.cpp @@ -48,7 +48,7 @@ class MyMotionModel : public fuse_core::AsyncMotionModel virtual ~MyMotionModel() = default; - bool applyCallback(fuse_core::Transaction& /*transaction*/) + bool applyCallback(fuse_core::Transaction& /*transaction*/) override { rclcpp::sleep_for(std::chrono::milliseconds(1000)); transaction_received = true; diff --git a/fuse_core/test/test_async_publisher.cpp b/fuse_core/test/test_async_publisher.cpp index fb3cf3e20..5d222a158 100644 --- a/fuse_core/test/test_async_publisher.cpp +++ b/fuse_core/test/test_async_publisher.cpp @@ -33,8 +33,6 @@ */ #include -#include - #include #include @@ -50,7 +48,8 @@ class MyPublisher : public fuse_core::AsyncPublisher virtual ~MyPublisher() = default; - void notifyCallback(fuse_core::Transaction::ConstSharedPtr /*transaction*/, fuse_core::Graph::ConstSharedPtr /*graph*/) + void notifyCallback(fuse_core::Transaction::ConstSharedPtr /*transaction*/, + fuse_core::Graph::ConstSharedPtr /*graph*/) override { rclcpp::sleep_for(std::chrono::milliseconds(10)); callback_processed = true; diff --git a/fuse_core/test/test_parameter.cpp b/fuse_core/test/test_parameter.cpp index 3013c89fe..d27d15c76 100644 --- a/fuse_core/test/test_parameter.cpp +++ b/fuse_core/test/test_parameter.cpp @@ -33,7 +33,6 @@ */ #include -#include #include #include diff --git a/fuse_core/test/test_timestamp_manager.cpp b/fuse_core/test/test_timestamp_manager.cpp index b5a7a8530..2759721d5 100644 --- a/fuse_core/test/test_timestamp_manager.cpp +++ b/fuse_core/test/test_timestamp_manager.cpp @@ -34,7 +34,6 @@ #include #include -#include #include #include diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index eeab5dbcd..095ab2bf1 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -33,7 +33,6 @@ */ #include -#include #include #include @@ -186,7 +185,8 @@ TEST(Util, quaternionProduct) // Atm only the jacobian wrt the second quaternion is implemented. If the computation will be // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. - double J_analytic_q1[16], J_analytic_q2[16]; // Analytical Jacobians wrt first and second quaternion + // double J_analytic_q1[16]; // Analytical Jacobians wrt first quaternion + double J_analytic_q2[16]; // Analytical Jacobian wrt second quaternion double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); diff --git a/fuse_graphs/include/fuse_graphs/hash_graph.h b/fuse_graphs/include/fuse_graphs/hash_graph.h deleted file mode 100644 index 8c66e1692..000000000 --- a/fuse_graphs/include/fuse_graphs/hash_graph.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_GRAPHS__HASH_GRAPH_H_ -#define FUSE_GRAPHS__HASH_GRAPH_H_ - -#warning This header is obsolete, please include fuse_graphs/hash_graph.hpp instead - -#include - -#endif // FUSE_GRAPHS__HASH_GRAPH_H_ diff --git a/fuse_graphs/include/fuse_graphs/hash_graph_params.h b/fuse_graphs/include/fuse_graphs/hash_graph_params.h deleted file mode 100644 index 90bc47872..000000000 --- a/fuse_graphs/include/fuse_graphs/hash_graph_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_GRAPHS__HASH_GRAPH_PARAMS_H_ -#define FUSE_GRAPHS__HASH_GRAPH_PARAMS_H_ - -#warning This header is obsolete, please include fuse_graphs/hash_graph_params.hpp instead - -#include - -#endif // FUSE_GRAPHS__HASH_GRAPH_PARAMS_H_ diff --git a/fuse_loss/include/fuse_loss/arctan_loss.h b/fuse_loss/include/fuse_loss/arctan_loss.h deleted file mode 100644 index 888286599..000000000 --- a/fuse_loss/include/fuse_loss/arctan_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__ARCTAN_LOSS_H_ -#define FUSE_LOSS__ARCTAN_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/arctan_loss.hpp instead - -#include - -#endif // FUSE_LOSS__ARCTAN_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/cauchy_loss.h b/fuse_loss/include/fuse_loss/cauchy_loss.h deleted file mode 100644 index 0a885e8c1..000000000 --- a/fuse_loss/include/fuse_loss/cauchy_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__CAUCHY_LOSS_H_ -#define FUSE_LOSS__CAUCHY_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/cauchy_loss.hpp instead - -#include - -#endif // FUSE_LOSS__CAUCHY_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/composed_loss.h b/fuse_loss/include/fuse_loss/composed_loss.h deleted file mode 100644 index c66351e28..000000000 --- a/fuse_loss/include/fuse_loss/composed_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__COMPOSED_LOSS_H_ -#define FUSE_LOSS__COMPOSED_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/composed_loss.hpp instead - -#include - -#endif // FUSE_LOSS__COMPOSED_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/dcs_loss.h b/fuse_loss/include/fuse_loss/dcs_loss.h deleted file mode 100644 index ba94dffe9..000000000 --- a/fuse_loss/include/fuse_loss/dcs_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__DCS_LOSS_H_ -#define FUSE_LOSS__DCS_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/dcs_loss.hpp instead - -#include - -#endif // FUSE_LOSS__DCS_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/fair_loss.h b/fuse_loss/include/fuse_loss/fair_loss.h deleted file mode 100644 index ba0f82a81..000000000 --- a/fuse_loss/include/fuse_loss/fair_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__FAIR_LOSS_H_ -#define FUSE_LOSS__FAIR_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/fair_loss.hpp instead - -#include - -#endif // FUSE_LOSS__FAIR_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/geman_mcclure_loss.h b/fuse_loss/include/fuse_loss/geman_mcclure_loss.h deleted file mode 100644 index ccff6815a..000000000 --- a/fuse_loss/include/fuse_loss/geman_mcclure_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__GEMAN_MCCLURE_LOSS_H_ -#define FUSE_LOSS__GEMAN_MCCLURE_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/geman_mcclure_loss.hpp instead - -#include - -#endif // FUSE_LOSS__GEMAN_MCCLURE_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/huber_loss.h b/fuse_loss/include/fuse_loss/huber_loss.h deleted file mode 100644 index 43348c817..000000000 --- a/fuse_loss/include/fuse_loss/huber_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__HUBER_LOSS_H_ -#define FUSE_LOSS__HUBER_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/huber_loss.hpp instead - -#include - -#endif // FUSE_LOSS__HUBER_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/loss_function.h b/fuse_loss/include/fuse_loss/loss_function.h deleted file mode 100644 index d5dad0563..000000000 --- a/fuse_loss/include/fuse_loss/loss_function.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__LOSS_FUNCTION_H_ -#define FUSE_LOSS__LOSS_FUNCTION_H_ - -#warning This header is obsolete, please include fuse_loss/loss_function.hpp instead - -#include - -#endif // FUSE_LOSS__LOSS_FUNCTION_H_ diff --git a/fuse_loss/include/fuse_loss/qwt_loss_plot.h b/fuse_loss/include/fuse_loss/qwt_loss_plot.h deleted file mode 100644 index 7a9ba3270..000000000 --- a/fuse_loss/include/fuse_loss/qwt_loss_plot.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__QWT_LOSS_PLOT_H_ -#define FUSE_LOSS__QWT_LOSS_PLOT_H_ - -#warning This header is obsolete, please include fuse_loss/qwt_loss_plot.hpp instead - -#include - -#endif // FUSE_LOSS__QWT_LOSS_PLOT_H_ diff --git a/fuse_loss/include/fuse_loss/scaled_loss.h b/fuse_loss/include/fuse_loss/scaled_loss.h deleted file mode 100644 index 5058e4b98..000000000 --- a/fuse_loss/include/fuse_loss/scaled_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__SCALED_LOSS_H_ -#define FUSE_LOSS__SCALED_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/scaled_loss.hpp instead - -#include - -#endif // FUSE_LOSS__SCALED_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/softlone_loss.h b/fuse_loss/include/fuse_loss/softlone_loss.h deleted file mode 100644 index 6a856d888..000000000 --- a/fuse_loss/include/fuse_loss/softlone_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__SOFTLONE_LOSS_H_ -#define FUSE_LOSS__SOFTLONE_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/softlone_loss.hpp instead - -#include - -#endif // FUSE_LOSS__SOFTLONE_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/tolerant_loss.h b/fuse_loss/include/fuse_loss/tolerant_loss.h deleted file mode 100644 index a12a45eaf..000000000 --- a/fuse_loss/include/fuse_loss/tolerant_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__TOLERANT_LOSS_H_ -#define FUSE_LOSS__TOLERANT_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/tolerant_loss.hpp instead - -#include - -#endif // FUSE_LOSS__TOLERANT_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/trivial_loss.h b/fuse_loss/include/fuse_loss/trivial_loss.h deleted file mode 100644 index 0dbf5a042..000000000 --- a/fuse_loss/include/fuse_loss/trivial_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__TRIVIAL_LOSS_H_ -#define FUSE_LOSS__TRIVIAL_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/trivial_loss.hpp instead - -#include - -#endif // FUSE_LOSS__TRIVIAL_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/tukey_loss.h b/fuse_loss/include/fuse_loss/tukey_loss.h deleted file mode 100644 index 7df96ea20..000000000 --- a/fuse_loss/include/fuse_loss/tukey_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__TUKEY_LOSS_H_ -#define FUSE_LOSS__TUKEY_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/tukey_loss.hpp instead - -#include - -#endif // FUSE_LOSS__TUKEY_LOSS_H_ diff --git a/fuse_loss/include/fuse_loss/welsch_loss.h b/fuse_loss/include/fuse_loss/welsch_loss.h deleted file mode 100644 index 14db82144..000000000 --- a/fuse_loss/include/fuse_loss/welsch_loss.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_LOSS__WELSCH_LOSS_H_ -#define FUSE_LOSS__WELSCH_LOSS_H_ - -#warning This header is obsolete, please include fuse_loss/welsch_loss.hpp instead - -#include - -#endif // FUSE_LOSS__WELSCH_LOSS_H_ diff --git a/fuse_models/include/fuse_models/acceleration_2d.h b/fuse_models/include/fuse_models/acceleration_2d.h deleted file mode 100644 index 208657367..000000000 --- a/fuse_models/include/fuse_models/acceleration_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__ACCELERATION_2D_H_ -#define FUSE_MODELS__ACCELERATION_2D_H_ - -#warning This header is obsolete, please include fuse_models/acceleration_2d.hpp instead - -#include - -#endif // FUSE_MODELS__ACCELERATION_2D_H_ diff --git a/fuse_models/include/fuse_models/common/sensor_config.h b/fuse_models/include/fuse_models/common/sensor_config.h deleted file mode 100644 index 144fefa70..000000000 --- a/fuse_models/include/fuse_models/common/sensor_config.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__COMMON__SENSOR_CONFIG_H_ -#define FUSE_MODELS__COMMON__SENSOR_CONFIG_H_ - -#warning This header is obsolete, please include fuse_models/common/sensor_config.hpp instead - -#include - -#endif // FUSE_MODELS__COMMON__SENSOR_CONFIG_H_ diff --git a/fuse_models/include/fuse_models/common/sensor_proc.h b/fuse_models/include/fuse_models/common/sensor_proc.h deleted file mode 100644 index ab0d552e9..000000000 --- a/fuse_models/include/fuse_models/common/sensor_proc.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__COMMON__SENSOR_PROC_H_ -#define FUSE_MODELS__COMMON__SENSOR_PROC_H_ - -#warning This header is obsolete, please include fuse_models/common/sensor_proc.hpp instead - -#include - -#endif // FUSE_MODELS__COMMON__SENSOR_PROC_H_ diff --git a/fuse_models/include/fuse_models/common/variable_traits.h b/fuse_models/include/fuse_models/common/variable_traits.h deleted file mode 100644 index 78bcba5a2..000000000 --- a/fuse_models/include/fuse_models/common/variable_traits.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__COMMON__VARIABLE_TRAITS_H_ -#define FUSE_MODELS__COMMON__VARIABLE_TRAITS_H_ - -#warning This header is obsolete, please include fuse_models/common/variable_traits.hpp instead - -#include - -#endif // FUSE_MODELS__COMMON__VARIABLE_TRAITS_H_ diff --git a/fuse_models/include/fuse_models/graph_ignition.h b/fuse_models/include/fuse_models/graph_ignition.h deleted file mode 100644 index ca668da77..000000000 --- a/fuse_models/include/fuse_models/graph_ignition.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__GRAPH_IGNITION_H_ -#define FUSE_MODELS__GRAPH_IGNITION_H_ - -#warning This header is obsolete, please include fuse_models/graph_ignition.hpp instead - -#include - -#endif // FUSE_MODELS__GRAPH_IGNITION_H_ diff --git a/fuse_models/include/fuse_models/imu_2d.h b/fuse_models/include/fuse_models/imu_2d.h deleted file mode 100644 index 22c42b7a8..000000000 --- a/fuse_models/include/fuse_models/imu_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__IMU_2D_H_ -#define FUSE_MODELS__IMU_2D_H_ - -#warning This header is obsolete, please include fuse_models/imu_2d.hpp instead - -#include - -#endif // FUSE_MODELS__IMU_2D_H_ diff --git a/fuse_models/include/fuse_models/odometry_2d.h b/fuse_models/include/fuse_models/odometry_2d.h deleted file mode 100644 index 0cac4c2f1..000000000 --- a/fuse_models/include/fuse_models/odometry_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__ODOMETRY_2D_H_ -#define FUSE_MODELS__ODOMETRY_2D_H_ - -#warning This header is obsolete, please include fuse_models/odometry_2d.hpp instead - -#include - -#endif // FUSE_MODELS__ODOMETRY_2D_H_ diff --git a/fuse_models/include/fuse_models/odometry_2d_publisher.h b/fuse_models/include/fuse_models/odometry_2d_publisher.h deleted file mode 100644 index a882e941e..000000000 --- a/fuse_models/include/fuse_models/odometry_2d_publisher.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__ODOMETRY_2D_PUBLISHER_H_ -#define FUSE_MODELS__ODOMETRY_2D_PUBLISHER_H_ - -#warning This header is obsolete, please include fuse_models/odometry_2d_publisher.hpp instead - -#include - -#endif // FUSE_MODELS__ODOMETRY_2D_PUBLISHER_H_ diff --git a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h b/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h deleted file mode 100644 index f427cd491..000000000 --- a/fuse_models/include/fuse_models/parameters/acceleration_2d_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__ACCELERATION_2D_PARAMS_H_ -#define FUSE_MODELS__PARAMETERS__ACCELERATION_2D_PARAMS_H_ - -#warning This header is obsolete, please include fuse_models/parameters/acceleration_2d_params.hpp instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__ACCELERATION_2D_PARAMS_H_ diff --git a/fuse_models/include/fuse_models/parameters/graph_ignition_params.h b/fuse_models/include/fuse_models/parameters/graph_ignition_params.h deleted file mode 100644 index 7739b4fd8..000000000 --- a/fuse_models/include/fuse_models/parameters/graph_ignition_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__GRAPH_IGNITION_PARAMS_H_ -#define FUSE_MODELS__PARAMETERS__GRAPH_IGNITION_PARAMS_H_ - -#warning This header is obsolete, please include fuse_models/parameters/graph_ignition_params.hpp instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__GRAPH_IGNITION_PARAMS_H_ diff --git a/fuse_models/include/fuse_models/parameters/imu_2d_params.h b/fuse_models/include/fuse_models/parameters/imu_2d_params.h deleted file mode 100644 index 262f3267c..000000000 --- a/fuse_models/include/fuse_models/parameters/imu_2d_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__IMU_2D_PARAMS_H_ -#define FUSE_MODELS__PARAMETERS__IMU_2D_PARAMS_H_ - -#warning This header is obsolete, please include fuse_models/parameters/imu_2d_params.hpp instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__IMU_2D_PARAMS_H_ diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_params.h b/fuse_models/include/fuse_models/parameters/odometry_2d_params.h deleted file mode 100644 index 1925cc5f4..000000000 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PARAMS_H_ -#define FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PARAMS_H_ - -#warning This header is obsolete, please include fuse_models/parameters/odometry_2d_params.hpp instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PARAMS_H_ diff --git a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h b/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h deleted file mode 100644 index 7bec22d88..000000000 --- a/fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PUBLISHER_PARAMS_H_ -#define FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PUBLISHER_PARAMS_H_ - -#warning This header is obsolete, please include fuse_models/parameters/odometry_2d_publisher_params.hpp \ - instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__ODOMETRY_2D_PUBLISHER_PARAMS_H_ diff --git a/fuse_models/include/fuse_models/parameters/parameter_base.h b/fuse_models/include/fuse_models/parameters/parameter_base.h deleted file mode 100644 index fd72994c2..000000000 --- a/fuse_models/include/fuse_models/parameters/parameter_base.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__PARAMETER_BASE_H_ -#define FUSE_MODELS__PARAMETERS__PARAMETER_BASE_H_ - -#warning This header is obsolete, please include fuse_models/parameters/parameter_base.hpp instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__PARAMETER_BASE_H_ diff --git a/fuse_models/include/fuse_models/parameters/pose_2d_params.h b/fuse_models/include/fuse_models/parameters/pose_2d_params.h deleted file mode 100644 index 91d228db7..000000000 --- a/fuse_models/include/fuse_models/parameters/pose_2d_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__POSE_2D_PARAMS_H_ -#define FUSE_MODELS__PARAMETERS__POSE_2D_PARAMS_H_ - -#warning This header is obsolete, please include fuse_models/parameters/pose_2d_params.hpp instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__POSE_2D_PARAMS_H_ diff --git a/fuse_models/include/fuse_models/parameters/transaction_params.h b/fuse_models/include/fuse_models/parameters/transaction_params.h deleted file mode 100644 index 014e45d5b..000000000 --- a/fuse_models/include/fuse_models/parameters/transaction_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__TRANSACTION_PARAMS_H_ -#define FUSE_MODELS__PARAMETERS__TRANSACTION_PARAMS_H_ - -#warning This header is obsolete, please include fuse_models/parameters/transaction_params.hpp instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__TRANSACTION_PARAMS_H_ diff --git a/fuse_models/include/fuse_models/parameters/twist_2d_params.h b/fuse_models/include/fuse_models/parameters/twist_2d_params.h deleted file mode 100644 index c887e9fa4..000000000 --- a/fuse_models/include/fuse_models/parameters/twist_2d_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__TWIST_2D_PARAMS_H_ -#define FUSE_MODELS__PARAMETERS__TWIST_2D_PARAMS_H_ - -#warning This header is obsolete, please include fuse_models/parameters/twist_2d_params.hpp instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__TWIST_2D_PARAMS_H_ diff --git a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h b/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h deleted file mode 100644 index 8cba776a3..000000000 --- a/fuse_models/include/fuse_models/parameters/unicycle_2d_ignition_params.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__PARAMETERS__UNICYCLE_2D_IGNITION_PARAMS_H_ -#define FUSE_MODELS__PARAMETERS__UNICYCLE_2D_IGNITION_PARAMS_H_ - -#warning This header is obsolete, please include fuse_models/parameters/unicycle_2d_ignition_params.hpp \ - instead - -#include - -#endif // FUSE_MODELS__PARAMETERS__UNICYCLE_2D_IGNITION_PARAMS_H_ diff --git a/fuse_models/include/fuse_models/pose_2d.h b/fuse_models/include/fuse_models/pose_2d.h deleted file mode 100644 index 3757552dc..000000000 --- a/fuse_models/include/fuse_models/pose_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__POSE_2D_H_ -#define FUSE_MODELS__POSE_2D_H_ - -#warning This header is obsolete, please include fuse_models/pose_2d.hpp instead - -#include - -#endif // FUSE_MODELS__POSE_2D_H_ diff --git a/fuse_models/include/fuse_models/transaction.h b/fuse_models/include/fuse_models/transaction.h deleted file mode 100644 index aa51e36d0..000000000 --- a/fuse_models/include/fuse_models/transaction.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__TRANSACTION_H_ -#define FUSE_MODELS__TRANSACTION_H_ - -#warning This header is obsolete, please include fuse_models/transaction.hpp instead - -#include - -#endif // FUSE_MODELS__TRANSACTION_H_ diff --git a/fuse_models/include/fuse_models/twist_2d.h b/fuse_models/include/fuse_models/twist_2d.h deleted file mode 100644 index 1aa85690c..000000000 --- a/fuse_models/include/fuse_models/twist_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__TWIST_2D_H_ -#define FUSE_MODELS__TWIST_2D_H_ - -#warning This header is obsolete, please include fuse_models/twist_2d.hpp instead - -#include - -#endif // FUSE_MODELS__TWIST_2D_H_ diff --git a/fuse_models/include/fuse_models/unicycle_2d.h b/fuse_models/include/fuse_models/unicycle_2d.h deleted file mode 100644 index 55827e26f..000000000 --- a/fuse_models/include/fuse_models/unicycle_2d.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__UNICYCLE_2D_H_ -#define FUSE_MODELS__UNICYCLE_2D_H_ - -#warning This header is obsolete, please include fuse_models/unicycle_2d.hpp instead - -#include - -#endif // FUSE_MODELS__UNICYCLE_2D_H_ diff --git a/fuse_models/include/fuse_models/unicycle_2d_ignition.h b/fuse_models/include/fuse_models/unicycle_2d_ignition.h deleted file mode 100644 index 37ca0d1e3..000000000 --- a/fuse_models/include/fuse_models/unicycle_2d_ignition.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__UNICYCLE_2D_IGNITION_H_ -#define FUSE_MODELS__UNICYCLE_2D_IGNITION_H_ - -#warning This header is obsolete, please include fuse_models/unicycle_2d_ignition.hpp instead - -#include - -#endif // FUSE_MODELS__UNICYCLE_2D_IGNITION_H_ diff --git a/fuse_models/include/fuse_models/unicycle_2d_predict.h b/fuse_models/include/fuse_models/unicycle_2d_predict.h deleted file mode 100644 index dfd5487a8..000000000 --- a/fuse_models/include/fuse_models/unicycle_2d_predict.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__UNICYCLE_2D_PREDICT_H_ -#define FUSE_MODELS__UNICYCLE_2D_PREDICT_H_ - -#warning This header is obsolete, please include fuse_models/unicycle_2d_predict.hpp instead - -#include - -#endif // FUSE_MODELS__UNICYCLE_2D_PREDICT_H_ diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h b/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h deleted file mode 100644 index e67ff4e1b..000000000 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_function.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTION_H_ -#define FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTION_H_ - -#warning This header is obsolete, please include fuse_models/unicycle_2d_state_cost_function.hpp instead - -#include - -#endif // FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTION_H_ diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h b/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h deleted file mode 100644 index c7a330df2..000000000 --- a/fuse_models/include/fuse_models/unicycle_2d_state_cost_functor.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTOR_H_ -#define FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTOR_H_ - -#warning This header is obsolete, please include fuse_models/unicycle_2d_state_cost_functor.hpp instead - -#include - -#endif // FUSE_MODELS__UNICYCLE_2D_STATE_COST_FUNCTOR_H_ diff --git a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h b/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h deleted file mode 100644 index b714233a7..000000000 --- a/fuse_models/include/fuse_models/unicycle_2d_state_kinematic_constraint.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_MODELS__UNICYCLE_2D_STATE_KINEMATIC_CONSTRAINT_H_ -#define FUSE_MODELS__UNICYCLE_2D_STATE_KINEMATIC_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_models/unicycle_2d_state_kinematic_constraint.hpp \ - instead - -#include - -#endif // FUSE_MODELS__UNICYCLE_2D_STATE_KINEMATIC_CONSTRAINT_H_ diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h deleted file mode 100644 index 6709f38d0..000000000 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_OPTIMIZERS__BATCH_OPTIMIZER_H_ -#define FUSE_OPTIMIZERS__BATCH_OPTIMIZER_H_ - -#warning This header is obsolete, please include fuse_optimizers/batch_optimizer.hpp instead - -#include - -#endif // FUSE_OPTIMIZERS__BATCH_OPTIMIZER_H_ diff --git a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h b/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h deleted file mode 100644 index 32d05d4b6..000000000 --- a/fuse_optimizers/include/fuse_optimizers/batch_optimizer_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_OPTIMIZERS__BATCH_OPTIMIZER_PARAMS_H_ -#define FUSE_OPTIMIZERS__BATCH_OPTIMIZER_PARAMS_H_ - -#warning This header is obsolete, please include fuse_optimizers/batch_optimizer_params.hpp instead - -#include - -#endif // FUSE_OPTIMIZERS__BATCH_OPTIMIZER_PARAMS_H_ diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h deleted file mode 100644 index 04117b2c3..000000000 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_H_ -#define FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_H_ - -#warning This header is obsolete, please include fuse_optimizers/fixed_lag_smoother.hpp instead - -#include - -#endif // FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_H_ diff --git a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h b/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h deleted file mode 100644 index 735b6e3cc..000000000 --- a/fuse_optimizers/include/fuse_optimizers/fixed_lag_smoother_params.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_PARAMS_H_ -#define FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_PARAMS_H_ - -#warning This header is obsolete, please include fuse_optimizers/fixed_lag_smoother_params.hpp instead - -#include - -#endif // FUSE_OPTIMIZERS__FIXED_LAG_SMOOTHER_PARAMS_H_ diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h deleted file mode 100644 index 5f63488c0..000000000 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_OPTIMIZERS__OPTIMIZER_H_ -#define FUSE_OPTIMIZERS__OPTIMIZER_H_ - -#warning This header is obsolete, please include fuse_optimizers/optimizer.hpp instead - -#include - -#endif // FUSE_OPTIMIZERS__OPTIMIZER_H_ diff --git a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h b/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h deleted file mode 100644 index 2058bdcde..000000000 --- a/fuse_optimizers/include/fuse_optimizers/variable_stamp_index.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_OPTIMIZERS__VARIABLE_STAMP_INDEX_H_ -#define FUSE_OPTIMIZERS__VARIABLE_STAMP_INDEX_H_ - -#warning This header is obsolete, please include fuse_optimizers/variable_stamp_index.hpp instead - -#include - -#endif // FUSE_OPTIMIZERS__VARIABLE_STAMP_INDEX_H_ diff --git a/fuse_publishers/include/fuse_publishers/path_2d_publisher.h b/fuse_publishers/include/fuse_publishers/path_2d_publisher.h deleted file mode 100644 index 126c070ee..000000000 --- a/fuse_publishers/include/fuse_publishers/path_2d_publisher.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_PUBLISHERS__PATH_2D_PUBLISHER_H_ -#define FUSE_PUBLISHERS__PATH_2D_PUBLISHER_H_ - -#warning This header is obsolete, please include fuse_publishers/path_2d_publisher.hpp instead - -#include - -#endif // FUSE_PUBLISHERS__PATH_2D_PUBLISHER_H_ diff --git a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.h b/fuse_publishers/include/fuse_publishers/pose_2d_publisher.h deleted file mode 100644 index 1169b687f..000000000 --- a/fuse_publishers/include/fuse_publishers/pose_2d_publisher.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_PUBLISHERS__POSE_2D_PUBLISHER_H_ -#define FUSE_PUBLISHERS__POSE_2D_PUBLISHER_H_ - -#warning This header is obsolete, please include fuse_publishers/pose_2d_publisher.hpp instead - -#include - -#endif // FUSE_PUBLISHERS__POSE_2D_PUBLISHER_H_ diff --git a/fuse_publishers/include/fuse_publishers/serialized_publisher.h b/fuse_publishers/include/fuse_publishers/serialized_publisher.h deleted file mode 100644 index 1910885a7..000000000 --- a/fuse_publishers/include/fuse_publishers/serialized_publisher.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_PUBLISHERS__SERIALIZED_PUBLISHER_H_ -#define FUSE_PUBLISHERS__SERIALIZED_PUBLISHER_H_ - -#warning This header is obsolete, please include fuse_publishers/serialized_publisher.hpp instead - -#include - -#endif // FUSE_PUBLISHERS__SERIALIZED_PUBLISHER_H_ diff --git a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h b/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h deleted file mode 100644 index 5f390ea09..000000000 --- a/fuse_publishers/include/fuse_publishers/stamped_variable_synchronizer.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_H_ -#define FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_H_ - -#warning This header is obsolete, please include fuse_publishers/stamped_variable_synchronizer.hpp instead - -#include - -#endif // FUSE_PUBLISHERS__STAMPED_VARIABLE_SYNCHRONIZER_H_ diff --git a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.h b/fuse_tutorials/include/fuse_tutorials/beacon_publisher.h deleted file mode 100644 index 2c2b7dc7c..000000000 --- a/fuse_tutorials/include/fuse_tutorials/beacon_publisher.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_TUTORIALS__BEACON_PUBLISHER_H_ -#define FUSE_TUTORIALS__BEACON_PUBLISHER_H_ - -#warning This header is obsolete, please include fuse_tutorials/beacon_publisher.hpp instead - -#include - -#endif // FUSE_TUTORIALS__BEACON_PUBLISHER_H_ diff --git a/fuse_tutorials/include/fuse_tutorials/range_constraint.h b/fuse_tutorials/include/fuse_tutorials/range_constraint.h deleted file mode 100644 index da01fce0b..000000000 --- a/fuse_tutorials/include/fuse_tutorials/range_constraint.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_TUTORIALS__RANGE_CONSTRAINT_H_ -#define FUSE_TUTORIALS__RANGE_CONSTRAINT_H_ - -#warning This header is obsolete, please include fuse_tutorials/range_constraint.hpp instead - -#include - -#endif // FUSE_TUTORIALS__RANGE_CONSTRAINT_H_ diff --git a/fuse_tutorials/include/fuse_tutorials/range_cost_functor.h b/fuse_tutorials/include/fuse_tutorials/range_cost_functor.h deleted file mode 100644 index 72982ae56..000000000 --- a/fuse_tutorials/include/fuse_tutorials/range_cost_functor.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_TUTORIALS__RANGE_COST_FUNCTOR_H_ -#define FUSE_TUTORIALS__RANGE_COST_FUNCTOR_H_ - -#warning This header is obsolete, please include fuse_tutorials/range_cost_functor.hpp instead - -#include - -#endif // FUSE_TUTORIALS__RANGE_COST_FUNCTOR_H_ diff --git a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.h b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.h deleted file mode 100644 index 80fa45835..000000000 --- a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_TUTORIALS__RANGE_SENSOR_MODEL_H_ -#define FUSE_TUTORIALS__RANGE_SENSOR_MODEL_H_ - -#warning This header is obsolete, please include fuse_tutorials/range_sensor_model.hpp instead - -#include - -#endif // FUSE_TUTORIALS__RANGE_SENSOR_MODEL_H_ diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h deleted file mode 100644 index e4e32b8a6..000000000 --- a/fuse_variables/include/fuse_variables/acceleration_angular_2d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__ACCELERATION_ANGULAR_2D_STAMPED_H_ -#define FUSE_VARIABLES__ACCELERATION_ANGULAR_2D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/acceleration_angular_2d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__ACCELERATION_ANGULAR_2D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h deleted file mode 100644 index 26c2d3346..000000000 --- a/fuse_variables/include/fuse_variables/acceleration_angular_3d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__ACCELERATION_ANGULAR_3D_STAMPED_H_ -#define FUSE_VARIABLES__ACCELERATION_ANGULAR_3D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/acceleration_angular_3d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__ACCELERATION_ANGULAR_3D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h deleted file mode 100644 index 52287c600..000000000 --- a/fuse_variables/include/fuse_variables/acceleration_linear_2d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__ACCELERATION_LINEAR_2D_STAMPED_H_ -#define FUSE_VARIABLES__ACCELERATION_LINEAR_2D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/acceleration_linear_2d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__ACCELERATION_LINEAR_2D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h b/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h deleted file mode 100644 index b330ecfd8..000000000 --- a/fuse_variables/include/fuse_variables/acceleration_linear_3d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__ACCELERATION_LINEAR_3D_STAMPED_H_ -#define FUSE_VARIABLES__ACCELERATION_LINEAR_3D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/acceleration_linear_3d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__ACCELERATION_LINEAR_3D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/fixed_size_variable.h b/fuse_variables/include/fuse_variables/fixed_size_variable.h deleted file mode 100644 index a11c717c6..000000000 --- a/fuse_variables/include/fuse_variables/fixed_size_variable.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__FIXED_SIZE_VARIABLE_H_ -#define FUSE_VARIABLES__FIXED_SIZE_VARIABLE_H_ - -#warning This header is obsolete, please include fuse_variables/fixed_size_variable.hpp instead - -#include - -#endif // FUSE_VARIABLES__FIXED_SIZE_VARIABLE_H_ diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.h b/fuse_variables/include/fuse_variables/orientation_2d_stamped.h deleted file mode 100644 index 7473ab190..000000000 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__ORIENTATION_2D_STAMPED_H_ -#define FUSE_VARIABLES__ORIENTATION_2D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/orientation_2d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__ORIENTATION_2D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.h b/fuse_variables/include/fuse_variables/orientation_3d_stamped.h deleted file mode 100644 index 8b8e615bf..000000000 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__ORIENTATION_3D_STAMPED_H_ -#define FUSE_VARIABLES__ORIENTATION_3D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/orientation_3d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__ORIENTATION_3D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h deleted file mode 100644 index 74522ac33..000000000 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__POINT_2D_FIXED_LANDMARK_H_ -#define FUSE_VARIABLES__POINT_2D_FIXED_LANDMARK_H_ - -#warning This header is obsolete, please include fuse_variables/point_2d_fixed_landmark.hpp instead - -#include - -#endif // FUSE_VARIABLES__POINT_2D_FIXED_LANDMARK_H_ diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.h b/fuse_variables/include/fuse_variables/point_2d_landmark.h deleted file mode 100644 index e730176c2..000000000 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__POINT_2D_LANDMARK_H_ -#define FUSE_VARIABLES__POINT_2D_LANDMARK_H_ - -#warning This header is obsolete, please include fuse_variables/point_2d_landmark.hpp instead - -#include - -#endif // FUSE_VARIABLES__POINT_2D_LANDMARK_H_ diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h deleted file mode 100644 index d65eb27f0..000000000 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__POINT_3D_FIXED_LANDMARK_H_ -#define FUSE_VARIABLES__POINT_3D_FIXED_LANDMARK_H_ - -#warning This header is obsolete, please include fuse_variables/point_3d_fixed_landmark.hpp instead - -#include - -#endif // FUSE_VARIABLES__POINT_3D_FIXED_LANDMARK_H_ diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.h b/fuse_variables/include/fuse_variables/point_3d_landmark.h deleted file mode 100644 index 7053b5426..000000000 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__POINT_3D_LANDMARK_H_ -#define FUSE_VARIABLES__POINT_3D_LANDMARK_H_ - -#warning This header is obsolete, please include fuse_variables/point_3d_landmark.hpp instead - -#include - -#endif // FUSE_VARIABLES__POINT_3D_LANDMARK_H_ diff --git a/fuse_variables/include/fuse_variables/position_2d_stamped.h b/fuse_variables/include/fuse_variables/position_2d_stamped.h deleted file mode 100644 index bacda37f9..000000000 --- a/fuse_variables/include/fuse_variables/position_2d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__POSITION_2D_STAMPED_H_ -#define FUSE_VARIABLES__POSITION_2D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/position_2d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__POSITION_2D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/position_3d_stamped.h b/fuse_variables/include/fuse_variables/position_3d_stamped.h deleted file mode 100644 index 6fe523f14..000000000 --- a/fuse_variables/include/fuse_variables/position_3d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__POSITION_3D_STAMPED_H_ -#define FUSE_VARIABLES__POSITION_3D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/position_3d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__POSITION_3D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/stamped.h b/fuse_variables/include/fuse_variables/stamped.h deleted file mode 100644 index 8943dab92..000000000 --- a/fuse_variables/include/fuse_variables/stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__STAMPED_H_ -#define FUSE_VARIABLES__STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h b/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h deleted file mode 100644 index 7f8ad356d..000000000 --- a/fuse_variables/include/fuse_variables/velocity_angular_2d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__VELOCITY_ANGULAR_2D_STAMPED_H_ -#define FUSE_VARIABLES__VELOCITY_ANGULAR_2D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/velocity_angular_2d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__VELOCITY_ANGULAR_2D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h b/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h deleted file mode 100644 index 0a5dfe2d8..000000000 --- a/fuse_variables/include/fuse_variables/velocity_angular_3d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__VELOCITY_ANGULAR_3D_STAMPED_H_ -#define FUSE_VARIABLES__VELOCITY_ANGULAR_3D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/velocity_angular_3d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__VELOCITY_ANGULAR_3D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h b/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h deleted file mode 100644 index 04aaf0413..000000000 --- a/fuse_variables/include/fuse_variables/velocity_linear_2d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__VELOCITY_LINEAR_2D_STAMPED_H_ -#define FUSE_VARIABLES__VELOCITY_LINEAR_2D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/velocity_linear_2d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__VELOCITY_LINEAR_2D_STAMPED_H_ diff --git a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h b/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h deleted file mode 100644 index cc8048e16..000000000 --- a/fuse_variables/include/fuse_variables/velocity_linear_3d_stamped.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VARIABLES__VELOCITY_LINEAR_3D_STAMPED_H_ -#define FUSE_VARIABLES__VELOCITY_LINEAR_3D_STAMPED_H_ - -#warning This header is obsolete, please include fuse_variables/velocity_linear_3d_stamped.hpp instead - -#include - -#endif // FUSE_VARIABLES__VELOCITY_LINEAR_3D_STAMPED_H_ diff --git a/fuse_viz/include/fuse_viz/conversions.h b/fuse_viz/include/fuse_viz/conversions.h deleted file mode 100644 index c2de0d797..000000000 --- a/fuse_viz/include/fuse_viz/conversions.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VIZ__CONVERSIONS_H_ -#define FUSE_VIZ__CONVERSIONS_H_ - -#warning This header is obsolete, please include fuse_viz/conversions.hpp instead - -#include - -#endif // FUSE_VIZ__CONVERSIONS_H_ diff --git a/fuse_viz/include/fuse_viz/mapped_covariance_property.h b/fuse_viz/include/fuse_viz/mapped_covariance_property.h deleted file mode 100644 index 4bbd4245f..000000000 --- a/fuse_viz/include/fuse_viz/mapped_covariance_property.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VIZ__MAPPED_COVARIANCE_PROPERTY_H_ -#define FUSE_VIZ__MAPPED_COVARIANCE_PROPERTY_H_ - -#warning This header is obsolete, please include fuse_viz/mapped_covariance_property.hpp instead - -#include - -#endif // FUSE_VIZ__MAPPED_COVARIANCE_PROPERTY_H_ diff --git a/fuse_viz/include/fuse_viz/mapped_covariance_visual.h b/fuse_viz/include/fuse_viz/mapped_covariance_visual.h deleted file mode 100644 index 9dbebd2f8..000000000 --- a/fuse_viz/include/fuse_viz/mapped_covariance_visual.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VIZ__MAPPED_COVARIANCE_VISUAL_H_ -#define FUSE_VIZ__MAPPED_COVARIANCE_VISUAL_H_ - -#warning This header is obsolete, please include fuse_viz/mapped_covariance_visual.hpp instead - -#include - -#endif // FUSE_VIZ__MAPPED_COVARIANCE_VISUAL_H_ diff --git a/fuse_viz/include/fuse_viz/pose_2d_stamped_property.h b/fuse_viz/include/fuse_viz/pose_2d_stamped_property.h deleted file mode 100644 index a3f08be31..000000000 --- a/fuse_viz/include/fuse_viz/pose_2d_stamped_property.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VIZ__POSE_2D_STAMPED_PROPERTY_H_ -#define FUSE_VIZ__POSE_2D_STAMPED_PROPERTY_H_ - -#warning This header is obsolete, please include fuse_viz/pose_2d_stamped_property.hpp instead - -#include - -#endif // FUSE_VIZ__POSE_2D_STAMPED_PROPERTY_H_ diff --git a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.h b/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.h deleted file mode 100644 index cda68a386..000000000 --- a/fuse_viz/include/fuse_viz/pose_2d_stamped_visual.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VIZ__POSE_2D_STAMPED_VISUAL_H_ -#define FUSE_VIZ__POSE_2D_STAMPED_VISUAL_H_ - -#warning This header is obsolete, please include fuse_viz/pose_2d_stamped_visual.hpp instead - -#include - -#endif // FUSE_VIZ__POSE_2D_STAMPED_VISUAL_H_ diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h deleted file mode 100644 index d879ee071..000000000 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_property.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_PROPERTY_H_ -#define FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_PROPERTY_H_ - -#warning This header is obsolete, please include fuse_viz/relative_pose_2d_stamped_constraint_property.hpp instead - -#include - -#endif // FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_PROPERTY_H_ diff --git a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h b/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h deleted file mode 100644 index fb3f22d0f..000000000 --- a/fuse_viz/include/fuse_viz/relative_pose_2d_stamped_constraint_visual.h +++ /dev/null @@ -1,43 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_H_ -#define FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_H_ - -#warning This header is obsolete, please include fuse_viz/relative_pose_2d_stamped_constraint_visual.hpp \ - instead - -#include - -#endif // FUSE_VIZ__RELATIVE_POSE_2D_STAMPED_CONSTRAINT_VISUAL_H_ diff --git a/fuse_viz/include/fuse_viz/serialized_graph_display.h b/fuse_viz/include/fuse_viz/serialized_graph_display.h deleted file mode 100644 index 8d748fc92..000000000 --- a/fuse_viz/include/fuse_viz/serialized_graph_display.h +++ /dev/null @@ -1,42 +0,0 @@ -/* - * Software License Agreement (BSD License) - * - * Copyright (c) 2022, Locus Robotics - * All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * * Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * * Redistributions in binary form must reproduce the above - * copyright notice, this list of conditions and the following - * disclaimer in the documentation and/or other materials provided - * with the distribution. - * * Neither the name of the copyright holder nor the names of its - * contributors may be used to endorse or promote products derived - * from this software without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; - * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER - * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - */ - -#ifndef FUSE_VIZ__SERIALIZED_GRAPH_DISPLAY_H_ -#define FUSE_VIZ__SERIALIZED_GRAPH_DISPLAY_H_ - -#warning This header is obsolete, please include fuse_viz/serialized_graph_display.hpp instead - -#include - -#endif // FUSE_VIZ__SERIALIZED_GRAPH_DISPLAY_H_ From d91474e36ebad2d329f61be6f20803f54ac56c83 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 24 Sep 2024 20:10:46 +0000 Subject: [PATCH 10/46] fix test tolerance and only test fuse packages, not vcs deps --- .github/workflows/ci.yaml | 2 +- fuse_constraints/test/cost_function_gtest.hpp | 7 ++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 03bdea8c8..4bd9b87a7 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -61,7 +61,7 @@ jobs: run: | . /opt/ros/rolling/setup.sh . /colcon_ws/install/local_setup.sh - colcon test --event-handlers console_direct+ --retest-until-pass 3 + colcon test --event-handlers console_direct+ --packages-select-regex fuse* working-directory: /colcon_ws # `colcon test` does not actually error on failure - run `colcon test-result` to generate a summary and an error code. diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index 9b1c9da16..b5178c359 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -56,6 +56,7 @@ static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function, const ceres::CostFunction& actual_cost_function) { + constexpr double tol = 1e-12; EXPECT_EQ(cost_function.num_residuals(), actual_cost_function.num_residuals()); const size_t num_residuals = cost_function.num_residuals(); const std::vector& parameter_block_sizes = cost_function.parameter_block_sizes(); @@ -114,7 +115,7 @@ static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(), actual_residuals.get(), nullptr)); for (size_t i = 0; i < num_residuals; ++i) { - EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual id: " << i; + EXPECT_NEAR(residuals[i], actual_residuals[i], tol) << "residual id: " << i; } EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), residuals.get(), jacobian_blocks.get())); @@ -122,12 +123,12 @@ static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function actual_cost_function.Evaluate(parameter_blocks.get(), actual_residuals.get(), actual_jacobian_blocks.get())); for (size_t i = 0; i < num_residuals; ++i) { - EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual : " << i; + EXPECT_NEAR(residuals[i], actual_residuals[i], tol) << "residual : " << i; } for (size_t i = 0; i < num_residuals * num_parameters; ++i) { - EXPECT_DOUBLE_EQ(jacobians[i], actual_jacobians[i]) + EXPECT_NEAR(jacobians[i], actual_jacobians[i], tol) << "jacobian : " << i << " " << jacobians[i] << " " << actual_jacobians[i]; } } From edd25a14f40604a87b6e67efb9dcad06e3622c0d Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 24 Sep 2024 20:12:52 +0000 Subject: [PATCH 11/46] bump to correct link --- doc/Constraints.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/doc/Constraints.md b/doc/Constraints.md index 377738a5d..263c3e12b 100644 --- a/doc/Constraints.md +++ b/doc/Constraints.md @@ -34,7 +34,7 @@ by accepting, at a minimum, the UUID of each involved Variable. The derived Constraints provided by the [fuse_constraints](../fuse_constraints) package go one step further, and require instances of full Variable types rather than just the Variable's UUID. This is done solely to enforce type-safety; e.g. a specific Constraint must involve two -[Position2DStamped](../fuse_variables/include/fuse_variables/position_2d_stamped.h) variables, no other variable types +[Position2DStamped](../fuse_variables/include/fuse_variables/position_2d_stamped.hpp) variables, no other variable types are acceptable. ### Cost Function From d70df287003242ff2cf04f7483d83ec8c6629213 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Tue, 24 Sep 2024 22:55:15 +0000 Subject: [PATCH 12/46] initial pass at 3D tutorial --- fuse_models/include/fuse_models/imu_2d.hpp | 1 + fuse_models/include/fuse_models/imu_3d.hpp | 1 + .../include/fuse_models/odometry_2d.hpp | 1 + .../fuse_models/omnidirectional_3d.hpp | 9 +- fuse_tutorials/CMakeLists.txt | 5 + fuse_tutorials/config/fuse_3d_tutorial.rviz | 205 ++++++++++++++ fuse_tutorials/config/fuse_3d_tutorial.yaml | 47 ++++ .../fuse_tutorials/range_sensor_model.hpp | 1 - .../launch/fuse_3d_tutorial.launch.py | 59 ++++ .../src/three_dimensional_imu_simulator.cpp | 258 ++++++++++++++++++ 10 files changed, 581 insertions(+), 6 deletions(-) create mode 100644 fuse_tutorials/config/fuse_3d_tutorial.rviz create mode 100644 fuse_tutorials/config/fuse_3d_tutorial.yaml create mode 100644 fuse_tutorials/launch/fuse_3d_tutorial.launch.py create mode 100644 fuse_tutorials/src/three_dimensional_imu_simulator.cpp diff --git a/fuse_models/include/fuse_models/imu_2d.hpp b/fuse_models/include/fuse_models/imu_2d.hpp index c95a36349..61a4d5324 100644 --- a/fuse_models/include/fuse_models/imu_2d.hpp +++ b/fuse_models/include/fuse_models/imu_2d.hpp @@ -47,6 +47,7 @@ #include #include +#include #include #include diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp index 2ff2fc0f6..46c8979b9 100644 --- a/fuse_models/include/fuse_models/imu_3d.hpp +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -47,6 +47,7 @@ #include #include +#include #include #include diff --git a/fuse_models/include/fuse_models/odometry_2d.hpp b/fuse_models/include/fuse_models/odometry_2d.hpp index 8edbc5d20..d43083ce5 100644 --- a/fuse_models/include/fuse_models/odometry_2d.hpp +++ b/fuse_models/include/fuse_models/odometry_2d.hpp @@ -47,6 +47,7 @@ #include #include +#include #include #include diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index cdb016689..e1764b58d 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -31,12 +31,11 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#ifndef FUSE_MODELS__UNICYCLE_3D_HPP_ -#define FUSE_MODELS__UNICYCLE_3D_HPP_ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_HPP_ #include #include -#include #include #include @@ -229,8 +228,8 @@ class Omnidirectional3D : public fuse_core::AsyncMotionModel StateHistory state_history_; //!< History of optimized graph pose estimates }; -std::ostream& operator<<(std::ostream& stream, const Omnidirectional3D& unicycle_2d); +std::ostream& operator<<(std::ostream& stream, const Omnidirectional3D& omnidirectional_3d); } // namespace fuse_models -#endif // FUSE_MODELS__UNICYCLE_3D_HPP_ +#endif // FUSE_MODELS__OMNIDIRECTIONAL_3D_HPP_ diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index c45b22c37..2b0c6a183 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -45,6 +45,10 @@ target_link_libraries( add_executable(range_sensor_simulator src/range_sensor_simulator.cpp) target_link_libraries(range_sensor_simulator ${PROJECT_NAME}) +add_executable(three_dimensional_IMU_simulator + src/three_dimensional_imu_simulator.cpp) +target_link_libraries(three_dimensional_IMU_simulator ${PROJECT_NAME}) + # ############################################################################## # Install ## # ############################################################################## @@ -59,6 +63,7 @@ install( install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(TARGETS range_sensor_simulator DESTINATION lib/${PROJECT_NAME}) +install(TARGETS three_dimensional_IMU_simulator DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch config data DESTINATION share/${PROJECT_NAME}) diff --git a/fuse_tutorials/config/fuse_3d_tutorial.rviz b/fuse_tutorials/config/fuse_3d_tutorial.rviz new file mode 100644 index 000000000..764a4aeb0 --- /dev/null +++ b/fuse_tutorials/config/fuse_3d_tutorial.rviz @@ -0,0 +1,205 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Fused Odom1/Shape1 + Splitter Ratio: 0.5 + Tree Height: 719 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: "" +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 100 + Reference Frame: + Value: true + - Angle Tolerance: 0.05000000074505806 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 10000 + Name: Fused Odom + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 26; 95; 180 + Head Length: 0.07500000298023224 + Head Radius: 0.02500000037252903 + Shaft Length: 0.25 + Shaft Radius: 0.012500000186264515 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_filtered + Value: true + - Angle Tolerance: 0.05000000074505806 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: true + Keep: 10000 + Name: Odom + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 25; 0 + Head Length: 0.07500000298023224 + Head Radius: 0.02500000037252903 + Shaft Length: 0.25 + Shaft Radius: 0.012500000186264515 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: odom + Frame Rate: 24 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 168.0489959716797 + Target Frame: + Value: TopDownOrtho (rviz_default_plugins) + X: 2 + Y: 0 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1016 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005da0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1846 + X: 74 + Y: 27 diff --git a/fuse_tutorials/config/fuse_3d_tutorial.yaml b/fuse_tutorials/config/fuse_3d_tutorial.yaml new file mode 100644 index 000000000..95a126317 --- /dev/null +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -0,0 +1,47 @@ +state_estimator: + ros__parameters: + # Fixed-lag smoother configuration + optimization_frequency: 20.0 + transaction_timeout: 0.01 + lag_duration: 0.5 + + motion_models: + 3d_motion_model: + type: fuse_models::Omnidirectional3D + + 3d_motion_model: + # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc + process_noise_diagonal: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + + sensor_models: + initial_localization_sensor: + type: fuse_models::Omnidirectional3DIgnition + motion_models: [3d_motion_model] + ignition: true + imu_sensor: + type: fuse_models::Imu3D + motion_models: [3d_motion_model] + + initial_localization_sensor: + publish_on_startup: true + # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc + initial_state: [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] + initial_sigma: [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] + + imu_sensor: + topic: 'imu' + twist_target_frame: 'base_footprint' + angular_velocity_dimensions: ['yaw'] + + publishers: + filtered_publisher: + type: fuse_models::Odometry3DPublisher + + filtered_publisher: + topic: 'odom_filtered' + base_link_frame_id: 'base_footprint' + odom_frame_id: 'odom' + map_frame_id: 'map' + world_frame_id: 'odom' + publish_tf: true + publish_frequency: 10.0 diff --git a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp index 52e5baa99..29dc12400 100644 --- a/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp +++ b/fuse_tutorials/include/fuse_tutorials/range_sensor_model.hpp @@ -34,7 +34,6 @@ #ifndef FUSE_TUTORIALS__RANGE_SENSOR_MODEL_HPP_ #define FUSE_TUTORIALS__RANGE_SENSOR_MODEL_HPP_ -#include #include #include diff --git a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py new file mode 100644 index 000000000..440991b5a --- /dev/null +++ b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py @@ -0,0 +1,59 @@ +#! /usr/bin/env python3 + +# Copyright 2022 Open Source Robotics Foundation, Inc. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch.substitutions import PathJoinSubstitution +from launch_ros.actions import Node, SetParameter +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + pkg_dir = FindPackageShare("fuse_tutorials") + + return LaunchDescription( + [ + Node( + package="fuse_tutorials", + executable="three_dimensional_IMU_simulator", + name="three_dimensional_IMU_simulator", + output="screen", + # prefix=["gdbserver localhost:3000"], + ), + SetParameter(name="use_sim_time", value=True), + Node( + package="fuse_optimizers", + executable="fixed_lag_smoother_node", + name="state_estimator", + parameters=[ + PathJoinSubstitution([pkg_dir, "config", "fuse_3d_tutorial.yaml"]) + ], + ), + Node( + package="rviz2", + executable="rviz2", + name="rviz", + # arguments=[ + # "-d", + # [ + # PathJoinSubstitution( + # [pkg_dir, "config", "fuse_3d_tutorial.rviz"] + # ) + # ], + # ], + ), + ] + ) diff --git a/fuse_tutorials/src/three_dimensional_imu_simulator.cpp b/fuse_tutorials/src/three_dimensional_imu_simulator.cpp new file mode 100644 index 000000000..34ea52067 --- /dev/null +++ b/fuse_tutorials/src/three_dimensional_imu_simulator.cpp @@ -0,0 +1,258 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2021, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame id used when + //!< publishing sensor data +static constexpr char MAP_FRAME[] = "map"; //!< The map frame id used when publishing ground truth + //!< data +static constexpr double IMU_SIGMA = 0.1; //!< Std dev of simulated Imu measurement noise + +/** + * @brief The true pose and velocity of the robot + */ +struct Robot +{ + rclcpp::Time stamp; + + double mass; + + double x = 0; + double y = 0; + double z = 0; + double roll = 0; + double pitch = 0; + double yaw = 0; + double vx = 0; + double vy = 0; + double vz = 0; + double vroll = 0; + double vpitch = 0; + double vyaw = 0; + double ax = 0; + double ay = 0; + double az = 0; +}; + +/** + * @brief Convert the robot state into a ground truth odometry message + */ +nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot& state) +{ + auto msg = std::make_shared(); + msg->header.stamp = state.stamp; + msg->header.frame_id = MAP_FRAME; + msg->child_frame_id = BASELINK_FRAME; + msg->pose.pose.position.x = state.x; + msg->pose.pose.position.y = state.y; + msg->pose.pose.position.z = state.z; + + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + msg->pose.pose.orientation.w = q.w(); + msg->pose.pose.orientation.x = q.x(); + msg->pose.pose.orientation.y = q.y(); + msg->pose.pose.orientation.z = q.z(); + msg->twist.twist.linear.x = state.vx; + msg->twist.twist.linear.y = state.vy; + msg->twist.twist.linear.z = state.vz; + msg->twist.twist.angular.x = state.vroll; + msg->twist.twist.angular.y = state.vpitch; + msg->twist.twist.angular.z = state.vyaw; + + // set covariances + msg->pose.covariance[0] = 0.1; + msg->pose.covariance[7] = 0.1; + msg->pose.covariance[14] = 0.1; + msg->pose.covariance[21] = 0.1; + msg->pose.covariance[28] = 0.1; + msg->pose.covariance[35] = 0.1; + msg->twist.covariance[0] = 0.1; + msg->twist.covariance[7] = 0.1; + msg->twist.covariance[14] = 0.1; + msg->twist.covariance[21] = 0.1; + msg->twist.covariance[28] = 0.1; + msg->twist.covariance[35] = 0.1; + return msg; +} + +/** + * @brief Compute the next robot state given the current robot state and a simulated step time + */ +Robot simulateRobotMotion(const Robot& previous_state, const rclcpp::Time& now, Eigen::Vector3d external_force) +{ + auto dt = (now - previous_state.stamp).seconds(); + auto next_state = Robot(); + next_state.stamp = now; + next_state.mass = previous_state.mass; + + // just euler integrate to get the next position and velocity + next_state.x = previous_state.x + previous_state.vx * dt; + next_state.y = previous_state.y + previous_state.vy * dt; + next_state.z = previous_state.z + previous_state.vz * dt; + next_state.vx = previous_state.vx + previous_state.ax * dt; + next_state.vy = previous_state.vy + previous_state.ay * dt; + next_state.vz = previous_state.vz + previous_state.az * dt; + + // let's not deal with 3D orientation dynamics for this tutorial + next_state.roll = 0; + next_state.pitch = 0; + next_state.yaw = 0; + next_state.vroll = 0; + next_state.vpitch = 0; + next_state.vyaw = 0; + + // get the current acceleration from the current applied force + next_state.ax = external_force.x() / next_state.mass; + next_state.ay = external_force.y() / next_state.mass; + next_state.az = external_force.z() / next_state.mass; + + return next_state; +} + +/** + * @brief Create a simulated Imu measurement from the current state + */ +sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) +{ + static std::random_device rd{}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> noise{ 0.0, IMU_SIGMA }; + + auto msg = std::make_shared(); + msg->header.stamp = robot.stamp; + msg->header.frame_id = BASELINK_FRAME; + + // measure accel + msg->linear_acceleration.x = robot.ax + noise(generator); + msg->linear_acceleration.y = robot.ay + noise(generator); + msg->linear_acceleration.z = robot.az + noise(generator); + msg->linear_acceleration_covariance[0] = IMU_SIGMA * IMU_SIGMA; + msg->linear_acceleration_covariance[4] = IMU_SIGMA * IMU_SIGMA; + msg->linear_acceleration_covariance[8] = IMU_SIGMA * IMU_SIGMA; + + // Simulated IMU does not provide orientation + msg->orientation_covariance[0] = -1; + msg->orientation_covariance[4] = -1; + msg->orientation_covariance[8] = -1; + + msg->angular_velocity.x = robot.vroll + noise(generator); + msg->angular_velocity.y = robot.vpitch + noise(generator); + msg->angular_velocity.z = robot.vyaw + noise(generator); + msg->angular_velocity_covariance[0] = IMU_SIGMA * IMU_SIGMA; + msg->angular_velocity_covariance[4] = IMU_SIGMA * IMU_SIGMA; + msg->angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA; + return msg; +} + +int main(int argc, char** argv) +{ + double const motion_duration = 5; + double const N_cycles = 2; + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("range_sensor_simulator"); + auto logger = node->get_logger(); + auto clock = node->get_clock(); + + auto latched_qos = rclcpp::QoS(1); + latched_qos.transient_local(); + + auto imu_publisher = node->create_publisher("imu", 1); + auto ground_truth_publisher = node->create_publisher("ground_truth", 1); + + // Initialize the robot state + auto state = Robot(); + state.stamp = node->now(); + state.mass = 10; // kg + + auto rate = rclcpp::Rate(10.0); + + while (rclcpp::ok()) + { + static auto const first_time = node->now(); + // Apply a harmonic force in x, y, and z sequentially to the robot + auto const now = node->now(); + + double now_d = (now - first_time).seconds(); + + double mod_time = std::fmod(now_d, motion_duration); + + // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) + double const force = 100 * std::cos(2 * M_PI * N_cycles * mod_time / motion_duration); + Eigen::Vector3d external_force = { 0, 0, 0 }; + + // switch oscillation axes every `motion_duration` seconds (with one rest period) + if (std::fmod(now_d, 4 * motion_duration) < motion_duration) + { + external_force.x() = force; + } + else if (std::fmod(now_d, 4 * motion_duration) < 2 * motion_duration) + { + external_force.y() = force; + } + else if (std::fmod(now_d, 4 * motion_duration) < 3 * motion_duration) + { + external_force.z() = force; + } + + // Simulate the robot motion + auto new_state = simulateRobotMotion(state, now, external_force); + + // Publish the new ground truth + ground_truth_publisher->publish(*robotToOdometry(new_state)); + + // Generate and publish simulated measurement from the new robot state + imu_publisher->publish(*simulateImu(new_state)); + + // Wait for the next time step + state = new_state; + rclcpp::spin_some(node); + rate.sleep(); + } + + rclcpp::shutdown(); + return 0; +} From 6570a94d2269ca36e2c70932b85ff995848ca2b3 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 25 Sep 2024 00:03:13 +0000 Subject: [PATCH 13/46] wip 3d tutorial --- fuse_tutorials/config/fuse_3d_tutorial.yaml | 11 +-- .../launch/fuse_3d_tutorial.launch.py | 2 +- .../src/three_dimensional_imu_simulator.cpp | 69 ++++++++++++++++++- 3 files changed, 75 insertions(+), 7 deletions(-) diff --git a/fuse_tutorials/config/fuse_3d_tutorial.yaml b/fuse_tutorials/config/fuse_3d_tutorial.yaml index 95a126317..7f12fa75a 100644 --- a/fuse_tutorials/config/fuse_3d_tutorial.yaml +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -25,13 +25,14 @@ state_estimator: initial_localization_sensor: publish_on_startup: true # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc - initial_state: [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] - initial_sigma: [0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000, 0.000] + initial_state: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] imu_sensor: topic: 'imu' - twist_target_frame: 'base_footprint' - angular_velocity_dimensions: ['yaw'] + twist_target_frame: 'base_link' + linear_acceleration_dimensions: ['x', 'y', 'z'] + angular_velocity_dimensions: ['roll', 'pitch', 'yaw'] publishers: filtered_publisher: @@ -39,7 +40,7 @@ state_estimator: filtered_publisher: topic: 'odom_filtered' - base_link_frame_id: 'base_footprint' + base_link_frame_id: 'base_link' odom_frame_id: 'odom' map_frame_id: 'map' world_frame_id: 'odom' diff --git a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py index 440991b5a..a7b678255 100644 --- a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py @@ -33,7 +33,6 @@ def generate_launch_description(): output="screen", # prefix=["gdbserver localhost:3000"], ), - SetParameter(name="use_sim_time", value=True), Node( package="fuse_optimizers", executable="fixed_lag_smoother_node", @@ -41,6 +40,7 @@ def generate_launch_description(): parameters=[ PathJoinSubstitution([pkg_dir, "config", "fuse_3d_tutorial.yaml"]) ], + # prefix=["gdbserver localhost:3000"], ), Node( package="rviz2", diff --git a/fuse_tutorials/src/three_dimensional_imu_simulator.cpp b/fuse_tutorials/src/three_dimensional_imu_simulator.cpp index 34ea52067..754e5e35e 100644 --- a/fuse_tutorials/src/three_dimensional_imu_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_imu_simulator.cpp @@ -32,7 +32,10 @@ * POSSIBILITY OF SUCH DAMAGE. */ #include +#include #include +#include +#include #include #include @@ -185,9 +188,71 @@ sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) msg->angular_velocity_covariance[0] = IMU_SIGMA * IMU_SIGMA; msg->angular_velocity_covariance[4] = IMU_SIGMA * IMU_SIGMA; msg->angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA; + + Eigen::Matrix3d acov; + Eigen::Matrix3d cov; + for (std::size_t i = 0; i < 9; ++i) + { + acov(i) = msg->angular_velocity_covariance[i]; + cov(i) = msg->linear_acceleration_covariance[i]; + } + if (!fuse_core::isSymmetric(cov) || !fuse_core::isSymmetric(acov)) + { + std::cout << "Help" << std::endl; + } return msg; } +void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces interfaces, + const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger) +{ + // Send the initial localization signal to the state estimator + auto srv = std::make_shared(); + srv->pose.header.frame_id = MAP_FRAME; + srv->pose.pose.pose.position.x = state.x; + srv->pose.pose.pose.position.y = state.y; + srv->pose.pose.pose.position.z = state.z; + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + srv->pose.pose.pose.orientation.w = q.w(); + srv->pose.pose.pose.orientation.x = q.x(); + srv->pose.pose.pose.orientation.y = q.y(); + srv->pose.pose.pose.orientation.z = q.z(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 1.0; + srv->pose.pose.covariance[14] = 1.0; + srv->pose.pose.covariance[21] = 1.0; + srv->pose.pose.covariance[28] = 1.0; + srv->pose.pose.covariance[35] = 1.0; + + auto client = rclcpp::create_client( + interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), + interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", rclcpp::ServicesQoS()); + + while (!client->wait_for_service(std::chrono::seconds(30)) && + interfaces.get_node_base_interface()->get_context()->is_valid()) + { + RCLCPP_WARN_STREAM(logger, "Waiting for '" << client->get_service_name() << "' service to become available."); + } + + auto success = false; + while (!success) + { + clock->sleep_for(std::chrono::milliseconds(100)); + srv->pose.header.stamp = clock->now(); + auto result_future = client->async_send_request(srv); + + if (rclcpp::spin_until_future_complete(interfaces.get_node_base_interface(), result_future, + std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(logger, "service call failed :("); + client->remove_pending_request(result_future); + return; + } + success = result_future.get()->success; + } +} + int main(int argc, char** argv) { double const motion_duration = 5; @@ -208,7 +273,9 @@ int main(int argc, char** argv) state.stamp = node->now(); state.mass = 10; // kg - auto rate = rclcpp::Rate(10.0); + auto rate = rclcpp::Rate(100.0); + + // initializeStateEstimation(*node, state, clock, logger); while (rclcpp::ok()) { From bcc20ad82991acbd032158d246f1b66ea977329c Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 25 Sep 2024 23:49:46 +0000 Subject: [PATCH 14/46] fix typos --- fuse_models/include/fuse_models/omnidirectional_3d.hpp | 2 +- .../include/fuse_models/omnidirectional_3d_ignition.hpp | 4 ++-- fuse_tutorials/config/fuse_3d_tutorial.yaml | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp index e1764b58d..4706d2985 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -55,7 +55,7 @@ namespace fuse_models * @brief A fuse_models 3D kinematic model that generates kinematic constraints between provided * time stamps, and adds those constraints to the fuse graph. * - * This class uses a unicycle kinematic model for the robot. It is equivalent to the motion model + * This class uses an omnidirectional kinematic model for the robot. It is equivalent to the motion model * in the robot_localization state estimation nodes. * * Parameters: diff --git a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp index a9757f1cb..93620c865 100644 --- a/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -57,7 +57,7 @@ namespace fuse_models * motion model. * * This class publishes a transaction that contains a prior on each state subvariable used in the - * Omnidirectional 3D motion model (x, y, z, qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * Omnidirectional 3D motion model (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). When the sensor is first loaded, it publishes a single transaction * with the configured initial state and covariance. * Additionally, whenever a pose is received, either on the set_pose service or the topic, this @@ -77,7 +77,7 @@ namespace fuse_models * yaw_vel, x_acc, y_acc, z_acc). * - ~initial_state (vector of doubles) A 15-dimensional vector containing the initial values for * the state. Variable order is (x, y, z, - * qx, qy, qz, x_vel, y_vel, z_vel, roll_vel, pitch_vel, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, * yaw_vel, x_acc, y_acc, z_acc). * - ~queue_size (int, default: 10) The subscriber queue size for the pose messages * - ~reset_service (string, default: "~/reset") The name of the reset service to call before diff --git a/fuse_tutorials/config/fuse_3d_tutorial.yaml b/fuse_tutorials/config/fuse_3d_tutorial.yaml index 7f12fa75a..213a105b0 100644 --- a/fuse_tutorials/config/fuse_3d_tutorial.yaml +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -25,7 +25,7 @@ state_estimator: initial_localization_sensor: publish_on_startup: true # x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc - initial_state: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] imu_sensor: From 8df2fc81510c7cc0a36938fec8eac3339474ca27 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 26 Sep 2024 03:04:18 +0000 Subject: [PATCH 15/46] fix 3d tutorial --- .../include/fuse_models/common/sensor_proc.hpp | 3 ++- fuse_tutorials/config/fuse_3d_tutorial.yaml | 2 +- .../src/three_dimensional_imu_simulator.cpp | 12 ------------ 3 files changed, 3 insertions(+), 14 deletions(-) diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 7e051cb58..2e0c6d71d 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -2000,7 +2000,8 @@ inline bool processAccel3DWithCovariance(const std::string& source, const fuse_c accel_mean << transformed_message.accel.accel.linear.x, transformed_message.accel.accel.linear.y, transformed_message.accel.accel.linear.z; - Eigen::Map accel_covariance_map(transformed_message.accel.covariance.data()); + Eigen::Map> accel_covariance_map( + transformed_message.accel.covariance.data()); // Build the sub-vector and sub-matrices based on the requested indices fuse_core::VectorXd accel_mean_partial(indices.size()); diff --git a/fuse_tutorials/config/fuse_3d_tutorial.yaml b/fuse_tutorials/config/fuse_3d_tutorial.yaml index 213a105b0..67352f026 100644 --- a/fuse_tutorials/config/fuse_3d_tutorial.yaml +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -30,7 +30,7 @@ state_estimator: imu_sensor: topic: 'imu' - twist_target_frame: 'base_link' + acceleration_target_frame: 'base_link' linear_acceleration_dimensions: ['x', 'y', 'z'] angular_velocity_dimensions: ['roll', 'pitch', 'yaw'] diff --git a/fuse_tutorials/src/three_dimensional_imu_simulator.cpp b/fuse_tutorials/src/three_dimensional_imu_simulator.cpp index 754e5e35e..f14a6d0a9 100644 --- a/fuse_tutorials/src/three_dimensional_imu_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_imu_simulator.cpp @@ -188,18 +188,6 @@ sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) msg->angular_velocity_covariance[0] = IMU_SIGMA * IMU_SIGMA; msg->angular_velocity_covariance[4] = IMU_SIGMA * IMU_SIGMA; msg->angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA; - - Eigen::Matrix3d acov; - Eigen::Matrix3d cov; - for (std::size_t i = 0; i < 9; ++i) - { - acov(i) = msg->angular_velocity_covariance[i]; - cov(i) = msg->linear_acceleration_covariance[i]; - } - if (!fuse_core::isSymmetric(cov) || !fuse_core::isSymmetric(acov)) - { - std::cout << "Help" << std::endl; - } return msg; } From 2c5ee140f04acbb25b9b2c258347311cd5ffb223 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 26 Sep 2024 19:15:15 +0000 Subject: [PATCH 16/46] finish tutorial, add comments, fix visualization --- .../include/fuse_models/odometry_3d.hpp | 1 + fuse_tutorials/CMakeLists.txt | 7 +- fuse_tutorials/config/fuse_3d_tutorial.rviz | 82 +++++++++-------- fuse_tutorials/config/fuse_3d_tutorial.yaml | 15 +++- .../launch/fuse_3d_tutorial.launch.py | 32 ++++--- ...or.cpp => three_dimensional_simulator.cpp} | 89 +++++++++++++++---- 6 files changed, 154 insertions(+), 72 deletions(-) rename fuse_tutorials/src/{three_dimensional_imu_simulator.cpp => three_dimensional_simulator.cpp} (74%) diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp index 0ba4df86c..6a8f6e552 100644 --- a/fuse_models/include/fuse_models/odometry_3d.hpp +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -47,6 +47,7 @@ #include #include +#include #include #include diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index 2b0c6a183..83387db16 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -45,9 +45,8 @@ target_link_libraries( add_executable(range_sensor_simulator src/range_sensor_simulator.cpp) target_link_libraries(range_sensor_simulator ${PROJECT_NAME}) -add_executable(three_dimensional_IMU_simulator - src/three_dimensional_imu_simulator.cpp) -target_link_libraries(three_dimensional_IMU_simulator ${PROJECT_NAME}) +add_executable(three_dimensional_simulator src/three_dimensional_simulator.cpp) +target_link_libraries(three_dimensional_simulator ${PROJECT_NAME}) # ############################################################################## # Install ## @@ -63,7 +62,7 @@ install( install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(TARGETS range_sensor_simulator DESTINATION lib/${PROJECT_NAME}) -install(TARGETS three_dimensional_IMU_simulator DESTINATION lib/${PROJECT_NAME}) +install(TARGETS three_dimensional_simulator DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY launch config data DESTINATION share/${PROJECT_NAME}) diff --git a/fuse_tutorials/config/fuse_3d_tutorial.rviz b/fuse_tutorials/config/fuse_3d_tutorial.rviz index 764a4aeb0..863814ccc 100644 --- a/fuse_tutorials/config/fuse_3d_tutorial.rviz +++ b/fuse_tutorials/config/fuse_3d_tutorial.rviz @@ -4,9 +4,12 @@ Panels: Name: Displays Property Tree Widget: Expanded: - - /Fused Odom1/Shape1 + - /Global Options1 + - /Status1 + - /Odometry1/Shape1 + - /Odometry2/Shape1 Splitter Ratio: 0.5 - Tree Height: 719 + Tree Height: 555 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -43,10 +46,10 @@ Visualization Manager: Y: 0 Z: 0 Plane: XY - Plane Cell Count: 100 + Plane Cell Count: 10 Reference Frame: Value: true - - Angle Tolerance: 0.05000000074505806 + - Angle Tolerance: 0.10000000149011612 Class: rviz_default_plugins/Odometry Covariance: Orientation: @@ -64,18 +67,18 @@ Visualization Manager: Value: true Value: false Enabled: true - Keep: 10000 - Name: Fused Odom + Keep: 1 + Name: Odometry Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 - Color: 26; 95; 180 - Head Length: 0.07500000298023224 - Head Radius: 0.02500000037252903 - Shaft Length: 0.25 - Shaft Radius: 0.012500000186264515 + Color: 255; 0; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 Value: Arrow Topic: Depth: 5 @@ -83,9 +86,9 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /odom_filtered + Value: /ground_truth Value: true - - Angle Tolerance: 0.05000000074505806 + - Angle Tolerance: 0.10000000149011612 Class: rviz_default_plugins/Odometry Covariance: Orientation: @@ -101,20 +104,20 @@ Visualization Manager: Color: 204; 51; 204 Scale: 1 Value: true - Value: false + Value: true Enabled: true - Keep: 10000 - Name: Odom + Keep: 1 + Name: Odometry Position Tolerance: 0.10000000149011612 Shape: Alpha: 1 Axes Length: 1 Axes Radius: 0.10000000149011612 - Color: 255; 25; 0 - Head Length: 0.07500000298023224 - Head Radius: 0.02500000037252903 - Shaft Length: 0.25 - Shaft Radius: 0.012500000186264515 + Color: 0; 255; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 Value: Arrow Topic: Depth: 5 @@ -122,13 +125,13 @@ Visualization Manager: Filter size: 10 History Policy: Keep Last Reliability Policy: Reliable - Value: /odom + Value: /odom_filtered Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: odom - Frame Rate: 24 + Fixed Frame: map + Frame Rate: 30 Name: root Tools: - Class: rviz_default_plugins/Interact @@ -169,29 +172,34 @@ Visualization Manager: Value: true Views: Current: - Angle: 0 - Class: rviz_default_plugins/TopDownOrtho + Class: rviz_default_plugins/Orbit + Distance: 10.72309398651123 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Scale: 168.0489959716797 + Pitch: 0.785398006439209 Target Frame: - Value: TopDownOrtho (rviz_default_plugins) - X: 2 - Y: 0 + Value: Orbit (rviz) + Yaw: 0.785398006439209 Saved: ~ Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 846 Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd0000000400000000000001560000035afc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d0000035a000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000035afc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d0000035a000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007360000003efc0100000002fb0000000800540069006d0065010000000000000736000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005da0000035a00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -199,7 +207,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true - Width: 1846 - X: 74 - Y: 27 + collapsed: false + Width: 1200 + X: 60 + Y: 60 diff --git a/fuse_tutorials/config/fuse_3d_tutorial.yaml b/fuse_tutorials/config/fuse_3d_tutorial.yaml index 67352f026..410682a53 100644 --- a/fuse_tutorials/config/fuse_3d_tutorial.yaml +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -5,6 +5,7 @@ state_estimator: transaction_timeout: 0.01 lag_duration: 0.5 + # all our sensors will be using this motion model motion_models: 3d_motion_model: type: fuse_models::Omnidirectional3D @@ -18,6 +19,9 @@ state_estimator: type: fuse_models::Omnidirectional3DIgnition motion_models: [3d_motion_model] ignition: true + odometry_sensor: + type: fuse_models::Odometry3D + motion_models: [3d_motion_model] imu_sensor: type: fuse_models::Imu3D motion_models: [3d_motion_model] @@ -28,16 +32,25 @@ state_estimator: initial_state: [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] initial_sigma: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] + odometry_sensor: + topic: 'odom' + twist_target_frame: 'base_link' + # we only want position and orientation, but you can get a full odometry measurement from this sensor + position_dimensions: ['x', 'y', 'z'] + orientation_dimensions: ['roll', 'pitch', 'yaw'] + imu_sensor: topic: 'imu' acceleration_target_frame: 'base_link' + # we only care about linear acceleration for this tutorial linear_acceleration_dimensions: ['x', 'y', 'z'] - angular_velocity_dimensions: ['roll', 'pitch', 'yaw'] + # this publishes our estimated odometry publishers: filtered_publisher: type: fuse_models::Odometry3DPublisher + # the configuration of our filtered_publisher: topic: 'odom_filtered' base_link_frame_id: 'base_link' diff --git a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py index a7b678255..3864bde7f 100644 --- a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py @@ -26,13 +26,21 @@ def generate_launch_description(): return LaunchDescription( [ + # tell tf2 that map is the same as odom + # without this, visualization won't work as we have no reference + Node( + package="tf2_ros", + executable="static_transform_publisher", + arguments=["0", "0", "0", "0", "0", "0", "map", "odom"], + ), + # run our simulator Node( package="fuse_tutorials", - executable="three_dimensional_IMU_simulator", - name="three_dimensional_IMU_simulator", + executable="three_dimensional_simulator", + name="three_dimensional_simulator", output="screen", - # prefix=["gdbserver localhost:3000"], ), + # run our estimator Node( package="fuse_optimizers", executable="fixed_lag_smoother_node", @@ -40,20 +48,20 @@ def generate_launch_description(): parameters=[ PathJoinSubstitution([pkg_dir, "config", "fuse_3d_tutorial.yaml"]) ], - # prefix=["gdbserver localhost:3000"], ), + # run visualization Node( package="rviz2", executable="rviz2", name="rviz", - # arguments=[ - # "-d", - # [ - # PathJoinSubstitution( - # [pkg_dir, "config", "fuse_3d_tutorial.rviz"] - # ) - # ], - # ], + arguments=[ + "-d", + [ + PathJoinSubstitution( + [pkg_dir, "config", "fuse_3d_tutorial.rviz"] + ) + ], + ], ), ] ) diff --git a/fuse_tutorials/src/three_dimensional_imu_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp similarity index 74% rename from fuse_tutorials/src/three_dimensional_imu_simulator.cpp rename to fuse_tutorials/src/three_dimensional_simulator.cpp index f14a6d0a9..d15b0a890 100644 --- a/fuse_tutorials/src/three_dimensional_imu_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -53,6 +53,9 @@ static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame static constexpr char MAP_FRAME[] = "map"; //!< The map frame id used when publishing ground truth //!< data static constexpr double IMU_SIGMA = 0.1; //!< Std dev of simulated Imu measurement noise +static constexpr char ODOM_FRAME[] = "odom"; //!< The odom frame id used when publishing wheel +static constexpr double ODOM_POSITION_SIGMA = 0.5; //!< Std dev of simulated odom position measurement noise +static constexpr double ODOM_ORIENTATION_SIGMA = 0.1; //!< Std dev of simulated odom orientation measurement noise /** * @brief The true pose and velocity of the robot @@ -191,6 +194,38 @@ sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) return msg; } +nav_msgs::msg::Odometry::SharedPtr simulateOdometry(const Robot& robot) +{ + static std::random_device rd{}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> position_noise{ 0.0, ODOM_POSITION_SIGMA }; + + auto msg = std::make_shared(); + msg->header.stamp = robot.stamp; + msg->header.frame_id = ODOM_FRAME; + msg->child_frame_id = BASELINK_FRAME; + + // noisy position measurement + msg->pose.pose.position.x = robot.x + position_noise(generator); + msg->pose.pose.position.y = robot.y + position_noise(generator); + msg->pose.pose.position.z = robot.z + position_noise(generator); + msg->pose.covariance[0] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg->pose.covariance[7] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg->pose.covariance[14] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + + // noisy orientation measurement + tf2::Quaternion q; + q.setEuler(robot.yaw, robot.pitch, robot.roll); + msg->pose.pose.orientation.w = q.w(); + msg->pose.pose.orientation.x = q.x(); + msg->pose.pose.orientation.y = q.y(); + msg->pose.pose.orientation.z = q.z(); + msg->pose.covariance[21] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg->pose.covariance[28] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg->pose.covariance[35] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + return msg; +} + void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfaces interfaces, const Robot& state, const rclcpp::Clock::SharedPtr& clock, const rclcpp::Logger& logger) { @@ -243,54 +278,71 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfacesget_logger(); - auto clock = node->get_clock(); - - auto latched_qos = rclcpp::QoS(1); - latched_qos.transient_local(); + auto node = rclcpp::Node::make_shared("three_dimensional_simulator"); + // create our sensor publishers auto imu_publisher = node->create_publisher("imu", 1); + auto odom_publisher = node->create_publisher("odom", 1); + + // create the ground truth publisher auto ground_truth_publisher = node->create_publisher("ground_truth", 1); - // Initialize the robot state + // Initialize the robot state (state variables are zero-initialized) auto state = Robot(); state.stamp = node->now(); state.mass = 10; // kg + // you can modify the rate at which this loop runs to see the different performance of the estimator and the effect of + // integration inaccuracy on the ground truth auto rate = rclcpp::Rate(100.0); - // initializeStateEstimation(*node, state, clock, logger); + // normally we would have to initialize the state estimation, but we included an ignition 'sensor' in our config, + // which takes care of that. + + // parameters that control the motion pattern of the robot + double const motion_duration = 5; // length of time to oscillate on a given axis, in seconds + double const N_cycles = 2; // number of oscillations per `motion_duration` while (rclcpp::ok()) { + // store the first time this runs (since it won't start running exactly at a multiple of `motion_duration`) static auto const first_time = node->now(); - // Apply a harmonic force in x, y, and z sequentially to the robot auto const now = node->now(); + // compensate for the original time offset double now_d = (now - first_time).seconds(); - + // store how long it has been (resetting at `motion_duration` seconds) double mod_time = std::fmod(now_d, motion_duration); // apply a harmonic force (oscillates `N_cycles` times per `motion_duration`) - double const force = 100 * std::cos(2 * M_PI * N_cycles * mod_time / motion_duration); + double const force_magnitude = 100 * std::cos(2 * M_PI * N_cycles * mod_time / motion_duration); Eigen::Vector3d external_force = { 0, 0, 0 }; - // switch oscillation axes every `motion_duration` seconds (with one rest period) + // switch oscillation axes every `motion_duration` seconds (with one 'rest period') if (std::fmod(now_d, 4 * motion_duration) < motion_duration) { - external_force.x() = force; + external_force.x() = force_magnitude; } else if (std::fmod(now_d, 4 * motion_duration) < 2 * motion_duration) { - external_force.y() = force; + external_force.y() = force_magnitude; } else if (std::fmod(now_d, 4 * motion_duration) < 3 * motion_duration) { - external_force.z() = force; + external_force.z() = force_magnitude; + } + else + { + // reset the robot's position and velocity, leave the external force as 0 + // we do this so the ground truth doesn't drift (due to inaccuracy from euler integration) + state.x = 0; + state.y = 0; + state.z = 0; + state.vx = 0; + state.vy = 0; + state.vz = 0; } // Simulate the robot motion @@ -299,8 +351,9 @@ int main(int argc, char** argv) // Publish the new ground truth ground_truth_publisher->publish(*robotToOdometry(new_state)); - // Generate and publish simulated measurement from the new robot state + // Generate and publish simulated measurements from the new robot state imu_publisher->publish(*simulateImu(new_state)); + odom_publisher->publish(*simulateOdometry(new_state)); // Wait for the next time step state = new_state; From 1a7ef62d14683fa79da3d9e3a9f7f5086f5e135b Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 27 Sep 2024 20:27:12 +0000 Subject: [PATCH 17/46] fix incomplete comment --- fuse_tutorials/config/fuse_3d_tutorial.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_tutorials/config/fuse_3d_tutorial.yaml b/fuse_tutorials/config/fuse_3d_tutorial.yaml index 410682a53..59073b3bd 100644 --- a/fuse_tutorials/config/fuse_3d_tutorial.yaml +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -50,7 +50,7 @@ state_estimator: filtered_publisher: type: fuse_models::Odometry3DPublisher - # the configuration of our + # the configuration of our output publisher filtered_publisher: topic: 'odom_filtered' base_link_frame_id: 'base_link' From 85d4cfa7ed39094b6c3068ed3569a38e36127944 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 27 Sep 2024 20:52:22 +0000 Subject: [PATCH 18/46] make launch file executable --- fuse_tutorials/launch/fuse_3d_tutorial.launch.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 fuse_tutorials/launch/fuse_3d_tutorial.launch.py diff --git a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py old mode 100644 new mode 100755 From b4e76b69f6597d7b28d3ea8b9b86ae94d3b5f528 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 27 Sep 2024 22:30:15 +0000 Subject: [PATCH 19/46] change clang tidy conf and remove jenkinsfile --- .clang-tidy | 47 +++++++++++++++++++++++++---------------------- Jenkinsfile | 20 -------------------- 2 files changed, 25 insertions(+), 42 deletions(-) delete mode 100644 Jenkinsfile diff --git a/.clang-tidy b/.clang-tidy index 693d15f05..9f4f1a806 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,26 +1,29 @@ ---- -# This was copied over from the moveit_studio repository. -# -# TODO(henningkayser): Re-enable performance-unnecessary-value-param once #214 is resolved -Checks: '-*, - performance-*, - -performance-unnecessary-value-param, - llvm-namespace-comment, - modernize-redundant-void-arg, - modernize-use-nullptr, - modernize-use-default, - modernize-use-override, - modernize-loop-convert, - readability-braces-around-statements, - readability-named-parameter, - readability-redundant-smartptr-get, - readability-redundant-string-cstr, - readability-simplify-boolean-expr, - readability-container-size-empty, - readability-identifier-naming, - ' +Checks: -*, + bugprone-*, + clang-analyzer-*, + concurrency-*, + cppcoreguidelines-*, + google-*, + misc-*, + modernize-*, + performance-*, + portability-*, + readability-*, + -bugprone-easily-swappable-parameters, + -bugprone-exception-escape, + -cppcoreguidelines-avoid-magic-numbers, + -google-readability-casting, + -misc-non-private-member-variables-in-classes, + -misc-use-anonymous-namespace, + -modernize-use-trailing-return-type, + -readability-identifier-length, + -readability-function-cognitive-complexity, + -readability-magic-numbers, + -readability-uppercase-literal-suffix, + -cppcoreguidelines-pro-type-union-access, + -cppcoreguidelines-non-private-member-variables-in-classes, + -cppcoreguidelines-pro-type-vararg, HeaderFilterRegex: '' -AnalyzeTemporaryDtors: false CheckOptions: - key: llvm-namespace-comment.ShortNamespaceLines value: '10' diff --git a/Jenkinsfile b/Jenkinsfile deleted file mode 100644 index 06c18cd4a..000000000 --- a/Jenkinsfile +++ /dev/null @@ -1,20 +0,0 @@ -#!/usr/bin/env groovy -@Library('tailor-meta@0.1.24')_ -tailorTestPipeline( - // Name of job that generated this test definition. - rosdistro_job: '/ci/rosdistro/master', - // Distribution name - rosdistro_name: 'ros2', - // Release track to test branch against. - release_track: 'hotdog', - // Release label to pull test images from. - release_label: 'hotdog', - // OS distributions to test. - distributions: ['jammy'], - // Version of tailor_meta to build against - tailor_meta: '0.1.24', - // Master or release branch associated with this track - source_branch: 'rolling', - // Docker registry where test image is stored - docker_registry: 'https://084758475884.dkr.ecr.us-east-1.amazonaws.com/locus-tailor' -) From a98ef09e9bc0baf4f2d67a62f0f737f1d2fd07d5 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 2 Oct 2024 18:21:30 +0000 Subject: [PATCH 20/46] fix mistake in quat util --- fuse_core/include/fuse_core/util.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index f6924a824..14f255ec0 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -317,9 +317,9 @@ static inline void quaternionToAngleAxis(const double* q, double* angle_axis, do else { jacobian_map.setZero(); + jacobian_map(0, 0) = 2.0; jacobian_map(1, 1) = 2.0; jacobian_map(2, 2) = 2.0; - jacobian_map(3, 3) = 2.0; } } } From ea4e374c606e6e618eb32137d30891d40ca973bb Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 2 Oct 2024 12:30:10 -0600 Subject: [PATCH 21/46] Update fuse_tutorials/src/three_dimensional_simulator.cpp Co-authored-by: Bilal Gill --- fuse_tutorials/src/three_dimensional_simulator.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/fuse_tutorials/src/three_dimensional_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp index d15b0a890..7629fcfc8 100644 --- a/fuse_tutorials/src/three_dimensional_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -130,7 +130,7 @@ nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot& state) */ Robot simulateRobotMotion(const Robot& previous_state, const rclcpp::Time& now, Eigen::Vector3d external_force) { - auto dt = (now - previous_state.stamp).seconds(); + const auto dt = (now - previous_state.stamp).seconds(); auto next_state = Robot(); next_state.stamp = now; next_state.mass = previous_state.mass; From f0f0cc6de0eb9231f5e06ada397320c9befb68fc Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 2 Oct 2024 18:39:53 +0000 Subject: [PATCH 22/46] apply PR feedback --- fuse_tutorials/config/fuse_3d_tutorial.yaml | 1 + .../launch/fuse_3d_tutorial.launch.py | 2 +- .../src/three_dimensional_simulator.cpp | 159 +++++++++--------- 3 files changed, 83 insertions(+), 79 deletions(-) diff --git a/fuse_tutorials/config/fuse_3d_tutorial.yaml b/fuse_tutorials/config/fuse_3d_tutorial.yaml index 59073b3bd..1e95b1740 100644 --- a/fuse_tutorials/config/fuse_3d_tutorial.yaml +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -1,3 +1,4 @@ +# this yaml file is adapted from `fuse_simple_tutorial.yaml` state_estimator: ros__parameters: # Fixed-lag smoother configuration diff --git a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py index 3864bde7f..f57d66dc1 100755 --- a/fuse_tutorials/launch/fuse_3d_tutorial.launch.py +++ b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py @@ -1,6 +1,6 @@ #! /usr/bin/env python3 -# Copyright 2022 Open Source Robotics Foundation, Inc. +# Copyright 2024 PickNik Robotics # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. diff --git a/fuse_tutorials/src/three_dimensional_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp index 7629fcfc8..6bfb873fa 100644 --- a/fuse_tutorials/src/three_dimensional_simulator.cpp +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2021, Locus Robotics + * Copyright (c) 2024, PickNik Robotics * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -48,6 +48,8 @@ #include #include +namespace +{ static constexpr char BASELINK_FRAME[] = "base_link"; //!< The base_link frame id used when //!< publishing sensor data static constexpr char MAP_FRAME[] = "map"; //!< The map frame id used when publishing ground truth @@ -56,6 +58,7 @@ static constexpr double IMU_SIGMA = 0.1; //!< Std dev of simulated static constexpr char ODOM_FRAME[] = "odom"; //!< The odom frame id used when publishing wheel static constexpr double ODOM_POSITION_SIGMA = 0.5; //!< Std dev of simulated odom position measurement noise static constexpr double ODOM_ORIENTATION_SIGMA = 0.1; //!< Std dev of simulated odom orientation measurement noise +} // namespace /** * @brief The true pose and velocity of the robot @@ -86,51 +89,51 @@ struct Robot /** * @brief Convert the robot state into a ground truth odometry message */ -nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot& state) +nav_msgs::msg::Odometry robotToOdometry(Robot const& state) { - auto msg = std::make_shared(); - msg->header.stamp = state.stamp; - msg->header.frame_id = MAP_FRAME; - msg->child_frame_id = BASELINK_FRAME; - msg->pose.pose.position.x = state.x; - msg->pose.pose.position.y = state.y; - msg->pose.pose.position.z = state.z; + nav_msgs::msg::Odometry msg; + msg.header.stamp = state.stamp; + msg.header.frame_id = MAP_FRAME; + msg.child_frame_id = BASELINK_FRAME; + msg.pose.pose.position.x = state.x; + msg.pose.pose.position.y = state.y; + msg.pose.pose.position.z = state.z; tf2::Quaternion q; q.setEuler(state.yaw, state.pitch, state.roll); - msg->pose.pose.orientation.w = q.w(); - msg->pose.pose.orientation.x = q.x(); - msg->pose.pose.orientation.y = q.y(); - msg->pose.pose.orientation.z = q.z(); - msg->twist.twist.linear.x = state.vx; - msg->twist.twist.linear.y = state.vy; - msg->twist.twist.linear.z = state.vz; - msg->twist.twist.angular.x = state.vroll; - msg->twist.twist.angular.y = state.vpitch; - msg->twist.twist.angular.z = state.vyaw; + msg.pose.pose.orientation.w = q.w(); + msg.pose.pose.orientation.x = q.x(); + msg.pose.pose.orientation.y = q.y(); + msg.pose.pose.orientation.z = q.z(); + msg.twist.twist.linear.x = state.vx; + msg.twist.twist.linear.y = state.vy; + msg.twist.twist.linear.z = state.vz; + msg.twist.twist.angular.x = state.vroll; + msg.twist.twist.angular.y = state.vpitch; + msg.twist.twist.angular.z = state.vyaw; // set covariances - msg->pose.covariance[0] = 0.1; - msg->pose.covariance[7] = 0.1; - msg->pose.covariance[14] = 0.1; - msg->pose.covariance[21] = 0.1; - msg->pose.covariance[28] = 0.1; - msg->pose.covariance[35] = 0.1; - msg->twist.covariance[0] = 0.1; - msg->twist.covariance[7] = 0.1; - msg->twist.covariance[14] = 0.1; - msg->twist.covariance[21] = 0.1; - msg->twist.covariance[28] = 0.1; - msg->twist.covariance[35] = 0.1; + msg.pose.covariance[0] = 0.1; + msg.pose.covariance[7] = 0.1; + msg.pose.covariance[14] = 0.1; + msg.pose.covariance[21] = 0.1; + msg.pose.covariance[28] = 0.1; + msg.pose.covariance[35] = 0.1; + msg.twist.covariance[0] = 0.1; + msg.twist.covariance[7] = 0.1; + msg.twist.covariance[14] = 0.1; + msg.twist.covariance[21] = 0.1; + msg.twist.covariance[28] = 0.1; + msg.twist.covariance[35] = 0.1; return msg; } /** * @brief Compute the next robot state given the current robot state and a simulated step time */ -Robot simulateRobotMotion(const Robot& previous_state, const rclcpp::Time& now, Eigen::Vector3d external_force) +Robot simulateRobotMotion(Robot const& previous_state, rclcpp::Time const& now, Eigen::Vector3d const& external_force) { - const auto dt = (now - previous_state.stamp).seconds(); + auto const dt = (now - previous_state.stamp).seconds(); auto next_state = Robot(); next_state.stamp = now; next_state.mass = previous_state.mass; @@ -162,67 +165,67 @@ Robot simulateRobotMotion(const Robot& previous_state, const rclcpp::Time& now, /** * @brief Create a simulated Imu measurement from the current state */ -sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot& robot) +sensor_msgs::msg::Imu simulateImu(Robot const& robot) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; static std::normal_distribution<> noise{ 0.0, IMU_SIGMA }; - auto msg = std::make_shared(); - msg->header.stamp = robot.stamp; - msg->header.frame_id = BASELINK_FRAME; + sensor_msgs::msg::Imu msg; + msg.header.stamp = robot.stamp; + msg.header.frame_id = BASELINK_FRAME; // measure accel - msg->linear_acceleration.x = robot.ax + noise(generator); - msg->linear_acceleration.y = robot.ay + noise(generator); - msg->linear_acceleration.z = robot.az + noise(generator); - msg->linear_acceleration_covariance[0] = IMU_SIGMA * IMU_SIGMA; - msg->linear_acceleration_covariance[4] = IMU_SIGMA * IMU_SIGMA; - msg->linear_acceleration_covariance[8] = IMU_SIGMA * IMU_SIGMA; - - // Simulated IMU does not provide orientation - msg->orientation_covariance[0] = -1; - msg->orientation_covariance[4] = -1; - msg->orientation_covariance[8] = -1; - - msg->angular_velocity.x = robot.vroll + noise(generator); - msg->angular_velocity.y = robot.vpitch + noise(generator); - msg->angular_velocity.z = robot.vyaw + noise(generator); - msg->angular_velocity_covariance[0] = IMU_SIGMA * IMU_SIGMA; - msg->angular_velocity_covariance[4] = IMU_SIGMA * IMU_SIGMA; - msg->angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA; + msg.linear_acceleration.x = robot.ax + noise(generator); + msg.linear_acceleration.y = robot.ay + noise(generator); + msg.linear_acceleration.z = robot.az + noise(generator); + msg.linear_acceleration_covariance[0] = IMU_SIGMA * IMU_SIGMA; + msg.linear_acceleration_covariance[4] = IMU_SIGMA * IMU_SIGMA; + msg.linear_acceleration_covariance[8] = IMU_SIGMA * IMU_SIGMA; + + // Simulated IMU does not provide orientation (negative covariance indicates this) + msg.orientation_covariance[0] = -1; + msg.orientation_covariance[4] = -1; + msg.orientation_covariance[8] = -1; + + msg.angular_velocity.x = robot.vroll + noise(generator); + msg.angular_velocity.y = robot.vpitch + noise(generator); + msg.angular_velocity.z = robot.vyaw + noise(generator); + msg.angular_velocity_covariance[0] = IMU_SIGMA * IMU_SIGMA; + msg.angular_velocity_covariance[4] = IMU_SIGMA * IMU_SIGMA; + msg.angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA; return msg; } -nav_msgs::msg::Odometry::SharedPtr simulateOdometry(const Robot& robot) +nav_msgs::msg::Odometry simulateOdometry(const Robot& robot) { static std::random_device rd{}; static std::mt19937 generator{ rd() }; static std::normal_distribution<> position_noise{ 0.0, ODOM_POSITION_SIGMA }; - auto msg = std::make_shared(); - msg->header.stamp = robot.stamp; - msg->header.frame_id = ODOM_FRAME; - msg->child_frame_id = BASELINK_FRAME; + nav_msgs::msg::Odometry msg; + msg.header.stamp = robot.stamp; + msg.header.frame_id = ODOM_FRAME; + msg.child_frame_id = BASELINK_FRAME; // noisy position measurement - msg->pose.pose.position.x = robot.x + position_noise(generator); - msg->pose.pose.position.y = robot.y + position_noise(generator); - msg->pose.pose.position.z = robot.z + position_noise(generator); - msg->pose.covariance[0] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; - msg->pose.covariance[7] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; - msg->pose.covariance[14] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg.pose.pose.position.x = robot.x + position_noise(generator); + msg.pose.pose.position.y = robot.y + position_noise(generator); + msg.pose.pose.position.z = robot.z + position_noise(generator); + msg.pose.covariance[0] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg.pose.covariance[7] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg.pose.covariance[14] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; // noisy orientation measurement tf2::Quaternion q; q.setEuler(robot.yaw, robot.pitch, robot.roll); - msg->pose.pose.orientation.w = q.w(); - msg->pose.pose.orientation.x = q.x(); - msg->pose.pose.orientation.y = q.y(); - msg->pose.pose.orientation.z = q.z(); - msg->pose.covariance[21] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; - msg->pose.covariance[28] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; - msg->pose.covariance[35] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg.pose.pose.orientation.w = q.w(); + msg.pose.pose.orientation.x = q.x(); + msg.pose.pose.orientation.y = q.y(); + msg.pose.pose.orientation.z = q.z(); + msg.pose.covariance[21] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg.pose.covariance[28] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg.pose.covariance[35] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; return msg; } @@ -248,7 +251,7 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfacespose.pose.covariance[28] = 1.0; srv->pose.pose.covariance[35] = 1.0; - auto client = rclcpp::create_client( + auto const client = rclcpp::create_client( interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", rclcpp::ServicesQoS()); @@ -268,7 +271,7 @@ void initializeStateEstimation(fuse_core::node_interfaces::NodeInterfacesremove_pending_request(result_future); return; } @@ -349,11 +352,11 @@ int main(int argc, char** argv) auto new_state = simulateRobotMotion(state, now, external_force); // Publish the new ground truth - ground_truth_publisher->publish(*robotToOdometry(new_state)); + ground_truth_publisher->publish(robotToOdometry(new_state)); // Generate and publish simulated measurements from the new robot state - imu_publisher->publish(*simulateImu(new_state)); - odom_publisher->publish(*simulateOdometry(new_state)); + imu_publisher->publish(simulateImu(new_state)); + odom_publisher->publish(simulateOdometry(new_state)); // Wait for the next time step state = new_state; From 8c00d8dfe86555a763ca3d5848fcb6f77fd04ce1 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Wed, 2 Oct 2024 23:17:37 +0000 Subject: [PATCH 23/46] add clang-tidy workflow --- .github/workflows/ci.yaml | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 03bdea8c8..3768cfb88 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -70,3 +70,18 @@ jobs: if: always() run: colcon test-result --verbose working-directory: /colcon_ws + + clang-tidy: + name: Run clang-tidy + needs: + # Ensure the test job runs after the build job finishes instead of attempting to run in parallel + - build-ws + runs-on: ubuntu-24.04 + container: + # Run on the Docker image we tagged and pushed to a private repo in the job above + image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} + steps: + - name: Unit test workspace + run: | + run-clang-tidy -j $(nproc --all) -p /colcon_ws/build/ + working-directory: /colcon_ws From 41e36927eb00679ee025949a972232fe87be8b84 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 01:18:13 +0000 Subject: [PATCH 24/46] add clang tidy pre-commit --- .pre-commit-config.yaml | 8 ++++++++ run_clang_tidy.bash | 4 ++++ 2 files changed, 12 insertions(+) create mode 100755 run_clang_tidy.bash diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 1b5c22bcb..50b744fea 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -81,3 +81,11 @@ repos: - "prettier@3.1.0" - "@prettier/plugin-xml@3.3.1" files: \.(xml|xacro)$ + + - repo: local + hooks: + - id: clang-tidy + name: clang tidy + entry: run_clang_tidy.bash + files: \.(cpp|hpp)$ + language: script diff --git a/run_clang_tidy.bash b/run_clang_tidy.bash new file mode 100755 index 000000000..0be2d6e2a --- /dev/null +++ b/run_clang_tidy.bash @@ -0,0 +1,4 @@ +#!/bin/bash + +# -j $(nproc --all) runs with all cores, but the prepended nice runs with a low priority so it won't make your computer unusable while clang tidy is going. The "$@" at the end passes all the filenames from pre-commit so it should only look for clang tidy fixes in the files you directly changed in the commit that is being checked. +nice run-clang-tidy -p ../../build_dbg -j $(nproc --all) -quiet -export-fixes=clang-tidy-fixes.yaml -fix "$@" From c45e85c0bcfe328c654209adf8f6c1ec86e0dd2d Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 01:22:27 +0000 Subject: [PATCH 25/46] add clang-tidy to pre-commit --- .gitignore | 3 ++ .../test/launch_tests/test_parameters.cpp | 40 +++++++++++-------- 2 files changed, 26 insertions(+), 17 deletions(-) diff --git a/.gitignore b/.gitignore index 60cf591f7..d613a68a4 100644 --- a/.gitignore +++ b/.gitignore @@ -33,3 +33,6 @@ # Python compiled bytecode files *.pyc + +# clang tidy fixes +clang-tidy-fixes.yaml diff --git a/fuse_core/test/launch_tests/test_parameters.cpp b/fuse_core/test/launch_tests/test_parameters.cpp index 17197b019..920903634 100644 --- a/fuse_core/test/launch_tests/test_parameters.cpp +++ b/fuse_core/test/launch_tests/test_parameters.cpp @@ -33,12 +33,18 @@ */ #include -#include +#include +#include +#include +#include +#include +#include +#include #include #include #include -#include +#include "fuse_core/eigen.hpp" class TestParameters : public ::testing::Test { @@ -102,15 +108,15 @@ TEST_F(TestParameters, getPositiveParam) TEST_F(TestParameters, GetCovarianceDiagonalParam) { // Build expected covariance matrix: - constexpr int Size = 3; + constexpr int size = 3; constexpr double variance = 1.0e-3; - constexpr double default_variance = 0.0; + constexpr double defaultVariance = 0.0; fuse_core::Matrix3d expected_covariance = fuse_core::Matrix3d::Identity(); expected_covariance *= variance; fuse_core::Matrix3d default_covariance = fuse_core::Matrix3d::Identity(); - default_covariance *= default_variance; + default_covariance *= defaultVariance; auto node = rclcpp::Node::make_shared("test_parameters_node"); @@ -123,10 +129,10 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) try { - const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); + const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance); - EXPECT_EQ(Size, covariance.rows()); - EXPECT_EQ(Size, covariance.cols()); + EXPECT_EQ(size, covariance.rows()); + EXPECT_EQ(size, covariance.cols()); EXPECT_EQ(expected_covariance.rows() * expected_covariance.cols(), expected_covariance.cwiseEqual(covariance).count()) @@ -148,10 +154,10 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) try { - const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance); + const auto covariance = fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance); - EXPECT_EQ(Size, covariance.rows()); - EXPECT_EQ(Size, covariance.cols()); + EXPECT_EQ(size, covariance.rows()); + EXPECT_EQ(size, covariance.cols()); EXPECT_EQ(default_covariance.rows() * default_covariance.cols(), default_covariance.cwiseEqual(covariance).count()) << "Expected\n" @@ -170,7 +176,7 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance), std::invalid_argument); } @@ -180,7 +186,7 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance), std::invalid_argument); } @@ -190,7 +196,7 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance), std::invalid_argument); } @@ -200,7 +206,7 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) const std::string parameter_name{ "covariance_diagonal_with_strings" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance), rclcpp::exceptions::InvalidParameterTypeException); // NOTE(CH3): A covariance diagonal with invalid element type used to not throw, and used to @@ -217,7 +223,7 @@ TEST_F(TestParameters, GetCovarianceDiagonalParam) const std::string parameter_name{ "covariance_diagonal_with_string" }; ASSERT_FALSE(node->has_parameter(parameter_name)); - EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, default_variance), + EXPECT_THROW(fuse_core::getCovarianceDiagonalParam(*node, parameter_name, defaultVariance), rclcpp::exceptions::InvalidParameterTypeException); // NOTE(CH3): A covariance diagonal with invalid element type used to not throw, and used to @@ -234,7 +240,7 @@ int main(int argc, char** argv) { rclcpp::init(argc, argv); testing::InitGoogleTest(&argc, argv); - int ret = RUN_ALL_TESTS(); + int const ret = RUN_ALL_TESTS(); rclcpp::shutdown(); return ret; } From a33b7178d63e93f3f78674d83f888ad3171e4eed Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 01:36:43 +0000 Subject: [PATCH 26/46] move pre-commit to after build in CI and make clang-tidy optional --- .github/workflows/ci.yaml | 25 +++++++++++++++++++------ .github/workflows/pre-commit.yml | 29 ----------------------------- run_clang_tidy.bash | 6 +++++- 3 files changed, 24 insertions(+), 36 deletions(-) delete mode 100644 .github/workflows/pre-commit.yml diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 3768cfb88..9a98a404e 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -71,8 +71,8 @@ jobs: run: colcon test-result --verbose working-directory: /colcon_ws - clang-tidy: - name: Run clang-tidy + pre-commit: + name: Pre-commit needs: # Ensure the test job runs after the build job finishes instead of attempting to run in parallel - build-ws @@ -81,7 +81,20 @@ jobs: # Run on the Docker image we tagged and pushed to a private repo in the job above image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} steps: - - name: Unit test workspace - run: | - run-clang-tidy -j $(nproc --all) -p /colcon_ws/build/ - working-directory: /colcon_ws + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: '3.10' + - name: Install clang-format + run: sudo apt-get install clang-format + - name: Install clang-tidy + run: sudo apt-get install clang-tidy + - name: enable clang tidy in pre-commit + run: export PRE_COMMIT_CLANG_TIDY=1 + - uses: pre-commit/action@v3.0.1 + id: precommit + - name: Upload pre-commit changes + if: failure() && steps.precommit.outcome == 'failure' + uses: rhaschke/upload-git-patch-action@main + with: + name: pre-commit diff --git a/.github/workflows/pre-commit.yml b/.github/workflows/pre-commit.yml deleted file mode 100644 index 416508d7a..000000000 --- a/.github/workflows/pre-commit.yml +++ /dev/null @@ -1,29 +0,0 @@ -# see https://github.com/pre-commit/action - -name: pre-commit - -on: - workflow_dispatch: - pull_request: - push: - branches: - - main - -jobs: - pre-commit: - name: Format - runs-on: ubuntu-24.04 - steps: - - uses: actions/checkout@v4 - - uses: actions/setup-python@v5 - with: - python-version: '3.10' - - name: Install clang-format-18 - run: sudo apt-get install clang-format-18 - - uses: pre-commit/action@v3.0.1 - id: precommit - - name: Upload pre-commit changes - if: failure() && steps.precommit.outcome == 'failure' - uses: rhaschke/upload-git-patch-action@main - with: - name: pre-commit diff --git a/run_clang_tidy.bash b/run_clang_tidy.bash index 0be2d6e2a..8e98cc58a 100755 --- a/run_clang_tidy.bash +++ b/run_clang_tidy.bash @@ -1,4 +1,8 @@ #!/bin/bash +if [ "$PRE_COMMIT_CLANG_TIDY" != "1" ] + exit +fi + # -j $(nproc --all) runs with all cores, but the prepended nice runs with a low priority so it won't make your computer unusable while clang tidy is going. The "$@" at the end passes all the filenames from pre-commit so it should only look for clang tidy fixes in the files you directly changed in the commit that is being checked. -nice run-clang-tidy -p ../../build_dbg -j $(nproc --all) -quiet -export-fixes=clang-tidy-fixes.yaml -fix "$@" +nice run-clang-tidy -p ../../build_dbg -j $(nproc --all) -quiet -fix "$@" From 81ef290c9f56f135fbe0e037aab772462de67ddd Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 01:48:04 +0000 Subject: [PATCH 27/46] default to not running clang tidy, but let the user know --- .pre-commit-config.yaml | 2 +- run_clang_tidy.bash | 10 +++++++++- 2 files changed, 10 insertions(+), 2 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 50b744fea..76cafc845 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -85,7 +85,7 @@ repos: - repo: local hooks: - id: clang-tidy - name: clang tidy + name: clang-tidy entry: run_clang_tidy.bash files: \.(cpp|hpp)$ language: script diff --git a/run_clang_tidy.bash b/run_clang_tidy.bash index 8e98cc58a..011f305ac 100755 --- a/run_clang_tidy.bash +++ b/run_clang_tidy.bash @@ -1,7 +1,15 @@ #!/bin/bash if [ "$PRE_COMMIT_CLANG_TIDY" != "1" ] - exit +then + if [ "$PRE_COMMIT_CLANG_TIDY" != "0" ] + then + >&2 echo "Skipped clang tidy (it is a slow check). To run clang tidy, use 'export PRE_COMMIT_CLANG_TIDY=1' and then run again." + >&2 echo "To skip clang tidy and make this check pass, 'export PRE_COMMIT_CLANG_TIDY=0' and then run again." + exit 1 + fi + # the user intends to skip this check + exit 0 fi # -j $(nproc --all) runs with all cores, but the prepended nice runs with a low priority so it won't make your computer unusable while clang tidy is going. The "$@" at the end passes all the filenames from pre-commit so it should only look for clang tidy fixes in the files you directly changed in the commit that is being checked. From e31c89bca7984ecc02868d5927fbd12ccc2cef98 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 20:50:12 +0000 Subject: [PATCH 28/46] specify version --- .github/workflows/ci.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 9a98a404e..2c8f0875f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -86,9 +86,9 @@ jobs: with: python-version: '3.10' - name: Install clang-format - run: sudo apt-get install clang-format + run: sudo apt-get install clang-format-18 - name: Install clang-tidy - run: sudo apt-get install clang-tidy + run: sudo apt-get install clang-tidy-18 - name: enable clang tidy in pre-commit run: export PRE_COMMIT_CLANG_TIDY=1 - uses: pre-commit/action@v3.0.1 From 69af8ea304077cdfc58f783ca39d7171dd6171d4 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 21:19:50 +0000 Subject: [PATCH 29/46] update first? --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 2c8f0875f..18ff90fb0 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -86,7 +86,7 @@ jobs: with: python-version: '3.10' - name: Install clang-format - run: sudo apt-get install clang-format-18 + run: sudo apt-get update && sudo apt-get install clang-format-18 - name: Install clang-tidy run: sudo apt-get install clang-tidy-18 - name: enable clang tidy in pre-commit From 2d663023eb4df05508ecda18834ecdc987ddc401 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 22:04:13 +0000 Subject: [PATCH 30/46] cd into correct wd --- .github/workflows/ci.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 18ff90fb0..3b0e5274d 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -89,6 +89,8 @@ jobs: run: sudo apt-get update && sudo apt-get install clang-format-18 - name: Install clang-tidy run: sudo apt-get install clang-tidy-18 + - name: Change directory + run: cd /colcon_ws/src/fuse - name: enable clang tidy in pre-commit run: export PRE_COMMIT_CLANG_TIDY=1 - uses: pre-commit/action@v3.0.1 From 04b59110a7512124d992de4da92a669a9a1eb269 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 22:47:26 +0000 Subject: [PATCH 31/46] change pip cache owner --- .github/workflows/ci.yaml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 3b0e5274d..f6ee94abe 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -93,6 +93,8 @@ jobs: run: cd /colcon_ws/src/fuse - name: enable clang tidy in pre-commit run: export PRE_COMMIT_CLANG_TIDY=1 + - name: change pip cache owner + run: chown -R $(whoami) /github - uses: pre-commit/action@v3.0.1 id: precommit - name: Upload pre-commit changes From 60c46d64674318153c4de74c28c396379ac87660 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 23:04:43 +0000 Subject: [PATCH 32/46] remove industrial CI --- .github/workflows/ros2.yml | 15 --------------- 1 file changed, 15 deletions(-) delete mode 100644 .github/workflows/ros2.yml diff --git a/.github/workflows/ros2.yml b/.github/workflows/ros2.yml deleted file mode 100644 index 02f231083..000000000 --- a/.github/workflows/ros2.yml +++ /dev/null @@ -1,15 +0,0 @@ -name: ros2 - -on: [push, pull_request] - -jobs: - industrial_ci: - strategy: - matrix: - env: - - {ROS_DISTRO: rolling, ROS_REPO: main} - runs-on: ubuntu-latest - steps: - - uses: actions/checkout@v1 - - uses: 'ros-industrial/industrial_ci@master' - env: ${{matrix.env}} From 10abfa45b32363a0ccd917da623c92932e59d8ef Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 23:08:26 +0000 Subject: [PATCH 33/46] chown more stuff --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index f6ee94abe..c0c4e3627 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -94,7 +94,7 @@ jobs: - name: enable clang tidy in pre-commit run: export PRE_COMMIT_CLANG_TIDY=1 - name: change pip cache owner - run: chown -R $(whoami) /github + run: chown -R $(whoami) /github && chown -R $(whoami) /home/runner/work/ - uses: pre-commit/action@v3.0.1 id: precommit - name: Upload pre-commit changes From 9a6c4ce69de71b76cf9c569658eda89a5ad35c72 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 23:32:02 +0000 Subject: [PATCH 34/46] try without container --- .github/workflows/ci.yaml | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c0c4e3627..7fbc10d0b 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -77,9 +77,6 @@ jobs: # Ensure the test job runs after the build job finishes instead of attempting to run in parallel - build-ws runs-on: ubuntu-24.04 - container: - # Run on the Docker image we tagged and pushed to a private repo in the job above - image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} steps: - uses: actions/checkout@v4 - uses: actions/setup-python@v5 @@ -94,7 +91,7 @@ jobs: - name: enable clang tidy in pre-commit run: export PRE_COMMIT_CLANG_TIDY=1 - name: change pip cache owner - run: chown -R $(whoami) /github && chown -R $(whoami) /home/runner/work/ + run: chown -R $(whoami) /github - uses: pre-commit/action@v3.0.1 id: precommit - name: Upload pre-commit changes From 66534b7de27b858e24aa23be7820132292275389 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 23:41:19 +0000 Subject: [PATCH 35/46] try without checkout --- .github/workflows/ci.yaml | 1 - run_clang_tidy.bash | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 7fbc10d0b..6b59df3ec 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -78,7 +78,6 @@ jobs: - build-ws runs-on: ubuntu-24.04 steps: - - uses: actions/checkout@v4 - uses: actions/setup-python@v5 with: python-version: '3.10' diff --git a/run_clang_tidy.bash b/run_clang_tidy.bash index 011f305ac..84fb12ae2 100755 --- a/run_clang_tidy.bash +++ b/run_clang_tidy.bash @@ -13,4 +13,4 @@ then fi # -j $(nproc --all) runs with all cores, but the prepended nice runs with a low priority so it won't make your computer unusable while clang tidy is going. The "$@" at the end passes all the filenames from pre-commit so it should only look for clang tidy fixes in the files you directly changed in the commit that is being checked. -nice run-clang-tidy -p ../../build_dbg -j $(nproc --all) -quiet -fix "$@" +nice run-clang-tidy -p ../../build -j $(nproc --all) -quiet -fix "$@" From 6b8611488de364f56180c241a6a405777e720cef Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Thu, 3 Oct 2024 23:41:56 +0000 Subject: [PATCH 36/46] add container back --- .github/workflows/ci.yaml | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 6b59df3ec..1f8b02152 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -77,6 +77,9 @@ jobs: # Ensure the test job runs after the build job finishes instead of attempting to run in parallel - build-ws runs-on: ubuntu-24.04 + container: + # Run on the Docker image we tagged and pushed to a private repo in the job above + image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} steps: - uses: actions/setup-python@v5 with: From 8748a8f58982ae7ff1746fb1d4ff9bc4a6299e1d Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 00:37:27 +0000 Subject: [PATCH 37/46] try clang tidy outside pre-commit --- .github/workflows/ci.yaml | 28 +++++++--------------------- .github/workflows/pre-commit.yaml | 29 +++++++++++++++++++++++++++++ .pre-commit-config.yaml | 8 -------- Dockerfile | 1 + run_clang_tidy.bash | 14 +------------- 5 files changed, 38 insertions(+), 42 deletions(-) create mode 100644 .github/workflows/pre-commit.yaml diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 1f8b02152..28dec0e8d 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -71,33 +71,19 @@ jobs: run: colcon test-result --verbose working-directory: /colcon_ws - pre-commit: - name: Pre-commit + clang_tidy: needs: # Ensure the test job runs after the build job finishes instead of attempting to run in parallel - build-ws + name: clang-tidy runs-on: ubuntu-24.04 container: # Run on the Docker image we tagged and pushed to a private repo in the job above image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} steps: - - uses: actions/setup-python@v5 - with: - python-version: '3.10' - - name: Install clang-format - run: sudo apt-get update && sudo apt-get install clang-format-18 - - name: Install clang-tidy - run: sudo apt-get install clang-tidy-18 - - name: Change directory - run: cd /colcon_ws/src/fuse - - name: enable clang tidy in pre-commit - run: export PRE_COMMIT_CLANG_TIDY=1 - - name: change pip cache owner - run: chown -R $(whoami) /github - - uses: pre-commit/action@v3.0.1 - id: precommit - - name: Upload pre-commit changes - if: failure() && steps.precommit.outcome == 'failure' - uses: rhaschke/upload-git-patch-action@main + - run: run-clang-tidy -j $(nproc --all) -p build/ src/fuse -export-fixes clang-tidy-fixes.yaml + working-directory: /colcon_ws + - uses: asarium/clang-tidy-action@v1.0 with: - name: pre-commit + fixesFile: /colcon_ws/clang-tidy-fixes.yaml + noFailOnIssue: false diff --git a/.github/workflows/pre-commit.yaml b/.github/workflows/pre-commit.yaml new file mode 100644 index 000000000..416508d7a --- /dev/null +++ b/.github/workflows/pre-commit.yaml @@ -0,0 +1,29 @@ +# see https://github.com/pre-commit/action + +name: pre-commit + +on: + workflow_dispatch: + pull_request: + push: + branches: + - main + +jobs: + pre-commit: + name: Format + runs-on: ubuntu-24.04 + steps: + - uses: actions/checkout@v4 + - uses: actions/setup-python@v5 + with: + python-version: '3.10' + - name: Install clang-format-18 + run: sudo apt-get install clang-format-18 + - uses: pre-commit/action@v3.0.1 + id: precommit + - name: Upload pre-commit changes + if: failure() && steps.precommit.outcome == 'failure' + uses: rhaschke/upload-git-patch-action@main + with: + name: pre-commit diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 76cafc845..1b5c22bcb 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -81,11 +81,3 @@ repos: - "prettier@3.1.0" - "@prettier/plugin-xml@3.3.1" files: \.(xml|xacro)$ - - - repo: local - hooks: - - id: clang-tidy - name: clang-tidy - entry: run_clang_tidy.bash - files: \.(cpp|hpp)$ - language: script diff --git a/Dockerfile b/Dockerfile index d82059ae6..ac748b2bf 100644 --- a/Dockerfile +++ b/Dockerfile @@ -20,6 +20,7 @@ WORKDIR /colcon_ws # hadolint ignore=SC1091 RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ --mount=type=cache,target=/var/lib/apt,sharing=locked \ + --mount=type=cache,target=/$HOME/.cache,sharing=locked \ apt-get update && apt-get upgrade -y && \ . /opt/ros/rolling/setup.sh && \ rosdep install --from-paths src -y --ignore-src && \ diff --git a/run_clang_tidy.bash b/run_clang_tidy.bash index 84fb12ae2..9fb7598e8 100755 --- a/run_clang_tidy.bash +++ b/run_clang_tidy.bash @@ -1,16 +1,4 @@ #!/bin/bash -if [ "$PRE_COMMIT_CLANG_TIDY" != "1" ] -then - if [ "$PRE_COMMIT_CLANG_TIDY" != "0" ] - then - >&2 echo "Skipped clang tidy (it is a slow check). To run clang tidy, use 'export PRE_COMMIT_CLANG_TIDY=1' and then run again." - >&2 echo "To skip clang tidy and make this check pass, 'export PRE_COMMIT_CLANG_TIDY=0' and then run again." - exit 1 - fi - # the user intends to skip this check - exit 0 -fi - # -j $(nproc --all) runs with all cores, but the prepended nice runs with a low priority so it won't make your computer unusable while clang tidy is going. The "$@" at the end passes all the filenames from pre-commit so it should only look for clang tidy fixes in the files you directly changed in the commit that is being checked. -nice run-clang-tidy -p ../../build -j $(nproc --all) -quiet -fix "$@" +nice run-clang-tidy -p ../../build_dbg -j $(nproc --all) -quiet -fix "$@" From e7ca98ff1096e70c8569a6b93f4304a3853ee595 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 01:02:17 +0000 Subject: [PATCH 38/46] run only on changed cpp/hpp files --- .github/workflows/ci.yaml | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 28dec0e8d..f3732e67f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -81,7 +81,15 @@ jobs: # Run on the Docker image we tagged and pushed to a private repo in the job above image: ghcr.io/picknikrobotics/fuse:${{ github.run_id }} steps: - - run: run-clang-tidy -j $(nproc --all) -p build/ src/fuse -export-fixes clang-tidy-fixes.yaml + - name: Changed Files + id: changed-cpp-files + uses: tj-actions/changed-files@v45.0.3 + with: + # Avoid using single or double quotes for multiline patterns + files: | + **.cpp + **.hpp + - run: run-clang-tidy -j $(nproc --all) -p build/ src/fuse ${{ steps.changed-cpp-files.outputs.all_changed_files }} -export-fixes clang-tidy-fixes.yaml working-directory: /colcon_ws - uses: asarium/clang-tidy-action@v1.0 with: From 2ae9f73154b03ec127aa538bb63bc3ffbb601050 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 15:06:29 +0000 Subject: [PATCH 39/46] point to config file --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index f3732e67f..5f6a89ce7 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -89,7 +89,7 @@ jobs: files: | **.cpp **.hpp - - run: run-clang-tidy -j $(nproc --all) -p build/ src/fuse ${{ steps.changed-cpp-files.outputs.all_changed_files }} -export-fixes clang-tidy-fixes.yaml + - run: run-clang-tidy -j $(nproc --all) -p build/ src/fuse ${{ steps.changed-cpp-files.outputs.all_changed_files }} -export-fixes clang-tidy-fixes.yaml -config-file src/fuse/.clang-tidy working-directory: /colcon_ws - uses: asarium/clang-tidy-action@v1.0 with: From 58d3ca63064902d51c4cc731b571339cf38176ce Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 15:39:18 +0000 Subject: [PATCH 40/46] change files to last --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 5f6a89ce7..8beaa2d51 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -89,7 +89,7 @@ jobs: files: | **.cpp **.hpp - - run: run-clang-tidy -j $(nproc --all) -p build/ src/fuse ${{ steps.changed-cpp-files.outputs.all_changed_files }} -export-fixes clang-tidy-fixes.yaml -config-file src/fuse/.clang-tidy + - run: run-clang-tidy -j $(nproc --all) -p build/ src/fuse -export-fixes clang-tidy-fixes.yaml -config-file src/fuse/.clang-tidy ${{ steps.changed-cpp-files.outputs.all_changed_files }} working-directory: /colcon_ws - uses: asarium/clang-tidy-action@v1.0 with: From 5c4a5fa63df7c9cbe6a52d4e2de5b89e94ce574f Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 17:01:23 +0000 Subject: [PATCH 41/46] fix clang tidy path --- .github/workflows/ci.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 8beaa2d51..e13deb86f 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -89,7 +89,7 @@ jobs: files: | **.cpp **.hpp - - run: run-clang-tidy -j $(nproc --all) -p build/ src/fuse -export-fixes clang-tidy-fixes.yaml -config-file src/fuse/.clang-tidy ${{ steps.changed-cpp-files.outputs.all_changed_files }} + - run: run-clang-tidy -j $(nproc --all) -p build/ -export-fixes clang-tidy-fixes.yaml -config-file src/fuse/.clang-tidy ${{ steps.changed-cpp-files.outputs.all_changed_files }} working-directory: /colcon_ws - uses: asarium/clang-tidy-action@v1.0 with: From 6424c7a93f846ae710b688a310741a1674c98c0a Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 17:45:27 +0000 Subject: [PATCH 42/46] try adding back some checks --- .clang-tidy | 4 ---- 1 file changed, 4 deletions(-) diff --git a/.clang-tidy b/.clang-tidy index 9f4f1a806..3d6d474cf 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -13,15 +13,11 @@ Checks: -*, -bugprone-exception-escape, -cppcoreguidelines-avoid-magic-numbers, -google-readability-casting, - -misc-non-private-member-variables-in-classes, - -misc-use-anonymous-namespace, -modernize-use-trailing-return-type, -readability-identifier-length, -readability-function-cognitive-complexity, -readability-magic-numbers, -readability-uppercase-literal-suffix, - -cppcoreguidelines-pro-type-union-access, - -cppcoreguidelines-non-private-member-variables-in-classes, -cppcoreguidelines-pro-type-vararg, HeaderFilterRegex: '' CheckOptions: From 445fb27370a0f30158768ed705f4be87b63146b6 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 17:52:25 +0000 Subject: [PATCH 43/46] try removing cache --- Dockerfile | 1 - 1 file changed, 1 deletion(-) diff --git a/Dockerfile b/Dockerfile index ac748b2bf..d82059ae6 100644 --- a/Dockerfile +++ b/Dockerfile @@ -20,7 +20,6 @@ WORKDIR /colcon_ws # hadolint ignore=SC1091 RUN --mount=type=cache,target=/var/cache/apt,sharing=locked \ --mount=type=cache,target=/var/lib/apt,sharing=locked \ - --mount=type=cache,target=/$HOME/.cache,sharing=locked \ apt-get update && apt-get upgrade -y && \ . /opt/ros/rolling/setup.sh && \ rosdep install --from-paths src -y --ignore-src && \ From ad64883c49b3eb592ccbca05caba82e2576e5582 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 18:08:53 +0000 Subject: [PATCH 44/46] clang tidy fix --- fuse_core/test/launch_tests/test_parameters.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/fuse_core/test/launch_tests/test_parameters.cpp b/fuse_core/test/launch_tests/test_parameters.cpp index 920903634..aff635707 100644 --- a/fuse_core/test/launch_tests/test_parameters.cpp +++ b/fuse_core/test/launch_tests/test_parameters.cpp @@ -65,6 +65,7 @@ class TestParameters : public ::testing::Test executor_.reset(); } +private: std::thread spinner_; //!< Internal thread for spinning the executor rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; }; From 9d22bde720dbecfaf6c4a6d9e4f0ee5e01b85701 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 20:30:27 +0000 Subject: [PATCH 45/46] remove most oft-violated checks --- .clang-tidy | 3 +++ 1 file changed, 3 insertions(+) diff --git a/.clang-tidy b/.clang-tidy index 3d6d474cf..5c44be683 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -12,8 +12,11 @@ Checks: -*, -bugprone-easily-swappable-parameters, -bugprone-exception-escape, -cppcoreguidelines-avoid-magic-numbers, + -cppcoreguidelines-non-private-member-variables-in-classes, -google-readability-casting, + -misc-include-cleaner, -modernize-use-trailing-return-type, + -modernize-avoid-bind, -readability-identifier-length, -readability-function-cognitive-complexity, -readability-magic-numbers, From b874508745264cfff8c6a81035afe6fae9d2b315 Mon Sep 17 00:00:00 2001 From: Henry Moore Date: Fri, 4 Oct 2024 20:30:40 +0000 Subject: [PATCH 46/46] don't run clang tidy on main --- .github/workflows/ci.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index c412c8763..d58661793 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -72,6 +72,7 @@ jobs: working-directory: /colcon_ws clang_tidy: + if: github.ref != 'refs/heads/main' needs: # Ensure the test job runs after the build job finishes instead of attempting to run in parallel - build-ws