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, diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index e13deb86f..d58661793 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. @@ -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 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 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 diff --git a/fuse.repos b/fuse.repos new file mode 100644 index 000000000..d569f4ec3 --- /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 diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 06091a383..5f3a5d4cc 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -116,7 +116,7 @@ include(suitesparse-extras.cmake) # 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() @@ -129,6 +129,7 @@ add_library( 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 @@ -136,11 +137,15 @@ add_library( 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) target_include_directories( 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.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_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index 0e02561bd..e4b90b549 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 @@ -193,23 +197,31 @@ 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 f28ceb3bf..d46e41927 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp @@ -149,6 +149,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 { @@ -179,6 +191,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_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/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..e367e1742 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -0,0 +1,193 @@ +/* + * 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/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_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..4c1201650 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,143 @@ +/* + * 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_ 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.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp new file mode 100644 index 000000000..bcb2ddb8f --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp @@ -0,0 +1,96 @@ +/* + * 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_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.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp new file mode 100644 index 000000000..8088539d7 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -0,0 +1,96 @@ +/* + * 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_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/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp new file mode 100644 index 000000000..723fc3fbd --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -0,0 +1,102 @@ +/* + * 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..7085b79a2 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,127 @@ +/* + * 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_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/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..ab59f7915 --- /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/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/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index b30a4b77b..26c223a36 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -36,17 +36,25 @@ #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); 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_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index 41d91c68f..7a113262b 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 @@ -75,8 +75,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..e3bec0ff4 --- /dev/null +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,139 @@ +/* + * 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_core/include/fuse_core/util.h b/fuse_constraints/src/normal_prior_orientation_3d.cpp similarity index 51% rename from fuse_core/include/fuse_core/util.h rename to fuse_constraints/src/normal_prior_orientation_3d.cpp index 2daf0d29c..c0537dc24 100644 --- a/fuse_core/include/fuse_core/util.h +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -1,7 +1,7 @@ /* * Software License Agreement (BSD License) * - * Copyright (c) 2022, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,12 +31,55 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include -#ifndef FUSE_CORE__UTIL_H_ -#define FUSE_CORE__UTIL_H_ +#include +#include -#warning This header is obsolete, please include fuse_core/util.hpp instead +namespace fuse_constraints +{ -#include +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; +} -#endif // FUSE_CORE__UTIL_H_ +} // 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..4fa0864a3 --- /dev/null +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -0,0 +1,97 @@ +/* + * 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..098168027 --- /dev/null +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp @@ -0,0 +1,93 @@ +/* + * 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 f4ca63a16..2b6913fe7 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -70,6 +70,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..91de1542d --- /dev/null +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,123 @@ +/* + * 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 276317e07..3b2c63dd4 100644 --- a/fuse_constraints/test/CMakeLists.txt +++ b/fuse_constraints/test/CMakeLists.txt @@ -7,13 +7,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 e77f593e0..b5178c359 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -38,6 +38,9 @@ #include #include +#include +#include +#include /** * @brief A helper function to compare a expected and actual cost function. @@ -53,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(); @@ -67,9 +71,25 @@ static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function } std::unique_ptr parameters(new double[num_parameters]); - for (size_t i = 0; i < num_parameters; ++i) + if ((num_parameters == 7) && (parameter_block_sizes[0] == 3) && (parameter_block_sizes[1] == 4)) { - parameters[i] = static_cast(i) + 1.0; + // 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]); @@ -95,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())); @@ -103,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]; } } 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 3929b84ce..e139bfdc8 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 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..43b92de91 --- /dev/null +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,460 @@ +/* + * 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_constraints::AbsolutePose3DStampedEulerConstraint; +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; + +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..57a4d18d0 --- /dev/null +++ b/fuse_constraints/test/test_normal_prior_pose_3d.cpp @@ -0,0 +1,86 @@ +/* + * 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 "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); +} 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..59150f134 --- /dev/null +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -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. + */ +#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..283b7535a --- /dev/null +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,624 @@ +/* + * 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_constraints::AbsolutePose3DStampedConstraint; +using fuse_constraints::AbsolutePose3DStampedEulerConstraint; +using fuse_constraints::RelativePose3DStampedEulerConstraint; +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; + +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/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.hpp b/fuse_core/include/fuse_core/eigen.hpp index 23d794559..be9d74403 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/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.hpp b/fuse_core/include/fuse_core/util.hpp index 444f7e574..14f255ec0 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 @@ -153,6 +154,176 @@ 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(0, 0) = 2.0; + jacobian_map(1, 1) = 2.0; + jacobian_map(2, 2) = 2.0; + } + } +} + /** * @brief Create a compound ROS topic name from two components * 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/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 49851fe4b..095ab2bf1 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -33,10 +33,58 @@ */ #include -#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 +129,110 @@ 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]; // 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); + + 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_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/CMakeLists.txt b/fuse_models/CMakeLists.txt index 2d825c356..faf8ec3ef 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) @@ -44,20 +45,27 @@ add_library( 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/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 @@ -104,6 +112,7 @@ pluginlib_export_plugin_description_file(fuse_core fuse_plugins.xml) 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/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_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index b402f9dab..71e81a24a 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 namespace fuse_models @@ -95,6 +99,37 @@ std::enable_if_t::value, size_t> toIndex(const std::string& dime 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 * @@ -118,6 +153,60 @@ std::enable_if_t::value, size_t> toIndex(const std::string& dim 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.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/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index d60b2df1f..2e0c6d71d 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 static auto sensor_proc_clock = rclcpp::Clock(); @@ -181,6 +197,27 @@ inline void populatePartialMeasurement(const fuse_core::VectorXd& mean_full, con } } +/** + * @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 @@ -211,6 +248,27 @@ inline void validatePartialMeasurement(const fuse_core::VectorXd& mean_partial, } } +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 * @@ -338,7 +396,7 @@ inline bool processAbsolutePoseWithCovariance(const std::string& source, const f { try { - validatePartialMeasurement(pose_mean_partial, pose_covariance_partial); + validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); } catch (const std::runtime_error& ex) { @@ -365,6 +423,159 @@ inline bool processAbsolutePoseWithCovariance(const std::string& source, const f 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 @@ -688,6 +899,244 @@ inline bool processDifferentialPoseWithCovariance(const std::string& source, con 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 @@ -862,46 +1311,232 @@ inline bool processDifferentialPoseWithTwistCovariance(const std::string& source } /** - * @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(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)) +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::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) { - // Make sure we actually have work to do - if (linear_indices.empty() && angular_indices.empty()) + if (position_indices.empty() && orientation_indices.empty()) { return false; } - geometry_msgs::msg::TwistWithCovarianceStamped transformed_message; + // 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, + 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; @@ -1025,6 +1660,184 @@ inline bool processTwistWithCovariance(const std::string& source, const fuse_cor 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 @@ -1127,6 +1940,110 @@ inline bool processAccelWithCovariance(const std::string& source, const fuse_cor 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 * @@ -1165,6 +2082,29 @@ inline void scaleProcessNoiseCovariance(fuse_core::Matrix8d& process_noise_covar 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.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/common/variable_traits.hpp b/fuse_models/include/fuse_models/common/variable_traits.hpp index 10006456f..803d5f62d 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 { @@ -70,6 +75,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 { @@ -88,6 +117,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/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/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 new file mode 100644 index 000000000..46c8979b9 --- /dev/null +++ b/fuse_models/include/fuse_models/imu_3d.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__IMU_3D_HPP_ +#define FUSE_MODELS__IMU_3D_HPP_ + +#include +#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 + 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_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.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/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/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp new file mode 100644 index 000000000..6a8f6e552 --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -0,0 +1,179 @@ +/* + * 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 +#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 + 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..dc623ad99 --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -0,0 +1,239 @@ +/* + * 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 + 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..4706d2985 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -0,0 +1,235 @@ +/* + * 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__OMNIDIRECTIONAL_3D_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_3D_HPP_ + +#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 an omnidirectional 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 + 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& omnidirectional_3d); + +} // namespace fuse_models + +#endif // FUSE_MODELS__OMNIDIRECTIONAL_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..93620c865 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -0,0 +1,208 @@ +/* + * 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, 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 + * 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, + * 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 + * 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 + 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..167554624 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -0,0 +1,506 @@ +/* + * 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..0fcaf9063 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp @@ -0,0 +1,281 @@ +/* + * 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..0ccafd8fc --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.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_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..18273e190 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp @@ -0,0 +1,192 @@ +/* + * 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/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/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp new file mode 100644 index 000000000..c0df539a7 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -0,0 +1,153 @@ +/* + * 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 + 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_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/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp new file mode 100644 index 000000000..734e75c98 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -0,0 +1,148 @@ +/* + * 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 + 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..21cb0a29e --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -0,0 +1,169 @@ +/* + * 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 + 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..b84d1c253 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp @@ -0,0 +1,178 @@ +/* + * 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 + 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/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_models/package.xml b/fuse_models/package.xml index d90eb3c39..1eefe87c9 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..1a12217cf --- /dev/null +++ b/fuse_models/src/imu_3d.cpp @@ -0,0 +1,266 @@ +/* + * 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..cd05a3c22 --- /dev/null +++ b/fuse_models/src/odometry_3d.cpp @@ -0,0 +1,216 @@ +/* + * 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, &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..ec2649f91 --- /dev/null +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -0,0 +1,588 @@ +/* + * 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..5def0f0c3 --- /dev/null +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -0,0 +1,553 @@ +/* + * 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..55c64a60f --- /dev/null +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -0,0 +1,428 @@ +/* + * 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..7b222f762 --- /dev/null +++ b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.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 +#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 05f6747be..535843431 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -1,9 +1,14 @@ # CORE GTESTS # ====================================================================================== set(TEST_TARGETS - test_unicycle_2d test_unicycle_2d_predict - test_unicycle_2d_state_cost_function test_graph_ignition - test_unicycle_2d_ignition) + test_unicycle_2d + test_unicycle_2d_predict + 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}) ament_add_gtest("${test_name}" "${test_name}.cpp") @@ -12,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}) diff --git a/fuse_models/test/test_omnidirectional_3d.cpp b/fuse_models/test/test_omnidirectional_3d.cpp new file mode 100644 index 000000000..01fbf7f6b --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d.cpp @@ -0,0 +1,365 @@ +/*************************************************************************** + * 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::StateHistory; + using fuse_models::Omnidirectional3D::StateHistoryElement; + using fuse_models::Omnidirectional3D::updateStateHistoryEstimates; +}; + +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..26e97be53 --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d_predict.cpp @@ -0,0 +1,492 @@ +/* + * 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); +} 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..db0b93b63 --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp @@ -0,0 +1,143 @@ +/* + * 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::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); + + 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..5c1caaa88 --- /dev/null +++ b/fuse_models/test/test_sensor_proc_3d.cpp @@ -0,0 +1,127 @@ +/*************************************************************************** + * 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/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_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 36e22e6f7..10c4e819b 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -229,6 +229,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()); @@ -376,7 +378,6 @@ void FixedLagSmoother::processQueue(fuse_core::Transaction& transaction, const r return; } } - // Use the most recent transaction time as the current time const auto current_time = pending_transactions_.front().stamp(); 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/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index c45b22c37..83387db16 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -45,6 +45,9 @@ 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_simulator src/three_dimensional_simulator.cpp) +target_link_libraries(three_dimensional_simulator ${PROJECT_NAME}) + # ############################################################################## # Install ## # ############################################################################## @@ -59,6 +62,7 @@ install( install(DIRECTORY include/ DESTINATION include/${PROJECT_NAME}) install(TARGETS range_sensor_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 new file mode 100644 index 000000000..863814ccc --- /dev/null +++ b/fuse_tutorials/config/fuse_3d_tutorial.rviz @@ -0,0 +1,213 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + - /Odometry1/Shape1 + - /Odometry2/Shape1 + Splitter Ratio: 0.5 + Tree Height: 555 + - 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: 10 + Reference Frame: + Value: true + - Angle Tolerance: 0.10000000149011612 + 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: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 255; 0; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /ground_truth + Value: true + - Angle Tolerance: 0.10000000149011612 + 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: true + Enabled: true + Keep: 1 + Name: Odometry + Position Tolerance: 0.10000000149011612 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Color: 0; 255; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /odom_filtered + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: map + Frame Rate: 30 + 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: + 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 + Pitch: 0.785398006439209 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.785398006439209 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 846 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000002b4000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000023f000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + 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 new file mode 100644 index 000000000..1e95b1740 --- /dev/null +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -0,0 +1,62 @@ +# this yaml file is adapted from `fuse_simple_tutorial.yaml` +state_estimator: + ros__parameters: + # Fixed-lag smoother configuration + optimization_frequency: 20.0 + 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 + + 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 + odometry_sensor: + type: fuse_models::Odometry3D + motion_models: [3d_motion_model] + 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.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'] + + # this publishes our estimated odometry + publishers: + filtered_publisher: + type: fuse_models::Odometry3DPublisher + + # the configuration of our output publisher + filtered_publisher: + topic: 'odom_filtered' + base_link_frame_id: 'base_link' + 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/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_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 100755 index 000000000..f57d66dc1 --- /dev/null +++ b/fuse_tutorials/launch/fuse_3d_tutorial.launch.py @@ -0,0 +1,67 @@ +#! /usr/bin/env python3 + +# 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. +# 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( + [ + # 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_simulator", + name="three_dimensional_simulator", + output="screen", + ), + # run our estimator + Node( + package="fuse_optimizers", + executable="fixed_lag_smoother_node", + name="state_estimator", + parameters=[ + PathJoinSubstitution([pkg_dir, "config", "fuse_3d_tutorial.yaml"]) + ], + ), + # run visualization + 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_simulator.cpp b/fuse_tutorials/src/three_dimensional_simulator.cpp new file mode 100644 index 000000000..6bfb873fa --- /dev/null +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -0,0 +1,369 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik 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 +#include +#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 + //!< 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 +} // namespace + +/** + * @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 robotToOdometry(Robot const& state) +{ + 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; + + // 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(Robot const& previous_state, rclcpp::Time const& now, Eigen::Vector3d const& external_force) +{ + auto const 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 simulateImu(Robot const& robot) +{ + static std::random_device rd{}; + static std::mt19937 generator{ rd() }; + static std::normal_distribution<> noise{ 0.0, IMU_SIGMA }; + + 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 (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 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 }; + + 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; + + // 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) +{ + // 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 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()); + + 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, "set pose service call failed"); + client->remove_pending_request(result_future); + return; + } + success = result_future.get()->success; + } +} + +int main(int argc, char** argv) +{ + // set up our ROS node + rclcpp::init(argc, argv); + 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 (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); + + // 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(); + 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_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') + if (std::fmod(now_d, 4 * motion_duration) < motion_duration) + { + external_force.x() = force_magnitude; + } + else if (std::fmod(now_d, 4 * motion_duration) < 2 * motion_duration) + { + external_force.y() = force_magnitude; + } + else if (std::fmod(now_d, 4 * motion_duration) < 3 * motion_duration) + { + 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 + auto new_state = simulateRobotMotion(state, now, external_force); + + // Publish the new ground truth + 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)); + + // Wait for the next time step + state = new_state; + rclcpp::spin_some(node); + rate.sleep(); + } + + rclcpp::shutdown(); + return 0; +} 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_