diff --git a/fuse.repos b/fuse.repos new file mode 100644 index 000000000..8ad27e25b --- /dev/null +++ b/fuse.repos @@ -0,0 +1,9 @@ +repositories: + locusrobotics/tf2_2d: + type: git + url: https://github.com/locusrobotics/tf2_2d.git + version: rolling + covariance_geometry_ros: + type: git + url: https://github.com/giafranchini/covariance_geometry_ros.git + version: iron \ No newline at end of file diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 8640fe075..2031533ee 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -78,6 +78,7 @@ add_library(${PROJECT_NAME} src/absolute_orientation_3d_stamped_euler_constraint.cpp src/absolute_pose_2d_stamped_constraint.cpp src/absolute_pose_3d_stamped_constraint.cpp + src/absolute_pose_3d_stamped_euler_constraint.cpp src/marginal_constraint.cpp src/marginal_cost_function.cpp src/marginalize_variables.cpp @@ -85,11 +86,15 @@ add_library(${PROJECT_NAME} src/normal_delta_orientation_2d.cpp src/normal_delta_pose_2d.cpp src/normal_prior_orientation_2d.cpp + src/normal_prior_orientation_3d.cpp src/normal_prior_pose_2d.cpp + src/normal_prior_pose_3d.cpp + src/normal_prior_pose_3d_euler.cpp src/relative_constraint.cpp src/relative_orientation_3d_stamped_constraint.cpp src/relative_pose_2d_stamped_constraint.cpp src/relative_pose_3d_stamped_constraint.cpp + src/relative_pose_3d_stamped_euler_constraint.cpp src/uuid_ordering.cpp src/variable_constraints.cpp ) diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index aa6b02c54..71e7b8d9a 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -5,12 +5,24 @@ the 2D angular acceleration. + + + A constraint that represents either prior information about a 3D angular acceleration, or a direct measurement of + the 3D angular acceleration. + + A constraint that represents either prior information about a 2D linear acceleration, or a direct measurement of the 2D linear acceleration. + + + A constraint that represents either prior information about a 3D linear acceleration, or a direct measurement of + the 3D linear acceleration. + + A constraint that represents either prior information about a 2D orientation, or a direct measurement of the @@ -35,12 +47,24 @@ the 2D angular velocity. + + + A constraint that represents either prior information about a 3D angular velocity, or a direct measurement of + the 3D angular velocity. + + A constraint that represents either prior information about a 2D linear velocity, or a direct measurement of the 2D linear velocity. + + + A constraint that represents either prior information about a 3D linear velocity, or a direct measurement of + the 3D linear velocity. + + A constraint that represents either prior information about a 3D orientation, or a direct measurement of the @@ -63,6 +87,12 @@ A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose. + + + A constraint that represents either prior information about a 3D pose, or a direct measurement of the 3D pose, + orientation parametrized in RPY. + + A constraint that represents remaining marginal information on a set of variables. @@ -118,4 +148,9 @@ A constraint that represents a measurement on the difference between two 3D poses. + + + A constraint that represents a measurement on the difference between two 3D poses, orientation parametrized in RPY. + + diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp index 8bb4ac4c7..995e0e8ea 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.hpp @@ -46,11 +46,15 @@ #include #include #include +#include #include +#include #include #include #include #include +#include +#include #include #include @@ -196,27 +200,39 @@ class AbsoluteConstraint : public fuse_core::Constraint // Define unique names for the different variations of the absolute constraint using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationAngular3DStampedConstraint = + AbsoluteConstraint; using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationLinear3DStampedConstraint = + AbsoluteConstraint; using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition3DStampedConstraint = AbsoluteConstraint; using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityAngular3DStampedConstraint = + AbsoluteConstraint; using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityLinear3DStampedConstraint = + AbsoluteConstraint; } // namespace fuse_constraints // Include the template implementation #include BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteOrientation2DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition2DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePosition3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint); BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); #endif // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp index cbb689b54..197db9c5e 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint_impl.hpp @@ -155,6 +155,18 @@ inline std::string AbsoluteConstraint +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint"; +} + +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint"; +} + template<> inline std::string AbsoluteConstraint::type() const { @@ -185,6 +197,18 @@ inline std::string AbsoluteConstraint:: return "fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint"; } +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint"; +} + +template<> +inline std::string AbsoluteConstraint::type() const +{ + return "fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint"; +} + } // namespace fuse_constraints #endif // FUSE_CONSTRAINTS__ABSOLUTE_CONSTRAINT_IMPL_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp new file mode 100644 index 000000000..672b52dac --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.hpp @@ -0,0 +1,194 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +#ifndef FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ +#define FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents either prior information about a 3D pose, or a direct + * measurement of the 3D pose. + * + * A 3D pose is the combination of a 3D position and a 3D orientation variable. As a convenience, + * this class applies an absolute constraint on both variables at once. This type of constraint + * arises in many situations. In mapping it is common to define the very first pose as the origin. + * Some sensors, such as GPS, provide direct measurements of the robot's pose in the global frame. + * And localization systems often match laserscans or pointclouds to a prior map (scan-to-map + * measurements). This constraint holds the measured 3D pose: orientation is parametrized as roll-pitch-yaw + * Euler angles and the covariance represents the error around each translational and rotational axis. + * This constraint allows measurement of a subset of the pose components given in the variable. + */ + +class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(AbsolutePose3DStampedEulerConstraint) + + /** + * @brief Default constructor + */ + AbsolutePose3DStampedEulerConstraint() = default; + + /** + * @brief Create a constraint using a measurement/prior of the 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the pose + * @param[in] orientation The variable representing the orientation components of the pose + * @param[in] mean The measured/prior pose as a vector + * (6x1 vector: x, y, z, roll, pitch, yaw) + * @param[in] covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) + */ + AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & mean, + const fuse_core::Matrix6d & covariance); + + /** + * @brief Create a constraint using a partial measurement/prior of the 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position The variable representing the position components of the pose + * @param[in] orientation The variable representing the orientation components of the pose + * @param[in] partial_mean The measured/prior pose as a vector + * (6x1 vector: x, y, z, roll, pitch, yaw) + * @param[in] partial_covariance The measurement/prior covariance (6x6 matrix: x, y, z, roll, pitch, yaw) + * @param[in] variable_indices The indices of the measured variables + */ + AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & partial_mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices); + + /** + * @brief Destructor + */ + virtual ~AbsolutePose3DStampedEulerConstraint() = default; + + /** + * @brief Read-only access to the measured/prior vector of mean values. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + const fuse_core::Vector6d & mean() const {return mean_;} + + /** + * @brief Read-only access to the square root information matrix. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, roll, pitch, yaw) + */ + fuse_core::Matrix6d covariance() const; + + /** + * @brief Print a human-readable description of the constraint to the provided stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream & stream = std::cout) const override; + + /** + * @brief Construct an instance of this constraint's cost function + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + +protected: + fuse_core::Vector6d mean_; //!< The measured/prior mean vector for this variable + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @brief The Boost Serialize method that serializes all of the data members in to/out of the + * archive + * + * @param[in/out] archive - The archive object that holds the serialized class members + * @param[in] version - The version of the archive being read/written. Generally unused. + */ + template + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & mean_; + archive & sqrt_information_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::AbsolutePose3DStampedEulerConstraint); + +#endif // FUSE_CONSTRAINTS__ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp new file mode 100644 index 000000000..6ac3eb1c2 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,167 @@ +/* + * 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_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp new file mode 100644 index 000000000..4e6c4b60e --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d.hpp @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_HPP_ + +#include + +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on a 3D orientation variable (quaternion) + * + * The cost function is of the form: + * + * || ||^2 + * cost(x) = || A * AngleAxis(b^-1 * q) || + * || || + * + * where the matrix A and the vector b are fixed, and q is the variable being measured, represented + * as a quaternion. The AngleAxis function converts a quaternion into a 3-vector of the form + * theta*k, where k is the unit vector axis of rotation and theta is the angle of rotation. The A + * matrix is applied to the angle-axis 3-vector. + * + * In case the user is interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorOrientation3D : public ceres::SizedCostFunction<3, 4> +{ +public: + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (qx, qy, qz) + * @param[in] b The orientation measurement or prior in order (qw, qx, qy, qz) + */ + NormalPriorOrientation3D(const fuse_core::Matrix3d & A, const fuse_core::Vector4d & b); + + /** + * @brief Destructor + */ + virtual ~NormalPriorOrientation3D() = default; + + /** + * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided + * variable/parameter values + */ + virtual bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const; + +private: + fuse_core::Matrix3d A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector4d b_; //!< The measured 3D orientation value +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_ORIENTATION_3D_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp new file mode 100644 index 000000000..adc281aac --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d.hpp @@ -0,0 +1,100 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2020, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_HPP_ + +#include + +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the position and orientation variables at once. + * + * The cost function is of the form: + * + * || [ x - b(0)] ||^2 + * cost(x) = || A * [ y - b(1)] || + * || [ z - b(2)] || + * || [ quat2angleaxis(b(3-6)^-1 * q)] || + * + * In case the user is interested in implementing a cost function of the form: + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3D : public ceres::SizedCostFunction<6, 3, 4> +{ +public: + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The pose measurement or prior in order (x, y, z, qw, qx, qy, qz) + */ + NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b); + + /** + * @brief Destructor + */ + virtual ~NormalPriorPose3D() = default; + + /** + * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided + * variable/parameter values + */ + virtual bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const; + +private: + fuse_core::Matrix6d A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector7d b_; //!< The measured 3D pose value +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp new file mode 100644 index 000000000..f2d1b3c64 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler.hpp @@ -0,0 +1,106 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_HPP_ + +#include + +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the position and orientation variables at once. + * + * The cost function is of the form: + * + * || [ x - b(0)] ||^2 + * cost(x) = || A * [ y - b(1)] || + * || [ z - b(2)] || + * || [ quat2eul(q) - b(3:5) ] || + * + * Here, the matrix A can be of variable size, thereby permitting the computation of errors for + * partial measurements. The vector b is a fixed-size 6x1. In case the user is interested in + * implementing a cost function of the form: + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3DEuler : public ceres::SizedCostFunction +{ +public: + /** + * @brief Construct a cost function instance + * + * The residual weighting matrix can vary in size, as this cost functor can be used to compute + * costs for partial vectors. The number of rows of A will be the number of dimensions for which + * you want to compute the error, and the number of columns in A will be fixed at 6. For example, + * if we just want to use the values of x, y and yaw, then \p A will be of size 3x6. + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The pose measurement or prior in order (x, y, z, roll, pitch, yaw) + */ + NormalPriorPose3DEuler(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + + /** + * @brief Destructor + */ + virtual ~NormalPriorPose3DEuler() = default; + + /** + * @brief Compute the cost values/residuals, and optionally the Jacobians, using the provided + * variable/parameter values + */ + virtual bool Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const; + +private: + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root + //!< information matrix + fuse_core::Vector6d b_; //!< The measured 3D pose value +}; + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp new file mode 100644 index 000000000..085bc2fd6 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.hpp @@ -0,0 +1,133 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ +#define FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_HPP_ + +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on both the 3D position and orientation variables at once. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost + * function that applies a prior constraint on both the 3D position and orientation variables at + * once. + * + * The cost function is of the form: + * + * cost(x) = || A * [ p - b(0:2) ] ||^2 + * || [ quat2eul(q) - b(3:5) ] || + * + * where, the matrix A and the vector b are fixed, p is the position variable, and q is the + * orientation variable. In case the user is interested in implementing a cost function of the form + * + * cost(X) = (X - mu)^T S^{-1} (X - mu) + * + * where, mu is a vector and S is a covariance matrix, then, A = S^{-1/2}, i.e the matrix A is the + * square root information matrix (the inverse of the covariance). + */ +class NormalPriorPose3DEulerCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW() + + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely the square root information matrix in + * order (x, y, z, roll, pitch, yaw) + * @param[in] b The 3D pose measurement or prior in order (x, y, z, roll, pitch, yaw) + */ + NormalPriorPose3DEulerCostFunctor(const fuse_core::MatrixXd & A, const fuse_core::Vector6d & b); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + */ + template + bool operator()(const T * const position, const T * const orientation, T * residual) const; + +private: + fuse_core::MatrixXd A_; + fuse_core::Vector6d b_; +}; + +NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor( + const fuse_core::MatrixXd & A, + const fuse_core::Vector6d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); +} + +template +bool NormalPriorPose3DEulerCostFunctor::operator()( + const T * const position, const T * const orientation, + T * residual) const +{ + T full_residuals[6]; + T orientation_rpy[3] = { + fuse_core::getRoll(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getPitch(orientation[0], orientation[1], orientation[2], orientation[3]), + fuse_core::getYaw(orientation[0], orientation[1], orientation[2], orientation[3]) + }; + + // Compute the position residual + full_residuals[0] = position[0] - T(b_(0)); + full_residuals[1] = position[1] - T(b_(1)); + full_residuals[2] = position[2] - T(b_(2)); + // Compute the orientation residual + full_residuals[3] = orientation_rpy[0] - T(b_(3)); + full_residuals[4] = orientation_rpy[1] - T(b_(4)); + full_residuals[5] = orientation_rpy[2] - T(b_(5)); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> full_residuals_map(full_residuals); + Eigen::Map> residuals_map(residual, A_.rows()); + residuals_map = A_.template cast() * full_residuals_map; + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS__NORMAL_PRIOR_POSE_3D_COST_FUNCTOR_HPP_ diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp new file mode 100644 index 000000000..424515bed --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.hpp @@ -0,0 +1,189 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ +#define FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ + +#include + +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents a measurement on the difference between two 3D poses. + * + * This type of constraint arises in many situations. Many types of incremental odometry + * measurements (e.g., visual odometry) measure the change in the pose, not the pose directly. This + * constraint holds the measured 3D pose change and the measurement uncertainty/covariance. + */ +class RelativePose3DStampedEulerConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(RelativePose3DStampedEulerConstraint) + + /** + * @brief Default constructor + */ + RelativePose3DStampedEulerConstraint() = default; + + /** + * @brief Create a constraint using a measurement/prior of the relative 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 The variable representing the position components of the first pose + * @param[in] orientation1 The variable representing the orientation components of the first pose + * @param[in] position2 The variable representing the position components of the second pose + * @param[in] orientation2 The variable representing the orientation components of the second pose + * @param[in] delta The measured change in the pose + * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) + * @param[in] covariance The measurement covariance (6x6 matrix: dx, dy, dz, droll, dpitch, dyaw) + */ + RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector6d & delta, + const fuse_core::Matrix6d & covariance); + + /** + * @brief Create a constraint using a measurement/prior of the relative 3D pose + * + * @param[in] source The name of the sensor or motion model that generated this constraint + * @param[in] position1 The variable representing the position components of the first pose + * @param[in] orientation1 The variable representing the orientation components of the first pose + * @param[in] position2 The variable representing the position components of the second pose + * @param[in] orientation2 The variable representing the orientation components of the second pose + * @param[in] partial_delta The measured change in the pose + * (6x1 vector: dx, dy, dz, droll, dpitch, dyaw) + * @param[in] partial_covariance The measurement subset covariance (max 6x6 matrix: x, y, z, droll, dpitch, dyaw) + * @param[in] variable_indices The indices of the measured variables + */ + RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector6d & partial_delta, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices); + + /** + * @brief Destructor + */ + virtual ~RelativePose3DStampedEulerConstraint() = default; + + /** + * @brief Read-only access to the measured pose change. + */ + const fuse_core::Vector6d & delta() const {return delta_;} + + /** + * @brief Read-only access to the square root information matrix. + */ + const fuse_core::MatrixXd & sqrtInformation() const {return sqrt_information_;} + + /** + * @brief Compute the measurement covariance matrix. + */ + fuse_core::Matrix6d covariance() const; + + /** + * @brief Print a human-readable description of the constraint to the provided stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream & stream = std::cout) const override; + + /** + * @brief Access the cost function for this constraint + * + * The function caller will own the new cost function instance. It is the responsibility of the + * caller to delete the cost function object when it is no longer needed. If the pointer is + * provided to a Ceres::Problem object, the Ceres::Problem object will takes ownership of the + * pointer and delete it during destruction. + * + * @return A base pointer to an instance of a derived CostFunction. + */ + ceres::CostFunction * costFunction() const override; + +protected: + fuse_core::Vector6d delta_; //!< The measured pose change (dx, dy, dz, droll, dpitch, dyaw) + fuse_core::MatrixXd sqrt_information_; //!< The square root information matrix (derived from the + //!< covariance matrix) + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + + /** + * @brief The Boost Serialize method that serializes all of the data members in to/out of the + * archive + * + * @param[in/out] archive - The archive object that holds the serialized class members + * @param[in] version - The version of the archive being read/written. Generally unused. + */ + template + void serialize(Archive & archive, const unsigned int /* version */) + { + archive & boost::serialization::base_object(*this); + archive & delta_; + archive & sqrt_information_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::RelativePose3DStampedEulerConstraint); + +#endif // FUSE_CONSTRAINTS__RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_HPP_ diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index 9aa7593fc..68dd8a7a1 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -36,19 +36,29 @@ #include BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteOrientation2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePosition3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint, + fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint, + fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteOrientation2DStampedConstraint, fuse_core::Constraint); @@ -61,6 +71,12 @@ PLUGINLIB_EXPORT_CLASS( PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint, + fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS( fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint, + fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index cc64e8d75..324b7fbc6 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -31,14 +32,13 @@ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE * POSSIBILITY OF SUCH DAMAGE. */ -#include #include #include #include #include -#include +#include #include namespace fuse_constraints @@ -74,8 +74,7 @@ void AbsolutePose3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * AbsolutePose3DStampedConstraint::costFunction() const { - return new ceres::AutoDiffCostFunction( - new NormalPriorPose3DCostFunctor(sqrt_information_, mean_)); + return new NormalPriorPose3D(sqrt_information_, mean_); } } // namespace fuse_constraints diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..8afee0c88 --- /dev/null +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,141 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include + +#include + +#include +#include +#include + +namespace fuse_constraints +{ + +AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & mean, + const fuse_core::Matrix6d & covariance) +: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT + mean_(mean), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position, + const fuse_variables::Orientation3DStamped & orientation, + const fuse_core::Vector6d & partial_mean, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices) +: fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}), // NOLINT + mean_(partial_mean) +{ + // Compute the partial sqrt information matrix of the provided cov matrix + fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + + // Assemble a sqrt information matrix from the provided values, but in proper Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); + for (size_t i = 0; i < variable_indices.size(); ++i) { + sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); + } +} + +fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const +{ + if (sqrt_information_.rows() == 6) { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + // Otherwise we need to compute the pseudoinverse + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + +void AbsolutePose3DStampedEulerConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position variable: " << variables().at(0) << "\n" + << " orientation variable: " << variables().at(1) << "\n" + << " mean: " << mean().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; + + if (loss()) { + stream << " loss: "; + loss()->print(stream); + } +} + +ceres::CostFunction * AbsolutePose3DStampedEulerConstraint::costFunction() const +{ + return new NormalPriorPose3DEuler(sqrt_information_, mean_); + + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could + // use automatic differentiation as follows: + + // return new ceres::AutoDiffCostFunction( + // new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), + // sqrt_information_.rows()); + + // And including the followings: + // #include + // #include +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsolutePose3DStampedEulerConstraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::AbsolutePose3DStampedEulerConstraint, + fuse_core::Constraint); diff --git a/fuse_constraints/src/normal_prior_orientation_3d.cpp b/fuse_constraints/src/normal_prior_orientation_3d.cpp new file mode 100644 index 000000000..e8c44bab0 --- /dev/null +++ b/fuse_constraints/src/normal_prior_orientation_3d.cpp @@ -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. + */ +#include +#include + +#include +#include + +namespace fuse_constraints +{ + +NormalPriorOrientation3D::NormalPriorOrientation3D( + const fuse_core::Matrix3d & A, + const fuse_core::Vector4d & b) +: A_(A), + b_(b) +{ +} + +bool NormalPriorOrientation3D::Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const +{ + double variable[4] = + { + parameters[0][0], + parameters[0][1], + parameters[0][2], + parameters[0][3], + }; + + double observation_inverse[4] = + { + b_(0), + -b_(1), + -b_(2), + -b_(3) + }; + + double difference[4]; + double j_product[16]; + double j_quat2angle[12]; + + // TODO(giafranchini): these jacobians should be populated only if jacobians[1] != nullptr + fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); + fuse_core::quaternionToAngleAxis(difference, residuals, j_quat2angle); // orientation angle-axis + + // Scale the residuals by the square root information matrix to account for the measurement + // uncertainty. + Eigen::Map residuals_map(residuals); + residuals_map.applyOnTheLeft(A_); + + if (jacobians != nullptr) { + if (jacobians[0] != nullptr) { + Eigen::Map> j_map(jacobians[0]); + Eigen::Map j_product_map(j_product); + Eigen::Map> j_quat2angle_map(j_quat2angle); + j_map = A_ * j_quat2angle_map * j_product_map; + } + } + return true; +} + +} // namespace fuse_constraints diff --git a/fuse_constraints/src/normal_prior_pose_3d.cpp b/fuse_constraints/src/normal_prior_pose_3d.cpp new file mode 100644 index 000000000..c3743a3f3 --- /dev/null +++ b/fuse_constraints/src/normal_prior_pose_3d.cpp @@ -0,0 +1,107 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include + +namespace fuse_constraints +{ + +NormalPriorPose3D::NormalPriorPose3D(const fuse_core::Matrix6d & A, const fuse_core::Vector7d & b) +: A_(A), + b_(b) +{ +} + +bool NormalPriorPose3D::Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const +{ + + double variable[4] = + { + parameters[1][0], + parameters[1][1], + parameters[1][2], + parameters[1][3], + }; + + double observation_inverse[4] = + { + b_(3), + -b_(4), + -b_(5), + -b_(6) + }; + + double difference[4]; + double j_product[16]; + double j_quat2angle[12]; + + // Compute position residuals + residuals[0] = parameters[0][0] - b_[0]; + residuals[1] = parameters[0][1] - b_[1]; + residuals[2] = parameters[0][2] - b_[2]; + // Compute orientation residuals + + // TODO(giafranchini): this is repeated code, it should be better to include normal_prior_orientation_3d.hpp + // and use that to compute residuals and jacobians. + fuse_core::quaternionProduct(observation_inverse, variable, difference, j_product); + fuse_core::quaternionToAngleAxis(difference, &residuals[3], j_quat2angle); // orientation angle-axis + + // Scale the residuals by the square root information matrix to account for the measurement + // uncertainty. + Eigen::Map residuals_map(residuals); + residuals_map.applyOnTheLeft(A_); + + if (jacobians != nullptr) { + // Jacobian of the residuals wrt position parameter block + if (jacobians[0] != nullptr) { + Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); + } + // Jacobian of the residuals wrt orientation parameter block + if (jacobians[1] != nullptr) { + Eigen::Map j_product_map(j_product); + Eigen::Map> j_quat2angle_map(j_quat2angle); + Eigen::Map j1_map(jacobians[1], num_residuals(), 4); + j1_map = A_.rightCols<3>() * j_quat2angle_map * j_product_map; + } + } + return true; +} + +} // namespace fuse_constraints diff --git a/fuse_constraints/src/normal_prior_pose_3d_euler.cpp b/fuse_constraints/src/normal_prior_pose_3d_euler.cpp new file mode 100644 index 000000000..033083baa --- /dev/null +++ b/fuse_constraints/src/normal_prior_pose_3d_euler.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 +#include +#include + +namespace fuse_constraints +{ + +NormalPriorPose3DEuler::NormalPriorPose3DEuler( + const fuse_core::MatrixXd & A, + const fuse_core::Vector6d & b) +: A_(A), + b_(b) +{ + CHECK_GT(A_.rows(), 0); + CHECK_EQ(A_.cols(), 6); + set_num_residuals(A_.rows()); +} + +bool NormalPriorPose3DEuler::Evaluate( + double const * const * parameters, + double * residuals, + double ** jacobians) const +{ + fuse_core::Vector6d full_residuals; + double orientation_rpy[3]; + double j_quat2rpy[12]; + + fuse_core::quaternion2rpy(parameters[1], orientation_rpy, j_quat2rpy); + + // Compute the position residual + full_residuals(0) = parameters[0][0] - b_(0); + full_residuals(1) = parameters[0][1] - b_(1); + full_residuals(2) = parameters[0][2] - b_(2); + + // Compute the orientation residual + full_residuals(3) = orientation_rpy[0] - b_(3); + full_residuals(4) = orientation_rpy[1] - b_(4); + full_residuals(5) = orientation_rpy[2] - b_(5); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> residuals_map(residuals, A_.rows()); + residuals_map = A_ * full_residuals; + + if (jacobians != nullptr) { + // Jacobian wrt position + if (jacobians[0] != nullptr) { + Eigen::Map(jacobians[0], num_residuals(), 3) = A_.leftCols<3>(); + } + // Jacobian wrt orientation + if (jacobians[1] != nullptr) { + Eigen::Map> j_quat2angle_map(j_quat2rpy); + Eigen::Map(jacobians[1], num_residuals(), 4) = + A_.rightCols<3>() * j_quat2angle_map; + } + } + return true; +} + +} // namespace fuse_constraints diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp index 7d627e117..1aefeb5fe 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -74,6 +74,7 @@ void RelativePose3DStampedConstraint::print(std::ostream & stream) const ceres::CostFunction * RelativePose3DStampedConstraint::costFunction() const { + // TODO(giafranchini): implement cost function with analytical derivatives return new ceres::AutoDiffCostFunction( new NormalDeltaPose3DCostFunctor(sqrt_information_, delta_)); } diff --git a/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..a2bc7706a --- /dev/null +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,135 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include + +#include +#include +#include +#include + +namespace fuse_constraints +{ + +RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector6d & delta, + const fuse_core::Matrix6d & covariance) +: fuse_core::Constraint( + source, + {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT + delta_(delta), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +RelativePose3DStampedEulerConstraint::RelativePose3DStampedEulerConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_core::Vector6d & partial_delta, + const fuse_core::MatrixXd & partial_covariance, + const std::vector & variable_indices) +: fuse_core::Constraint( + source, + {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}), // NOLINT + delta_(partial_delta) +{ + // Compute the partial sqrt information matrix of the provided cov matrix + fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + + // Assemble a sqrt information matrix from the provided values, but in proper Variable order + // + // What are we doing here? The constraint equation is defined as: cost(x) = ||A * (x - b)||^2 + // If we are measuring a subset of dimensions, we only want to produce costs for the measured + // dimensions. But the variable vectors will be full sized. We can make this all work out by + // creating a non-square A, where each row computes a cost for one measured dimensions, + // and the columns are in the order defined by the variable. + + // Fill in the rows of the sqrt information matrix corresponding to the measured dimensions + sqrt_information_ = fuse_core::MatrixXd::Zero(variable_indices.size(), 6); + for (size_t i = 0; i < variable_indices.size(); ++i) { + sqrt_information_.col(variable_indices[i]) = partial_sqrt_information.col(i); + } +} + +fuse_core::Matrix6d RelativePose3DStampedEulerConstraint::covariance() const +{ + // We need to compute the pseudoinverse + // cov = (sqrt_info' * sqrt_info)^-1 + // With some linear algebra, we can swap the transpose and the inverse. + // cov = (sqrt_info^-1) * (sqrt_info^-1)' + // But sqrt_info _may_ not be square. So we need to compute the pseudoinverse instead. + // Eigen doesn't have a pseudoinverse function (for probably very legitimate reasons). + // So we set the right hand side to identity, then solve using one of Eigen's many decompositions. + auto I = fuse_core::MatrixXd::Identity(sqrt_information_.rows(), sqrt_information_.cols()); + fuse_core::MatrixXd pinv = sqrt_information_.colPivHouseholderQr().solve(I); + return pinv * pinv.transpose(); +} + +void RelativePose3DStampedEulerConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position1 variable: " << variables().at(0) << "\n" + << " orientation1 variable: " << variables().at(1) << "\n" + << " position2 variable: " << variables().at(2) << "\n" + << " orientation2 variable: " << variables().at(3) << "\n" + << " delta: " << delta().transpose() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; +} + +ceres::CostFunction * RelativePose3DStampedEulerConstraint::costFunction() const +{ + // TODO(giafranchini): implement cost function with analytical Jacobians + return new ceres::AutoDiffCostFunction( + new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_), + sqrt_information_.rows()); +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::RelativePose3DStampedEulerConstraint); +PLUGINLIB_EXPORT_CLASS( + fuse_constraints::RelativePose3DStampedEulerConstraint, + fuse_core::Constraint); diff --git a/fuse_constraints/test/CMakeLists.txt b/fuse_constraints/test/CMakeLists.txt index 426322c5c..ec69a351b 100644 --- a/fuse_constraints/test/CMakeLists.txt +++ b/fuse_constraints/test/CMakeLists.txt @@ -6,13 +6,17 @@ set(TEST_TARGETS test_absolute_orientation_3d_stamped_euler_constraint test_absolute_pose_2d_stamped_constraint test_absolute_pose_3d_stamped_constraint + test_absolute_pose_3d_stamped_euler_constraint test_marginal_constraint test_marginalize_variables test_normal_delta_pose_2d test_normal_prior_pose_2d + test_normal_prior_pose_3d + test_normal_prior_pose_3d_euler test_relative_constraint test_relative_pose_2d_stamped_constraint test_relative_pose_3d_stamped_constraint + test_relative_pose_3d_stamped_euler_constraint test_uuid_ordering test_variable_constraints ) diff --git a/fuse_constraints/test/cost_function_gtest.hpp b/fuse_constraints/test/cost_function_gtest.hpp index 3687d0504..9fd98e470 100644 --- a/fuse_constraints/test/cost_function_gtest.hpp +++ b/fuse_constraints/test/cost_function_gtest.hpp @@ -38,6 +38,7 @@ #include #include +#include /** * @brief A helper function to compare a expected and actual cost function. @@ -52,7 +53,8 @@ */ static void ExpectCostFunctionsAreEqual( const ceres::CostFunction & cost_function, - const ceres::CostFunction & actual_cost_function) + const ceres::CostFunction & actual_cost_function, + const double tol = 1e-16) { EXPECT_EQ(cost_function.num_residuals(), actual_cost_function.num_residuals()); const size_t num_residuals = cost_function.num_residuals(); @@ -68,8 +70,22 @@ static void ExpectCostFunctionsAreEqual( } std::unique_ptr parameters(new double[num_parameters]); - for (size_t i = 0; i < num_parameters; ++i) { - parameters[i] = static_cast(i) + 1.0; + if ((num_parameters == 7) && (parameter_block_sizes[0] == 3) && + (parameter_block_sizes[1] == 4)) + { + // Special case for parameters[1] as quaternion + for (size_t i = 0; i < 3; i++) { + parameters[i] = static_cast(i) + 1.0; + } + Eigen::Quaterniond q = Eigen::Quaterniond::UnitRandom(); + parameters[3] = q.w(); + parameters[4] = q.x(); + parameters[5] = q.y(); + parameters[6] = q.z(); + } else { + for (size_t i = 0; i < num_parameters; ++i) { + parameters[i] = static_cast(i) + 1.0; + } } std::unique_ptr residuals(new double[num_residuals]); @@ -96,7 +112,7 @@ static void ExpectCostFunctionsAreEqual( 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( @@ -108,11 +124,11 @@ static void ExpectCostFunctionsAreEqual( 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 9787d83f6..5b79011fc 100644 --- a/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/test/test_absolute_orientation_3d_stamped_euler_constraint.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -150,7 +151,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationFull) ceres::Solve(options, &problem, &summary); // Check - Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * + Eigen::Quaterniond expected = + Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); @@ -229,7 +231,8 @@ TEST(AbsoluteOrientation3DStampedEulerConstraint, OptimizationPartial) ceres::Solve(options, &problem, &summary); // Check - Eigen::Quaterniond expected = Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * + Eigen::Quaterniond expected = + Eigen::AngleAxisd(0.5, Eigen::Vector3d::UnitZ()) * Eigen::AngleAxisd(1.5, Eigen::Vector3d::UnitY()) * Eigen::AngleAxisd(1.0, Eigen::Vector3d::UnitX()); EXPECT_NEAR(expected.w(), orientation_variable->w(), 5.0e-3); diff --git a/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp new file mode 100644 index 000000000..5d605f42d --- /dev/null +++ b/fuse_constraints/test/test_absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,501 @@ +/* + * 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 + +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; +using fuse_constraints::AbsolutePose3DStampedEulerConstraint; + + +TEST(AbsolutePose3DStampedEulerConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + AbsolutePose3DStampedEulerConstraint constraint( + "test", position_variable, orientation_variable, + mean, cov)); +} + +TEST(AbsolutePose3DStampedEulerConstraint, ConstructorPartial) +{ + // Construct a constraint just to make sure it compiles. + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector6d mean_partial; + mean_partial << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; + + fuse_core::Matrix5d cov_partial; + /* *INDENT-OFF* */ + cov_partial << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + AbsolutePose3DStampedEulerConstraint constraint( + "test", position_variable, orientation_variable, + mean_partial, cov_partial, variable_indices)); +} + +TEST(AbsolutePose3DStampedEulerConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( + "mo")); + + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, + mean, + cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix6d expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + /* *INDENT-ON* */ + fuse_core::Matrix6d expected_cov = cov; + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); +} + +TEST(AbsolutePose3DStampedEulerConstraint, CovariancePartial) +{ + // Verify the covariance <--> sqrt information conversions are correct + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("mo")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate( + "mo")); + + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector6d mean; + mean << 1.0, 0.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.02943174290333, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedEulerConstraint constraint("test", position_variable, orientation_variable, + mean, cov, variable_indices); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 1.90431982, 0.0, 1.57962714, -0.37235015, -1.81200356, -0.51202133, // NOLINT + 0. , 0.0, 3.82674687, 2.80341172 , -2.68168479, -2.88943844, // NOLINT + 0. , 0.0, 0. , 1.83006791 , -0.69691741, -1.17412835, // NOLINT + 0. , 0.0, 0. , 0. , 0.95330283, -0.76965441, // NOLINT + 0. , 0.0, 0. , 0. , 0. , 0.68147774; // NOLINT + /* *INDENT-ON* */ + + fuse_core::Matrix6d expected_cov; + /* *INDENT-OFF* */ + expected_cov << 2.0847236144069, 0.0, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.02943174290333, 0.0, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 0.0, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 0.0, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 0.0, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-8); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-8); +} + +TEST(AbsolutePose3DStampedEulerConstraint, Optimization) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are + // generated. Create the variables + auto position_variable = Position3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = -3.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + // Create an absolute pose constraint + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.0, 0.0, 0.0; + + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( + "test", + *position_variable, + *orientation_variable, + mean, + cov); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + position_variable->data(), + position_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position_variable->localParameterization()); +#else + position_variable->manifold()); +#endif + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(2.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); + covariance.GetCovarianceBlock( + position_variable->data(), + position_variable->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + // High tolerance here, but also high values of covariance + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); +} + +TEST(AbsolutePose3DStampedEulerConstraint, OptimizationPartial) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are + // generated. Create the variables. Version for partial measurements + auto position_variable = Position3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position_variable->x() = 1.5; + position_variable->y() = 1.0; + position_variable->z() = 10.0; + + auto orientation_variable = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->w() = 0.952; + orientation_variable->x() = 0.038; + orientation_variable->y() = -0.189; + orientation_variable->z() = 0.239; + + // Create an absolute pose constraint + std::vector variable_indices {0, 2, 3, 4, 5}; + + fuse_core::Vector6d mean; + mean << 1.0, 0.0, 3.0, 0.0, 0.0, 0.0; + + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 1.0, 0.2, 0.3, 0.4, 0.5, + 0.2, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + auto constraint = AbsolutePose3DStampedEulerConstraint::make_shared( + "test", + *position_variable, + *orientation_variable, + mean, + cov, + variable_indices); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + position_variable->data(), + position_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position_variable->localParameterization()); +#else + position_variable->manifold()); +#endif + problem.AddParameterBlock( + orientation_variable->data(), + orientation_variable->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation_variable->localParameterization()); +#else + orientation_variable->manifold()); +#endif + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + + problem.AddResidualBlock( + constraint->costFunction(), + constraint->lossFunction(), + parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(1.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(1.0, position_variable->y(), 1.0e-5); // This is not measured so it will not change + EXPECT_NEAR(3.0, position_variable->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation_variable->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_variable->z(), 1.0e-3); + + // Compute the covariance + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + covariance_blocks.emplace_back(position_variable->data(), orientation_variable->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::MatrixXd cov_pos_pos(position_variable->size(), position_variable->size()); + covariance.GetCovarianceBlock( + position_variable->data(), + position_variable->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation_variable->localSize(), + orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position_variable->data(), orientation_variable->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.2, 0.3, 0.4, 0.5, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.2, 0.0, 3.0, 0.2, 0.1, 0.2, + 0.3, 0.0, 0.2, 4.0, 0.3, 0.4, + 0.4, 0.0, 0.1, 0.3, 5.0, 0.5, + 0.5, 0.0, 0.2, 0.4, 0.5, 6.0; + /* *INDENT-ON* */ + + // High tolerance here, but also high values of covariance + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); +} + +TEST(AbsolutePose3DStampedEulerConstraint, Serialization) +{ + // Construct a constraint + Position3DStamped position_variable(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(rclcpp::Time(1234, 5678), + fuse_core::uuid::generate("walle")); + + fuse_core::Vector6d mean; + mean << 1.0, 2.0, 3.0, 0.1, 0.1, 1.0; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + AbsolutePose3DStampedEulerConstraint expected("test", position_variable, orientation_variable, + mean, + cov); + + // Serialize the constraint into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new constraint from that same stream + AbsolutePose3DStampedEulerConstraint actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_MATRIX_EQ(expected.mean(), actual.mean()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); +} diff --git a/fuse_constraints/test/test_normal_prior_pose_3d.cpp b/fuse_constraints/test/test_normal_prior_pose_3d.cpp new file mode 100644 index 000000000..f3ba07a3d --- /dev/null +++ b/fuse_constraints/test/test_normal_prior_pose_3d.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 "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, 1e-14); +} 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..50301c277 --- /dev/null +++ b/fuse_constraints/test/test_normal_prior_pose_3d_euler.cpp @@ -0,0 +1,207 @@ +/* + * 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, 1e-13); +} + +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, 1e-13); +} + +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, 1e-13); +} + +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, 1e-13); +} + +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, 1e-13); +} 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..ee0724d21 --- /dev/null +++ b/fuse_constraints/test/test_relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,731 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +using fuse_variables::Orientation3DStamped; +using fuse_variables::Position3DStamped; +using fuse_constraints::AbsolutePose3DStampedConstraint; +using fuse_constraints::AbsolutePose3DStampedEulerConstraint; +using fuse_constraints::RelativePose3DStampedEulerConstraint; + + +TEST(RelativePose3DStampedEulerConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + RelativePose3DStampedEulerConstraint constraint( + "test", position1, orientation1, position2, + orientation2, delta, cov)); +} + +TEST(RelativePose3DStampedEulerConstraint, ConstructorPartial) +{ + // Construct a constraint just to make sure it compiles. + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + std::vector indices {0, 1, 3, 4, 5}; + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + EXPECT_NO_THROW( + RelativePose3DStampedEulerConstraint constraint( + "test", position1, orientation1, position2, + orientation2, delta, cov, indices)); +} + +TEST(RelativePose3DStampedEulerConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + RelativePose3DStampedEulerConstraint constraint( + "test", + position1, + orientation1, + position2, + orientation2, + delta, + cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix6d expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 2.12658752275893, 1.20265444927878, 4.71225672571804, 1.43587520991272, -4.12764062992821, -3.19509486240291, // NOLINT + 0.0, 2.41958656956248, 5.93151964116945, 3.72535320852517, -4.23326858606213, -5.27776664777548, // NOLINT + 0.0, 0.0, 3.82674686590005, 2.80341171946161, -2.68168478581452, -2.8894384435255, // NOLINT + 0.0, 0.0, 0.0, 1.83006791372784, -0.696917410192509, -1.17412835464633, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.953302832761324, -0.769654414882847, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.681477739760948; // NOLINT + /* *INDENT-ON* */ + fuse_core::Matrix6d expected_cov = cov; + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-9); +} + +TEST(RelativePose3DStampedEulerConstraint, CovariancePartial) +{ + // Verify the covariance <--> sqrt information conversions are correct + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + std::vector indices {0, 1, 3, 4, 5}; + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 0.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octiave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix5d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.96120532313878, 1.35471905449013, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + RelativePose3DStampedEulerConstraint constraint( + "test", + position1, + orientation1, + position2, + orientation2, + delta, + cov, + indices); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix expected_sqrt_info; + /* *INDENT-OFF* */ + expected_sqrt_info << 1.7687, -0.1286, 0.0, -1.3877, -0.6508, 0.6747, // NOLINT + 0.0, 1.3117, 0.0, -0.3361, -0.0415, -0.4332, // NOLINT + 0.0, 0.0, 0.0, 1.8301, -0.6969, -1.1741, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.9533, -0.7697, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.6815; // NOLINT + /* *INDENT-ON* */ + /* *INDENT-ON* */ + fuse_core::Matrix6d expected_cov; + + expected_cov << 2.0847236144069, 1.10752598122138, 0.0, 1.96120532313878, 1.96735470687891, + 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.0, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, // NOLINT + 1.96120532313878, 1.35471905449013, 0.0, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 0.0, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 0.0, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + + // Compare + EXPECT_MATRIX_NEAR(expected_cov, constraint.covariance(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_sqrt_info, constraint.sqrtInformation(), 1.0e-4); +} + +TEST(RelativePose3DStampedEulerConstraint, Optimization) +{ + // Optimize a two-pose system with a pose prior and a relative pose constraint + // Verify the expected poses and covariances are generated. + // Create two poses + auto position1 = + Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position1->x() = 1.5; + position1->y() = -3.0; + position1->z() = 10.0; + + auto orientation1 = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation1->w() = 0.952; + orientation1->x() = 0.038; + orientation1->y() = -0.189; + orientation1->z() = 0.239; + + auto position2 = + Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + position2->x() = -1.5; + position2->y() = 3.0; + position2->z() = -10.0; + + auto orientation2 = Orientation3DStamped::make_shared( + rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + orientation2->w() = 0.944; + orientation2->x() = -0.128; + orientation2->y() = 0.145; + orientation2->z() = -0.269; + + // Create an absolute pose constraint at the origin + fuse_core::Vector7d mean_origin; + mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); + auto prior = AbsolutePose3DStampedConstraint::make_shared( + "test", + *position1, + *orientation1, + mean_origin, + cov_origin); + + // Create a relative pose constraint for 1m in the x direction + fuse_core::Vector6d mean_delta; + mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_delta = fuse_core::Matrix6d::Identity(); + auto relative = RelativePose3DStampedEulerConstraint::make_shared( + "test", + *position1, + *orientation1, + *position2, + *orientation2, + mean_delta, + cov_delta); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation1->data(), + orientation1->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation1->localParameterization()); +#else + orientation1->manifold()); +#endif + problem.AddParameterBlock( + position1->data(), + position1->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position1->localParameterization()); +#else + position1->manifold()); +#endif + problem.AddParameterBlock( + orientation2->data(), + orientation2->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation2->localParameterization()); +#else + orientation2->manifold()); +#endif + problem.AddParameterBlock( + position2->data(), + position2->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position2->localParameterization()); +#else + position2->manifold()); +#endif + std::vector prior_parameter_blocks; + prior_parameter_blocks.push_back(position1->data()); + prior_parameter_blocks.push_back(orientation1->data()); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); + std::vector relative_parameter_blocks; + relative_parameter_blocks.push_back(position1->data()); + relative_parameter_blocks.push_back(orientation1->data()); + relative_parameter_blocks.push_back(position2->data()); + relative_parameter_blocks.push_back(orientation2->data()); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position1->x(), 1.0e-5); + EXPECT_NEAR(0.0, position1->y(), 1.0e-5); + EXPECT_NEAR(0.0, position1->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation1->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->z(), 1.0e-3); + EXPECT_NEAR(1.0, position2->x(), 1.0e-5); + EXPECT_NEAR(0.0, position2->y(), 1.0e-5); + EXPECT_NEAR(0.0, position2->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation2->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->z(), 1.0e-3); + + // Compute the marginal covariance for pose1 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position1->data(), position1->data()); + covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); + covariance_blocks.emplace_back(position1->data(), orientation1->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position1->size(), position1->size()); + covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(3, 3); + covariance.GetCovarianceBlockInTangentSpace( + orientation1->data(), + orientation1->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->size(), 3); + covariance.GetCovarianceBlockInTangentSpace( + position1->data(), + orientation1->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); + } + + // Compute the marginal covariance for pose2 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position2->data(), position2->data()); + covariance_blocks.emplace_back(position2->data(), orientation2->data()); + covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); + + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position2->size(), position2->size()); + covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(3, 3); + covariance.GetCovarianceBlockInTangentSpace( + orientation2->data(), + orientation2->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->size(), 3); + covariance.GetCovarianceBlockInTangentSpace( + position2->data(), + orientation2->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 2.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 3.0, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 3.0, 0.0, -1.0, 0.0, + 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, + 0.0, 0.0, -1.0, 0.0, 2.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 2.0; + /* *INDENT-ON* */ + + // High tolerance here, but also high values of covariance + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); + } +} + +TEST(RelativePose3DStampedEulerConstraint, OptimizationPartial) +{ + // Optimize a two-pose system with a pose prior and a relative pose constraint + // Verify the expected poses and covariances are generated. + // Create two poses + auto position1 = + Position3DStamped::make_shared(rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + position1->x() = 1.5; + position1->y() = -3.0; + position1->z() = 10.0; + + auto orientation1 = Orientation3DStamped::make_shared( + rclcpp::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation1->w() = 0.952; + orientation1->x() = 0.038; + orientation1->y() = -0.189; + orientation1->z() = 0.239; + + auto position2 = + Position3DStamped::make_shared(rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + position2->x() = -1.5; + position2->y() = 3.0; + position2->z() = -10.0; + + auto orientation2 = Orientation3DStamped::make_shared( + rclcpp::Time(2, 0), fuse_core::uuid::generate("spra")); + orientation2->w() = 0.944; + orientation2->x() = -0.128; + orientation2->y() = 0.145; + orientation2->z() = -0.269; + + std::vector indices {0, 1, 3, 4, 5}; + + // Create an absolute pose constraint at the origin + fuse_core::Vector7d mean_origin; + mean_origin << 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0; + fuse_core::Matrix6d cov_origin = fuse_core::Matrix6d::Identity(); + auto prior = AbsolutePose3DStampedConstraint::make_shared( + "test", + *position1, + *orientation1, + mean_origin, + cov_origin); + + // Create a relative pose constraint for 1m in the x direction + fuse_core::Vector6d mean_delta; + mean_delta << 1.0, 0.0, 0.0, 0.0, 0.0, 0.0; + fuse_core::Matrix5d cov_delta = fuse_core::Matrix5d::Identity(); + auto relative = RelativePose3DStampedEulerConstraint::make_shared( + "test", + *position1, + *orientation1, + *position2, + *orientation2, + mean_delta, + cov_delta, + indices); + + // Create a relative pose constraint for 1m in the y direction + std::vector indices_y {1, 2, 3, 4, 5}; + fuse_core::Vector6d mean_delta_y; + mean_delta_y << 0.0, 1.0, 0.0, 0.0, 0.0, 0.0; + fuse_core::Matrix5d cov_delta_y = fuse_core::Matrix5d::Identity(); + auto relative_y = RelativePose3DStampedEulerConstraint::make_shared( + "test", + *position1, + *orientation1, + *position2, + *orientation2, + mean_delta_y, + cov_delta_y, + indices_y); + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + problem.AddParameterBlock( + orientation1->data(), + orientation1->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation1->localParameterization()); +#else + orientation1->manifold()); +#endif + problem.AddParameterBlock( + position1->data(), + position1->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position1->localParameterization()); +#else + position1->manifold()); +#endif + problem.AddParameterBlock( + orientation2->data(), + orientation2->size(), +#if !CERES_SUPPORTS_MANIFOLDS + orientation2->localParameterization()); +#else + orientation2->manifold()); +#endif + problem.AddParameterBlock( + position2->data(), + position2->size(), +#if !CERES_SUPPORTS_MANIFOLDS + position2->localParameterization()); +#else + position2->manifold()); +#endif + std::vector prior_parameter_blocks; + prior_parameter_blocks.push_back(position1->data()); + prior_parameter_blocks.push_back(orientation1->data()); + problem.AddResidualBlock( + prior->costFunction(), + prior->lossFunction(), + prior_parameter_blocks); + std::vector relative_parameter_blocks; + relative_parameter_blocks.push_back(position1->data()); + relative_parameter_blocks.push_back(orientation1->data()); + relative_parameter_blocks.push_back(position2->data()); + relative_parameter_blocks.push_back(orientation2->data()); + problem.AddResidualBlock( + relative->costFunction(), + relative->lossFunction(), + relative_parameter_blocks); + std::vector relative_parameter_blocks_y; + relative_parameter_blocks_y.push_back(position1->data()); + relative_parameter_blocks_y.push_back(orientation1->data()); + relative_parameter_blocks_y.push_back(position2->data()); + relative_parameter_blocks_y.push_back(orientation2->data()); + problem.AddResidualBlock( + relative_y->costFunction(), + relative_y->lossFunction(), + relative_parameter_blocks_y); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(0.0, position1->x(), 1.0e-5); + EXPECT_NEAR(0.0, position1->y(), 1.0e-5); + EXPECT_NEAR(0.0, position1->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation1->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation1->z(), 1.0e-3); + EXPECT_NEAR(1.0, position2->x(), 1.0e-5); + EXPECT_NEAR(0.5, position2->y(), 1.0e-5); + EXPECT_NEAR(0.0, position2->z(), 1.0e-5); + EXPECT_NEAR(1.0, orientation2->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation2->z(), 1.0e-3); + + // Compute the marginal covariance for pose1 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position1->data(), position1->data()); + covariance_blocks.emplace_back(orientation1->data(), orientation1->data()); + covariance_blocks.emplace_back(position1->data(), orientation1->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position1->size(), position1->size()); + covariance.GetCovarianceBlock(position1->data(), position1->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation1->data(), + orientation1->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position1->data(), + orientation1->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 1.0; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-9); + } + + // Compute the marginal covariance for pose2 + { + std::vector> covariance_blocks; + covariance_blocks.emplace_back(position2->data(), position2->data()); + covariance_blocks.emplace_back(position2->data(), orientation2->data()); + covariance_blocks.emplace_back(orientation2->data(), orientation2->data()); + + ceres::Covariance::Options cov_options; + cov_options.algorithm_type = ceres::DENSE_SVD; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + + fuse_core::MatrixXd cov_pos_pos(position2->size(), position2->size()); + covariance.GetCovarianceBlock(position2->data(), position2->data(), cov_pos_pos.data()); + + fuse_core::MatrixXd cov_or_or(orientation1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation2->data(), + orientation2->data(), cov_or_or.data()); + + fuse_core::MatrixXd cov_pos_or(position1->localSize(), orientation1->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + position2->data(), + orientation2->data(), cov_pos_or.data()); + + // Assemble the full covariance from the covariance blocks + fuse_core::Matrix6d actual_covariance; + actual_covariance << cov_pos_pos, cov_pos_or, cov_pos_or.transpose(), cov_or_or; + + // Define the expected covariance + fuse_core::Matrix6d expected_covariance; + /* *INDENT-OFF* */ + expected_covariance << + 2.25, -0.5, 0.0, 0.0, 0.0, -0.5, + -0.5, 2.5, 0.0, 0.0, 0.0, 1.0, + 0.0, 0.0, 3.25, 0.5, -1.0, 0.0, + 0.0, 0.0, 0.5, 1.5, 0.0, 0.0, + 0.0, 0.0, -1.0, 0.0, 1.5, 0.0, + -0.5, 1.0, 0.0, 0.0, 0.0, 1.5; + /* *INDENT-ON* */ + + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-2); + } +} + +TEST(RelativePose3DStampedEulerConstraint, Serialization) +{ + // Construct a constraint + Orientation3DStamped orientation1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position1(rclcpp::Time(1234, 5678), fuse_core::uuid::generate("r5d4")); + Orientation3DStamped orientation2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + Position3DStamped position2(rclcpp::Time(1235, 5678), fuse_core::uuid::generate("r5d4")); + + fuse_core::Vector6d delta; + delta << 1.0, 2.0, 3.0, 0.988, 0.094, 0.079; + + // Generated PD matrix using Octave: R = rand(6, 6); A = R * R' (use format long g to get the + // required precision) + fuse_core::Matrix6d cov; + /* *INDENT-OFF* */ + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, // NOLINT + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, // NOLINT + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, // NOLINT + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, // NOLINT + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, // NOLINT + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; // NOLINT + /* *INDENT-ON* */ + + RelativePose3DStampedEulerConstraint expected("test", position1, orientation1, position2, + orientation2, + delta, cov); + + // Serialize the constraint into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new constraint from that same stream + RelativePose3DStampedEulerConstraint actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.variables(), actual.variables()); + EXPECT_MATRIX_EQ(expected.delta(), actual.delta()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); +} diff --git a/fuse_core/include/fuse_core/constraint.hpp b/fuse_core/include/fuse_core/constraint.hpp index 1abd6d16d..db4f4b4a8 100644 --- a/fuse_core/include/fuse_core/constraint.hpp +++ b/fuse_core/include/fuse_core/constraint.hpp @@ -89,15 +89,15 @@ void serialize(fuse_core::BinaryOutputArchive & archive) const override \ { \ archive << *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void serialize(fuse_core::TextOutputArchive & archive) const override \ { \ archive << *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void deserialize(fuse_core::BinaryInputArchive & archive) override \ { \ archive >> *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void deserialize(fuse_core::TextInputArchive & archive) override \ { \ archive >> *this; \ @@ -124,8 +124,8 @@ static std::string type() \ { \ return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ + } /* NOLINT */ \ + }; /* NOLINT */ \ std::string type() const override \ { \ return detail::type(); \ diff --git a/fuse_core/include/fuse_core/eigen.hpp b/fuse_core/include/fuse_core/eigen.hpp index 6d9ced923..447325539 100644 --- a/fuse_core/include/fuse_core/eigen.hpp +++ b/fuse_core/include/fuse_core/eigen.hpp @@ -54,6 +54,7 @@ using Vector6d = Eigen::Matrix; using Vector7d = Eigen::Matrix; using Vector8d = Eigen::Matrix; using Vector9d = Eigen::Matrix; +using Vector15d = Eigen::Matrix; using MatrixXd = Eigen::Matrix; using Matrix1d = Eigen::Matrix; @@ -65,6 +66,7 @@ using Matrix6d = Eigen::Matrix; using Matrix7d = Eigen::Matrix; using Matrix8d = Eigen::Matrix; using Matrix9d = Eigen::Matrix; +using Matrix15d = Eigen::Matrix; template using Matrix = Eigen::Matrix; diff --git a/fuse_core/include/fuse_core/graph.hpp b/fuse_core/include/fuse_core/graph.hpp index a9f366c84..c0a918e3e 100644 --- a/fuse_core/include/fuse_core/graph.hpp +++ b/fuse_core/include/fuse_core/graph.hpp @@ -74,15 +74,15 @@ void serialize(fuse_core::BinaryOutputArchive & archive) const override \ { \ archive << *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void serialize(fuse_core::TextOutputArchive & archive) const override \ { \ archive << *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void deserialize(fuse_core::BinaryInputArchive & archive) override \ { \ archive >> *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void deserialize(fuse_core::TextInputArchive & archive) override \ { \ archive >> *this; \ @@ -109,8 +109,8 @@ static std::string type() \ { \ return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ + } /* NOLINT */ \ + }; /* NOLINT */ \ std::string type() const override \ { \ return detail::type(); \ diff --git a/fuse_core/include/fuse_core/loss.hpp b/fuse_core/include/fuse_core/loss.hpp index 8465e574f..bf0fb4eb5 100644 --- a/fuse_core/include/fuse_core/loss.hpp +++ b/fuse_core/include/fuse_core/loss.hpp @@ -81,15 +81,15 @@ void serialize(fuse_core::BinaryOutputArchive & archive) const override \ { \ archive << *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void serialize(fuse_core::TextOutputArchive & archive) const override \ { \ archive << *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void deserialize(fuse_core::BinaryInputArchive & archive) override \ { \ archive >> *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void deserialize(fuse_core::TextInputArchive & archive) override \ { \ archive >> *this; \ @@ -116,8 +116,8 @@ static std::string type() \ { \ return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ + } /* NOLINT */ \ + }; /* NOLINT */ \ std::string type() const override \ { \ return detail::type(); \ diff --git a/fuse_core/include/fuse_core/util.hpp b/fuse_core/include/fuse_core/util.hpp index b3bdc4a55..1b10f088c 100644 --- a/fuse_core/include/fuse_core/util.hpp +++ b/fuse_core/include/fuse_core/util.hpp @@ -35,9 +35,11 @@ #define FUSE_CORE__UTIL_HPP_ #include +#include #include #include +#include #include #include @@ -150,6 +152,216 @@ 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::hypot(q1, q2, q3); + const double cos_theta = q0; + + const double cond = std::pow(sin_theta2, 1.5); + if (std::fpclassify(cond) != FP_ZERO) { + const double two_theta = 2.0 * + (cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta)); + const double a = two_theta / sin_theta; + const double b = sin_theta2 * q_sum2; + const double c = two_theta / cond; + jacobian_map(0, 0) = -2.0 * q1 / q_sum2; + jacobian_map(0, 1) = + two_theta / sin_theta + + (2.0 * q0 * q1 * q1) / b - + (q1 * q1 * c); + jacobian_map(0, 2) = + (2.0 * q0 * q1 * q2) / b - + (q1 * q2 * c); + jacobian_map(0, 3) = + (2.0 * q0 * q1 * q3) / b - + (q1 * q3 * c); + + jacobian_map(1, 0) = -2.0 * q2 / q_sum2; + jacobian_map(1, 1) = + (2.0 * q0 * q1 * q2) / b - + (q1 * q2 * c); + jacobian_map(1, 2) = + two_theta / sin_theta + + (2.0 * q0 * q2 * q2) / b - + (q2 * q2 * c); + jacobian_map(1, 3) = + (2.0 * q0 * q2 * q3) / b - + (q2 * q3 * c); + + jacobian_map(2, 0) = -2.0 * q3 / q_sum2; + jacobian_map(2, 1) = + (2.0 * q0 * q1 * q3) / b - + (q1 * q3 * c); + jacobian_map(2, 2) = + (2.0 * q0 * q2 * q3) / b - + (q2 * q3 * c); + jacobian_map(2, 3) = + two_theta / sin_theta + + (2.0 * q0 * q3 * q3) / b - + (q3 * q3 * c); + } else { + jacobian_map.setZero(); + jacobian_map(0, 1) = 2.0; + jacobian_map(1, 2) = 2.0; + jacobian_map(2, 3) = 2.0; + } + } +} + /** * @brief Create a compound ROS topic name from two components * diff --git a/fuse_core/include/fuse_core/variable.hpp b/fuse_core/include/fuse_core/variable.hpp index 91570d966..aa480ebac 100644 --- a/fuse_core/include/fuse_core/variable.hpp +++ b/fuse_core/include/fuse_core/variable.hpp @@ -84,15 +84,15 @@ void serialize(fuse_core::BinaryOutputArchive & archive) const override \ { \ archive << *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void serialize(fuse_core::TextOutputArchive & archive) const override \ { \ archive << *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void deserialize(fuse_core::BinaryInputArchive & archive) override \ { \ archive >> *this; \ - } /* NOLINT */ \ + } /* NOLINT */ \ void deserialize(fuse_core::TextInputArchive & archive) override \ { \ archive >> *this; \ @@ -119,8 +119,8 @@ static std::string type() \ { \ return boost::typeindex::stl_type_index::type_id<__VA_ARGS__>().pretty_name(); \ - } /* NOLINT */ \ - }; /* NOLINT */ \ + } /* NOLINT */ \ + }; /* NOLINT */ \ std::string type() const override \ { \ return detail::type(); \ diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 49851fe4b..e6b4b2063 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2020, Clearpath Robotics + * Copyright (c) 2024, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -36,7 +37,59 @@ #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 +134,183 @@ TEST(Util, wrapAngle2D) EXPECT_EQ("~b", fuse_core::joinTopicName("a/", "~b")); } } + +TEST(Util, quaternion2rpy) +{ + // Test correct conversion from quaternion to roll-pitch-yaw + double q[4] = {1.0, 0.0, 0.0, 0.0}; + double rpy[3]; + fuse_core::quaternion2rpy(q, rpy); + EXPECT_EQ(0.0, rpy[0]); + EXPECT_EQ(0.0, rpy[1]); + EXPECT_EQ(0.0, rpy[2]); + + q[0] = 0.9818562; + q[1] = 0.0640713; + q[2] = 0.0911575; + q[3] = -0.1534393; + + fuse_core::quaternion2rpy(q, rpy); + EXPECT_NEAR(0.1, rpy[0], 1e-6); + EXPECT_NEAR(0.2, rpy[1], 1e-6); + EXPECT_NEAR(-0.3, rpy[2], 1e-6); + + // Test correct quaternion to roll-pitch-yaw function jacobian + const Eigen::Quaterniond q_eigen = Eigen::Quaterniond::UnitRandom(); + double J_analytic[12], J_autodiff[12]; + q[0] = q_eigen.w(); + q[1] = q_eigen.x(); + q[2] = q_eigen.y(); + q[3] = q_eigen.z(); + + fuse_core::quaternion2rpy(q, rpy, J_analytic); + + double * jacobians[1] = {J_autodiff}; + const double * parameters[1] = {q}; + + ceres::CostFunction * quat2rpy_cf = Quat2RPY::Create(); + double rpy_autodiff[3]; + quat2rpy_cf->Evaluate(parameters, rpy_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); +} + +TEST(Util, quaternionProduct) +{ + // Test correct quaternion product function jacobian + const Eigen::Quaterniond q1_eigen = Eigen::Quaterniond::UnitRandom(); + const Eigen::Quaterniond q2_eigen = Eigen::Quaterniond::UnitRandom(); + double q_out[4]; + double q1[4] + { + q1_eigen.w(), + q1_eigen.x(), + q1_eigen.y(), + q1_eigen.z() + }; + + double q2[4] + { + q2_eigen.w(), + q2_eigen.x(), + q2_eigen.y(), + q2_eigen.z() + }; + + // Atm only the jacobian wrt the second quaternion is implemented. If the computation will be + // extended in future, we just have to compare J_analytic_q1 with the other automatic J_autodiff_q1. + double J_analytic_q1[16], J_analytic_q2[16]; // Analytical Jacobians wrt first and second quaternion + double J_autodiff_q1[16], J_autodiff_q2[16]; // Autodiff Jacobians wrt first and second quaternion + + fuse_core::quaternionProduct(q1, q2, q_out, J_analytic_q2); + + double * jacobians[2]; + jacobians[0] = J_autodiff_q1; + jacobians[1] = J_autodiff_q2; + + const double * parameters[2]; + parameters[0] = q1; + parameters[1] = q2; + + ceres::CostFunction * quat_prod_cf = QuatProd::Create(); + double q_out_autodiff[4]; + quat_prod_cf->Evaluate(parameters, q_out_autodiff, jacobians); + + Eigen::Map> J_autodiff_q1_map(jacobians[0]); + Eigen::Map> J_autodiff_q2_map(jacobians[1]); + + // Eigen::Map> J_analytic_q1_map(J_analytic_q1); + Eigen::Map> J_analytic_q2_map(J_analytic_q2); + + EXPECT_TRUE(J_analytic_q2_map.isApprox(J_autodiff_q2_map)); +} + +TEST(Util, quaternionToAngleAxis) +{ + // Test correct quaternion to angle-axis function jacobian, for quaternions representing non-zero rotation + // The implementation of quaternionToAngleAxis changes slightly between ceres 2.1.0 and 2.2.0. We checked for + // both the versions for the test to pass. + { + 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)); + } + + // Test correct quaternion to angle-axis function jacobian, for quaternions representing zero rotation + { + double angle_axis[3]; + double q[4] {1.0, 0.0, 0.0, 0.0}; + + 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)); + } + + { + // Test that approximate conversion and jacobian computation work for very small angles that + // could potentially cause underflow. + + double theta = std::pow(std::numeric_limits::min(), 0.75); + double q[4] = {cos(theta / 2.0), sin(theta / 2.0), 0, 0}; + double angle_axis[3]; + double expected[3] = {theta, 0, 0}; + double J_analytic[12]; + double J_autodiff[12]; + + fuse_core::quaternionToAngleAxis(q, angle_axis, J_analytic); + EXPECT_DOUBLE_EQ(angle_axis[0], expected[0]); + EXPECT_DOUBLE_EQ(angle_axis[1], expected[1]); + EXPECT_DOUBLE_EQ(angle_axis[2], expected[2]); + + double * jacobians[1] = {J_autodiff}; + const double * parameters[1] = {q}; + + ceres::CostFunction * quat2angle_axis_cf = Quat2AngleAxis::Create(); + double angle_axis_autodiff[3]; + quat2angle_axis_cf->Evaluate(parameters, angle_axis_autodiff, jacobians); + + Eigen::Map> J_autodiff_map(jacobians[0]); + Eigen::Map> J_analytic_map(J_analytic); + + EXPECT_TRUE(J_analytic_map.isApprox(J_autodiff_map)); + } +} diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index fe3bc9452..0df2d164d 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -13,6 +13,7 @@ endif() find_package(ament_cmake_ros REQUIRED) +find_package(covariance_geometry_ros REQUIRED) find_package(fuse_constraints REQUIRED) find_package(fuse_core REQUIRED) find_package(fuse_graphs REQUIRED) @@ -43,20 +44,27 @@ add_library(${PROJECT_NAME} src/acceleration_2d.cpp src/graph_ignition.cpp src/imu_2d.cpp + src/imu_3d.cpp src/odometry_2d.cpp src/odometry_2d_publisher.cpp + src/odometry_3d.cpp + src/odometry_3d_publisher.cpp src/pose_2d.cpp src/transaction.cpp src/twist_2d.cpp src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp src/unicycle_2d_state_kinematic_constraint.cpp + src/omnidirectional_3d.cpp + src/omnidirectional_3d_ignition.cpp + src/omnidirectional_3d_state_kinematic_constraint.cpp ) target_include_directories(${PROJECT_NAME} PUBLIC "$" "$" ) target_link_libraries(${PROJECT_NAME} + covariance_geometry_ros::covariance_geometry_ros Ceres::ceres fuse_constraints::fuse_constraints fuse_core::fuse_core @@ -115,6 +123,7 @@ ament_export_targets(${PROJECT_NAME}-export HAS_LIBRARY_TARGET) ament_export_dependencies( ament_cmake_ros + covariance_geometry_ros fuse_constraints fuse_core fuse_graphs diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index f6c860934..7830e9a81 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -12,6 +12,19 @@ + + + A class that represents a kinematic constraint between 3D states at two different times. + + + + + + A fuse_models 3D kinematic model that generates kinematic constraints between provided time stamps, and adds + those constraints to the fuse graph. + + + Publisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts a tf transform for optimized 2D @@ -20,6 +33,14 @@ + + + Publisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts a tf transform for optimized 3D + state data (combination of Position3DStamped, Orientation3DStamped, VelocityLinear3DStamped, and + VelocityAngular3DStamped). + + + An adapter-type sensor that produces 2D linear acceleration constraints from information published by @@ -40,12 +61,24 @@ acceleration constraints from IMU sensor data published by another node + + + An adapter-type sensor that produces 3D orientation (relative or absolute), angular velocity, and linear + acceleration constraints from IMU sensor data published by another node + + An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data published by another node + + + An adapter-type sensor that produces pose (relative or absolute) and velocity constraints from sensor data + published by another node + + An adapter-type sensor that produces absolute or relative pose constraints from information published by @@ -68,4 +101,9 @@ A fuse_models ignition sensor designed to be used in conjunction with the unicycle 2D motion model. + + + A fuse_models ignition sensor designed to be used in conjunction with the Omnidirectional 3D motion model. + + diff --git a/fuse_models/include/fuse_models/common/sensor_config.hpp b/fuse_models/include/fuse_models/common/sensor_config.hpp index b9f2b3505..dcd51424f 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.hpp +++ b/fuse_models/include/fuse_models/common/sensor_config.hpp @@ -44,10 +44,14 @@ #include #include +#include #include +#include #include #include +#include #include +#include #include @@ -90,6 +94,28 @@ std::enable_if_t::value, size_t> toIndex(const std::string & dim return 0u; } +/** + * @brief Method that converts from 3D linear axis dimension names to index values + * + * This method is enabled only for variables that contain _only_ 3D linear quantities + * + * @param[in] dimension - The dimension name to convert + * @return the index of the enumerated dimension for that type + * @throws runtime_error if the dimension name is invalid + */ +template +std::enable_if_t::value, size_t> toIndex(const std::string & dimension) +{ + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "x") {return static_cast(T::X);} + if (lower_dim == "y") {return static_cast(T::Y);} + if (lower_dim == "z") {return static_cast(T::Z);} + + throwDimensionError(dimension); + + return 0u; +} + /** * @brief Method that converts from 2D angular axis dimension names to index values * @@ -112,6 +138,56 @@ std::enable_if_t::value, size_t> toIndex(const std::string & di return 0u; } +/** + * @brief Method that converts from 3D angular axis dimension names to index values + * + * This method is enabled only for variables that contain _only_ 3D angular quantities + * + * @param[in] dimension - The dimension name to convert + * @return the index of the enumerated dimension for that type + * @throws runtime_error if the dimension name is invalid + */ +template +std::enable_if_t::value && !is_orientation::value, size_t> toIndex( + const std::string & dimension) +{ + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "roll" || lower_dim == "x") { + return static_cast(T::ROLL); + } + if (lower_dim == "pitch" || lower_dim == "y") { + return static_cast(T::PITCH); + } + if (lower_dim == "yaw" || lower_dim == "z") { + return static_cast(T::YAW); + } + + throwDimensionError(dimension); + + return 0u; +} + +template +std::enable_if_t::value && is_orientation::value, size_t> toIndex( + const std::string & dimension) +{ + // Trick to get roll, pitch, yaw indexes as 0, 1, 2 + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "roll" || lower_dim == "x") { + return static_cast(fuse_variables::Orientation3DStamped::Euler::ROLL) - 4UL; + } + if (lower_dim == "pitch" || lower_dim == "y") { + return static_cast(fuse_variables::Orientation3DStamped::Euler::PITCH) - 4UL; + } + if (lower_dim == "yaw" || lower_dim == "z") { + return static_cast(fuse_variables::Orientation3DStamped::Euler::YAW) - 4UL; + } + + throwDimensionError(dimension); + + return 0u; +} + /** * @brief Utility method to convert a vector of dimension names to a vector of dimension indices * diff --git a/fuse_models/include/fuse_models/common/sensor_proc.hpp b/fuse_models/include/fuse_models/common/sensor_proc.hpp index 56d58ff0b..344887a2e 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.hpp +++ b/fuse_models/include/fuse_models/common/sensor_proc.hpp @@ -2,6 +2,7 @@ * Software License Agreement (BSD License) * * Copyright (c) 2018, Locus Robotics + * Copyright (c) 2023, Giacomo Franchini * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -45,18 +46,28 @@ #include #include +#include #include +#include +#include #include +#include +#include #include #include #include #include #include #include +#include #include +#include #include +#include #include +#include #include +#include #include #include @@ -69,6 +80,11 @@ #include #include +#include "covariance_geometry/pose_composition.hpp" +#include "covariance_geometry/pose_covariance_representation.hpp" +#include "covariance_geometry/pose_covariance_composition.hpp" +#include "covariance_geometry_ros/utils.hpp" + #include @@ -194,6 +210,27 @@ inline void populatePartialMeasurement( } } +/** + * @brief Method to create covariances of sub-measurements from covariances of full measurements and append + * them to existing partial covariances + * + * @param[in] covariance_full - The full covariance matrix from which we will generate the covariances of the + * sub-measurement + * @param[in] indices - The indices we want to include in the sub-measurement + * @param[in,out] covariance_partial - The partial measurement covariance to which we want to append + */ +inline void populatePartialMeasurement( + const fuse_core::MatrixXd & covariance_full, + const std::vector & indices, + fuse_core::MatrixXd & covariance_partial) +{ + for (size_t r = 0; r < indices.size(); ++r) { + for (size_t c = 0; c < indices.size(); ++c) { + covariance_partial(r, c) = covariance_full(indices[r], indices[c]); + } + } +} + /** * @brief Method to validate partial measurements, that checks for finite values and covariance * properties @@ -224,6 +261,28 @@ inline void validatePartialMeasurement( } } +inline void validateMeasurement( + const fuse_core::VectorXd & mean, + const fuse_core::MatrixXd & covariance, + const double precision = Eigen::NumTraits::dummy_precision()) +{ + if (!mean.allFinite()) { + throw std::runtime_error("Invalid mean " + fuse_core::to_string(mean)); + } + + if (!fuse_core::isSymmetric(covariance, precision)) { + throw std::runtime_error( + "Non-symmetric covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } + + if (!fuse_core::isPositiveDefinite(covariance)) { + throw std::runtime_error( + "Non-positive-definite covariance matrix\n" + + fuse_core::to_string(covariance, Eigen::FullPrecision)); + } +} + /** * @brief Transforms a ROS geometry message from its frame to the frame of the output message * @@ -357,7 +416,7 @@ inline bool processAbsolutePoseWithCovariance( if (validate) { try { - validatePartialMeasurement(pose_mean_partial, pose_covariance_partial); + validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); } catch (const std::runtime_error & ex) { RCLCPP_ERROR_STREAM_THROTTLE( rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, @@ -387,6 +446,171 @@ inline bool processAbsolutePoseWithCovariance( return true; } +/** + * @brief Extracts 3D pose data from a PoseWithCovarianceStamped message and adds that data to a + * fuse Transaction + * + * This method effectively adds two variables (3D position and 3D orientation) and a 3D pose + * constraint to the given \p transaction. The pose data is extracted from the \p pose message. + * The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] pose - The PoseWithCovarianceStamped message from which we will extract the pose data + * @param[in] loss - The loss function for the 3D pose constraint generated + * @param[in] target_frame - The frame ID into which the pose data will be transformed before it is + * used + * @param[in] position_indices - The indices of the position variables to be added to the transaction + * @param[in] orientation_indices - The indices of the orientation variables to be added to the transaction + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processAbsolutePose3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const fuse_core::Loss::SharedPtr & loss, + const std::string & target_frame, + const std::vector & position_indices, + const std::vector & orientation_indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + if (position_indices.empty() && orientation_indices.empty()) { + return false; + } + + geometry_msgs::msg::PoseWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = pose; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, pose, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform pose message with stamp " << rclcpp::Time( + pose.header.stamp).nanoseconds() << ". Cannot create constraint."); + return false; + } + } + // Create the pose variable + auto position = fuse_variables::Position3DStamped::make_shared(pose.header.stamp, device_id); + position->x() = transformed_message.pose.pose.position.x; + position->y() = transformed_message.pose.pose.position.y; + position->z() = transformed_message.pose.pose.position.z; + auto orientation = + fuse_variables::Orientation3DStamped::make_shared(pose.header.stamp, device_id); + orientation->w() = transformed_message.pose.pose.orientation.w; + orientation->x() = transformed_message.pose.pose.orientation.x; + orientation->y() = transformed_message.pose.pose.orientation.y; + orientation->z() = transformed_message.pose.pose.orientation.z; + + if (position_indices.size() == 3 && orientation_indices.size() == 3) { + // Full pose measurement, no need to create a partial one + fuse_core::Vector7d pose_mean; + pose_mean << transformed_message.pose.pose.position.x, + transformed_message.pose.pose.position.y, + transformed_message.pose.pose.position.z, + transformed_message.pose.pose.orientation.w, + transformed_message.pose.pose.orientation.x, + transformed_message.pose.pose.orientation.y, + transformed_message.pose.pose.orientation.z; + + Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); + + if (validate) { + try { + validatePartialMeasurement(pose_mean, pose_covariance, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); + return false; + } + } + + auto constraint = fuse_constraints::AbsolutePose3DStampedConstraint::make_shared( + source, + *position, + *orientation, + pose_mean, + pose_covariance); + + constraint->loss(loss); + + transaction.addVariable(position); + transaction.addVariable(orientation); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose.header.stamp); + + return true; + } + + // Convert the ROS message into tf2 transform + tf2::Transform tf2_pose; + tf2::fromMsg(transformed_message.pose.pose, tf2_pose); + // Fill eigen pose in RPY representation + fuse_core::Vector6d pose_mean_partial; + pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), + tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY( + pose_mean_partial(3), pose_mean_partial( + 4), pose_mean_partial(5)); + + Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); + + // Set the components which are not measured to zero + const auto indices = mergeIndices(position_indices, orientation_indices, 3); + std::replace_if( + pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), + [&indices, &pose_mean_partial](const double & value) { + return std::find( + indices.begin(), indices.end(), + &value - pose_mean_partial.data()) == indices.end(); + }, 0.0); + fuse_core::MatrixXd pose_covariance_partial(indices.size(), indices.size()); + populatePartialMeasurement( + pose_covariance, + indices, + pose_covariance_partial); + + if (validate) { + try { + validatePartialMeasurement(pose_mean_partial, pose_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); + return false; + } + } + + auto constraint = fuse_constraints::AbsolutePose3DStampedEulerConstraint::make_shared( + source, + *position, + *orientation, + pose_mean_partial, + pose_covariance_partial, + indices); + + constraint->loss(loss); + + transaction.addVariable(position); + transaction.addVariable(orientation); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose.header.stamp); + + return true; +} + /** * @brief Extracts relative 2D pose data from a PoseWithCovarianceStamped and adds that data to a * fuse Transaction @@ -750,6 +974,280 @@ inline bool processDifferentialPoseWithCovariance( return true; } +/** + * @brief Extracts relative 3D pose data from a PoseWithCovarianceStamped and adds that data to a + * fuse Transaction + * + * This method computes the delta between two poses and creates the required fuse variables and + * constraints, and then adds them to the given \p transaction. The pose delta is calculated as + * + * pose_relative = pose_absolute1^-1 * pose_absolute2 + * + * Additionally, the covariance of each pose message is rotated into the robot's base frame at the + * time of pose_absolute1. They are then added in the constraint if the pose measurements are + * independent. Otherwise, if the pose measurements are dependent, the covariance of pose_absolute1 + * is substracted from the covariance of pose_absolute2. A small minimum relative covariance is + * added to avoid getting a zero or ill-conditioned covariance. This could happen if both covariance + * matrices are the same or very similar, e.g. when pose_absolute1 == pose_absolute2, it's possible + * that the covariance is the same for both poses. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] pose1 - The first (and temporally earlier) PoseWithCovarianceStamped message + * @param[in] pose2 - The second (and temporally later) PoseWithCovarianceStamped message + * @param[in] independent - Whether the pose measurements are independent or not + * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always + * added to the resulting pose relative covariance + * @param[in] loss - The loss function for the 3D pose constraint generated + * @param[in] position_indices - The indices of the position variables to be added to the transaction + * @param[in] orientation_indices - The indices of the orientation variables to be added to the transaction + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processDifferentialPose3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose1, + const geometry_msgs::msg::PoseWithCovarianceStamped & pose2, + const bool independent, + const fuse_core::Matrix6d & minimum_pose_relative_covariance, + const fuse_core::Loss::SharedPtr & loss, + const std::vector & position_indices, + const std::vector & orientation_indices, + const bool validate, + fuse_core::Transaction & transaction) +{ + // Create the pose variables + auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); + auto orientation1 = + fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); + position1->x() = pose1.pose.pose.position.x; + position1->y() = pose1.pose.pose.position.y; + position1->z() = pose1.pose.pose.position.z; + orientation1->x() = pose1.pose.pose.orientation.x; + orientation1->y() = pose1.pose.pose.orientation.y; + orientation1->z() = pose1.pose.pose.orientation.z; + orientation1->w() = pose1.pose.pose.orientation.w; + + auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); + auto orientation2 = + fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); + position2->x() = pose2.pose.pose.position.x; + position2->y() = pose2.pose.pose.position.y; + position2->z() = pose2.pose.pose.position.z; + orientation2->x() = pose2.pose.pose.orientation.x; + orientation2->y() = pose2.pose.pose.orientation.y; + orientation2->z() = pose2.pose.pose.orientation.z; + orientation2->w() = pose2.pose.pose.orientation.w; + + // Here we are using covariance_geometry types to compute the relative pose covariance: + // PoseQuaternionCovarianceRPY is std::pair, Covariance> + // Position is Eigen::Vector3d + // Quaternion is Eigen::Quaterniond + // Covariance is Eigen::Matrix6d + + // N.B. covariance_geometry implements functions for pose composition and covariance propagation + // which are based on "A tutorial on SE(3) transformation parameterizations and on-manifold optimization" + // by José Luis Blanco Claraco (https://arxiv.org/abs/2103.15980) + + // Convert from ROS msg to covariance geometry types + covariance_geometry::PoseQuaternionCovarianceRPY p1, p2, p12; + covariance_geometry::fromROS(pose1.pose, p1); + covariance_geometry::fromROS(pose2.pose, p2); + + // Create the delta for the constraint + if (independent) { + covariance_geometry::ComposePoseQuaternionCovarianceRPY( + covariance_geometry::InversePose3DQuaternionCovarianceRPY(p1), + p2, + p12 + ); + } else { + // If covariances of p1 and p2 are the same, then it's possible that p12 covariance will be + // zero or ill-conditioned. To avoid this, we skip the expensive following calculations and + // instead we just add a minimum covariance later + if (p1.second.isApprox(p2.second, 1e-9)) { + covariance_geometry::ComposePose3DQuaternion( + covariance_geometry::InversePose(p1.first), + p2.first, + p12.first + ); + p12.second.setZero(); + } else { + // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. + // We know cov1 and cov2 and we should substract the first to the second in order + // to obtain the relative pose covariance. But first the 2 of them have to be in the + // same reference frame, moreover we need to rotate the result in p12 reference frame + // The covariance propagation of p2 = p1 * p12 is: + // + // C2 = J_p1 * C1 * J_p1^T + J_p12 * C12 * J_p12^T + // + // where C1, C2, C12 are the covariance matrices of p1, p2 and dp, respectively, and J_p1 and + // J_p12 are the jacobians of the equation (pose composition) wrt p1 and p12, respectively. + // + // Therefore, the covariance C12 of the relative pose p12 is: + // + // C12 = J_p12^-1 * (C2 - J_p1 * C1 * J_p1^T) * J_p12^-T + + // First we need to convert covariances from RPY (6x6) to quaternion (7x7) + covariance_geometry::PoseQuaternionCovariance p1_q, p2_q, p12_q; + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( + p1, + p1_q + ); + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance( + p2, + p2_q + ); + // Then we need to compute the delta pose + covariance_geometry::ComposePose3DQuaternion( + covariance_geometry::InversePose(p1_q.first), + p2_q.first, + p12_q.first + ); + // Now we have to compute pose composition jacobians so we can rotate covariances + Eigen::Matrix7d j_p1, j_p12, j_p12_inv; + Eigen::Matrix4d j_qn; + + covariance_geometry::jacobianQuaternionNormalization(p12_q.first.second, j_qn); + + covariance_geometry::JacobianPosePoseCompositionA(p1_q.first, p12_q.first, j_p1); + j_p1.block<4, 4>(3, 3).applyOnTheLeft(j_qn); + j_p1.block<4, 3>(3, 0).setZero(); + + covariance_geometry::JacobianPosePoseCompositionB(p1_q.first, j_p12); + j_p12.block<4, 4>(3, 3).applyOnTheLeft(j_qn); + j_p12.block<3, 4>(0, 3).setZero(); + j_p12.block<4, 3>(3, 0).setZero(); + + // TODO(giafranchini): check if faster to use j12.llt().solve() instead + j_p12_inv = j_p12.colPivHouseholderQr().solve(Eigen::Matrix7d::Identity()); + p12_q.second = j_p12_inv * (p2_q.second - j_p1 * p1_q.second * j_p1.transpose()) * + j_p12_inv.transpose(); + + // Now again convert the delta pose covariance back to RPY(6x6) + covariance_geometry::Pose3DQuaternionCovarianceTo3DQuaternionCovarianceRPY( + p12_q, + p12 + ); + } + } + + if (position_indices.size() == 3 && orientation_indices.size() == 3) { + // Full pose measurement, no need to create a partial one + fuse_core::Vector7d pose_relative_mean; + pose_relative_mean << + p12.first.first.x(), p12.first.first.y(), p12.first.first.z(), + p12.first.second.w(), p12.first.second.x(), p12.first.second.y(), + p12.first.second.z(); + fuse_core::Matrix6d pose_relative_covariance = p12.second + minimum_pose_relative_covariance; + + if (validate) { + try { + validateMeasurement( + pose_relative_mean, pose_relative_covariance, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean, + pose_relative_covariance); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; + } + + // Convert the poses into RPY representation + fuse_core::Vector6d pose_relative_mean_partial; + + tf2::Quaternion q12(p12.first.second.x(), p12.first.second.y(), p12.first.second.z(), + p12.first.second.w()); + pose_relative_mean_partial.head<3>() << p12.first.first.x(), p12.first.first.y(), + p12.first.first.z(); + tf2::Matrix3x3(q12).getRPY( + pose_relative_mean_partial(3), pose_relative_mean_partial( + 4), pose_relative_mean_partial(5)); + + fuse_core::Matrix6d pose_relative_covariance = p12.second + minimum_pose_relative_covariance; + + const auto indices = mergeIndices(position_indices, orientation_indices, 3); + + fuse_core::MatrixXd pose_relative_covariance_partial(indices.size(), indices.size()); + + // Set the components which are not measured to zero + std::replace_if( + pose_relative_mean_partial.data(), + pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), + [&indices, &pose_relative_mean_partial](const double & value) { + return std::find( + indices.begin(), + indices.end(), + &value - pose_relative_mean_partial.data()) == indices.end(); + }, 0.0); + + populatePartialMeasurement( + pose_relative_covariance, + indices, + pose_relative_covariance_partial); + + if (validate) { + try { + validateMeasurement( + pose_relative_mean_partial, pose_relative_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean_partial, + pose_relative_covariance_partial, + indices); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; +} + /** * @brief Extracts relative 2D pose data from a PoseWithCovarianceStamped and adds that data to a * fuse Transaction @@ -948,28 +1446,234 @@ inline bool processDifferentialPoseWithTwistCovariance( } /** - * @brief Extracts velocity data from a TwistWithCovarianceStamped and adds that data to a fuse - * Transaction + * @brief Extracts relative 3D pose data from a PoseWithCovarianceStamped and adds that data to a + * fuse Transaction * - * This method effectively adds two variables (2D linear velocity and 2D angular velocity) and their - * respective constraints to the given \p transaction. The velocity data is extracted from the \p - * twist message. Only 2D data is used. The data will be automatically transformed into the \p - * target_frame before it is used. + * This method computes the delta between two poses and creates the required fuse variables and + * constraints, and then adds them to the given \p transaction. 3D data is used. The pose delta + * is calculated as + * + * pose_relative = pose_absolute1^-1 * pose_absolute2 + * + * Additionally, the twist covariance of the last message is used to compute the relative pose + * covariance using the time difference between the pose_absolute2 and pose_absolute1 time stamps. + * This assumes the pose measurements are dependent. A small minimum relative covariance is added to + * avoid getting a zero or ill-conditioned covariance. This could happen if the twist covariance is + * very small, e.g. when the twist is zero. * * @param[in] source - The name of the sensor or motion model that generated this constraint * @param[in] device_id - The UUID of the machine - * @param[in] twist - The TwistWithCovarianceStamped message from which we will extract the twist - * data - * @param[in] linear_velocity_loss - The loss function for the 2D linear velocity constraint - * generated - * @param[in] angular_velocity_loss - The loss function for the 2D angular velocity constraint - * generated - * @param[in] target_frame - The frame ID into which the twist data will be transformed before it is - * used - * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform - * @param[in] 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 + * @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 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) +{ + if (position_indices.empty() && orientation_indices.empty()) { + return false; + } + + // Convert the poses into tf2 transforms + tf2::Transform pose1_tf2, pose2_tf2; + tf2::fromMsg(pose1.pose.pose, pose1_tf2); + tf2::fromMsg(pose2.pose.pose, pose2_tf2); + + // Create the pose variables + auto position1 = fuse_variables::Position3DStamped::make_shared(pose1.header.stamp, device_id); + position1->x() = pose1_tf2.getOrigin().x(); + position1->y() = pose1_tf2.getOrigin().y(); + position1->z() = pose1_tf2.getOrigin().z(); + auto orientation1 = + fuse_variables::Orientation3DStamped::make_shared(pose1.header.stamp, device_id); + orientation1->x() = pose1_tf2.getRotation().x(); + orientation1->y() = pose1_tf2.getRotation().y(); + orientation1->z() = pose1_tf2.getRotation().z(); + orientation1->w() = pose1_tf2.getRotation().w(); + + auto position2 = fuse_variables::Position3DStamped::make_shared(pose2.header.stamp, device_id); + position2->x() = pose2_tf2.getOrigin().x(); + position2->y() = pose2_tf2.getOrigin().y(); + position2->z() = pose2_tf2.getOrigin().z(); + auto orientation2 = + fuse_variables::Orientation3DStamped::make_shared(pose2.header.stamp, device_id); + orientation2->x() = pose2_tf2.getRotation().x(); + orientation2->y() = pose2_tf2.getRotation().y(); + orientation2->z() = pose2_tf2.getRotation().z(); + orientation2->w() = pose2_tf2.getRotation().w(); + + // Create the delta for the constraint + const auto delta = pose1_tf2.inverseTimes(pose2_tf2); + + // Create the covariance components for the constraint + Eigen::Map cov(twist.twist.covariance.data()); + + const auto dt = (rclcpp::Time(pose2.header.stamp) - rclcpp::Time(pose1.header.stamp)).seconds(); + + if (dt < 1e-6) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Very small time difference " << dt << "s from '" << source << "' source."); + return false; + } + + fuse_core::Matrix6d j_twist; + j_twist.setIdentity(); + j_twist *= dt; + + fuse_core::Matrix6d pose_relative_covariance = + j_twist * (cov - twist_covariance_offset) * j_twist.transpose() + + minimum_pose_relative_covariance; + + if (position_indices.size() == 3 && orientation_indices.size() == 3) { + // Full pose measurement, no need to create a partial one + fuse_core::Vector7d pose_relative_mean; + pose_relative_mean << delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(), + delta.getRotation().w(), delta.getRotation().x(), delta.getRotation().y(), + delta.getRotation().z(); + + if (validate) { + try { + validatePartialMeasurement( + pose_relative_mean, pose_relative_covariance, + 1e-4); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement using the twist covariance from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean, + pose_relative_covariance); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; + } + + // Fill eigen pose in RPY representation + fuse_core::Vector6d pose_relative_mean_partial; + pose_relative_mean_partial.head<3>() << delta.getOrigin().x(), delta.getOrigin().y(), + delta.getOrigin().z(); + tf2::Matrix3x3(delta.getRotation()).getRPY( + pose_relative_mean_partial(3), pose_relative_mean_partial(4), pose_relative_mean_partial(5)); + + // Set the components which are not measured to zero + const auto indices = mergeIndices(position_indices, orientation_indices, 3); + std::replace_if( + pose_relative_mean_partial.data(), + pose_relative_mean_partial.data() + pose_relative_mean_partial.size(), + [&indices, &pose_relative_mean_partial](const double & value) { + return std::find( + indices.begin(), indices.end(), + &value - pose_relative_mean_partial.data()) == indices.end(); + }, 0.0); + fuse_core::MatrixXd pose_relative_covariance_partial(indices.size(), indices.size()); + populatePartialMeasurement( + pose_relative_covariance, + indices, + pose_relative_covariance_partial); + + if (validate) { + try { + validatePartialMeasurement( + pose_relative_mean_partial, pose_relative_covariance_partial, + 1e-4); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial differential pose measurement using the twist covariance from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // Create a relative pose constraint. + auto constraint = fuse_constraints::RelativePose3DStampedEulerConstraint::make_shared( + source, + *position1, + *orientation1, + *position2, + *orientation2, + pose_relative_mean_partial, + pose_relative_covariance_partial, + indices); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; +} + +/** + * @brief Extracts velocity data from a TwistWithCovarianceStamped and adds that data to a fuse + * Transaction + * + * This method effectively adds two variables (2D linear velocity and 2D angular velocity) and their + * respective constraints to the given \p transaction. The velocity data is extracted from the \p + * twist message. Only 2D data is used. The data will be automatically transformed into the \p + * target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] twist - The TwistWithCovarianceStamped message from which we will extract the twist + * data + * @param[in] linear_velocity_loss - The loss function for the 2D linear velocity constraint + * generated + * @param[in] angular_velocity_loss - The loss function for the 2D angular velocity constraint + * generated + * @param[in] target_frame - The frame ID into which the twist data will be transformed before it is + * used + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ inline bool processTwistWithCovariance( @@ -1114,6 +1818,193 @@ inline bool processTwistWithCovariance( return constraints_added; } +/** + * @brief Extracts velocity data from a TwistWithCovarianceStamped and adds that data to a fuse + * Transaction + * + * This method effectively adds two variables (3D linear velocity and 3D angular velocity) and their + * respective constraints to the given \p transaction. The velocity data is extracted from the \p + * twist message. The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] twist - The TwistWithCovarianceStamped message from which we will extract the twist + * data + * @param[in] linear_velocity_loss - The loss function for the 3D linear velocity constraint + * generated + * @param[in] angular_velocity_loss - The loss function for the 3D angular velocity constraint + * generated + * @param[in] target_frame - The frame ID into which the twist data will be transformed before it is + * used + * @param[in] linear_indices - The indices of the linear velocity vector to use. If empty, no + * linear velocity constraint is added + * @param[in] angular_indices - The indices of the angular velocity vector to use. If empty, no + * angular velocity constraint is added + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processTwist3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const fuse_core::Loss::SharedPtr & linear_velocity_loss, + const fuse_core::Loss::SharedPtr & angular_velocity_loss, + const std::string & target_frame, + const std::vector & linear_indices, + const std::vector & angular_indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + // Make sure we actually have work to do + if (linear_indices.empty() && angular_indices.empty()) { + return false; + } + + geometry_msgs::msg::TwistWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = twist; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, twist, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Failed to transform twist message with stamp " << rclcpp::Time( + twist.header.stamp).nanoseconds() << ". Cannot create constraint."); + return false; + } + } + + bool constraints_added = false; + + // Create two absolute constraints + if (!linear_indices.empty()) { + auto velocity_linear = + fuse_variables::VelocityLinear3DStamped::make_shared(twist.header.stamp, device_id); + velocity_linear->x() = transformed_message.twist.twist.linear.x; + velocity_linear->y() = transformed_message.twist.twist.linear.y; + velocity_linear->z() = transformed_message.twist.twist.linear.z; + + // Create the mean twist vectors for the constraints + fuse_core::Vector3d linear_vel_mean; + linear_vel_mean << transformed_message.twist.twist.linear.x, + transformed_message.twist.twist.linear.y, + transformed_message.twist.twist.linear.z; + + // Create the covariance for the constraint + Eigen::Map linear_vel_covariance_map( + transformed_message.twist.covariance.data()); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd linear_vel_mean_partial(linear_indices.size()); + + fuse_core::MatrixXd linear_vel_covariance_partial(linear_vel_mean_partial.rows(), + linear_vel_mean_partial.rows()); + + populatePartialMeasurement( + linear_vel_mean, + linear_vel_covariance_map.block<3, 3>(0, 0), + linear_indices, + linear_vel_mean_partial, + linear_vel_covariance_partial); + + bool add_constraint = true; + + if (validate) { + try { + validatePartialMeasurement(linear_vel_mean_partial, linear_vel_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear velocity measurement from '" + << source << "' source: " << ex.what()); + add_constraint = false; + } + } + + if (add_constraint) { + auto linear_vel_constraint = + fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint::make_shared( + source, *velocity_linear, linear_vel_mean_partial, linear_vel_covariance_partial, + linear_indices); + + linear_vel_constraint->loss(linear_velocity_loss); + + transaction.addVariable(velocity_linear); + transaction.addConstraint(linear_vel_constraint); + constraints_added = true; + } + } + + if (!angular_indices.empty()) { + // Create the twist variables + auto velocity_angular = + fuse_variables::VelocityAngular3DStamped::make_shared(twist.header.stamp, device_id); + velocity_angular->roll() = transformed_message.twist.twist.angular.x; + velocity_angular->pitch() = transformed_message.twist.twist.angular.y; + velocity_angular->yaw() = transformed_message.twist.twist.angular.z; + + fuse_core::Vector3d angular_vel_mean; + angular_vel_mean << transformed_message.twist.twist.angular.x, + transformed_message.twist.twist.angular.y, + transformed_message.twist.twist.angular.z; + + // Create the covariance for the constraint + Eigen::Map angular_vel_cov_map(transformed_message.twist.covariance. + data()); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd angular_vel_mean_partial(angular_indices.size()); + fuse_core::MatrixXd angular_vel_covariance_partial(angular_vel_mean_partial.rows(), + angular_vel_mean_partial.rows()); + + populatePartialMeasurement( + angular_vel_mean, + angular_vel_cov_map.block<3, 3>(3, 3), + angular_indices, + angular_vel_mean_partial, + angular_vel_covariance_partial); + + bool add_constraint = true; + + if (validate) { + try { + validatePartialMeasurement(angular_vel_mean_partial, angular_vel_covariance_partial, 1e-5); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Invalid partial angular velocity measurement from '" + << source << "' source: " << ex.what()); + add_constraint = false; + } + } + + if (add_constraint) { + auto angular_vel_constraint = + fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint::make_shared( + source, *velocity_angular, angular_vel_mean_partial, angular_vel_covariance_partial, + angular_indices); + + angular_vel_constraint->loss(angular_velocity_loss); + + transaction.addVariable(velocity_angular); + transaction.addConstraint(angular_vel_constraint); + constraints_added = true; + } + } + + if (constraints_added) { + transaction.addInvolvedStamp(twist.header.stamp); + } + + return constraints_added; +} + /** * @brief Extracts linear acceleration data from an AccelWithCovarianceStamped and adds that data to * a fuse Transaction @@ -1225,6 +2116,118 @@ inline bool processAccelWithCovariance( return true; } +/** + * @brief Extracts linear acceleration data from an AccelWithCovarianceStamped and adds that data to + * a fuse Transaction + * + * This method effectively adds a linear acceleration variable and constraint to the given to the + * given \p transaction. The acceleration data is extracted from the \p acceleration message. + * The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] acceleration - The AccelWithCovarianceStamped message from which we will extract the + * acceleration data + * @param[in] loss - The loss function for the 3D linear acceleration constraint generated + * @param[in] indices - The indices of the linear acceleration vector to use. If empty, no + * linear acceleration constraint is added + * @param[in] target_frame - The frame ID into which the acceleration data will be transformed + * before it is used + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no + * constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processAccel3DWithCovariance( + const std::string & source, + const fuse_core::UUID & device_id, + const geometry_msgs::msg::AccelWithCovarianceStamped & acceleration, + const fuse_core::Loss::SharedPtr & loss, + const std::string & target_frame, + const std::vector & indices, + const tf2_ros::Buffer & tf_buffer, + const bool validate, + fuse_core::Transaction & transaction, + const rclcpp::Duration & tf_timeout = rclcpp::Duration(0, 0)) +{ + // Make sure we actually have work to do + if (indices.empty()) { + return false; + } + + geometry_msgs::msg::AccelWithCovarianceStamped transformed_message; + if (target_frame.empty()) { + transformed_message = acceleration; + } else { + transformed_message.header.frame_id = target_frame; + + if (!transformMessage(tf_buffer, acceleration, transformed_message, tf_timeout)) { + RCLCPP_WARN_STREAM_SKIPFIRST_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0, + "Failed to transform acceleration message with stamp " << + rclcpp::Time(acceleration.header.stamp).nanoseconds() + << ". Cannot create constraint."); + return false; + } + } + + // Create the full mean vector and covariance for the constraint + fuse_core::Vector3d accel_mean; + accel_mean << + transformed_message.accel.accel.linear.x, + transformed_message.accel.accel.linear.y, + transformed_message.accel.accel.linear.z; + + Eigen::Map> accel_covariance_map(transformed_message.accel.covariance.data()); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd accel_mean_partial(indices.size()); + fuse_core::MatrixXd accel_covariance_partial(accel_mean_partial.rows(), + accel_mean_partial.rows()); + + populatePartialMeasurement( + accel_mean, accel_covariance_map, indices, accel_mean_partial, + accel_covariance_partial); + + if (validate) { + try { + validatePartialMeasurement(accel_mean_partial, accel_covariance_partial, 1e-4); + } catch (const std::runtime_error & ex) { + RCLCPP_ERROR_STREAM_THROTTLE( + rclcpp::get_logger("fuse"), sensor_proc_clock, 10.0 * 1000, + "Invalid partial linear acceleration measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // Create the acceleration variables + auto acceleration_linear = + fuse_variables::AccelerationLinear3DStamped::make_shared(acceleration.header.stamp, device_id); + acceleration_linear->x() = transformed_message.accel.accel.linear.x; + acceleration_linear->y() = transformed_message.accel.accel.linear.y; + acceleration_linear->z() = transformed_message.accel.accel.linear.z; + + // Create the constraint + auto linear_accel_constraint = + fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint::make_shared( + source, + *acceleration_linear, + accel_mean_partial, + accel_covariance_partial, + indices); + + linear_accel_constraint->loss(loss); + + transaction.addVariable(acceleration_linear); + transaction.addConstraint(linear_accel_constraint); + transaction.addInvolvedStamp(acceleration.header.stamp); + + return true; +} + /** * @brief Scales the process noise covariance pose by the norm of the velocity * @@ -1266,6 +2269,37 @@ inline void scaleProcessNoiseCovariance( velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); } +/** + * @brief Scales the process noise covariance pose by the norm of the velocity + * + * @param[in, out] process_noise_covariance - The process noise covariance to scale. Only the pose + * components (x, y, z, roll, pitch, yaw) are scaled, and they are assumed to be in the + * top left 6x6 corner + * @param[in] velocity_linear - The linear velocity + * @param[in] velocity_angular - The angular velocity + * @param[in] velocity_linear_norm_min - The minimum linear velocity norm + * @param[in] velocity_angular_norm_min - The minimum angular velocity norm + */ +inline void scaleProcessNoiseCovariance( + fuse_core::Matrix15d & process_noise_covariance, + const fuse_core::Vector3d & velocity_linear, + const fuse_core::Vector3d & velocity_angular, + const double velocity_linear_norm_min, + const double velocity_angular_norm_min) +{ + fuse_core::Matrix6d velocity; + velocity.setIdentity(); + velocity.topLeftCorner<3, 3>().diagonal() *= + std::max( + velocity_linear_norm_min, + velocity_linear.norm()); + velocity.bottomRightCorner<3, 3>().diagonal() *= + std::max( + velocity_angular_norm_min, + velocity_angular.norm()); + process_noise_covariance.topLeftCorner<6, 6>() = + velocity * process_noise_covariance.topLeftCorner<6, 6>() * velocity.transpose(); +} } // namespace common } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/common/variable_traits.hpp b/fuse_models/include/fuse_models/common/variable_traits.hpp index b5a77de98..19eef89e8 100644 --- a/fuse_models/include/fuse_models/common/variable_traits.hpp +++ b/fuse_models/include/fuse_models/common/variable_traits.hpp @@ -35,10 +35,15 @@ #define FUSE_MODELS__COMMON__VARIABLE_TRAITS_HPP_ #include +#include #include +#include #include +#include #include +#include #include +#include namespace fuse_models @@ -71,6 +76,30 @@ struct is_linear_2d static const bool value = true; }; +template +struct is_linear_3d +{ + static const bool value = false; +}; + +template<> +struct is_linear_3d +{ + static const bool value = true; +}; + +template<> +struct is_linear_3d +{ + static const bool value = true; +}; + +template<> +struct is_linear_3d +{ + static const bool value = true; +}; + template struct is_angular_2d { @@ -89,6 +118,37 @@ struct is_angular_2d static const bool value = true; }; +template +struct is_angular_3d +{ + static const bool value = false; +}; +template<> +struct is_angular_3d +{ + static const bool value = true; +}; +template<> +struct is_angular_3d +{ + static const bool value = true; +}; + +template +struct is_orientation +{ + static const bool value = false; +}; +template<> +struct is_orientation +{ + static const bool value = true; +}; +template<> +struct is_orientation +{ + static const bool value = true; +}; } // namespace common } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/imu_3d.hpp b/fuse_models/include/fuse_models/imu_3d.hpp new file mode 100644 index 000000000..8dc570000 --- /dev/null +++ b/fuse_models/include/fuse_models/imu_3d.hpp @@ -0,0 +1,195 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__IMU_3D_HPP_ +#define FUSE_MODELS__IMU_3D_HPP_ + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief An adapter-type sensor that produces orientation (relative or absolute), angular velocity, + * and linear acceleration constraints from IMU sensor data published by another node + * + * This sensor subscribes to a sensor_msgs::msg::Imu topic and: + * 1. Creates relative or absolute orientation and constraints. If the \p differential parameter + * is set to false (the default), the orientation measurement will be treated as an absolute + * constraint. If it is set to true, consecutive measurements will be used to generate relative + * orientation constraints. + * 2. Creates 3D velocity variables and constraints. + * + * This sensor really just separates out the orientation, angular velocity, and linear acceleration + * components of the message, and processes them just like the Pose3D, Twist3D, and Acceleration3D + * classes. + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - queue_size (int, default: 10) The subscriber queue size for the pose messages + * - topic (string) The topic to which to subscribe for the pose messages + * - differential (bool, default: false) Whether we should fuse orientation measurements + * absolutely, or to create relative orientation constraints + * using consecutive measurements. + * - remove_gravitational_acceleration (bool, default: false) Whether we should remove acceleration + * due to gravity from the acceleration + * values produced by the IMU before + * fusing + * - gravitational_acceleration (double, default: 9.80665) Acceleration due to gravity, in + * meters/sec^2. This value is only used if + * \p remove_gravitational_acceleration is + * true + * - orientation_target_frame (string) Orientation data will be transformed into this frame before + * it is fused. + * - twist_target_frame (string) Twist/velocity data will be transformed into this frame before it + * is fused. + * - acceleration_target_frame (string) Acceleration data will be transformed into this frame + * before it is fused. + * + * Subscribes: + * - \p topic (sensor_msgs::msg::Imu) IMU data at a given timestep + */ +class Imu3D : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Imu3D) + using ParameterType = parameters::Imu3DParams; + + /** + * @brief Default constructor + */ + Imu3D(); + + /** + * @brief Destructor + */ + virtual ~Imu3D() = default; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Callback for pose messages + * @param[in] msg - The IMU message to process + */ + void process(const sensor_msgs::msg::Imu & msg); + +protected: + fuse_core::UUID device_id_; //!< The UUID of this device + + /** + * @brief Perform any required initialization for the sensor model + * + * This could include things like reading from the parameter server or subscribing to topics. The + * class's node handles will be properly initialized before SensorModel::onInit() is called. + * Spinning of the callback queue will not begin until after the call to SensorModel::onInit() + * completes. + */ + void onInit() override; + + /** + * @brief Subscribe to the input topic to start sending transactions to the optimizer + */ + void onStart() override; + + /** + * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer + */ + void onStop() override; + + /** + * @brief Process a pose message in differential mode + * + * @param[in] pose - The pose message to process in differential mode + * @param[in] twist - The twist message used in case the twist covariance is used in differential + * mode + * @param[in] validate - Whether to validate the pose and twist coavriance or not + * @param[out] transaction - The generated variables and constraints are added to this transaction + */ + void processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const bool validate, + fuse_core::Transaction & transaction); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; + + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_; + + // NOTE(CH3): Unique ptr to defer till we have the node interfaces from initialize() + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr sub_; + + using ImuThrottledCallback = fuse_core::ThrottledMessageCallback; + ImuThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__IMU_3D_HPP_ diff --git a/fuse_models/include/fuse_models/odometry_3d.hpp b/fuse_models/include/fuse_models/odometry_3d.hpp new file mode 100644 index 000000000..8a888f3a8 --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d.hpp @@ -0,0 +1,186 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__ODOMETRY_3D_HPP_ +#define FUSE_MODELS__ODOMETRY_3D_HPP_ + +#include +#include + +#include +#include + +#include +#include + +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief An adapter-type sensor that produces pose (relative or absolute) and velocity constraints + * from sensor data published by another node + * + * This sensor subscribes to a nav_msgs::msg::Odometry topic and: + * 1. Creates relative or absolute pose variables and constraints. If the \p differential parameter + * is set to false (the default), the measurement will be treated as an absolute constraint. If + * it is set to true, consecutive measurements will be used to generate relative pose + * constraints. + * 2. Creates 3D velocity variables and constraints. + * + * This sensor really just separates out the pose and twist components of the message, and processes + * them just like the Pose3D and Twist3D classes. + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - queue_size (int, default: 10) The subscriber queue size for the pose messages + * - topic (string) The topic to which to subscribe for the pose messages + * - differential (bool, default: false) Whether we should fuse measurements absolutely, or to + * create relative pose constraints using consecutive + * measurements. + * - pose_target_frame (string) Pose data will be transformed into this frame before it is fused. + * This frame should be a world-fixed frame, typically 'odom' or + * 'map'. + * - twist_target_frame (string) Twist/velocity data will be transformed into this frame before it + * is fused. This frame should be a body-relative frame, typically + * 'base_link'. + * + * Subscribes: + * - \p topic (nav_msgs::msg::Odometry) Odometry information at a given timestep + */ +class Odometry3D : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Odometry3D) + using ParameterType = parameters::Odometry3DParams; + + /** + * @brief Default constructor + */ + Odometry3D(); + + /** + * @brief Destructor + */ + virtual ~Odometry3D() = default; + + /** + * @brief Shadowing extension to the AsyncSensorModel::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) override; + + /** + * @brief Callback for pose messages + * @param[in] msg - The pose message to process + */ + void process(const nav_msgs::msg::Odometry & msg); + +protected: + fuse_core::UUID device_id_; //!< The UUID of this device + + /** + * @brief Perform any required initialization for the sensor model + * + * This could include things like reading from the parameter server or subscribing to topics. The + * class's node handles will be properly initialized before SensorModel::onInit() is called. + * Spinning of the callback queue will not begin until after the call to SensorModel::onInit() + * completes. + */ + void onInit() override; + + /** + * @brief Subscribe to the input topic to start sending transactions to the optimizer + */ + void onStart() override; + + /** + * @brief Unsubscribe from the input topic to stop sending transactions to the optimizer + */ + void onStop() override; + + /** + * @brief Process a pose message in differential mode + * + * @param[in] pose - The pose message to process in differential mode + * @param[in] twist - The twist message used in case the twist covariance is used in differential + * mode + * @param[in] validate - Whether to validate the pose and twist coavriance or not + * @param[out] transaction - The generated variables and constraints are added to this transaction + */ + void processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const bool validate, + fuse_core::Transaction & transaction); + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; + + geometry_msgs::msg::PoseWithCovarianceStamped::UniquePtr previous_pose_; + + // NOTE(CH3): Unique ptr to defer till we have the node interfaces from initialize() + std::unique_ptr tf_buffer_; + std::unique_ptr tf_listener_; + + rclcpp::Subscription::SharedPtr sub_; + + using OdometryThrottledCallback = fuse_core::ThrottledMessageCallback; + OdometryThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__ODOMETRY_3D_HPP_ diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.hpp b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp new file mode 100644 index 000000000..f1c70f441 --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.hpp @@ -0,0 +1,255 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__ODOMETRY_3D_PUBLISHER_HPP_ +#define FUSE_MODELS__ODOMETRY_3D_PUBLISHER_HPP_ + +#include +#include +#include + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @class Odometry3DPublisher plugin that publishes a nav_msgs::msg::Odometry message and broadcasts + * a tf transform for optimized 3D state data (combination of Position3DStamped, + * Orientation3DStamped, VelocityLinear3DStamped, VelocityAngular3DStamped, and + * AccelerationLinear3DStamped). + * + * Parameters: + * - device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to + * publish + * - device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - publish_tf (bool, default: true) Whether to publish the generated pose data as a transform to + * the tf tree + * - predict_to_current_time (bool, default: false) The tf publication happens at a fixed rate. + * This parameter specifies whether we should + * predict, using the 3D omnidirectional model, the state + * at the time of the tf publication, rather than + * the last posterior (optimized) state. + * - publish_frequency (double, default: 10.0) How often, in Hz, we publish the filtered state + * data and broadcast the transform + * - tf_cache_time (double, default: 10.0) The length of our tf cache (only used if the + * world_frame_id and the map_frame_id are the same) + * - tf_timeout (double, default: 0.1) Our tf lookup timeout period (only used if the + * world_frame_id and the map_frame_id are the same) + * - queue_size (int, default: 1) The size of our ROS publication queue + * - map_frame_id (string, default: "map") Our map frame_id + * - odom_frame_id (string, default: "odom") Our odom frame_id + * - base_link_frame_id (string, default: "base_link") Our base_link (body) frame_id + * - world_frame_id (string, default: "odom") The frame_id that will be published as the parent + * frame for the output. Must be either the + * map_frame_id or the odom_frame_id. + * - topic (string, default: "odometry/filtered") The ROS topic to which we will publish the + * filtered state data + * + * Publishes: + * - odometry/filtered (nav_msgs::msg::Odometry) The most recent optimized state, gives as an + * odometry message + * - tf (via a tf2_ros::TransformBroadcaster) The most recent optimized state, as a tf transform + * + * Subscribes: + * - tf, tf_static (tf2_msgs::msg::TFMessage) Subscribes to tf data to obtain the requisite + * odom->base_link transform, but only if the + * world_frame_id is set to the value of the + * map_frame_id. + */ +class Odometry3DPublisher : public fuse_core::AsyncPublisher +{ +public: + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(Odometry3DPublisher) + using ParameterType = parameters::Odometry3DPublisherParams; + + /** + * @brief Constructor + */ + Odometry3DPublisher(); + + /** + * @brief Destructor + */ + virtual ~Odometry3DPublisher() = default; + + /** + * @brief Shadowing extension to the AsyncPublisher::initialize call + */ + void initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name) override; + +protected: + /** + * @brief Perform any required post-construction initialization, such as advertising publishers or + * reading from the parameter server. + */ + void onInit() override; + + /** + * @brief Fires whenever an optimized graph has been computed + * + * @param[in] transaction A Transaction object, describing the set of variables that have been + * added and/or removed + * @param[in] graph A read-only pointer to the graph object, allowing queries to be + * performed whenever needed + */ + void notifyCallback( + fuse_core::Transaction::ConstSharedPtr transaction, + fuse_core::Graph::ConstSharedPtr graph) override; + + /** + * @brief Perform any required operations before the first call to notify() occurs + */ + void onStart() override; + + /** + * @brief Perform any required operations to stop publications + */ + void onStop() override; + + /** + * @brief Retrieves the given variable values at the requested time from the graph + * @param[in] graph The graph from which we will retrieve the state + * @param[in] stamp The time stamp at which we want the state + * @param[in] device_id The device ID for which we want the given variables + * @param[out] position_uuid The UUID of the position variable that gets extracted from the graph + * @param[out] orientation_uuid The UUID of the orientation variable that gets extracted from the + * graph + * @param[out] velocity_linear_uuid The UUID of the linear velocity variable that gets extracted + * from the graph + * @param[out] velocity_angular_uuid The UUID of the angular velocity variable that gets extracted + * from the graph + * @param[out] acceleration_linear_uuid The UUID of the linear acceleration variable that gets + * extracted from the graph + * @param[out] odometry All of the fuse pose and velocity variable values get packed into this + * structure + * @param[out] acceleration All of the fuse acceleration variable values get packed into this + * structure + * @return true if the checks pass, false otherwise + */ + bool getState( + const fuse_core::Graph & graph, + const rclcpp::Time & stamp, + const fuse_core::UUID & device_id, + fuse_core::UUID & position_uuid, + fuse_core::UUID & orientation_uuid, + fuse_core::UUID & velocity_linear_uuid, + fuse_core::UUID & velocity_angular_uuid, + fuse_core::UUID & acceleration_linear_uuid, + nav_msgs::msg::Odometry & odometry, + geometry_msgs::msg::AccelWithCovarianceStamped & acceleration); + + /** + * @brief Timer callback method for the filtered state publication and tf broadcasting + * @param[in] event The timer event parameters that are associated with the given invocation + */ + void publishTimerCallback(); + + /** + * @brief Object that searches for the most recent common timestamp for a set of variables + */ + using Synchronizer = fuse_publishers::StampedVariableSynchronizer< + fuse_variables::Orientation3DStamped, + fuse_variables::Position3DStamped, + fuse_variables::VelocityLinear3DStamped, + fuse_variables::VelocityAngular3DStamped, + fuse_variables::AccelerationLinear3DStamped>; + + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Timers, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncPublisher interfaces_ + + fuse_core::UUID device_id_; //!< The UUID of this device + rclcpp::Clock::SharedPtr clock_; //!< The publisher's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The publisher's logger + + ParameterType params_; + + rclcpp::Time latest_stamp_; + rclcpp::Time latest_covariance_stamp_; + bool latest_covariance_valid_{false}; //!< Whether the latest covariance computed is valid or not + nav_msgs::msg::Odometry odom_output_; + geometry_msgs::msg::AccelWithCovarianceStamped acceleration_output_; + + //!< Object that tracks the latest common timestamp of multiple variables + Synchronizer synchronizer_; + + std::unique_ptr tf_buffer_; + + rclcpp::Publisher::SharedPtr odom_pub_; + rclcpp::Publisher::SharedPtr acceleration_pub_; + + std::shared_ptr tf_broadcaster_ = nullptr; + std::unique_ptr tf_listener_; + + fuse_core::DelayedThrottleFilter delayed_throttle_filter_{10.0}; //!< A ros::console filter to + //!< print delayed throttle + //!< messages, that can be reset + //!< on start + + rclcpp::TimerBase::SharedPtr publish_timer_; + + std::mutex mutex_; //!< A mutex to protect the access to the attributes used concurrently by the + //!< notifyCallback and publishTimerCallback methods: + //!< latest_stamp_, latest_covariance_stamp_, odom_output_ and + //!< acceleration_output_ +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__ODOMETRY_3D_PUBLISHER_HPP_ diff --git a/fuse_models/include/fuse_models/omnidirectional_3d.hpp b/fuse_models/include/fuse_models/omnidirectional_3d.hpp new file mode 100644 index 000000000..5cec25309 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d.hpp @@ -0,0 +1,243 @@ +/* + * 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 a 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< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping and logging + rclcpp::Logger logger_; //!< The sensor model's logger + + rclcpp::Duration buffer_length_; //!< The length of the state history + fuse_core::UUID device_id_; //!< The UUID of the device to be published + fuse_core::TimestampManager timestamp_manager_; //!< Tracks timestamps and previously created + //!< motion model segments + fuse_core::Matrix15d process_noise_covariance_; //!< Process noise covariance matrix + bool scale_process_noise_{false}; //!< Whether to scale the process noise + //!< covariance pose by the norm of the current + //!< state twist + double velocity_linear_norm_min_{1e-3}; //!< The minimum linear velocity norm allowed when + //!< scaling the process noise covariance + double velocity_angular_norm_min_{1e-3}; //!< The minimum twist norm allowed when + //!< scaling the process noise covariance + bool disable_checks_{false}; //!< Whether to disable the validation checks for the current and + //!< predicted state, including the process noise covariance after + //!< it is scaled and multiplied by dt + StateHistory state_history_; //!< History of optimized graph pose estimates +}; + +std::ostream & operator<<(std::ostream & stream, const Omnidirectional3D & 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..da92fb2d3 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_ignition.hpp @@ -0,0 +1,220 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;\ + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_IGNITION_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_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< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Clock, + fuse_core::node_interfaces::Graph, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters, + fuse_core::node_interfaces::Services, + fuse_core::node_interfaces::Topics, + fuse_core::node_interfaces::Waitables + > interfaces_; //!< Shadows AsyncSensorModel interfaces_ + + std::atomic_bool started_; //!< Flag indicating the sensor has been started + bool initial_transaction_sent_; //!< Flag indicating an initial transaction has been sent already + fuse_core::UUID device_id_; //!< The UUID of this device + rclcpp::Clock::SharedPtr clock_; //!< The sensor model's clock, for timestamping + rclcpp::Logger logger_; //!< The sensor model's logger + + ParameterType params_; //!< Object containing all of the configuration parameters + + //!< Service client used to call the "reset" service on the optimizer + rclcpp::Client::SharedPtr reset_client_; + rclcpp::Service::SharedPtr set_pose_service_; + rclcpp::Service::SharedPtr set_pose_deprecated_service_; + + rclcpp::Subscription::SharedPtr sub_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS__OMNIDIRECTIONAL_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..f5a0030ac --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_predict.hpp @@ -0,0 +1,671 @@ +/* + * 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_PREDICT_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_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__OMNIDIRECTIONAL_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..874042136 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_function.hpp @@ -0,0 +1,293 @@ +/* + * 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_STATE_COST_FUNCTION_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_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__OMNIDIRECTIONAL_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..4e8b28ea5 --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_cost_functor.hpp @@ -0,0 +1,217 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_COST_FUNCTOR_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_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__OMNIDIRECTIONAL_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..7201cdbed --- /dev/null +++ b/fuse_models/include/fuse_models/omnidirectional_3d_state_kinematic_constraint.hpp @@ -0,0 +1,188 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ +#define FUSE_MODELS__OMNIDIRECTIONAL_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__OMNIDIRECTIONAL_3D_STATE_KINEMATIC_CONSTRAINT_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp new file mode 100644 index 000000000..4051799d0 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.hpp @@ -0,0 +1,221 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__PARAMETERS__IMU_3D_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__IMU_3D_PARAMS_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Imu3D class + */ +struct Imu3DParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + angular_velocity_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_dimensions")); + linear_acceleration_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "linear_acceleration_dimensions")); + orientation_indices = loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "orientation_dimensions")); + + differential = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "differential"), + differential); + disable_checks = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "disable_checks"), + disable_checks); + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_timeout"), tf_timeout, + false); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_period"), throttle_period, + false); + throttle_use_wall_time = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_use_wall_time"), + throttle_use_wall_time); + + remove_gravitational_acceleration = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, "remove_gravitational_acceleration"), remove_gravitational_acceleration); + gravitational_acceleration = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "gravitational_acceleration"), + gravitational_acceleration); + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + if (differential) { + independent = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "independent"), + independent); + use_twist_covariance = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + } + + acceleration_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "acceleration_target_frame"), + acceleration_target_frame); + orientation_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "orientation_target_frame"), + orientation_target_frame); + twist_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "twist_target_frame"), + twist_target_frame); + + pose_loss = + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + angular_velocity_loss = + fuse_core::loadLossConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_loss")); + linear_acceleration_loss = + fuse_core::loadLossConfig( + interfaces, + fuse_core::joinParameterName(ns, "linear_acceleration_loss")); + } + + bool differential {false}; + bool disable_checks {false}; + bool independent {true}; + bool use_twist_covariance {true}; + fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance + //!< matrix + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + bool remove_gravitational_acceleration {false}; + int queue_size {10}; + rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds + bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not + double gravitational_acceleration {9.80665}; + std::string acceleration_target_frame {}; + std::string orientation_target_frame {}; + std::string topic {}; + std::string twist_target_frame {}; + std::vector angular_velocity_indices; + std::vector linear_acceleration_indices; + std::vector orientation_indices; + fuse_core::Loss::SharedPtr pose_loss; + fuse_core::Loss::SharedPtr angular_velocity_loss; + fuse_core::Loss::SharedPtr linear_acceleration_loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__IMU_3D_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp new file mode 100644 index 000000000..ed9059fa6 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.hpp @@ -0,0 +1,213 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ + +#include +#include + +#include + +#include +#include +#include +#include +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Odometry3D class + */ +struct Odometry3DParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + position_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "position_dimensions")); + orientation_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "orientation_dimensions")); + linear_velocity_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "linear_velocity_dimensions")); + angular_velocity_indices = + loadSensorConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_dimensions")); + + differential = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "differential"), + differential); + disable_checks = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "disable_checks"), + disable_checks); + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_timeout"), tf_timeout, + false); + + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_period"), throttle_period, + false); + throttle_use_wall_time = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "throttle_use_wall_time"), + throttle_use_wall_time); + + fuse_core::getParamRequired(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + twist_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "twist_target_frame"), + twist_target_frame); + pose_target_frame = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "pose_target_frame"), + pose_target_frame); + + if (differential) { + independent = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "independent"), + independent); + use_twist_covariance = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "use_twist_covariance"), + use_twist_covariance); + + minimum_pose_relative_covariance = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "minimum_pose_relative_covariance_diagonal"), 0.0); + twist_covariance_offset = + fuse_core::getCovarianceDiagonalParam<6>( + interfaces, + fuse_core::joinParameterName(ns, "twist_covariance_offset_diagonal"), 0.0); + } + + pose_loss = + fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "pose_loss")); + linear_velocity_loss = + fuse_core::loadLossConfig( + interfaces, fuse_core::joinParameterName( + ns, + "linear_velocity_loss")); + angular_velocity_loss = + fuse_core::loadLossConfig( + interfaces, fuse_core::joinParameterName( + ns, + "angular_velocity_loss")); + } + + bool differential {false}; + bool disable_checks {false}; + bool independent {true}; + bool use_twist_covariance {true}; + fuse_core::Matrix6d minimum_pose_relative_covariance; //!< Minimum pose relative covariance + //!< matrix + fuse_core::Matrix6d twist_covariance_offset; //!< Offset already added to the twist covariance + //!< matrix, that will be substracted in order to + //!< recover the raw values + int queue_size {10}; + rclcpp::Duration tf_timeout {0, 0}; //!< The maximum time to wait for a transform to become + //!< available + rclcpp::Duration throttle_period {0, 0}; //!< The throttle period duration in seconds + bool throttle_use_wall_time {false}; //!< Whether to throttle using ros::WallTime or not + std::string topic {}; + std::string pose_target_frame {}; + std::string twist_target_frame {}; + std::vector position_indices; + std::vector orientation_indices; + std::vector linear_velocity_indices; + std::vector angular_velocity_indices; + fuse_core::Loss::SharedPtr pose_loss; + fuse_core::Loss::SharedPtr linear_velocity_loss; + fuse_core::Loss::SharedPtr angular_velocity_loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp new file mode 100644 index 000000000..64d2d1380 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.hpp @@ -0,0 +1,249 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PUBLISHER_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PUBLISHER_PARAMS_HPP_ + +#include + +#include +#include +#include +#include + +#include + +#include +#include +#include + +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Odometry3DPublisher class + */ +struct Odometry3DPublisherParams : public ParameterBase +{ +public: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + publish_tf = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "publish_tf"), + publish_tf); + invert_tf = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "invert_tf"), + invert_tf); + predict_to_current_time = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "predict_to_current_time"), + predict_to_current_time); + predict_with_acceleration = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "predict_with_acceleration"), + predict_with_acceleration); + publish_frequency = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "publish_frequency"), + publish_frequency); + + process_noise_covariance = fuse_core::getCovarianceDiagonalParam<15>( + interfaces, fuse_core::joinParameterName( + ns, + "process_noise_diagonal"), 0.0); + scale_process_noise = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "scale_process_noise"), + scale_process_noise); + velocity_linear_norm_min_ = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "velocity_linear_norm_min"), + velocity_linear_norm_min_); + velocity_angular_norm_min_ = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "velocity_angular_norm_min"), + velocity_angular_norm_min_); + fuse_core::getPositiveParam( + interfaces, + fuse_core::joinParameterName( + ns, + "covariance_throttle_period"), covariance_throttle_period, + false); + + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_cache_time"), tf_cache_time, + false); + fuse_core::getPositiveParam( + interfaces, fuse_core::joinParameterName( + ns, + "tf_timeout"), tf_timeout, + false); + + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + + map_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "map_frame_id"), + map_frame_id); + odom_frame_id = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "odom_frame_id"), + odom_frame_id); + base_link_frame_id = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "base_link_frame_id"), + base_link_frame_id); + base_link_output_frame_id = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "base_link_output_frame_id"), + base_link_output_frame_id); + world_frame_id = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "world_frame_id"), + world_frame_id); + + const bool frames_valid = + map_frame_id != odom_frame_id && + map_frame_id != base_link_frame_id && + map_frame_id != base_link_output_frame_id && + odom_frame_id != base_link_frame_id && + odom_frame_id != base_link_output_frame_id && + (world_frame_id == map_frame_id || world_frame_id == odom_frame_id); + + if (!frames_valid) { + RCLCPP_FATAL_STREAM( + interfaces.get_node_logging_interface()->get_logger(), + "Invalid frame configuration! Please note:\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_frame_id must be " + << "unique\n" + << " - The values for map_frame_id, odom_frame_id, and base_link_output_frame_id must be " + << "unique\n" + << " - The world_frame_id must be the same as the map_frame_id or odom_frame_id\n"); + + assert(frames_valid); + } + + topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + acceleration_topic = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "acceleration_topic"), + acceleration_topic); + + fuse_core::loadCovarianceOptionsFromROS(interfaces, covariance_options, "covariance_options"); + } + + bool publish_tf {true}; //!< Whether to publish/broadcast the TF transform or not + bool invert_tf{false}; //!< Whether to broadcast the inverse of the TF transform or not. When + //!< the inverse is broadcasted, the transform is inverted and the + //!< header.frame_id and child_frame_id are swapped, i.e. the odometry + //!< output header.frame_id is set to the base_link_output_frame_id and + //!< the child_frame_id to the world_frame_id + bool predict_to_current_time {false}; + bool predict_with_acceleration {false}; + double publish_frequency {10.0}; + fuse_core::Matrix15d process_noise_covariance; //!< Process noise covariance matrix + bool scale_process_noise{false}; + double velocity_linear_norm_min_{1e-3}; + double velocity_angular_norm_min_{1e-3}; + rclcpp::Duration covariance_throttle_period {0, 0}; //!< The throttle period duration in seconds + //!< to compute the covariance + rclcpp::Duration tf_cache_time {10, 0}; + rclcpp::Duration tf_timeout {0, static_cast(RCUTILS_S_TO_NS(0.1))}; + int queue_size {1}; + std::string map_frame_id {"map"}; + std::string odom_frame_id {"odom"}; + std::string base_link_frame_id {"base_link"}; + std::string base_link_output_frame_id {base_link_frame_id}; + std::string world_frame_id {odom_frame_id}; + std::string topic {"odometry/filtered"}; + std::string acceleration_topic {"acceleration/filtered"}; + ceres::Covariance::Options covariance_options; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__ODOMETRY_3D_PUBLISHER_PARAMS_HPP_ diff --git a/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp new file mode 100644 index 000000000..c16658d67 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/omnidirectional_3d_ignition_params.hpp @@ -0,0 +1,218 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_ +#define FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_ + +#include +#include +#include +#include + +#include + +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Omnidirectional3DIgnition class + */ +struct Omnidirectional3DIgnitionParams : public ParameterBase +{ +public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] interfaces - The node interfaces with which to load parameters + * @param[in] ns - The parameter namespace to use + */ + void loadFromROS( + fuse_core::node_interfaces::NodeInterfaces< + fuse_core::node_interfaces::Base, + fuse_core::node_interfaces::Logging, + fuse_core::node_interfaces::Parameters + > interfaces, + const std::string & ns) + { + publish_on_startup = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "publish_on_startup"), + publish_on_startup); + queue_size = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "queue_size"), + queue_size); + reset_service = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "reset_service"), + reset_service); + set_pose_service = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "set_pose_service"), + set_pose_service); + set_pose_deprecated_service = + fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "set_pose_deprecated_service"), + set_pose_deprecated_service); + topic = fuse_core::getParam(interfaces, fuse_core::joinParameterName(ns, "topic"), topic); + + std::vector sigma_vector; + sigma_vector = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "initial_sigma"), + sigma_vector); + if (!sigma_vector.empty()) { + if (sigma_vector.size() != 15) { + throw std::invalid_argument( + "The supplied initial_sigma parameter must be length 15, but " + "is actually length " + + std::to_string(sigma_vector.size())); + } + auto is_sigma_valid = [](const double sigma) + { + return std::isfinite(sigma) && (sigma > 0); + }; + if (!std::all_of(sigma_vector.begin(), sigma_vector.end(), is_sigma_valid)) { + throw std::invalid_argument( + "The supplied initial_sigma parameter must contain valid floating point values. " + "NaN, Inf, and values <= 0 are not acceptable."); + } + initial_sigma.swap(sigma_vector); + } + + std::vector state_vector; + state_vector = fuse_core::getParam( + interfaces, fuse_core::joinParameterName( + ns, + "initial_state"), + state_vector); + if (!state_vector.empty()) { + if (state_vector.size() != 15) { + throw std::invalid_argument( + "The supplied initial_state parameter must be length 15, but is actually length " + + std::to_string(state_vector.size())); + } + auto is_state_valid = [](const double state) + { + return std::isfinite(state); + }; + if (!std::all_of(state_vector.begin(), state_vector.end(), is_state_valid)) { + throw std::invalid_argument( + "The supplied initial_state parameter must contain valid floating point values. " + "NaN, Inf, etc are not acceptable."); + } + initial_state.swap(state_vector); + } + + loss = fuse_core::loadLossConfig(interfaces, fuse_core::joinParameterName(ns, "loss")); + } + + + /** + * @brief Flag indicating if an initial state transaction should be sent on startup, or only in + * response to a set_pose service call or topic message. + */ + bool publish_on_startup {true}; + + /** + * @brief The size of the subscriber queue for the set_pose topic + */ + int queue_size {10}; + + /** + * @brief The name of the reset service to call before sending transactions to the optimizer + */ + std::string reset_service {"~/reset"}; + + /** + * @brief The name of the set_pose service to advertise + */ + std::string set_pose_service {"set_pose"}; + + /** + * @brief The name of the deprecated set_pose service without return codes + */ + std::string set_pose_deprecated_service {"set_pose_deprecated"}; + + /** + * @brief The topic name for received PoseWithCovarianceStamped messages + */ + std::string topic {"set_pose"}; + + /** + * @brief The uncertainty of the initial state value + * + * Standard deviations are provided as an 15-dimensional vector in the order: + * (x, y, z, roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) + * The covariance matrix is created placing the squared standard deviations along the diagonal of + * an 15x15 matrix. + */ + std::vector initial_sigma {1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, + 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9, 1.0e-9 + }; + + /** + * @brief The initial value of the 15-dimension state vector (x, y, z, roll, pitch, yaw, + * x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, x_acc, y_acc, z_acc) + */ + std::vector initial_state {0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0}; + /** + * @brief Loss function + */ + fuse_core::Loss::SharedPtr loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS__PARAMETERS__OMNIDIRECTIONAL_3D_IGNITION_PARAMS_HPP_ diff --git a/fuse_models/package.xml b/fuse_models/package.xml index 2fdd895eb..adef11d4d 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..d01bf0997 --- /dev/null +++ b/fuse_models/src/imu_3d.cpp @@ -0,0 +1,320 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +// Register this sensor model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Imu3D, fuse_core::SensorModel) + +namespace fuse_models +{ + +Imu3D::Imu3D() +: fuse_core::AsyncSensorModel(1), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")), + throttled_callback_(std::bind(&Imu3D::process, this, std::placeholders::_1)) +{ +} + +void Imu3D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + +void Imu3D::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(clock_); + } + + if (params_.orientation_indices.empty() && + params_.linear_acceleration_indices.empty() && + params_.angular_velocity_indices.empty()) + { + RCLCPP_WARN_STREAM( + logger_, + "No dimensions were specified. Data from topic " << params_.topic + << " will be ignored."); + } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); +} + +void Imu3D::onStart() +{ + if (!params_.orientation_indices.empty() || + !params_.linear_acceleration_indices.empty() || + !params_.angular_velocity_indices.empty()) + { + previous_pose_.reset(); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + params_.topic, + params_.queue_size, + std::bind( + &ImuThrottledCallback::callback, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); + } +} + +void Imu3D::onStop() +{ + sub_.reset(); +} + +void Imu3D::process(const sensor_msgs::msg::Imu & msg) +{ + // Create a transaction object + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(msg.header.stamp); + + // Handle the orientation data (treat it as a pose, but with only orientation indices used) + auto pose = std::make_unique(); + pose->header = msg.header; + pose->pose.pose.orientation = msg.orientation; + pose->pose.covariance[21] = msg.orientation_covariance[0]; + pose->pose.covariance[22] = msg.orientation_covariance[1]; + pose->pose.covariance[23] = msg.orientation_covariance[2]; + pose->pose.covariance[27] = msg.orientation_covariance[3]; + pose->pose.covariance[28] = msg.orientation_covariance[4]; + pose->pose.covariance[29] = msg.orientation_covariance[5]; + pose->pose.covariance[33] = msg.orientation_covariance[6]; + pose->pose.covariance[34] = msg.orientation_covariance[7]; + pose->pose.covariance[35] = msg.orientation_covariance[8]; + + geometry_msgs::msg::TwistWithCovarianceStamped twist; + twist.header = msg.header; + twist.twist.twist.angular = msg.angular_velocity; + twist.twist.covariance[21] = msg.angular_velocity_covariance[0]; + twist.twist.covariance[22] = msg.angular_velocity_covariance[1]; + twist.twist.covariance[23] = msg.angular_velocity_covariance[2]; + twist.twist.covariance[27] = msg.angular_velocity_covariance[3]; + twist.twist.covariance[28] = msg.angular_velocity_covariance[4]; + twist.twist.covariance[29] = msg.angular_velocity_covariance[5]; + twist.twist.covariance[33] = msg.angular_velocity_covariance[6]; + twist.twist.covariance[34] = msg.angular_velocity_covariance[7]; + twist.twist.covariance[35] = msg.angular_velocity_covariance[8]; + + const bool validate = !params_.disable_checks; + + if (params_.differential) { + processDifferential(*pose, twist, validate, *transaction); + } else { + common::processAbsolutePose3DWithCovariance( + name(), + device_id_, + *pose, + params_.pose_loss, + params_.orientation_target_frame, + {}, + params_.orientation_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + } + + // Handle the twist data (only include indices for angular velocity) + common::processTwist3DWithCovariance( + name(), + device_id_, + twist, + nullptr, + params_.angular_velocity_loss, + params_.twist_target_frame, + {}, + params_.angular_velocity_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + + // Handle the acceleration data + geometry_msgs::msg::AccelWithCovarianceStamped accel; + accel.header = msg.header; + accel.accel.accel.linear = msg.linear_acceleration; + accel.accel.covariance[0] = msg.linear_acceleration_covariance[0]; + accel.accel.covariance[1] = msg.linear_acceleration_covariance[1]; + accel.accel.covariance[2] = msg.linear_acceleration_covariance[2]; + accel.accel.covariance[6] = msg.linear_acceleration_covariance[3]; + accel.accel.covariance[7] = msg.linear_acceleration_covariance[4]; + accel.accel.covariance[8] = msg.linear_acceleration_covariance[5]; + accel.accel.covariance[12] = msg.linear_acceleration_covariance[6]; + accel.accel.covariance[13] = msg.linear_acceleration_covariance[7]; + accel.accel.covariance[14] = msg.linear_acceleration_covariance[8]; + + // Optionally remove the acceleration due to gravity + if (params_.remove_gravitational_acceleration) { + geometry_msgs::msg::Vector3 accel_gravity; + accel_gravity.z = params_.gravitational_acceleration; + geometry_msgs::msg::TransformStamped orientation_trans; + tf2::Quaternion imu_orientation; + tf2::fromMsg(msg.orientation, imu_orientation); + orientation_trans.transform.rotation = tf2::toMsg(imu_orientation.inverse()); + tf2::doTransform(accel_gravity, accel_gravity, orientation_trans); // Doesn't use the stamp + accel.accel.accel.linear.x -= accel_gravity.x; + accel.accel.accel.linear.y -= accel_gravity.y; + accel.accel.accel.linear.z -= accel_gravity.z; + } + + common::processAccel3DWithCovariance( + name(), + device_id_, + accel, + params_.linear_acceleration_loss, + params_.acceleration_target_frame, + params_.linear_acceleration_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + + // Send the transaction object to the plugin's parent + sendTransaction(transaction); +} + +void Imu3D::processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const bool validate, + fuse_core::Transaction & transaction) +{ + auto transformed_pose = std::make_unique(); + transformed_pose->header.frame_id = + params_.orientation_target_frame.empty() ? pose.header.frame_id : params_. + orientation_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose, params_.tf_timeout)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " << rclcpp::Time( + pose.header.stamp).nanoseconds() + << " to orientation target frame " << + params_.orientation_target_frame); + return; + } + + if (!previous_pose_) { + previous_pose_ = std::move(transformed_pose); + return; + } + + if (params_.use_twist_covariance) { + geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; + transformed_twist.header.frame_id = + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " << rclcpp::Time( + twist.header.stamp).nanoseconds() + << " to twist target frame " << + params_.twist_target_frame); + } else { + common::processDifferentialPose3DWithTwistCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + twist, + params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, + params_.pose_loss, + {}, + params_.orientation_indices, + validate, + transaction); + } + } else { + common::processDifferentialPose3DWithCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + params_.independent, + params_.minimum_pose_relative_covariance, + params_.pose_loss, + {}, + params_.orientation_indices, + validate, + transaction); + } + + previous_pose_ = std::move(transformed_pose); +} + +} // namespace fuse_models diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp new file mode 100644 index 000000000..4afd0ee39 --- /dev/null +++ b/fuse_models/src/odometry_3d.cpp @@ -0,0 +1,257 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Register this sensor model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Odometry3D, fuse_core::SensorModel) + +namespace fuse_models +{ + +Odometry3D::Odometry3D() +: fuse_core::AsyncSensorModel(1), + device_id_(fuse_core::uuid::NIL), + logger_(rclcpp::get_logger("uninitialized")), + throttled_callback_(std::bind(&Odometry3D::process, this, std::placeholders::_1)) +{ +} + +void Odometry3D::initialize( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const std::string & name, + fuse_core::TransactionCallback transaction_callback) +{ + interfaces_ = interfaces; + fuse_core::AsyncSensorModel::initialize(interfaces, name, transaction_callback); +} + +void Odometry3D::onInit() +{ + logger_ = interfaces_.get_node_logging_interface()->get_logger(); + clock_ = interfaces_.get_node_clock_interface()->get_clock(); + + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(interfaces_); + + params_.loadFromROS(interfaces_, name_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + + if (!params_.throttle_use_wall_time) { + throttled_callback_.setClock(clock_); + } + + if (params_.position_indices.empty() && params_.orientation_indices.empty() && + params_.linear_velocity_indices.empty() && params_.angular_velocity_indices.empty()) + { + RCLCPP_WARN_STREAM( + logger_, + "No dimensions were specified. Data from topic " << params_.topic + << " will be ignored."); + } + + tf_buffer_ = std::make_unique(clock_); + tf_listener_ = std::make_unique( + *tf_buffer_, + interfaces_.get_node_base_interface(), + interfaces_.get_node_logging_interface(), + interfaces_.get_node_parameters_interface(), + interfaces_.get_node_topics_interface() + ); +} + +void Odometry3D::onStart() +{ + if (!params_.position_indices.empty() || !params_.orientation_indices.empty() || + !params_.linear_velocity_indices.empty() || !params_.angular_velocity_indices.empty()) + { + previous_pose_.reset(); + + rclcpp::SubscriptionOptions sub_options; + sub_options.callback_group = cb_group_; + + sub_ = rclcpp::create_subscription( + interfaces_, + params_.topic, + params_.queue_size, + std::bind( + &OdometryThrottledCallback::callback< + const nav_msgs::msg::Odometry &>, + &throttled_callback_, + std::placeholders::_1 + ), + sub_options + ); + } +} + +void Odometry3D::onStop() +{ + sub_.reset(); +} + +void Odometry3D::process(const nav_msgs::msg::Odometry & msg) +{ + // Create a transaction object + auto transaction = fuse_core::Transaction::make_shared(); + transaction->stamp(msg.header.stamp); + + // Handle the pose data + auto pose = std::make_unique(); + pose->header = msg.header; + pose->pose = msg.pose; + + geometry_msgs::msg::TwistWithCovarianceStamped twist; + twist.header = msg.header; + twist.header.frame_id = msg.child_frame_id; + twist.twist = msg.twist; + + const bool validate = !params_.disable_checks; + + if (params_.differential) { + processDifferential(*pose, twist, validate, *transaction); + } else { + common::processAbsolutePose3DWithCovariance( + name(), + device_id_, + *pose, + params_.pose_loss, + params_.pose_target_frame, + params_.position_indices, + params_.orientation_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + } + + // Handle the twist data + common::processTwist3DWithCovariance( + name(), + device_id_, + twist, + params_.linear_velocity_loss, + params_.angular_velocity_loss, + params_.twist_target_frame, + params_.linear_velocity_indices, + params_.angular_velocity_indices, + *tf_buffer_, + validate, + *transaction, + params_.tf_timeout); + + // Send the transaction object to the plugin's parent + sendTransaction(transaction); +} + +void Odometry3D::processDifferential( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::TwistWithCovarianceStamped & twist, + const bool validate, + fuse_core::Transaction & transaction) +{ + auto transformed_pose = std::make_unique(); + transformed_pose->header.frame_id = + params_.pose_target_frame.empty() ? pose.header.frame_id : params_.pose_target_frame; + + if (!common::transformMessage(*tf_buffer_, pose, *transformed_pose)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform pose message with stamp " + << rclcpp::Time( + pose.header.stamp).nanoseconds() << " to pose target frame " << params_.pose_target_frame); + return; + } + + if (!previous_pose_) { + previous_pose_ = std::move(transformed_pose); + return; + } + + if (params_.use_twist_covariance) { + geometry_msgs::msg::TwistWithCovarianceStamped transformed_twist; + transformed_twist.header.frame_id = + params_.twist_target_frame.empty() ? twist.header.frame_id : params_.twist_target_frame; + + if (!common::transformMessage(*tf_buffer_, twist, transformed_twist)) { + RCLCPP_WARN_STREAM_THROTTLE( + logger_, *clock_, 5.0 * 1000, + "Cannot transform twist message with stamp " << rclcpp::Time( + twist.header.stamp).nanoseconds() + << " to twist target frame " << + params_.twist_target_frame); + } else { + common::processDifferentialPose3DWithTwistCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + transformed_twist, + params_.minimum_pose_relative_covariance, + params_.twist_covariance_offset, + params_.pose_loss, + params_.position_indices, + params_.orientation_indices, + validate, + transaction); + } + } else { + common::processDifferentialPose3DWithCovariance( + name(), + device_id_, + *previous_pose_, + *transformed_pose, + params_.independent, + params_.minimum_pose_relative_covariance, + params_.pose_loss, + params_.position_indices, + params_.orientation_indices, + validate, + transaction); + } + previous_pose_ = std::move(transformed_pose); +} + +} // namespace fuse_models diff --git a/fuse_models/src/odometry_3d_publisher.cpp b/fuse_models/src/odometry_3d_publisher.cpp new file mode 100644 index 000000000..f1e00267a --- /dev/null +++ b/fuse_models/src/odometry_3d_publisher.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 +#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..ee925ef40 --- /dev/null +++ b/fuse_models/src/omnidirectional_3d.cpp @@ -0,0 +1,629 @@ +/* + * 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..422485418 --- /dev/null +++ b/fuse_models/src/omnidirectional_3d_ignition.cpp @@ -0,0 +1,469 @@ +/* + * 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]); + 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..b92b7f031 --- /dev/null +++ b/fuse_models/src/omnidirectional_3d_state_kinematic_constraint.cpp @@ -0,0 +1,119 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, Giacomo Franchini + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace fuse_models +{ + +Omnidirectional3DStateKinematicConstraint::Omnidirectional3DStateKinematicConstraint( + const std::string & source, + const fuse_variables::Position3DStamped & position1, + const fuse_variables::Orientation3DStamped & orientation1, + const fuse_variables::VelocityLinear3DStamped & velocity_linear1, + const fuse_variables::VelocityAngular3DStamped & velocity_angular1, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear1, + const fuse_variables::Position3DStamped & position2, + const fuse_variables::Orientation3DStamped & orientation2, + const fuse_variables::VelocityLinear3DStamped & velocity_linear2, + const fuse_variables::VelocityAngular3DStamped & velocity_angular2, + const fuse_variables::AccelerationLinear3DStamped & acceleration_linear2, + const fuse_core::Matrix15d & covariance) +: fuse_core::Constraint( + source, + {position1.uuid(), + orientation1.uuid(), + velocity_linear1.uuid(), + velocity_angular1.uuid(), + acceleration_linear1.uuid(), + position2.uuid(), + orientation2.uuid(), + velocity_linear2.uuid(), + velocity_angular2.uuid(), + acceleration_linear2.uuid()}), // NOLINT + dt_((position2.stamp() - position1.stamp()).seconds()), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +void Omnidirectional3DStateKinematicConstraint::print(std::ostream & stream) const +{ + stream << type() << "\n" + << " source: " << source() << "\n" + << " uuid: " << uuid() << "\n" + << " position variable 1: " << variables().at(0) << "\n" + << " orientation variable 1: " << variables().at(1) << "\n" + << " linear velocity variable 1: " << variables().at(2) << "\n" + << " angular velocity variable 1: " << variables().at(3) << "\n" + << " linear acceleration variable 1: " << variables().at(4) << "\n" + << " position variable 2: " << variables().at(5) << "\n" + << " orientation variable 2: " << variables().at(6) << "\n" + << " linear velocity variable 2: " << variables().at(7) << "\n" + << " angular velocity variable 2: " << variables().at(8) << "\n" + << " linear acceleration variable 2: " << variables().at(9) << "\n" + << " dt: " << dt() << "\n" + << " sqrt_info: " << sqrtInformation() << "\n"; +} + +ceres::CostFunction * Omnidirectional3DStateKinematicConstraint::costFunction() const +{ + return new Omnidirectional3DStateCostFunction(dt_, sqrt_information_); + // Here we return a cost function that computes the analytic derivatives/jacobians, but we could + // use automatic differentiation as follows: + // + // return new ceres::AutoDiffCostFunction( + // new Omnidirectional3DStateCostFunctor(dt_, sqrt_information_)); + + // And including the followings: + // #include + // #include +} + +} // namespace fuse_models + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_models::Omnidirectional3DStateKinematicConstraint); +PLUGINLIB_EXPORT_CLASS( + fuse_models::Omnidirectional3DStateKinematicConstraint, + fuse_core::Constraint); diff --git a/fuse_models/test/CMakeLists.txt b/fuse_models/test/CMakeLists.txt index f8e4347f5..048e96778 100644 --- a/fuse_models/test/CMakeLists.txt +++ b/fuse_models/test/CMakeLists.txt @@ -5,6 +5,10 @@ set(TEST_TARGETS test_unicycle_2d_state_cost_function test_graph_ignition test_unicycle_2d_ignition + test_omnidirectional_3d + test_omnidirectional_3d_predict + test_omnidirectional_3d_state_cost_function + test_omnidirectional_3d_ignition ) foreach(test_name ${TEST_TARGETS}) @@ -14,3 +18,6 @@ endforeach() ament_add_gmock(test_sensor_proc "test_sensor_proc.cpp") target_link_libraries(test_sensor_proc ${PROJECT_NAME}) + +ament_add_gmock(test_sensor_proc_3d "test_sensor_proc_3d.cpp") +target_link_libraries(test_sensor_proc_3d ${PROJECT_NAME}) \ No newline at end of file diff --git a/fuse_models/test/test_omnidirectional_3d.cpp b/fuse_models/test/test_omnidirectional_3d.cpp new file mode 100644 index 000000000..c525189af --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d.cpp @@ -0,0 +1,405 @@ +/*************************************************************************** + * Copyright (C) 2017 Locus Robotics. All rights reserved. + * Unauthorized copying of this file, via any medium, is strictly prohibited + * Proprietary and confidential + ***************************************************************************/ +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/** + * @brief Derived class used in unit tests to expose protected functions + */ +class Omnidirectional3DModelTest : public fuse_models::Omnidirectional3D +{ +public: + using fuse_models::Omnidirectional3D::updateStateHistoryEstimates; + using fuse_models::Omnidirectional3D::StateHistoryElement; + using fuse_models::Omnidirectional3D::StateHistory; +}; + +TEST(Omnidirectional3D, UpdateStateHistoryEstimates) +{ + // Create some variables + auto position1 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(1, 0)); + auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_velocity1 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(1, 0)); + auto angular_velocity1 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(1, 0)); + auto linear_acceleration1 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(1, 0)); + position1->x() = 1.1; + position1->y() = 2.1; + position1->z() = 0.0; + orientation1->w() = 1.0; + orientation1->x() = 0.0; + orientation1->y() = 0.0; + orientation1->z() = 0.0; + linear_velocity1->x() = 1.0; + linear_velocity1->y() = 0.0; + linear_velocity1->z() = 0.0; + angular_velocity1->roll() = 0.0; + angular_velocity1->pitch() = 0.0; + angular_velocity1->yaw() = 0.0; + linear_acceleration1->x() = 1.0; + linear_acceleration1->y() = 0.0; + linear_acceleration1->z() = 0.0; + auto position2 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(2, 0)); + auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_velocity2 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(2, 0)); + auto angular_velocity2 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(2, 0)); + auto linear_acceleration2 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(2, 0)); + position2->x() = 1.2; + position2->y() = 2.2; + position2->z() = 0.0; + orientation2->w() = 1.0; + orientation2->x() = 0.0; + orientation2->y() = 0.0; + orientation2->z() = 0.0; + linear_velocity2->x() = 0.0; + linear_velocity2->y() = 1.0; + linear_velocity2->z() = 0.0; + angular_velocity2->roll() = 0.0; + angular_velocity2->pitch() = 0.0; + angular_velocity2->yaw() = 0.0; + linear_acceleration2->x() = 0.0; + linear_acceleration2->y() = 1.0; + linear_acceleration2->z() = 0.0; + auto position3 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(3, 0)); + auto orientation3 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_velocity3 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(3, 0)); + auto angular_velocity3 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(3, 0)); + auto linear_acceleration3 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(3, 0)); + position3->x() = 1.3; + position3->y() = 2.3; + position3->z() = 0.0; + orientation3->w() = 1.0; + orientation3->x() = 0.0; + orientation3->y() = 0.0; + orientation3->z() = 0.0; + linear_velocity3->x() = 4.3; + linear_velocity3->y() = 5.3; + linear_velocity3->z() = 0.0; + angular_velocity3->roll() = 0.0; + angular_velocity3->pitch() = 0.0; + angular_velocity3->yaw() = 0.0; + linear_acceleration3->x() = 7.3; + linear_acceleration3->y() = 8.3; + linear_acceleration3->z() = 0.0; + auto position4 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(4, 0)); + auto orientation4 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_velocity4 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(4, 0)); + auto angular_velocity4 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(4, 0)); + auto linear_acceleration4 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(4, 0)); + position4->x() = 1.4; + position4->y() = 2.4; + position4->z() = 0.0; + orientation4->w() = 1.0; + orientation4->x() = 0.0; + orientation4->y() = 0.0; + orientation4->z() = 0.0; + linear_velocity4->x() = 4.4; + linear_velocity4->y() = 5.4; + linear_velocity4->z() = 0.0; + angular_velocity4->roll() = 0.0; + angular_velocity4->pitch() = 0.0; + angular_velocity4->yaw() = 0.0; + linear_acceleration4->x() = 7.4; + linear_acceleration4->y() = 8.4; + linear_acceleration4->z() = 0.0; + auto position5 = fuse_variables::Position3DStamped::make_shared(rclcpp::Time(5, 0)); + auto orientation5 = fuse_variables::Orientation3DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_velocity5 = fuse_variables::VelocityLinear3DStamped::make_shared(rclcpp::Time(5, 0)); + auto angular_velocity5 = + fuse_variables::VelocityAngular3DStamped::make_shared(rclcpp::Time(5, 0)); + auto linear_acceleration5 = + fuse_variables::AccelerationLinear3DStamped::make_shared(rclcpp::Time(5, 0)); + position5->x() = 1.5; + position5->y() = 2.5; + position5->z() = 0.0; + orientation5->w() = 1.0; + orientation5->x() = 0.0; + orientation5->y() = 0.0; + orientation5->z() = 0.0; + linear_velocity5->x() = 4.5; + linear_velocity5->y() = 5.5; + linear_velocity5->z() = 0.0; + angular_velocity5->roll() = 0.0; + angular_velocity5->pitch() = 0.0; + angular_velocity5->yaw() = 0.0; + linear_acceleration5->x() = 7.5; + linear_acceleration5->y() = 8.5; + linear_acceleration5->z() = 0.0; + + // Add a subset of the variables to a graph + fuse_graphs::HashGraph graph; + graph.addVariable(position2); + graph.addVariable(orientation2); + graph.addVariable(linear_velocity2); + graph.addVariable(angular_velocity2); + graph.addVariable(linear_acceleration2); + + graph.addVariable(position4); + graph.addVariable(orientation4); + graph.addVariable(linear_velocity4); + graph.addVariable(angular_velocity4); + graph.addVariable(linear_acceleration4); + + // Add all of the variables to the state history + Omnidirectional3DModelTest::StateHistory state_history; + state_history.emplace( + position1->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position1->uuid(), + orientation1->uuid(), + linear_velocity1->uuid(), + angular_velocity1->uuid(), + linear_acceleration1->uuid(), + fuse_core::Vector3d(1.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position2->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position2->uuid(), + orientation2->uuid(), + linear_velocity2->uuid(), + angular_velocity2->uuid(), + linear_acceleration2->uuid(), + fuse_core::Vector3d(2.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position3->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position3->uuid(), + orientation3->uuid(), + linear_velocity3->uuid(), + angular_velocity3->uuid(), + linear_acceleration3->uuid(), + fuse_core::Vector3d(3.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position4->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position4->uuid(), + orientation4->uuid(), + linear_velocity4->uuid(), + angular_velocity4->uuid(), + linear_acceleration4->uuid(), + fuse_core::Vector3d(4.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position5->stamp(), + Omnidirectional3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position5->uuid(), + orientation5->uuid(), + linear_velocity5->uuid(), + angular_velocity5->uuid(), + linear_acceleration5->uuid(), + fuse_core::Vector3d(5.0, 0.0, 0.0), + Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0), + fuse_core::Vector3d(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + + // Update the state history + Omnidirectional3DModelTest::updateStateHistoryEstimates( + graph, state_history, rclcpp::Duration::from_seconds( + 10.0)); + + // Check the state estimates in the state history + { + // The first entry is missing from the graph. It will not get updated. + auto expected_position = fuse_core::Vector3d(1.0, 0.0, 0.0); + auto expected_orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + auto actual_position = state_history[rclcpp::Time(1, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(1, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(1, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(1, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(1, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The second entry is included in the graph. It will get updated directly. + auto expected_position = fuse_core::Vector3d(1.2, 2.2, 0.0); // <-- value in the Graph + auto expected_orientation = Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0); + auto actual_position = state_history[rclcpp::Time(2, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(2, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 1.0, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(2, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(2, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(0.0, 1.0, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(2, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The third entry is missing from the graph. It will get predicted from previous state. + auto expected_position = fuse_core::Vector3d(1.2, 3.7, 0.0); + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto actual_position = state_history[rclcpp::Time(3, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(3, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(0.0, 2.0, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(3, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(3, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(0.0, 1.0, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(3, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The forth entry is included in the graph. It will get updated directly. + auto expected_position = fuse_core::Vector3d(1.4, 2.4, 0.0); // <-- value in the Graph + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto actual_position = state_history[rclcpp::Time(4, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(4, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(4.4, 5.4, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(4, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(4, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(7.4, 8.4, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(4, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } + { + // The fifth entry is missing from the graph. It will get predicted from previous state. + auto expected_position = fuse_core::Vector3d(9.5, 12.0, 0.0); + auto expected_orientation = + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + auto actual_position = state_history[rclcpp::Time(5, 0)].position; + auto actual_orientation = state_history[rclcpp::Time(5, 0)].orientation; + EXPECT_NEAR(expected_position.x(), actual_position.x(), 1.0e-9); + EXPECT_NEAR(expected_position.y(), actual_position.y(), 1.0e-9); + EXPECT_NEAR(expected_position.z(), actual_position.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.x(), actual_orientation.x(), 1.0e-9); + EXPECT_NEAR(expected_orientation.y(), actual_orientation.y(), 1.0e-9); + EXPECT_NEAR(expected_orientation.z(), actual_orientation.z(), 1.0e-9); + EXPECT_NEAR(expected_orientation.w(), actual_orientation.w(), 1.0e-9); + + auto expected_linear_velocity = fuse_core::Vector3d(11.8, 13.8, 0.0); + auto actual_linear_velocity = state_history[rclcpp::Time(5, 0)].vel_linear; + EXPECT_NEAR(expected_linear_velocity.x(), actual_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.y(), actual_linear_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_velocity.z(), actual_linear_velocity.z(), 1.0e-9); + + auto expected_angular_velocity = fuse_core::Vector3d(0.0, 0.0, 0.0); + auto actual_angular_velocity = state_history[rclcpp::Time(5, 0)].vel_angular; + EXPECT_NEAR(expected_angular_velocity.x(), actual_angular_velocity.x(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.y(), actual_angular_velocity.y(), 1.0e-9); + EXPECT_NEAR(expected_angular_velocity.z(), actual_angular_velocity.z(), 1.0e-9); + + auto expected_linear_acceleration = fuse_core::Vector3d(7.4, 8.4, 0.0); + auto actual_linear_acceleration = state_history[rclcpp::Time(5, 0)].acc_linear; + EXPECT_NEAR(expected_linear_acceleration.x(), actual_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.y(), actual_linear_acceleration.y(), 1.0e-9); + EXPECT_NEAR(expected_linear_acceleration.z(), actual_linear_acceleration.z(), 1.0e-9); + } +} diff --git a/fuse_models/test/test_omnidirectional_3d_ignition.cpp b/fuse_models/test/test_omnidirectional_3d_ignition.cpp new file mode 100644 index 000000000..ece200d38 --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d_ignition.cpp @@ -0,0 +1,475 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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 + +using fuse_constraints::AbsolutePosition3DStampedConstraint; +using fuse_constraints::AbsoluteOrientation3DStampedConstraint; +using fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint; +using fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint; +using fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint; + + +/** + * @brief Promise used to communicate between the tests and the callback + */ +std::promise callback_promise; + +/** + * @brief Transaction callback that forwards the transaction into the promise result + */ +void transactionCallback(fuse_core::Transaction::SharedPtr transaction) +{ + callback_promise.set_value(std::move(transaction)); +} + +/** + * @brief Helper function for fetching the desired constraint from a transaction + */ +template +const Derived * getConstraint(const fuse_core::Transaction & transaction) +{ + for (const auto & constraint : transaction.addedConstraints()) { + auto derived = dynamic_cast(&constraint); + if (derived) { + return derived; + } + } + return nullptr; +} + + +class Omnidirectional3DIgnitionTestFixture : public ::testing::Test +{ +public: + Omnidirectional3DIgnitionTestFixture() + { + } + + void SetUp() override + { + rclcpp::init(0, nullptr); + executor_ = std::make_shared(); + spinner_ = std::thread( + [&]() { + executor_->spin(); + }); + } + + void TearDown() override + { + executor_->cancel(); + if (spinner_.joinable()) { + spinner_.join(); + } + executor_.reset(); + rclcpp::shutdown(); + } + + std::thread spinner_; //!< Internal thread for spinning the executor + rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_; +}; + +TEST_F(Omnidirectional3DIgnitionTestFixture, InitialTransaction) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments( + { + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]"}); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + // The ignition sensor should publish a transaction immediately. Wait for the callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(status == std::future_status::ready); + + // Check the transaction + auto transaction = callback_future.get(); + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 1.2, 2.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 1.0, 0.0, 0.0, 0.0, 4.0, 0.0, 0.0, 0.0, 9.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 0.2, 0.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 16.0, 0.0, 0.0, 0.0, 25.0, 0.0, 0.0, 0.0, 36.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + fuse_variables::Orientation3DStamped orientation_actual; + orientation_actual.w() = actual->mean()[0]; + orientation_actual.x() = actual->mean()[1]; + orientation_actual.y() = actual->mean()[2]; + orientation_actual.z() = actual->mean()[3]; + EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-9); + EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-9); + EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 6.7, 7.8, 8.9; + fuse_core::Matrix3d expected_cov; + expected_cov << 49.0, 0.0, 0.0, 0.0, 64.0, 0.0, 0.0, 0.0, 81.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 9.1, 10.2, 11.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 100.0, 0.0, 0.0, 0.0, 121.0, 0.0, 0.0, 0.0, 144.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 12.4, 13.5, 14.6; + fuse_core::Matrix3d expected_cov; + expected_cov << 169.0, 0.0, 0.0, 0.0, 196.0, 0.0, 0.0, 0.0, 225.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } +} + +TEST_F(Omnidirectional3DIgnitionTestFixture, SkipInitialTransaction) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments( + { + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]", + "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false"}); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + // The ignition sensor should publish a transaction immediately. Wait for the callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_FALSE(status == std::future_status::ready); +} + +TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseService) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments( + { + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]", + "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false"}); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad + + // Call the SetPose service + auto srv = std::make_shared(); + srv->pose.header.stamp = rclcpp::Time(12, 345678910); + srv->pose.pose.pose.position.x = 1.0; + srv->pose.pose.pose.position.y = 2.0; + srv->pose.pose.pose.position.z = 3.0; + 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.pose.orientation.w = q.w(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 2.0; + srv->pose.pose.covariance[14] = 3.0; + srv->pose.pose.covariance[21] = 4.0; + srv->pose.pose.covariance[28] = 5.0; + srv->pose.pose.covariance[35] = 6.0; + auto client = node->create_client( + "/omnidirectional_3d_ignition_test/set_pose"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + EXPECT_TRUE(result.get()->success); + + // The ignition sensor should publish a transaction in response to the service call. Wait for the + // callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(status == std::future_status::ready); + + // Check the transaction + auto transaction = callback_future.get(); + { + fuse_core::Vector3d expected_mean; + expected_mean << 1.0, 2.0, 3.0; + fuse_core::Matrix3d expected_cov; + expected_cov << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 0.1, 0.1; + fuse_core::Matrix3d expected_cov; + expected_cov << 4.0, 0.0, 0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 6.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + fuse_variables::Orientation3DStamped orientation_actual; + orientation_actual.w() = actual->mean()[0]; + orientation_actual.x() = actual->mean()[1]; + orientation_actual.y() = actual->mean()[2]; + orientation_actual.z() = actual->mean()[3]; + EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-1); + EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-1); + EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-1); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 6.7, 7.8, 8.9; + fuse_core::Matrix3d expected_cov; + expected_cov << 49.0, 0.0, 0.0, 0.0, 64.0, 0.0, 0.0, 0.0, 81.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 9.1, 10.2, 11.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 100.0, 0.0, 0.0, 0.0, 121.0, 0.0, 0.0, 0.0, 144.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 12.4, 13.5, 14.6; + fuse_core::Matrix3d expected_cov; + expected_cov << 169.0, 0.0, 0.0, 0.0, 196.0, 0.0, 0.0, 0.0, 225.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } +} + +TEST_F(Omnidirectional3DIgnitionTestFixture, SetPoseDeprecatedService) +{ + // Set some configuration + rclcpp::NodeOptions options; + options.arguments( + { + "--ros-args", + "-p", "ignition_sensor.initial_state:=" + "[0.1, 1.2, 2.3, 0.1, 0.2, 0.3, 6.7, 7.8, 8.9, 9.1, 10.2, 11.3, 12.4, 13.5, 14.6]", + "-p", "ignition_sensor.initial_sigma:=" + "[1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 9.0, 10.0, 11.0, 12.0, 13.0, 14.0, 15.0]", + "-p", "ignition_sensor.reset_service:=''", + "-p", "ignition_sensor.publish_on_startup:=false"}); + auto node = rclcpp::Node::make_shared("omnidirectional_3d_ignition_test", options); + executor_->add_node(node); + + // Initialize the callback promise. Promises are single-use. + callback_promise = std::promise(); + auto callback_future = callback_promise.get_future(); + + // Create an ignition sensor and register the callback + fuse_models::Omnidirectional3DIgnition ignition_sensor; + ignition_sensor.initialize(*node, "ignition_sensor", &transactionCallback); + ignition_sensor.start(); + + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitX()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.1, Eigen::Vector3d::UnitZ()); // roll, pitch, yaw = 0.1 rad + + // Call the SetPose service + auto srv = std::make_shared(); + srv->pose.header.stamp = rclcpp::Time(12, 345678910); + srv->pose.pose.pose.position.x = 1.0; + srv->pose.pose.pose.position.y = 2.0; + srv->pose.pose.pose.position.z = 3.0; + 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.pose.orientation.w = q.w(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 2.0; + srv->pose.pose.covariance[14] = 3.0; + srv->pose.pose.covariance[21] = 4.0; + srv->pose.pose.covariance[28] = 5.0; + srv->pose.pose.covariance[35] = 6.0; + auto client = node->create_client( + "/omnidirectional_3d_ignition_test/set_pose_deprecated"); + ASSERT_TRUE(client->wait_for_service(std::chrono::seconds(1))); + auto result = client->async_send_request(srv); + ASSERT_EQ(std::future_status::ready, result.wait_for(std::chrono::seconds(10))); + + // The ignition sensor should publish a transaction in response to the service call. Wait for the + // callback to fire. + auto status = callback_future.wait_for(std::chrono::seconds(5)); + ASSERT_TRUE(status == std::future_status::ready); + + // Check the transaction + auto transaction = callback_future.get(); + { + fuse_core::Vector3d expected_mean; + expected_mean << 1.0, 2.0, 3.0; + fuse_core::Matrix3d expected_cov; + expected_cov << 1.0, 0.0, 0.0, 0.0, 2.0, 0.0, 0.0, 0.0, 3.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 0.1, 0.1, 0.1; + fuse_core::Matrix3d expected_cov; + expected_cov << 4.0, 0.0, 0.0, 0.0, 5.0, 0.0, 0.0, 0.0, 6.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + fuse_variables::Orientation3DStamped orientation_actual; + orientation_actual.w() = actual->mean()[0]; + orientation_actual.x() = actual->mean()[1]; + orientation_actual.y() = actual->mean()[2]; + orientation_actual.z() = actual->mean()[3]; + EXPECT_NEAR(expected_mean.x(), orientation_actual.roll(), 1.0e-1); // not high precision + EXPECT_NEAR(expected_mean.y(), orientation_actual.pitch(), 1.0e-1); + EXPECT_NEAR(expected_mean.z(), orientation_actual.yaw(), 1.0e-1); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 6.7, 7.8, 8.9; + fuse_core::Matrix3d expected_cov; + expected_cov << 49.0, 0.0, 0.0, 0.0, 64.0, 0.0, 0.0, 0.0, 81.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 9.1, 10.2, 11.3; + fuse_core::Matrix3d expected_cov; + expected_cov << 100.0, 0.0, 0.0, 0.0, 121.0, 0.0, 0.0, 0.0, 144.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 1.0e-9); + } + { + fuse_core::Vector3d expected_mean; + expected_mean << 12.4, 13.5, 14.6; + fuse_core::Matrix3d expected_cov; + expected_cov << 169.0, 0.0, 0.0, 0.0, 196.0, 0.0, 0.0, 0.0, 225.0; + auto actual = getConstraint(*transaction); + ASSERT_TRUE(static_cast(actual)); + EXPECT_MATRIX_NEAR(expected_mean, actual->mean(), 1.0e-9); + EXPECT_MATRIX_NEAR(expected_cov, actual->covariance(), 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..63648087a --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d_predict.cpp @@ -0,0 +1,651 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2018, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include + +#include +#include +#include + +#include +#include +#include + +TEST(Predict, predictDirectVals) +{ + fuse_core::Vector3d position1(0.0, 0.0, 0.0); + Eigen::Quaterniond orientation1(1.0, 0.0, 0.0, 0.0); + fuse_core::Vector3d vel_linear1(1.0, 0.0, 0.0); + fuse_core::Vector3d vel_angular1(0.0, 0.0, 1.570796327); + fuse_core::Vector3d acc_linear1(1.0, 0.0, 0.0); + double dt = 0.1; + fuse_core::Vector3d position2; + Eigen::Quaterniond orientation2; + fuse_core::Vector3d vel_linear2; + fuse_core::Vector3d vel_angular2; + fuse_core::Vector3d acc_linear2; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + Eigen::Quaterniond q; + q = Eigen::AngleAxisd(0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.105, position2.x()); + EXPECT_DOUBLE_EQ(0.0, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); + EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); + EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); + EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); + EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // // Carry on with the output state from last time - show in-place update support + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxisd(0.3141592654, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.21858415916807189, position2.x()); + EXPECT_DOUBLE_EQ(0.017989963481956205, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_DOUBLE_EQ(q.w(), orientation2.w()); + EXPECT_DOUBLE_EQ(q.x(), orientation2.x()); + EXPECT_DOUBLE_EQ(q.y(), orientation2.y()); + EXPECT_DOUBLE_EQ(q.z(), orientation2.z()); + EXPECT_DOUBLE_EQ(1.2, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // Use non-zero Y values + vel_linear1.y() = -1.0; + vel_angular1.z() = -1.570796327; + acc_linear1.y() = -1.0; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + q = Eigen::AngleAxisd(-0.1570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(0.0, Eigen::Vector3d::UnitX()); + + EXPECT_DOUBLE_EQ(0.105, position2.x()); + EXPECT_DOUBLE_EQ(-0.105, position2.y()); + EXPECT_DOUBLE_EQ(0.0, position2.z()); + EXPECT_TRUE(q.isApprox(orientation2)); + EXPECT_DOUBLE_EQ(1.1, vel_linear2.x()); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(-1.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.z()); + + // Out of plane motion + position1 = {0.0, 0.0, 0.0}; + orientation1 = {1.0, 0.0, 0.0, 0.0}; + vel_linear1 = {0.0, 0.0, 0.1}; + vel_angular1 = {1.570796327, 0.0, 0.0}; + acc_linear1 = {0.0, 0.0, 1.0}; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(0.0, position2.x()); + EXPECT_DOUBLE_EQ(0.0, position2.y()); + EXPECT_DOUBLE_EQ(0.015, position2.z()); + EXPECT_DOUBLE_EQ(0.99691733373232339, orientation2.w()); + EXPECT_DOUBLE_EQ(0.078459095738068516, orientation2.x()); + EXPECT_DOUBLE_EQ(0.0, orientation2.y()); + EXPECT_DOUBLE_EQ(0.0, orientation2.z()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.2, vel_linear2.z()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.x()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.y()); + EXPECT_DOUBLE_EQ(0.0, vel_angular2.z()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.x()); + EXPECT_DOUBLE_EQ(0.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.z()); + + // General 3D motion (these value are checked against rl predict() equations) + position1 = {0.0, 0.0, 0.0}; + orientation1 = {0.110, -0.003, -0.943, 0.314}; // RPY {-2.490, -0.206, 3.066} + vel_linear1 = {0.1, 0.2, 0.1}; + vel_angular1 = {1.570796327, 1.570796327, -1.570796327}; + acc_linear1 = {-0.5, 1.0, 1.0}; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(-0.012044123300410431, position2.x()); + EXPECT_DOUBLE_EQ(0.011755776496514461, position2.y()); + EXPECT_DOUBLE_EQ(-0.024959783911094033, position2.z()); + EXPECT_DOUBLE_EQ(0.20388993714859482, orientation2.w()); + EXPECT_DOUBLE_EQ(0.061993007799788086, orientation2.x()); + EXPECT_DOUBLE_EQ(-0.90147820778463239, orientation2.y()); + EXPECT_DOUBLE_EQ(0.3767264277999153, orientation2.z()); + EXPECT_DOUBLE_EQ(0.05, vel_linear2.x()); + EXPECT_DOUBLE_EQ(0.3, vel_linear2.y()); + EXPECT_DOUBLE_EQ(0.2, vel_linear2.z()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.x()); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2.y()); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2.z()); + EXPECT_DOUBLE_EQ(-0.5, acc_linear2.x()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.y()); + EXPECT_DOUBLE_EQ(1.0, acc_linear2.z()); +} + +TEST(Predict, predictFromDoublePointers) +{ + double position1[3] {0.0, 0.0, 0.0}; + double orientation1[3] {0.0, 0.0, 0.0}; + double vel_linear1[3] {1.0, 0.0, 0.0}; + double vel_angular1[3] {0.0, 0.0, 1.570796327}; + double acc_linear1[3] {1.0, 0.0, 0.0}; + double dt = 0.1; + double position2[3]; + double orientation2[3]; + double vel_linear2[3]; + double vel_angular2[3]; + double acc_linear2[3]; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(0.105, position2[0]); + EXPECT_DOUBLE_EQ(0.0, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(0.1570796327, orientation2[2]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + + // Carry on with the output state from last time - show in-place update support + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(0.21858415916807189, position2[0]); + EXPECT_DOUBLE_EQ(0.017989963481956205, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(0.3141592654, orientation2[2]); + EXPECT_DOUBLE_EQ(1.2, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + + // Use non-zero Y values + vel_linear1[1] = -1.0; + vel_angular1[2] = -1.570796327; + acc_linear1[1] = -1.0; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(0.105, position2[0]); + EXPECT_DOUBLE_EQ(-0.105, position2[1]); + EXPECT_DOUBLE_EQ(0.0, position2[2]); + EXPECT_DOUBLE_EQ(0.0, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(-0.1570796327, orientation2[2]); + EXPECT_DOUBLE_EQ(1.1, vel_linear2[0]); + EXPECT_DOUBLE_EQ(-1.1, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(-1.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[2]); + + // Out of plane motion + position1[0] = 0.0; + position1[1] = 0.0; + position1[2] = 0.0; + orientation1[0] = 0.0; + orientation1[1] = 0.0; + orientation1[2] = 0.0; + vel_linear1[0] = 0.0; + vel_linear1[1] = 0.0; + vel_linear1[2] = 0.1; + vel_angular1[0] = 1.570796327; + vel_angular1[1] = 0.0; + vel_angular1[2] = 0.0; + acc_linear1[0] = 0.0; + acc_linear1[1] = 0.0; + acc_linear1[2] = 1.0; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(0.0, position2[0]); + EXPECT_DOUBLE_EQ(0.0, position2[1]); + EXPECT_DOUBLE_EQ(0.015, position2[2]); + EXPECT_DOUBLE_EQ(0.15707963270000003, orientation2[0]); + EXPECT_DOUBLE_EQ(0.0, orientation2[1]); + EXPECT_DOUBLE_EQ(0.0, orientation2[2]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.2, vel_linear2[2]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[0]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[1]); + EXPECT_DOUBLE_EQ(0.0, vel_angular2[2]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[0]); + EXPECT_DOUBLE_EQ(0.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[2]); + + // General 3D motion (these value are checked against rl predict() equations) + position1[0] = 0.0; + position1[1] = 0.0; + position1[2] = 0.0; + orientation1[0] = -2.490; + orientation1[1] = -0.206; + orientation1[2] = 3.066; + vel_linear1[0] = 0.1; + vel_linear1[1] = 0.2; + vel_linear1[2] = 0.1; + vel_angular1[0] = 1.570796327; + vel_angular1[1] = 1.570796327; + vel_angular1[2] = -1.570796327; + acc_linear1[0] = -0.5; + acc_linear1[1] = 1.0; + acc_linear1[2] = 1.0; + dt = 0.1; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(-0.012031207341885572, position2[0]); + EXPECT_DOUBLE_EQ(0.011723254405731805, position2[1]); + EXPECT_DOUBLE_EQ(-0.024981300126995967, position2[2]); + EXPECT_DOUBLE_EQ(-2.3391131265098766, orientation2[0]); + EXPECT_DOUBLE_EQ(-0.4261584872792554, orientation2[1]); + EXPECT_DOUBLE_EQ(3.0962756133525855, orientation2[2]); + EXPECT_DOUBLE_EQ(0.05, vel_linear2[0]); + EXPECT_DOUBLE_EQ(0.3, vel_linear2[1]); + EXPECT_DOUBLE_EQ(0.2, vel_linear2[2]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[0]); + EXPECT_DOUBLE_EQ(1.570796327, vel_angular2[1]); + EXPECT_DOUBLE_EQ(-1.570796327, vel_angular2[2]); + EXPECT_DOUBLE_EQ(-0.5, acc_linear2[0]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[1]); + EXPECT_DOUBLE_EQ(1.0, acc_linear2[2]); +} + +TEST(Predict, predictFromJetPointers) +{ + using Jet = ceres::Jet; + + Jet position1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; + Jet orientation1[3] = {Jet(0.0), Jet(0.0), Jet(0.0)}; + Jet vel_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; + Jet vel_angular1[3] = {Jet(0.0), Jet(0.0), Jet(1.570796327)}; + Jet acc_linear1[3] = {Jet(1.0), Jet(0.0), Jet(0.0)}; + Jet dt = Jet(0.1); + Jet position2[3]; + Jet orientation2[3]; + Jet vel_linear2[3]; + Jet vel_angular2[3]; + Jet acc_linear2[3]; + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.1570796327).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + + // Carry on with the output state from last time - show in-place update support + fuse_models::predict( + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(Jet(0.21858415916807189).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.017989963481956205).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.3141592654).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.2).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + + // // Use non-zero Y values + vel_linear1[1] = Jet(-1.0); + vel_angular1[2] = Jet(-1.570796327); + acc_linear1[1] = Jet(-1.0); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2); + + EXPECT_DOUBLE_EQ(Jet(0.105).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.105).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(-0.1570796327).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.1).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.1).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(-1.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[2].a); + +// Out of plane motion + position1[0] = Jet(0.0); + position1[1] = Jet(0.0); + position1[2] = Jet(0.0); + orientation1[0] = Jet(0.0); + orientation1[1] = Jet(0.0); + orientation1[2] = Jet(0.0); + vel_linear1[0] = Jet(0.0); + vel_linear1[1] = Jet(0.0); + vel_linear1[2] = Jet(0.1); + vel_angular1[0] = Jet(1.570796327); + vel_angular1[1] = Jet(0.0); + vel_angular1[2] = Jet(0.0); + acc_linear1[0] = Jet(0.0); + acc_linear1[1] = Jet(0.0); + acc_linear1[2] = Jet(1.0); + dt = Jet(0.1); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.015).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.15707963270000003).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.2).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); + + // General 3D motion (these value are checked against rl predict() equations) + position1[0] = Jet(0.0); + position1[1] = Jet(0.0); + position1[2] = Jet(0.0); + orientation1[0] = Jet(-2.490); + orientation1[1] = Jet(-0.206); + orientation1[2] = Jet(3.066); + vel_linear1[0] = Jet(0.1); + vel_linear1[1] = Jet(0.2); + vel_linear1[2] = Jet(0.1); + vel_angular1[0] = Jet(1.570796327); + vel_angular1[1] = Jet(1.570796327); + vel_angular1[2] = Jet(-1.570796327); + acc_linear1[0] = Jet(-0.5); + acc_linear1[1] = Jet(1.0); + acc_linear1[2] = Jet(1.0); + dt = Jet(0.1); + + fuse_models::predict( + position1, + orientation1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + position2, + orientation2, + vel_linear2, + vel_angular2, + acc_linear2 + ); + + EXPECT_DOUBLE_EQ(Jet(-0.012031207341885572).a, position2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.011723254405731805).a, position2[1].a); + EXPECT_DOUBLE_EQ(Jet(-0.024981300126995967).a, position2[2].a); + EXPECT_DOUBLE_EQ(Jet(-2.3391131265098766).a, orientation2[0].a); + EXPECT_DOUBLE_EQ(Jet(-0.4261584872792554).a, orientation2[1].a); + EXPECT_DOUBLE_EQ(Jet(3.0962756133525855).a, orientation2[2].a); + EXPECT_DOUBLE_EQ(Jet(0.05).a, vel_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(0.3).a, vel_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(0.2).a, vel_linear2[2].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[0].a); + EXPECT_DOUBLE_EQ(Jet(1.570796327).a, vel_angular2[1].a); + EXPECT_DOUBLE_EQ(Jet(-1.570796327).a, vel_angular2[2].a); + EXPECT_DOUBLE_EQ(Jet(-0.5).a, acc_linear2[0].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[1].a); + EXPECT_DOUBLE_EQ(Jet(1.0).a, acc_linear2[2].a); +} 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..a5bc835f8 --- /dev/null +++ b/fuse_models/test/test_omnidirectional_3d_state_cost_function.cpp @@ -0,0 +1,150 @@ +/* + * 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..d45d3b987 --- /dev/null +++ b/fuse_models/test/test_sensor_proc_3d.cpp @@ -0,0 +1,153 @@ +/*************************************************************************** + * Copyright (C) 2024 Giacomo Franchini. All rights reserved. + * Unauthorized copying of this file, via any medium, is strictly prohibited + * Proprietary and confidential + ***************************************************************************/ +#include +#include + +#include + +#include + +namespace fm_common = fuse_models::common; +TEST(TestSuite, mergeFullPositionAndFullOrientationIndices) +{ + const std::vector position_indices{0, 1, 2}; + const std::vector orientation_indices{0, 1, 2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); + EXPECT_THAT( + position_indices, + testing::ElementsAreArray( + merged_indices.begin(), + merged_indices.begin() + position_indices.size())); + EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); +} + +TEST(TestSuite, mergeXYPositionAndRollYawOrientationIndices) +{ + const std::vector position_indices{0, 1}; + const std::vector orientation_indices{0, 2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(position_indices.size() + orientation_indices.size(), merged_indices.size()); + EXPECT_THAT( + position_indices, + testing::ElementsAreArray( + merged_indices.begin(), + merged_indices.begin() + position_indices.size())); + EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); +} + +TEST(TestSuite, mergeFullPositionAndEmptyOrientationIndices) +{ + const std::vector position_indices{0, 1, 2}; + const std::vector orientation_indices; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(position_indices.size(), merged_indices.size()); + EXPECT_THAT(position_indices, testing::ElementsAreArray(merged_indices)); +} + +TEST(TestSuite, mergeEmptyPositionAndFullOrientationIndices) +{ + const std::vector position_indices; + const std::vector orientation_indices{0, 1, 2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_EQ(orientation_indices.size(), merged_indices.size()); + EXPECT_EQ(orientation_indices.back() + orientation_offset, merged_indices.back()); +} + +TEST(TestSuite, mergeEmptyPositionAndEmptyOrientationIndices) +{ + const std::vector position_indices; + const std::vector orientation_indices; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + EXPECT_TRUE(merged_indices.empty()); +} + +TEST(TestSuite, populatePartialMeasurements) +{ + // Test both conversion from quaternion to RPY and partial measurement population + // This one is just to generate a random unit quaternion and have the reference in RPY + fuse_core::Vector3d rpy = fuse_core::Vector3d::Random(); + Eigen::Quaterniond q = + Eigen::AngleAxisd(rpy(2), Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(rpy(1), Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(rpy(0), Eigen::Vector3d::UnitX()); + + tf2::Transform tf2_pose; + tf2_pose.setOrigin(tf2::Vector3(1.0, 2.0, 3.0)); + tf2_pose.setRotation(tf2::Quaternion(q.x(), q.y(), q.z(), q.w())); + fuse_core::Vector6d pose_mean_partial; + pose_mean_partial.head<3>() << tf2_pose.getOrigin().x(), tf2_pose.getOrigin().y(), + tf2_pose.getOrigin().z(); + tf2::Matrix3x3(tf2_pose.getRotation()).getRPY( + pose_mean_partial(3), pose_mean_partial( + 4), pose_mean_partial(5)); + + fuse_core::Matrix6d pose_covariance = fuse_core::Matrix6d::Random(); + + const std::vector position_indices{0, 1}; + const std::vector orientation_indices{2}; + + const size_t orientation_offset = 3; + + const auto merged_indices = fm_common::mergeIndices( + position_indices, orientation_indices, + orientation_offset); + + std::replace_if( + pose_mean_partial.data(), pose_mean_partial.data() + pose_mean_partial.size(), + [&merged_indices, &pose_mean_partial](const double & value) { + return std::find( + merged_indices.begin(), + merged_indices.end(), + &value - pose_mean_partial.data()) == merged_indices.end(); + }, 0.0); + + fuse_core::MatrixXd pose_covariance_partial(3, 3); + + fm_common::populatePartialMeasurement( + pose_covariance, + merged_indices, + pose_covariance_partial); + + fuse_core::Vector6d expected_pose; + expected_pose << 1.0, 2.0, 0.0, 0.0, 0.0, rpy(2); + fuse_core::Matrix3d expected_covariance; + expected_covariance.col(0) << pose_covariance(0, 0), pose_covariance(1, 0), pose_covariance(5, 0); + expected_covariance.col(1) << pose_covariance(0, 1), pose_covariance(1, 1), pose_covariance(5, 1); + expected_covariance.col(2) << pose_covariance(0, 5), pose_covariance(1, 5), pose_covariance(5, 5); + EXPECT_TRUE(expected_pose.isApprox(pose_mean_partial)); + EXPECT_TRUE(expected_covariance.isApprox(pose_covariance_partial)); +} diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index 87de2e8fe..b67704194 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -399,7 +399,6 @@ void FixedLagSmoother::processQueue( return; } } - // Use the most recent transaction time as the current time const auto current_time = pending_transactions_.front().stamp(); diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index 8dd86a0da..d053b65fc 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -44,10 +44,13 @@ target_link_libraries(${PROJECT_NAME} PUBLIC ${sensor_msgs_TARGETS} ) -# tutorial_sim executable +# tutorial_sim executables 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}) + ############# ## Testing ## ############# @@ -75,6 +78,7 @@ install(TARGETS 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..59073b3bd --- /dev/null +++ b/fuse_tutorials/config/fuse_3d_tutorial.yaml @@ -0,0 +1,61 @@ +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/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..b4a899117 --- /dev/null +++ b/fuse_tutorials/src/three_dimensional_simulator.cpp @@ -0,0 +1,364 @@ +/* + * 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 + +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 + +/** + * @brief The true pose and velocity of the robot + */ +struct Robot +{ + rclcpp::Time stamp; + + double mass; + + double x = 0; + double y = 0; + double z = 0; + double roll = 0; + double pitch = 0; + double yaw = 0; + double vx = 0; + double vy = 0; + double vz = 0; + double vroll = 0; + double vpitch = 0; + double vyaw = 0; + double ax = 0; + double ay = 0; + double az = 0; +}; + +/** + * @brief Convert the robot state into a ground truth odometry message + */ +nav_msgs::msg::Odometry::SharedPtr robotToOdometry(const Robot & state) +{ + auto msg = std::make_shared(); + msg->header.stamp = state.stamp; + msg->header.frame_id = MAP_FRAME; + msg->child_frame_id = BASELINK_FRAME; + msg->pose.pose.position.x = state.x; + msg->pose.pose.position.y = state.y; + msg->pose.pose.position.z = state.z; + + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + msg->pose.pose.orientation.w = q.w(); + msg->pose.pose.orientation.x = q.x(); + msg->pose.pose.orientation.y = q.y(); + msg->pose.pose.orientation.z = q.z(); + msg->twist.twist.linear.x = state.vx; + msg->twist.twist.linear.y = state.vy; + msg->twist.twist.linear.z = state.vz; + msg->twist.twist.angular.x = state.vroll; + msg->twist.twist.angular.y = state.vpitch; + msg->twist.twist.angular.z = state.vyaw; + + // set covariances + msg->pose.covariance[0] = 0.1; + msg->pose.covariance[7] = 0.1; + msg->pose.covariance[14] = 0.1; + msg->pose.covariance[21] = 0.1; + msg->pose.covariance[28] = 0.1; + msg->pose.covariance[35] = 0.1; + msg->twist.covariance[0] = 0.1; + msg->twist.covariance[7] = 0.1; + msg->twist.covariance[14] = 0.1; + msg->twist.covariance[21] = 0.1; + msg->twist.covariance[28] = 0.1; + msg->twist.covariance[35] = 0.1; + return msg; +} + +/** + * @brief Compute the next robot state given the current robot state and a simulated step time + */ +Robot simulateRobotMotion( + const Robot & previous_state, const rclcpp::Time & now, + Eigen::Vector3d external_force) +{ + auto dt = (now - previous_state.stamp).seconds(); + auto next_state = Robot(); + next_state.stamp = now; + next_state.mass = previous_state.mass; + + // just euler integrate to get the next position and velocity + next_state.x = previous_state.x + previous_state.vx * dt; + next_state.y = previous_state.y + previous_state.vy * dt; + next_state.z = previous_state.z + previous_state.vz * dt; + next_state.vx = previous_state.vx + previous_state.ax * dt; + next_state.vy = previous_state.vy + previous_state.ay * dt; + next_state.vz = previous_state.vz + previous_state.az * dt; + + // let's not deal with 3D orientation dynamics for this tutorial + next_state.roll = 0; + next_state.pitch = 0; + next_state.yaw = 0; + next_state.vroll = 0; + next_state.vpitch = 0; + next_state.vyaw = 0; + + // get the current acceleration from the current applied force + next_state.ax = external_force.x() / next_state.mass; + next_state.ay = external_force.y() / next_state.mass; + next_state.az = external_force.z() / next_state.mass; + + return next_state; +} + +/** + * @brief Create a simulated Imu measurement from the current state + */ +sensor_msgs::msg::Imu::SharedPtr simulateImu(const Robot & robot) +{ + static std::random_device rd{}; + static std::mt19937 generator{rd()}; + static std::normal_distribution<> noise{0.0, IMU_SIGMA}; + + auto msg = std::make_shared(); + msg->header.stamp = robot.stamp; + msg->header.frame_id = BASELINK_FRAME; + + // measure accel + msg->linear_acceleration.x = robot.ax + noise(generator); + msg->linear_acceleration.y = robot.ay + noise(generator); + msg->linear_acceleration.z = robot.az + noise(generator); + msg->linear_acceleration_covariance[0] = IMU_SIGMA * IMU_SIGMA; + msg->linear_acceleration_covariance[4] = IMU_SIGMA * IMU_SIGMA; + msg->linear_acceleration_covariance[8] = IMU_SIGMA * IMU_SIGMA; + + // Simulated IMU does not provide orientation + msg->orientation_covariance[0] = -1; + msg->orientation_covariance[4] = -1; + msg->orientation_covariance[8] = -1; + + msg->angular_velocity.x = robot.vroll + noise(generator); + msg->angular_velocity.y = robot.vpitch + noise(generator); + msg->angular_velocity.z = robot.vyaw + noise(generator); + msg->angular_velocity_covariance[0] = IMU_SIGMA * IMU_SIGMA; + msg->angular_velocity_covariance[4] = IMU_SIGMA * IMU_SIGMA; + msg->angular_velocity_covariance[8] = IMU_SIGMA * IMU_SIGMA; + return msg; +} + +nav_msgs::msg::Odometry::SharedPtr simulateOdometry(const Robot & robot) +{ + static std::random_device rd{}; + static std::mt19937 generator{rd()}; + static std::normal_distribution<> position_noise{0.0, ODOM_POSITION_SIGMA}; + + auto msg = std::make_shared(); + msg->header.stamp = robot.stamp; + msg->header.frame_id = ODOM_FRAME; + msg->child_frame_id = BASELINK_FRAME; + + // noisy position measurement + msg->pose.pose.position.x = robot.x + position_noise(generator); + msg->pose.pose.position.y = robot.y + position_noise(generator); + msg->pose.pose.position.z = robot.z + position_noise(generator); + msg->pose.covariance[0] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg->pose.covariance[7] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + msg->pose.covariance[14] = ODOM_POSITION_SIGMA * ODOM_POSITION_SIGMA; + + // noisy orientation measurement + tf2::Quaternion q; + q.setEuler(robot.yaw, robot.pitch, robot.roll); + msg->pose.pose.orientation.w = q.w(); + msg->pose.pose.orientation.x = q.x(); + msg->pose.pose.orientation.y = q.y(); + msg->pose.pose.orientation.z = q.z(); + msg->pose.covariance[21] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg->pose.covariance[28] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + msg->pose.covariance[35] = ODOM_ORIENTATION_SIGMA * ODOM_ORIENTATION_SIGMA; + return msg; +} + +void initializeStateEstimation( + fuse_core::node_interfaces::NodeInterfaces interfaces, + const Robot & state, const rclcpp::Clock::SharedPtr & clock, const rclcpp::Logger & logger) +{ + // Send the initial localization signal to the state estimator + auto srv = std::make_shared(); + srv->pose.header.frame_id = MAP_FRAME; + srv->pose.pose.pose.position.x = state.x; + srv->pose.pose.pose.position.y = state.y; + srv->pose.pose.pose.position.z = state.z; + tf2::Quaternion q; + q.setEuler(state.yaw, state.pitch, state.roll); + srv->pose.pose.pose.orientation.w = q.w(); + srv->pose.pose.pose.orientation.x = q.x(); + srv->pose.pose.pose.orientation.y = q.y(); + srv->pose.pose.pose.orientation.z = q.z(); + srv->pose.pose.covariance[0] = 1.0; + srv->pose.pose.covariance[7] = 1.0; + srv->pose.pose.covariance[14] = 1.0; + srv->pose.pose.covariance[21] = 1.0; + srv->pose.pose.covariance[28] = 1.0; + srv->pose.pose.covariance[35] = 1.0; + + auto client = rclcpp::create_client( + interfaces.get_node_base_interface(), interfaces.get_node_graph_interface(), + interfaces.get_node_services_interface(), "/state_estimation/set_pose_service", + rclcpp::ServicesQoS()); + + while (!client->wait_for_service(std::chrono::seconds(30)) && + interfaces.get_node_base_interface()->get_context()->is_valid()) + { + RCLCPP_WARN_STREAM( + logger, + "Waiting for '" << client->get_service_name() << "' service to become available."); + } + + auto success = false; + while (!success) { + clock->sleep_for(std::chrono::milliseconds(100)); + srv->pose.header.stamp = clock->now(); + auto result_future = client->async_send_request(srv); + + if (rclcpp::spin_until_future_complete( + interfaces.get_node_base_interface(), result_future, + std::chrono::seconds(1)) != rclcpp::FutureReturnCode::SUCCESS) + { + RCLCPP_ERROR(logger, "service call failed :("); + client->remove_pending_request(result_future); + return; + } + success = result_future.get()->success; + } +} + +int main(int argc, char ** argv) +{ + // 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; +}