diff --git a/.github/workflows/main.yaml b/.github/workflows/main.yaml new file mode 100644 index 000000000..3eb17cd34 --- /dev/null +++ b/.github/workflows/main.yaml @@ -0,0 +1,25 @@ +name: CI + +on: + push: + branches: + - '*' + +jobs: + ci: + strategy: + matrix: + include: + - os: ubuntu-20.04 + distro: noetic + fail-fast: false + runs-on: ${{ matrix.os }} + steps: + - uses: actions/checkout@v1 + - uses: ros-tooling/setup-ros@0.7.0 + with: + required-ros-distributions: ${{ matrix.distro }} + - run: sudo apt remove python3-openssl -y + - uses: ros-tooling/action-ros-ci@0.3.5 + with: + target-ros1-distro: ${{ matrix.distro }} diff --git a/Jenkinsfile b/Jenkinsfile index 41e41edad..918ecf3b9 100644 --- a/Jenkinsfile +++ b/Jenkinsfile @@ -1,5 +1,5 @@ #!/usr/bin/env groovy -@Library('tailor-meta@0.1.20')_ +@Library('tailor-meta@0.1.24')_ tailorTestPipeline( // Name of job that generated this test definition. rosdistro_job: '/ci/rosdistro/master', @@ -10,9 +10,9 @@ tailorTestPipeline( // Release label to pull test images from. release_label: 'hotdog', // OS distributions to test. - distributions: ['focal'], + distributions: ['jammy'], // Version of tailor_meta to build against - tailor_meta: '0.1.20', + tailor_meta: '0.1.24', // Master or release branch associated with this track source_branch: 'devel', // Docker registry where test image is stored diff --git a/fuse/CHANGELOG.rst b/fuse/CHANGELOG.rst index 0ad10ea44..179f5edec 100644 --- a/fuse/CHANGELOG.rst +++ b/fuse/CHANGELOG.rst @@ -11,6 +11,14 @@ Changelog for package fuse * Update changelogs * Contributors: Gary Servin +0.7.0 (2023-09-25) +------------------ +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* Contributors: Gary Servin + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse/package.xml b/fuse/package.xml index 3db99de91..7a51678f2 100644 --- a/fuse/package.xml +++ b/fuse/package.xml @@ -1,7 +1,7 @@ fuse - 0.6.0 + 0.7.0 The fuse metapackage diff --git a/fuse_constraints/CHANGELOG.rst b/fuse_constraints/CHANGELOG.rst index 0e7a591bf..7eaf34b08 100644 --- a/fuse_constraints/CHANGELOG.rst +++ b/fuse_constraints/CHANGELOG.rst @@ -28,6 +28,33 @@ Changelog for package fuse_constraints * Adding doxygen to all packages (#241) * Contributors: Gary Servin, Stephen Williams, Tom Moore +0.7.0 (2023-09-25) +------------------ +* [RST-7809] Fix optimization errors when the orientation is initialized at +PI (#334) + * Add some unit tests for the 2D orientation constraints; Create getters/setters for the 2D orientation variable is preparation for a fix. + * Force the 2D orientation value to be is minimum phase +* Minor header fixes (#266) + * Use fuse_macros.h instead of deprecated macros.h + * Add missed header + * Sort headers +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* Adding doxygen to all packages (#241) +* Contributors: Enrique Fernandez Perdomo, Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_constraints/CMakeLists.txt b/fuse_constraints/CMakeLists.txt index 75be81486..750de6226 100644 --- a/fuse_constraints/CMakeLists.txt +++ b/fuse_constraints/CMakeLists.txt @@ -44,6 +44,8 @@ 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/fixed_3d_landmark_constraint.cpp src/marginal_constraint.cpp src/marginal_cost_function.cpp src/marginalize_variables.cpp @@ -56,6 +58,7 @@ add_library(${PROJECT_NAME} 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 ) @@ -78,7 +81,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -135,7 +138,33 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + + # Absolute Orientation 2D Stamped Constraint Tests + catkin_add_gtest(test_absolute_orientation_2d_stamped_constraint + test/test_absolute_orientation_2d_stamped_constraint.cpp + ) + add_dependencies(test_absolute_orientation_2d_stamped_constraint + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(test_absolute_orientation_2d_stamped_constraint + PRIVATE + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ${EIGEN3_INCLUDE_DIRS} + ) + target_link_libraries(test_absolute_orientation_2d_stamped_constraint + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ${EIGEN3_LIBRARIES} + ) + set_target_properties(test_absolute_orientation_2d_stamped_constraint + PROPERTIES + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -161,7 +190,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_orientation_3d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -187,7 +216,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_orientation_3d_stamped_euler_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -211,7 +240,31 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_pose_2d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + + # Fixed 3D Landmark Constraint Tests + catkin_add_gtest(test_fixed_3d_landmark_constraint + test/test_fixed_3d_landmark_constraint.cpp + ) + add_dependencies(test_fixed_3d_landmark_constraint + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(test_fixed_3d_landmark_constraint + PRIVATE + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ) + target_link_libraries(test_fixed_3d_landmark_constraint + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ) + set_target_properties(test_fixed_3d_landmark_constraint + PROPERTIES + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -235,7 +288,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_absolute_pose_3d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -257,7 +310,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_marginal_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -280,7 +333,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_marginalize_variables PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -304,7 +357,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_normal_delta_pose_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -328,7 +381,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_normal_prior_pose_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -352,7 +405,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_relative_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -376,7 +429,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_relative_pose_2d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -400,7 +453,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_relative_pose_3d_stamped_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -419,7 +472,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_uuid_ordering PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -436,7 +489,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_variable_constraints PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -458,7 +511,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(benchmark_normal_delta_pose_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() @@ -477,7 +530,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(benchmark_normal_prior_pose_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_constraints/fuse_plugins.xml b/fuse_constraints/fuse_plugins.xml index ad96dff9c..abdca4986 100644 --- a/fuse_constraints/fuse_plugins.xml +++ b/fuse_constraints/fuse_plugins.xml @@ -11,6 +11,18 @@ the 2D linear 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 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 @@ -41,6 +53,18 @@ the 2D linear 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 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, + with the 3D orientation as roll-pitch-yaw Euler angles. + + A constraint that represents remaining marginal information on a set of variables. @@ -118,4 +148,10 @@ 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 with the 3D orientation + as roll-pitch-yaw Euler angles. + + diff --git a/fuse_constraints/include/fuse_constraints/absolute_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_constraint.h index 508425c53..db94478c2 100644 --- a/fuse_constraints/include/fuse_constraints/absolute_constraint.h +++ b/fuse_constraints/include/fuse_constraints/absolute_constraint.h @@ -41,11 +41,15 @@ #include #include #include +#include +#include #include #include #include #include #include +#include +#include #include #include @@ -186,11 +190,15 @@ class AbsoluteConstraint : public fuse_core::Constraint // Define unique names for the different variations of the absolute constraint using AbsoluteAccelerationAngular2DStampedConstraint = AbsoluteConstraint; using AbsoluteAccelerationLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationAngular3DStampedConstraint = AbsoluteConstraint; +using AbsoluteAccelerationLinear3DStampedConstraint = AbsoluteConstraint; using AbsoluteOrientation2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition2DStampedConstraint = AbsoluteConstraint; using AbsolutePosition3DStampedConstraint = AbsoluteConstraint; using AbsoluteVelocityAngular2DStampedConstraint = AbsoluteConstraint; using AbsoluteVelocityLinear2DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityAngular3DStampedConstraint = AbsoluteConstraint; +using AbsoluteVelocityLinear3DStampedConstraint = AbsoluteConstraint; } // namespace fuse_constraints // Include the template implementation @@ -198,10 +206,14 @@ using AbsoluteVelocityLinear2DStampedConstraint = 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 { @@ -181,6 +193,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_H diff --git a/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.h b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.h new file mode 100644 index 000000000..91b4df298 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/absolute_pose_3d_stamped_euler_constraint.h @@ -0,0 +1,188 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_CONSTRAINTS_ABSOLUTE_POSE_3D_STAMPED_EULER_CONSTRAINT_H + +#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 and the measurement + * uncertainty/covariance. Orientations are represented as roll, pitch, yaw. + * It also permits measurement of a subset of the pose provided in the position and orientation variables. + */ +class AbsolutePose3DStampedEulerConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS(AbsolutePose3DStampedEulerConstraint); + + using Euler = fuse_variables::Orientation3DStamped::Euler; + + /** + * @brief Default constructor + */ + AbsolutePose3DStampedEulerConstraint() = default; + + /** + * @brief Create a constraint using a measurement/prior of the 3D pose + * + * Note that, when measuring subset of dimensions, empty axis vectors are permitted. This signifies, e.g., that you + * don't want to measure any of the quantities in that variable. + * + * The mean is given as a vector. The first components (if any) will be dictated, both in content and in ordering, by + * the value of the \p linear_indices. The final component (if any) is dictated by the \p angular_indices. + * The covariance matrix follows the same ordering. + * + * @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 (max 6x1 vector, components are dictated by + * \p linear_indices and \p angular_indices) + * @param[in] partial_covariance The measurement/prior covariance (max 6x6 matrix, components are dictated by + * \p linear_indices and \p angular_indices) + * @param[in] linear_indices The set of indices corresponding to the measured position dimensions + * e.g. "{fuse_variables::Position3DStamped::X, fuse_variables::Position3DStamped::Y}" + * @param[in] angular_indices The set of indices corresponding to the measured orientation dimensions + * e.g. "{static_cast(fuse_variables::Orientation3DStamped::Yaw)}" + */ + AbsolutePose3DStampedEulerConstraint( + const std::string& source, + const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& linear_indices = + {fuse_variables::Position3DStamped::X, // NOLINT + fuse_variables::Position3DStamped::Y, + fuse_variables::Position3DStamped::Z}, // NOLINT + const std::vector& angular_indices = {Euler::ROLL, Euler::PITCH, Euler::YAW}); // NOLINT + + /** + * @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). Note that the returned vector will be full sized (6x1) and in the stated order. + */ + const fuse_core::Vector6d& mean() const { return mean_; } + + /** + * @brief Read-only access to the square root information matrix. + * + * If only a partial covariance matrix was provided in the constructor, this covariance matrix will not be square. + */ + const fuse_core::MatrixXd& sqrtInformation() const { return sqrt_information_; } + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, roll, pitch, yaw). Note that the returned covariance matrix will be full sized (3x3) and in the stated order. + * If only a partial covariance matrix was provided in the constructor, this covariance matrix may be a different + * size and in a different order than the constructor input. + */ + 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_H diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h new file mode 100644 index 000000000..032638c2d --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_constraint.h @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created on Mon Nov 12 2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_FIXED_3D_LANDMARK_CONSTRAINT_H +#define FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_CONSTRAINT_H + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include +#include + +namespace fuse_constraints +{ + +/** + * @brief A constraint that represents an observation of a 3D landmark (ARTag or Similar) + * + * A landmark is represented as a 3D pose (3D position and a 3D orientation). This class takes + * the ground truth location of the 3D landmark and applies a reprojection-error based constraint + * an constraint on the position, orientation and calibration of the camera that observed the landmark. + * + * In most cases, the camera calibration should be held fixed as a single landmark does not present enough + * points to accurate constrain the pose AND the calibraton. + * + */ +class Fixed3DLandmarkConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(Fixed3DLandmarkConstraint); + + /** + * @brief Default constructor + */ + Fixed3DLandmarkConstraint() = default; + + /** + * @brief Create a constraint using a known 3D fiducial marker + * + * @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 landmark pose + * @param[in] orientation The variable representing the orientation components of the marker pose + * @param[in] calibraton The intrinsic calibration parameters of the camera (4x1 vector: cx, cy, fx, fy). + * NOTE: Best practice is to fix this variable unless we have several observations + * with the same camera + * @param[in] pts3d Matrix of 3D points in marker coordiate frame. + * @param[in] observations The 2D (pixel) observations of each marker's corners. + * @param[in] mean The measured/prior pose of the marker as a vector (7x1 vector: x, y, z, qw, qx, qy, qz) + * @param[in] covariance The measurement/prior marker pose covariance (6x6 matrix: x, y, z, qx, qy, qz) + */ + Fixed3DLandmarkConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_variables::PinholeCamera& calibraton, + const fuse_core::MatrixXd& pts3d, + const fuse_core::MatrixXd& observations, const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance); + + /** + * @brief Create a constraint using a known 3D fiducial marker. Convenience constructor for the special case of + * an ARTag with 4 corners. + * + * @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 marker pose + * @param[in] orientation The variable representing the orientation components of the marker pose + * @param[in] calibraton The intrinsic calibration parameters of the camera (4x1 vector: cx, cy, fx, fy). + * NOTE: Best practice is to fix this variable unless we have several observations + * with the same camera + * @param[in] marker_size The size of the marker, in meters. Assumed to be square. + * @param[in] observations The 2D (pixel) observations of each marker's corners. + * @param[in] mean The measured/prior pose of the marker as a vector (7x1 vector: x, y, z, qw, qx, qy, qz) + * @param[in] covariance The measurement/prior marker pose covariance (6x6 matrix: x, y, z, qx, qy, qz) + */ + Fixed3DLandmarkConstraint(const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_variables::PinholeCamera& calibraton, + const double& marker_size, + const fuse_core::MatrixXd& observations, const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance); + + /** + * @brief Destructor + */ + virtual ~Fixed3DLandmarkConstraint() = default; + + /** + * @brief Read-only access to the measured/prior vector of mean values. + * + * Order is (x, y, z, qw, qx, qy, qz) + */ + const fuse_core::Vector7d& mean() const + { + return mean_; + } + + /** + * @brief Read-only access to the square root information matrix. + * + * Order is (x, y, z, qx, qy, qz) + */ + const fuse_core::Matrix6d& sqrtInformation() const + { + return sqrt_information_; + } + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (x, y, z, qx, qy, qz) + */ + fuse_core::Matrix6d covariance() const + { + return (sqrt_information_.transpose() * sqrt_information_).inverse(); + } + + /** + * @brief Read-only access to the observation Matrix (Nx2). + * + * Order is (x, y) + */ + const fuse_core::MatrixXd& pts3d() const + { + return pts3d_; + } + + /** + * @brief Read-only access to the observation Matrix (Nx2). + * + * Order is (x, y) + */ + const fuse_core::MatrixXd& observations() const + { + return observations_; + } + + /** + * @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::MatrixXd pts3d_; //!< The 3D points in marker Coordinate frame + fuse_core::MatrixXd observations_; //!< The 2D observations (in pixel space) of the marker at postion mean_ + fuse_core::Vector7d mean_; //!< The measured/prior mean vector for this variable + fuse_core::Matrix6d 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& pts3d_; + archive& observations_; + archive& mean_; + archive& sqrt_information_; + } +}; + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_KEY(fuse_constraints::Fixed3DLandmarkConstraint); + +#endif // FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_CONSTRAINT_H diff --git a/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h new file mode 100644 index 000000000..caa76becf --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/fixed_3d_landmark_cost_functor.h @@ -0,0 +1,195 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created on Mon Nov 12 2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_FIXED_3D_LANDMARK_COST_FUNCTOR_H +#define FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_COST_FUNCTOR_H + +#include +#include +#include +#include + +#include + +namespace fuse_constraints +{ + +/** + * @brief Create a prior cost function on the marker position, minimising reprojection error. + * + * The Ceres::NormalPrior cost function only supports a single variable. This is a convenience cost function that + * applies a prior constraint on the 3D position, orientation and calibration variables at once. + * + * The cost function is of the form: + * + * cost(x) = || A * (K * [R_q | p] * [R_{b(3:6)} | b(0:2))] * X - x) || + * + * where, the matrix A and the vector b are fixed, p is the camera position variable, and q is the camera orientation + * variable, K is the calibration matrix created from the calibration variable, X is the set of marker 3D points, + * R_b(0:3) is the Rotation matrix from the fixed landmark orentation (b(3:6)), b(0:2) is the fixed landmark position + * and x is the 2D ovservations. + * + * Note that the covariance submatrix for the quaternion is 3x3, representing errors in the orientation local + * parameterization tangent space. 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 Fixed3DLandmarkCostFunctor +{ +public: + FUSE_MAKE_ALIGNED_OPERATOR_NEW(); + + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely derived from the square root information + * matrix in order (x, y, z, qx, qy, qz) + * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) + * + * @param[in] obs The 2D projection of marker points (Nx2 in order x, y). + * + * @param[in] marker_size The size of the marker (in meters). + **/ + Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector7d& b, const fuse_core::MatrixXd& obs, + const fuse_core::Vector1d& marker_size); + + /** + * @brief Construct a cost function instance + * + * @param[in] A The residual weighting matrix, most likely derived from the square root information + * matrix in order (x, y, z, qx, qy, qz) + * @param[in] b The 3D pose measurement or prior in order (x, y, z, qw, qx, qy, qz) + * + * @param[in] obs The 2D projection of marker points (Nx2 in order x, y). + * + * @param[in] pts3d The 3D points in marker coordinate frame (Nx3 in order x, y, z). + **/ + Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector7d& b, const fuse_core::MatrixXd& obs, + const fuse_core::MatrixXd& pts3d); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + */ + template + bool operator()(const T* const position, const T* const orientation, const T* const calibration, T* residual) const; + +private: + fuse_core::MatrixXd A_; + fuse_core::Vector7d b_; + fuse_core::MatrixXd obs_; + fuse_core::MatrixXd pts3d_; + + NormalPriorOrientation3DCostFunctor orientation_functor_; +}; + +Fixed3DLandmarkCostFunctor::Fixed3DLandmarkCostFunctor(const fuse_core::MatrixXd& A, const fuse_core::Vector7d& b, + const fuse_core::MatrixXd& obs, const fuse_core::MatrixXd& pts3d) + : A_(A) + , b_(b) + , obs_(obs) + , pts3d_(pts3d.transpose()) // Transpose from Nx3 to 3xN to make math easier. + , orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<4>()) // Delta will not be scaled +{ + assert(pts3d_.rows() == 3); // Check if we have 3xN + + // Create Marker Transform Matrix (Tm) + double qm[4] = { b_(3), b_(4), b_(5), b_(6) }; + double rm[9]; + ceres::QuaternionToRotation(qm, rm); + + Eigen::Matrix Tm; + Tm << rm[0], rm[1], rm[2], b_(0), // NOLINT + rm[3], rm[4], rm[5], b_(1), // NOLINT + rm[6], rm[7], rm[8], b_(2), // NOLINT + 0.0, 0.0, 0.0, 1.0; // NOLINT + + // TODO(omendez): Can we do this directly on quaternions? + // ceres::QuaternionProduct(q, position, difference); + // ceres::QuaternionRotatePoint() + + // Transform points to marker pose and store for later use. + pts3d_ = Tm * pts3d_.colwise().homogeneous(); +} + +template +bool Fixed3DLandmarkCostFunctor::operator()(const T* const position, const T* const orientation, + const T* const calibration, T* residual) const +{ + // Create Calibration Matrix K + Eigen::Matrix K; + K << calibration[0], T(0.0), calibration[2], T(0.0), // NOLINT + T(0.0), calibration[1], calibration[3], T(0.0), // NOLINT + T(0.0), T(0.0), T(1.0), T(0.0), // NOLINT + T(0.0), T(0.0), T(0.0), T(1.0); // NOLINT + + // Create Camera Translation Matrix from Params. + T q[4] = { orientation[0], orientation[1], orientation[2], orientation[3] }; + T r[9]; + ceres::QuaternionToRotation(q, r); + + Eigen::Matrix Ta; + Ta << r[0], r[1], r[2], position[0], // NOLINT + r[3], r[4], r[5], position[1], // NOLINT + r[6], r[7], r[8], position[2], // NOLINT + T(0.0), T(0.0), T(0.0), T(1.0); // NOLINT + + // Transform 3D Points to Marker Location + Eigen::Matrix xp; + + // Project to camera at Ta + xp = (K * Ta * pts3d_.cast()); + xp.transposeInPlace(); + xp = xp.array().colwise() / xp.col(2).array(); + + auto d = (obs_.cast() - xp.block(0, 0, xp.rows(), 2)); + + for (uint i = 0; i < 4; i++) + { + residual[i * 2] = d.row(i)[0]; + residual[i * 2 + 1] = d.row(i)[1]; + } + + // TODO(omendez): how do we apply covariance weigthing to point losses from a pose uncertainty? + + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS_FIXED_3D_LANDMARK_COST_FUNCTOR_H diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.h new file mode 100644 index 000000000..7470434ef --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_delta_orientation_3d_euler_cost_functor.h @@ -0,0 +1,179 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_CONSTRAINTS_NORMAL_DELTA_ORIENTATION_3D_EULER_COST_FUNCTOR_H +#define FUSE_CONSTRAINTS_NORMAL_DELTA_ORIENTATION_3D_EULER_COST_FUNCTOR_H + +#include +#include +#include +#include + +#include +#include + +#include +#include + + +namespace fuse_constraints +{ + +/** + * @brief Implements a cost function that models a difference between 3D orientation variables using Euler roll, pitch, and yaw measurements + * + * The functor can compute the cost of a subset of the axes, in the event that we are not interested in all the Euler + * angles in the variable. + * + * So, for example, if + * b_ = [ measured_yaw_difference ] + * [ measured_roll_difference ] + * + * then the cost function is of the form: + * + * cost(x) = || A * [ (yaw1(x) - yaw2(x)) - b_(0) ] ||^2 + * || [ (roll1(x) - roll2(x)) - b_(1) ] || + * + * where the matrix A and the vector b are fixed and (roll, pitch, yaw) are the components of the 3D 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 NormalDeltaOrientation3DEulerCostFunctor +{ +public: + using Euler = fuse_variables::Orientation3DStamped::Euler; + 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. Its order must match + * the values in \p axes. + * @param[in] b The measured change between the two orientation variables. Its order must match the values in \p axes. + * @param[in] axes The Euler angle axes for which we want to compute errors. Defaults to all axes. + */ + NormalDeltaOrientation3DEulerCostFunctor( + const fuse_core::MatrixXd& A, + const fuse_core::Vector3d& b, + const std::vector &axes = {Euler::ROLL, Euler::PITCH, Euler::YAW}); + + /** + * @brief Evaluate the cost function. Used by the Ceres optimization engine. + */ + template + bool operator()(const T* const orientation1, const T* const orientation2, T* residuals) const; + +private: + fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root information matrix + fuse_core::VectorXd b_; //!< The measured difference between orientation1 and orientation2. Its order must match + //!< the values in \p axes. + std::vector axes_; //!< The Euler angle axes that we're measuring +}; + +NormalDeltaOrientation3DEulerCostFunctor::NormalDeltaOrientation3DEulerCostFunctor( + const fuse_core::MatrixXd& A, + const fuse_core::Vector3d& b, + const std::vector &axes) : + A_(A), + b_(b), + axes_(axes) +{ + if (A_.cols() != b_.size()) + { + throw std::invalid_argument("The number of columns in the residual weighting matrix A need to match the size of " + "the measured difference b."); + } + if (A_.rows() != static_cast(axes_.size())) + { + throw std::invalid_argument("The number of rows in the residual weighting matrix A need to match the size of " + "the desired Euler angle axes."); + } +} + +template +bool NormalDeltaOrientation3DEulerCostFunctor::operator()( + const T* const orientation1, + const T* const orientation2, + T* residuals) const +{ + using fuse_variables::Orientation3DStamped; + + for (size_t i = 0; i < axes_.size(); ++i) + { + T angle1, angle2; + switch (axes_[i]) + { + case Euler::ROLL: + { + angle1 = fuse_core::getRoll(orientation1[0], orientation1[1], orientation1[2], orientation1[3]); + angle2 = fuse_core::getRoll(orientation2[0], orientation2[1], orientation2[2], orientation2[3]); + break; + } + case Euler::PITCH: + { + angle1 = fuse_core::getPitch(orientation1[0], orientation1[1], orientation1[2], orientation1[3]); + angle2 = fuse_core::getPitch(orientation2[0], orientation2[1], orientation2[2], orientation2[3]); + break; + } + case Euler::YAW: + { + angle1 = fuse_core::getYaw(orientation1[0], orientation1[1], orientation1[2], orientation1[3]); + angle2 = fuse_core::getYaw(orientation2[0], orientation2[1], orientation2[2], orientation2[3]); + break; + } + default: + { + throw std::runtime_error("The provided axis specified is unknown. " + "I should probably be more informative here"); + } + } + const auto difference = fuse_core::wrapAngle2D(angle2 - angle1); + residuals[i] = fuse_core::wrapAngle2D(difference - T(b_[i])); + } + + // Scale the residuals by the square root information matrix to account for the measurement uncertainty. + Eigen::Map> residuals_map(residuals, A_.rows()); + residuals_map.applyOnTheLeft(A_.template cast()); + + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS_NORMAL_DELTA_ORIENTATION_3D_EULER_COST_FUNCTOR_H diff --git a/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.h new file mode 100644 index 000000000..3d0d1f13d --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_delta_pose_3d_euler_cost_functor.h @@ -0,0 +1,178 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_CONSTRAINTS_NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_H + +#include +#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. The generic NormalDelta cost function + * only supports a single variable type, and computes the difference using per-element subtraction. This cost function + * computes the difference using standard 3D transformation math. + * + * || [ delta.x(p1,p2) - b(0)] ||^2 + * || [ delta.y(p1,p2) - b(1)] || + * cost(x) = || A * [ delta.z(p1,p2) - b(2)] || + * || [ delta.roll(q1,q2) - b(3)] || + * || [ delta.pitch(q1,q2) - b(4)] || + * || [ delta.yaw(q1,q2) - b(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, p1 and p2 are the position variables, and q1 and q2 are the quaternion orientation + * variables. Note that the covariance submatrix for the orientation should represent errors in roll, pitch, and yaw. + * 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 + * + * 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 and yaw, then \p A + * will be of size 2x6 where the first row represents the weighting for x to all dimensions including x itself and + * the second row represents the weighting for yaw to all dimensions including yaw itself. For weighting with 1 and + * no relation to other dimensions the matrix should be: + * [1, 0, 0, 0, 0, 0] + * [0, 0, 0, 0, 0, 1] + * + * @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_; //!< The residual weighting matrix, most likely the square root information matrix + fuse_core::Vector6d b_; //!< The measured difference between variable pose1 and variable pose2 + + NormalDeltaOrientation3DEulerCostFunctor orientation_functor_; +}; + +NormalDeltaPose3DEulerCostFunctor::NormalDeltaPose3DEulerCostFunctor( + const fuse_core::MatrixXd& A, + const fuse_core::Vector6d& b) : + A_(A), + b_(b), + orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<3>()) // Orientation residuals will not be scaled + // within the orientation functor but here at + // the cost function after computation of the + // orientation residuals without scaling. +{ + if (A.cols() != b.size()) + { + throw std::invalid_argument("The number of columns in the residual weighting matrix A need to match the size of " + "the measured difference b."); + } +} + +template +bool NormalDeltaPose3DEulerCostFunctor::operator()( + const T* const position1, + const T* const orientation1, + const T* const position2, + const T* const orientation2, + T* residual) const +{ + Eigen::Matrix full_residuals_vector; + + // Compute the position delta between pose1 and pose2 + T orientation1_inverse[4] = + { + orientation1[0], + -orientation1[1], + -orientation1[2], + -orientation1[3] + }; + T position_delta[3] = + { + position2[0] - position1[0], + position2[1] - position1[1], + position2[2] - position1[2] + }; + T position_delta_rotated[3]; + ceres::QuaternionRotatePoint( + orientation1_inverse, + position_delta, + position_delta_rotated); + + // Compute the first three residual terms as (position_delta - b) + full_residuals_vector[0] = position_delta_rotated[0] - T(b_[0]); + full_residuals_vector[1] = position_delta_rotated[1] - T(b_[1]); + full_residuals_vector[2] = position_delta_rotated[2] - T(b_[2]); + + // Use the 3D orientation cost functor to compute the orientation delta + orientation_functor_(orientation1, orientation2, full_residuals_vector.data() + 3); + + // Map it to Eigen, and weight it + Eigen::Map> residuals_vector(residual, A_.rows()); + residuals_vector = A_.template cast() * full_residuals_vector; + + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS_NORMAL_DELTA_POSE_3D_EULER_COST_FUNCTOR_H diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h index 1d949fed3..49da2fc8b 100644 --- a/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h +++ b/fuse_constraints/include/fuse_constraints/normal_prior_orientation_3d_euler_cost_functor.h @@ -55,7 +55,7 @@ namespace fuse_constraints * * So, for example, if * b_ = [ measured_yaw ] - * [ meausred_roll ] + * [ measured_roll ] * * then the cost function is of the form: * @@ -140,7 +140,7 @@ class NormalPriorOrientation3DEulerCostFunctor private: fuse_core::MatrixXd A_; //!< The residual weighting matrix, most likely the square root information matrix - fuse_core::VectorXd b_; //!< The measured 3D orientation (quaternion) value + fuse_core::VectorXd b_; //!< The orientation measurement or prior. Its order must match the values in \p axes. std::vector axes_; //!< The Euler angle axes that we're measuring }; diff --git a/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.h b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.h new file mode 100644 index 000000000..288c4299b --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/normal_prior_pose_3d_euler_cost_functor.h @@ -0,0 +1,136 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_CONSTRAINTS_NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_H + +#include +#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 + * || [ toEulerRollPitchYaw(q) - b(3:6) ] || + * + * 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, p is the position variable, and q is the orientation variable. + * Note that the covariance submatrix for the orientation should represent errors in roll, pitch, and yaw. + * 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 + * + * 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 and yaw, then \p A + * will be of size 2x6 where the first row represents the weighting for x to all dimensions including x itself and + * the second row represents the weighting for yaw to all dimensions including yaw itself. For weighting with 1 and + * no relation to other dimensions the matrix should be: + * [1, 0, 0, 0, 0, 0] + * [0, 0, 0, 0, 0, 1] + * + * @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_; + + NormalPriorOrientation3DEulerCostFunctor orientation_functor_; +}; + +NormalPriorPose3DEulerCostFunctor::NormalPriorPose3DEulerCostFunctor( + const fuse_core::MatrixXd& A, const fuse_core::Vector6d& b) + : A_(A), + b_(b), + orientation_functor_(fuse_core::Matrix3d::Identity(), b_.tail<3>()) // Delta will not be scaled +{ +} + +template +bool NormalPriorPose3DEulerCostFunctor::operator()( + const T* const position, const T* const orientation, T* residual) const +{ + Eigen::Matrix full_residuals_vector; + // Compute the position error + full_residuals_vector[0] = position[0] - T(b_(0)); + full_residuals_vector[1] = position[1] - T(b_(1)); + full_residuals_vector[2] = position[2] - T(b_(2)); + + // Use the 3D orientation cost functor to compute the orientation delta + orientation_functor_(orientation, full_residuals_vector.data() + 3); + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + Eigen::Map> residuals_vector(residual, A_.rows()); + residuals_vector = A_.template cast() * full_residuals_vector; + + return true; +} + +} // namespace fuse_constraints + +#endif // FUSE_CONSTRAINTS_NORMAL_PRIOR_POSE_3D_EULER_COST_FUNCTOR_H diff --git a/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.h b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.h new file mode 100644 index 000000000..7f1b65117 --- /dev/null +++ b/fuse_constraints/include/fuse_constraints/relative_pose_3d_stamped_euler_constraint.h @@ -0,0 +1,187 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_CONSTRAINTS_RELATIVE_POSE_3D_STAMPED_EULER_CONSTRAINT_H + +#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); + + using Euler = fuse_variables::Orientation3DStamped::Euler; + + /** + * @brief Default constructor + */ + RelativePose3DStampedEulerConstraint() = default; + + /** + * @brief Constructor + * + * Note that, when measuring subset of dimensions, empty axis vectors are permitted. This signifies that you + * don't want to measure any of the quantities in that variable. + * + * The mean is given as a vector. The first components (if any) will be dictated, both in content and in ordering, by + * the value of the \p linear_indices. The final component (if any) is dictated by the \p angular_indices. The + * covariance matrix follows the same ordering. + * + * @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 (max 6x1 vector, components are dictated by + * \p linear_indices and \p angular_indices) + * @param[in] partial_covariance The measurement covariance (max 6x6 matrix, components are dictated by + * \p linear_indices and \p angular_indices) + * @param[in] linear_indices The set of indices corresponding to the measured position dimensions + * e.g., "{fuse_variables::Position3DStamped::X, fuse_variables::Position3DStamped::Y}" + * @param[in] angular_indices The set of indices corresponding to the measured orientation dimensions + * e.g., "{fuse_variables::Orientation3DStamped::Euler::Yaw}" + */ + 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::VectorXd& partial_delta, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& linear_indices = + {fuse_variables::Position3DStamped::X, fuse_variables::Position3DStamped::Y, fuse_variables::Position3DStamped::Z}, // NOLINT + const std::vector& angular_indices = {Euler::ROLL, Euler::PITCH, Euler::YAW}); // NOLINT + + /** + * @brief Destructor + */ + virtual ~RelativePose3DStampedEulerConstraint() = default; + + /** + * @brief Read-only access to the measured pose change. + * + * Order is (dx, dy, dz, droll, dpitch, dyaw). Note that the returned vector will be full sized (6x1) + * and in the stated order. + */ + const fuse_core::Vector6d& delta() const { return delta_; } + + /** + * @brief Read-only access to the square root information matrix. + * + * If only a partial covariance matrix was provided in the constructor, this covariance matrix will not be square. + */ + const fuse_core::MatrixXd& sqrtInformation() const { return sqrt_information_; } + + /** + * @brief Compute the measurement covariance matrix. + * + * Order is (dx, dy, dz, droll, dpitch, dyaw). Note that the returned covariance matrix will be full sized (6x6) + * and in the stated order. If only a partial covariance matrix was provided in the constructor, this covariance + * matrix may be a different size and in a different order than the constructor input. + */ + 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_H diff --git a/fuse_constraints/package.xml b/fuse_constraints/package.xml index ee7cf7764..0045fe0c7 100644 --- a/fuse_constraints/package.xml +++ b/fuse_constraints/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_constraints - 0.6.0 + 0.7.0 The fuse_constraints package provides a set of commonly used constraint types, such as direct measurements on state variables (absolute constraints) or measurements of the state changes (relative constraints). diff --git a/fuse_constraints/src/absolute_constraint.cpp b/fuse_constraints/src/absolute_constraint.cpp index fb767654c..d3262a6a5 100644 --- a/fuse_constraints/src/absolute_constraint.cpp +++ b/fuse_constraints/src/absolute_constraint.cpp @@ -33,23 +33,31 @@ */ #include -#include +#include #include BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint); BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint); +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::AbsoluteVelocityLinear2DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint); +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint); PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationAngular2DStampedConstraint, fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationAngular3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteAccelerationLinear3DStampedConstraint, fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteOrientation2DStampedConstraint, fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition2DStampedConstraint, fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsolutePosition3DStampedConstraint, fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityAngular2DStampedConstraint, fuse_core::Constraint); PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityAngular3DStampedConstraint, fuse_core::Constraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::AbsoluteVelocityLinear3DStampedConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp index 34765b2d5..97386d106 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp index 6758c831c..bf052e80d 100644 --- a/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp +++ b/fuse_constraints/src/absolute_orientation_3d_stamped_euler_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp index 31c2044bd..1f3168579 100644 --- a/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_2d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp index ffabb271e..74c1acf88 100644 --- a/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/absolute_pose_3d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include 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..f471dfc96 --- /dev/null +++ b/fuse_constraints/src/absolute_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,151 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 + + +namespace fuse_constraints +{ + +AbsolutePose3DStampedEulerConstraint::AbsolutePose3DStampedEulerConstraint( + const std::string& source, + const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, + const fuse_core::VectorXd& partial_mean, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& linear_indices, + const std::vector& angular_indices) : + fuse_core::Constraint(source, {position.uuid(), orientation.uuid()}) // NOLINT(whitespace/braces) +{ + constexpr size_t total_variable_size = 6; // 3 position (x, y, z) and 3 orientation variables (roll, pitch, yaw) + const size_t total_indices = linear_indices.size() + angular_indices.size(); + + assert(partial_mean.rows() == static_cast(total_indices)); + assert(partial_covariance.rows() == static_cast(total_indices)); + assert(partial_covariance.cols() == static_cast(total_indices)); + + // Compute the sqrt information of the provided cov matrix + const fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + + // Assemble a mean vector and 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 + // matrix, where each row computes a cost for one measured dimensions, and the columns are in the order + // defined by the variable. + mean_ = fuse_core::VectorXd::Zero(total_variable_size); + sqrt_information_ = fuse_core::MatrixXd::Zero(total_indices, total_variable_size); + for (size_t i = 0; i < linear_indices.size(); ++i) + { + mean_(linear_indices[i]) = partial_mean(i); + sqrt_information_.col(linear_indices[i]) = partial_sqrt_information.col(i); + } + + for (size_t i = linear_indices.size(); i < total_indices; ++i) + { + size_t angular_ordered_index; + switch (angular_indices[i - linear_indices.size()]) + { + case Euler::ROLL: + angular_ordered_index = 0; + break; + case Euler::PITCH: + angular_ordered_index = 1; + break; + case Euler::YAW: + angular_ordered_index = 2; + break; + default: + ROS_FATAL_STREAM("angular index is set different than roll, pitch and yaw which should not be possible"); + return; + } + size_t final_index = position.size() + angular_ordered_index; + mean_(final_index) = partial_mean(i); + sqrt_information_.col(final_index) = partial_sqrt_information.col(i); + } +} + +fuse_core::Matrix6d AbsolutePose3DStampedEulerConstraint::covariance() const +{ + // We want to compute: + // 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 +{ + const auto num_residuals = sqrt_information_.rows(); + + return new ceres::AutoDiffCostFunction( + new NormalPriorPose3DEulerCostFunctor(sqrt_information_, mean_), num_residuals); +} + +} // 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/fixed_3d_landmark_constraint.cpp b/fuse_constraints/src/fixed_3d_landmark_constraint.cpp new file mode 100644 index 000000000..0250c62aa --- /dev/null +++ b/fuse_constraints/src/fixed_3d_landmark_constraint.cpp @@ -0,0 +1,117 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created on Mon Nov 12 2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 + +namespace fuse_constraints +{ + +Fixed3DLandmarkConstraint::Fixed3DLandmarkConstraint( + const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, const fuse_variables::PinholeCamera& calibration, + const fuse_core::MatrixXd& pts3d, const fuse_core::MatrixXd& observations, const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid(), calibration.uuid() }) + , pts3d_(pts3d) + , observations_(observations) + , mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) +{ + assert(pts3d_.cols() == 3); + assert(observations_.cols() == 2); + assert(pts3d_.rows() == observations_.rows()); +} + +Fixed3DLandmarkConstraint::Fixed3DLandmarkConstraint( + const std::string& source, const fuse_variables::Position3DStamped& position, + const fuse_variables::Orientation3DStamped& orientation, const fuse_variables::PinholeCamera& calibration, + const double& marker_size, const fuse_core::MatrixXd& observations, const fuse_core::Vector7d& mean, + const fuse_core::Matrix6d& covariance) + : fuse_core::Constraint(source, { position.uuid(), orientation.uuid(), calibration.uuid() }) + , pts3d_(4, 3) + , observations_(observations) + , mean_(mean) + , sqrt_information_(covariance.inverse().llt().matrixU()) +{ + // Define 3D Homogeneous 3D Points at origin, assume z-up + pts3d_ << -1.0, -1.0, 0.0, // NOLINT + -1.0, 1.0, 0.0, // NOLINT + 1.0, -1.0, 0.0, // NOLINT + 1.0, 1.0, 0.0; // NOLINT + pts3d_ *= marker_size; // Scalar Multiplication + + assert(pts3d_.cols() == 3); + assert(observations_.cols() == 2); + assert(pts3d_.rows() == observations_.rows()); +} + +void Fixed3DLandmarkConstraint::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" + << " observations: " << observations() << "\n"; + + if (loss()) + { + stream << " loss: "; + loss()->print(stream); + } +} + +ceres::CostFunction* Fixed3DLandmarkConstraint::costFunction() const +{ + return new ceres::AutoDiffCostFunction( + new Fixed3DLandmarkCostFunctor(sqrt_information_, mean_, observations_, pts3d_)); +} + +} // namespace fuse_constraints + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_constraints::Fixed3DLandmarkConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_constraints::Fixed3DLandmarkConstraint, fuse_core::Constraint); diff --git a/fuse_constraints/src/marginal_constraint.cpp b/fuse_constraints/src/marginal_constraint.cpp index 3e85498d3..c08e855b4 100644 --- a/fuse_constraints/src/marginal_constraint.cpp +++ b/fuse_constraints/src/marginal_constraint.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/relative_constraint.cpp b/fuse_constraints/src/relative_constraint.cpp index 1b9e63a92..7b3a890b3 100644 --- a/fuse_constraints/src/relative_constraint.cpp +++ b/fuse_constraints/src/relative_constraint.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include diff --git a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp index 73f522e3c..2efd3bf87 100644 --- a/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_orientation_3d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp index af9ddc156..9b91bc535 100644 --- a/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_2d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp index 1e688e1d5..7ff80f319 100644 --- a/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp +++ b/fuse_constraints/src/relative_pose_3d_stamped_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include 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..e092f6727 --- /dev/null +++ b/fuse_constraints/src/relative_pose_3d_stamped_euler_constraint.cpp @@ -0,0 +1,156 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 + + +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::VectorXd& partial_delta, + const fuse_core::MatrixXd& partial_covariance, + const std::vector& linear_indices, + const std::vector& angular_indices) : + fuse_core::Constraint( + source, + {position1.uuid(), orientation1.uuid(), position2.uuid(), orientation2.uuid()}) // NOLINT(whitespace/braces) +{ + constexpr size_t total_variable_size = 6; // 3 position (x, y, z) and 3 orientation variables (roll, pitch, yaw) + const size_t total_indices = linear_indices.size() + angular_indices.size(); + + assert(partial_delta.rows() == static_cast(total_indices)); + assert(partial_covariance.rows() == static_cast(total_indices)); + assert(partial_covariance.cols() == static_cast(total_indices)); + + // Compute the sqrt information of the provided cov matrix + const fuse_core::MatrixXd partial_sqrt_information = partial_covariance.inverse().llt().matrixU(); + + // Assemble a mean vector and 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 + // matrix, where each row computes a cost for one measured dimensions, and the columns are in the order + // defined by the variable. + delta_ = fuse_core::Vector6d::Zero(); + sqrt_information_ = fuse_core::MatrixXd::Zero(total_indices, total_variable_size); + for (size_t i = 0; i < linear_indices.size(); ++i) + { + delta_(linear_indices[i]) = partial_delta(i); + sqrt_information_.col(linear_indices[i]) = partial_sqrt_information.col(i); + } + + for (size_t i = linear_indices.size(); i < total_indices; ++i) + { + size_t angular_ordered_index; + switch (angular_indices[i - linear_indices.size()]) + { + case Euler::ROLL: + angular_ordered_index = 0; + break; + case Euler::PITCH: + angular_ordered_index = 1; + break; + case Euler::YAW: + angular_ordered_index = 2; + break; + default: + ROS_FATAL_STREAM("angular index is set different than roll, pitch and yaw which should not be possible"); + return; + } + size_t final_index = 3 + angular_ordered_index; + delta_(final_index) = partial_delta(i); + sqrt_information_.col(final_index) = partial_sqrt_information.col(i); + } +} + +fuse_core::Matrix6d RelativePose3DStampedEulerConstraint::covariance() const +{ + // We want to compute: + // 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"; + + if (loss()) + { + stream << " loss: "; + loss()->print(stream); + } +} + +ceres::CostFunction* RelativePose3DStampedEulerConstraint::costFunction() const +{ + const auto num_residuals = sqrt_information_.rows(); + + return new ceres::AutoDiffCostFunction( + new NormalDeltaPose3DEulerCostFunctor(sqrt_information_, delta_), num_residuals); +} + +} // 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/cost_function_gtest.h b/fuse_constraints/test/cost_function_gtest.h index ec272fe3b..3f73a2537 100644 --- a/fuse_constraints/test/cost_function_gtest.h +++ b/fuse_constraints/test/cost_function_gtest.h @@ -51,7 +51,7 @@ * @param[in] tolerance The tolerance to use when comparing the cost functions are equal. Defaults to 1e-18 */ static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function, - const ceres::CostFunction& actual_cost_function, double tolerance = 1e-18) + const ceres::CostFunction& actual_cost_function) { EXPECT_EQ(cost_function.num_residuals(), actual_cost_function.num_residuals()); const size_t num_residuals = cost_function.num_residuals(); @@ -95,7 +95,7 @@ static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function EXPECT_TRUE(actual_cost_function.Evaluate(parameter_blocks.get(), actual_residuals.get(), nullptr)); for (size_t i = 0; i < num_residuals; ++i) { - EXPECT_NEAR(residuals[i], actual_residuals[i], tolerance) << "residual id: " << i; + EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual id: " << i; } EXPECT_TRUE(cost_function.Evaluate(parameter_blocks.get(), residuals.get(), jacobian_blocks.get())); @@ -103,12 +103,12 @@ static void ExpectCostFunctionsAreEqual(const ceres::CostFunction& cost_function actual_cost_function.Evaluate(parameter_blocks.get(), actual_residuals.get(), actual_jacobian_blocks.get())); for (size_t i = 0; i < num_residuals; ++i) { - EXPECT_NEAR(residuals[i], actual_residuals[i], tolerance) << "residual : " << i; + EXPECT_DOUBLE_EQ(residuals[i], actual_residuals[i]) << "residual : " << i; } for (size_t i = 0; i < num_residuals * num_parameters; ++i) { - EXPECT_NEAR(jacobians[i], actual_jacobians[i], tolerance) + EXPECT_DOUBLE_EQ(jacobians[i], actual_jacobians[i]) << "jacobian : " << i << " " << jacobians[i] << " " << actual_jacobians[i]; } } diff --git a/fuse_constraints/test/test_absolute_constraint.cpp b/fuse_constraints/test/test_absolute_constraint.cpp index 47f6da61d..00d35c7bb 100644 --- a/fuse_constraints/test/test_absolute_constraint.cpp +++ b/fuse_constraints/test/test_absolute_constraint.cpp @@ -378,7 +378,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) auto variable = fuse_variables::Orientation2DStamped::make_shared( ros::Time(1234, 5678), fuse_core::uuid::generate("tiktok")); - variable->yaw() = 0.7; + variable->setYaw(0.7); // Create an absolute constraint fuse_core::Vector1d mean; mean << 7.0; @@ -408,7 +408,7 @@ TEST(AbsoluteConstraint, AbsoluteOrientation2DOptimization) ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); // Check - EXPECT_NEAR(7.0 - 2 * M_PI, variable->yaw(), 1.0e-5); + EXPECT_NEAR(7.0 - 2 * M_PI, variable->getYaw(), 1.0e-5); // Compute the covariance std::vector > covariance_blocks; covariance_blocks.emplace_back(variable->data(), variable->data()); diff --git a/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp new file mode 100644 index 000000000..07eb274be --- /dev/null +++ b/fuse_constraints/test/test_absolute_orientation_2d_stamped_constraint.cpp @@ -0,0 +1,368 @@ +/* + * 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 +#include +#include + +#include +#include + +using fuse_constraints::AbsoluteOrientation2DStampedConstraint; +using fuse_variables::Orientation2DStamped; + + +TEST(AbsoluteOrientation2DStampedConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Orientation2DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + fuse_core::Vector1d mean; + mean << 1.0; + fuse_core::Matrix1d cov; + cov << 1.0; + EXPECT_NO_THROW(AbsoluteOrientation2DStampedConstraint constraint("test", orientation_variable, mean, cov)); +} + +TEST(AbsoluteOrientation2DStampedConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Orientation2DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo")); + fuse_core::Vector1d mean; + mean << 1.0; + fuse_core::Matrix1d cov; + cov << 1.0; + AbsoluteOrientation2DStampedConstraint constraint("test", orientation_variable, mean, cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix1d expected_sqrt_info; + expected_sqrt_info << 1.0; + fuse_core::Matrix1d 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(AbsoluteOrientation2DStampedConstraint, Optimization) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are generated. + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->setYaw(1.0); + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << 1.0; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *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( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + 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); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + EXPECT_NEAR(1.0, orientation_variable->getYaw(), 1.0e-3); + + // Compute the covariance + std::vector > covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, OptimizationZero) +{ + // Optimize a single orientation at zero and single constraint, verify the expected value and covariance are + // generated. + + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->setYaw(0.0); + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << 0.0; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *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( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + 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); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + EXPECT_NEAR(0.0, orientation_variable->getYaw(), 1.0e-3); + + // Compute the covariance + std::vector > covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, OptimizationPositivePi) +{ + // Optimize a single orientation at +PI and single constraint, verify the expected value and covariance are + // generated. + + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->setYaw(M_PI); + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << M_PI; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *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( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + 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); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + // We expect +PI to roll over to -PI because our range is [-PI, PI) + EXPECT_NEAR(-M_PI, orientation_variable->getYaw(), 1.0e-3); + + // Compute the covariance + std::vector > covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, OptimizationNegativePi) +{ + // Optimize a single orientation at -PI and single constraint, verify the expected value and covariance are + // generated. + + // Create the variables + auto orientation_variable = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_variable->setYaw(-M_PI); + + // Create an absolute orientation constraint + fuse_core::Vector1d mean; + mean << -M_PI; + + fuse_core::Matrix1d cov; + cov << 1.0; + auto constraint = AbsoluteOrientation2DStampedConstraint::make_shared( + "test", + *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( + orientation_variable->data(), + orientation_variable->size(), + orientation_variable->localParameterization()); + + std::vector parameter_blocks; + 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); + EXPECT_TRUE(summary.IsSolutionUsable()) << summary.FullReport(); + + // Check + EXPECT_NEAR(-M_PI, orientation_variable->getYaw(), 1.0e-3); + + // Compute the covariance + std::vector > covariance_blocks; + covariance_blocks.emplace_back(orientation_variable->data(), orientation_variable->data()); + ceres::Covariance::Options cov_options; + ceres::Covariance covariance(cov_options); + covariance.Compute(covariance_blocks, &problem); + fuse_core::Matrix1d actual_covariance(orientation_variable->localSize(), orientation_variable->localSize()); + covariance.GetCovarianceBlockInTangentSpace( + orientation_variable->data(), orientation_variable->data(), actual_covariance.data()); + + // Define the expected covariance + fuse_core::Matrix1d expected_covariance; + expected_covariance << 1.0; + EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-3); +} + +TEST(AbsoluteOrientation2DStampedConstraint, Serialization) +{ + // Construct a constraint + Orientation2DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + fuse_core::Vector1d mean; + mean << 1.0; + fuse_core::Matrix1d cov; + cov << 1.0; + AbsoluteOrientation2DStampedConstraint expected("test", 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 + AbsoluteOrientation2DStampedConstraint 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()); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp index fbe4a49cf..1ca110310 100644 --- a/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_absolute_pose_2d_stamped_constraint.cpp @@ -91,7 +91,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) // Optimize a single pose and single constraint, verify the expected value and covariance are generated. // Create the variables auto orientation_variable = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); - orientation_variable->yaw() = 0.8; + orientation_variable->setYaw(0.8); auto position_variable = Position2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = -3.0; @@ -132,7 +132,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationFull) // 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, orientation_variable->yaw(), 1.0e-5); + EXPECT_NEAR(3.0, orientation_variable->getYaw(), 1.0e-5); // Compute the covariance std::vector > covariance_blocks; covariance_blocks.emplace_back(position_variable->data(), position_variable->data()); @@ -167,7 +167,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) // Optimize a single pose and single constraint, verify the expected value and covariance are generated. // Create the variables auto orientation_variable = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); - orientation_variable->yaw() = 0.8; + orientation_variable->setYaw(0.8); auto position_variable = Position2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); position_variable->x() = 1.5; position_variable->y() = -3.0; @@ -237,7 +237,7 @@ TEST(AbsolutePose2DStampedConstraint, OptimizationPartial) // 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, orientation_variable->yaw(), 1.0e-5); + EXPECT_NEAR(3.0, orientation_variable->getYaw(), 1.0e-5); // Compute the covariance std::vector > covariance_blocks; diff --git a/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp b/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp new file mode 100644 index 000000000..229d16d05 --- /dev/null +++ b/fuse_constraints/test/test_fixed_3d_landmark_constraint.cpp @@ -0,0 +1,681 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created on Fri Nov 17 2023 + * + * Copyright (c) 2023 Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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_constraints::Fixed3DLandmarkConstraint; +using fuse_variables::Orientation3DStamped; +using fuse_variables::PinholeCameraFixed; +using fuse_variables::Position3DStamped; + +TEST(Fixed3DLandmarkConstraint, Constructor) +{ + // Construct a constraint just to make sure it compiles. + Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + PinholeCameraFixed calibration_variable(0); + + double marker_size = 1.0; + + fuse_core::Vector7d mean; + mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.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; + 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 + + // 2D observations (arbitrary) + Eigen::Matrix obs; + obs << 320, 240, 320, 240, 320, 240, 320, 240; + + EXPECT_NO_THROW(Fixed3DLandmarkConstraint constraint("test", position_variable, orientation_variable, + calibration_variable, marker_size, obs, mean, cov)); +} + +TEST(Fixed3DLandmarkConstraint, Covariance) +{ + // Verify the covariance <--> sqrt information conversions are correct + Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo")); + Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("mo")); + PinholeCameraFixed calibration_variable(0); + + double marker_size = 1.0; + + fuse_core::Vector7d mean; + mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.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; + 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 + + // 2D observations (arbitrary) + Eigen::Matrix obs; + obs << 320, 240, 320, 240, 320, 240, 320, 240; + + Fixed3DLandmarkConstraint constraint("test", position_variable, orientation_variable, calibration_variable, + marker_size, obs, mean, cov); + + // Define the expected matrices (used Octave to compute sqrt_info: 'chol(inv(A))') + fuse_core::Matrix6d expected_sqrt_info; + 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 + 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(Fixed3DLandmarkConstraint, 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(ros::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(ros::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; + + auto calibration_variable = PinholeCameraFixed::make_shared(0); + calibration_variable->fx() = 638.34478759765620; + calibration_variable->fy() = 643.10717773437500; + calibration_variable->cx() = 310.29060457226840; + calibration_variable->cy() = 237.80861559081677; + + // Define Marker Size + double marker_size = 1.0; + + // Create an fixed position for the marker. + fuse_core::Vector7d mean; + mean << 0, 0, 10, 0.9238795, 0, -0.3826834, 0; + + // And observations... + Eigen::Matrix obs; + obs << 261.71822455, 168.60442225, // NOLINT + 261.71822455, 307.01280893, // NOLINT + 352.44745875, 177.74503448, // NOLINT + 352.44745875, 297.87219670; // NOLINT + + // Define Pose Covariance + fuse_core::Matrix6d cov; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, // NOLINT + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, // NOLINT + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, // NOLINT + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, // NOLINT + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, // NOLINT + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; // NOLINT + + auto constraint = Fixed3DLandmarkConstraint::make_shared("test", *position_variable, *orientation_variable, + *calibration_variable, marker_size, obs, 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(), + position_variable->localParameterization()); + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), + orientation_variable->localParameterization()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + parameter_blocks.push_back(calibration_variable->data()); + + if (calibration_variable->holdConstant()) + { + problem.SetParameterBlockConstant(calibration_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(0.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(0.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); + + EXPECT_NEAR(638.34478759765620, calibration_variable->fx(), 1.0e-3); + EXPECT_NEAR(643.10717773437500, calibration_variable->fy(), 1.0e-3); + EXPECT_NEAR(310.29060457226840, calibration_variable->cx(), 1.0e-3); + EXPECT_NEAR(237.80861559081677, calibration_variable->cy(), 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; + // 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; + + // EXPECT_MATRIX_NEAR(expected_covariance, actual_covariance, 1.0e-5); +} + +TEST(Fixed3DLandmarkConstraint, OptimizationScaledMarker) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are generated. + // Create the variables + auto position_variable = Position3DStamped::make_shared(ros::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(ros::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; + + auto calibration_variable = PinholeCameraFixed::make_shared(0); + calibration_variable->fx() = 638.34478759765620; + calibration_variable->fy() = 643.10717773437500; + calibration_variable->cx() = 310.29060457226840; + calibration_variable->cy() = 237.80861559081677; + + // Define Marker Size + double marker_size = 0.25; + + // Create an fixed position for the marker. + fuse_core::Vector7d mean; + mean << 0, 0, 10, 0.9238795, 0, -0.3826834, 0; + + // And observations... + Eigen::Matrix obs; + obs << 298.8030834636202, 221.44160554498040, // NOLINT + 298.8030834636202, 254.17562563665314, // NOLINT + 321.3790354517349, 222.01021505859384, // NOLINT + 321.3790354517349, 253.60701612303970; // NOLINT + + // Define Pose Covariance + fuse_core::Matrix6d cov; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, // NOLINT + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, // NOLINT + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, // NOLINT + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, // NOLINT + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, // NOLINT + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; // NOLINT + + auto constraint = Fixed3DLandmarkConstraint::make_shared("test", *position_variable, *orientation_variable, + *calibration_variable, marker_size, obs, 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(), + position_variable->localParameterization()); + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), + orientation_variable->localParameterization()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + parameter_blocks.push_back(calibration_variable->data()); + + if (calibration_variable->holdConstant()) + { + problem.SetParameterBlockConstant(calibration_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(0.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(0.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); + + EXPECT_NEAR(638.34478759765620, calibration_variable->fx(), 1.0e-3); + EXPECT_NEAR(643.10717773437500, calibration_variable->fy(), 1.0e-3); + EXPECT_NEAR(310.29060457226840, calibration_variable->cx(), 1.0e-3); + EXPECT_NEAR(237.80861559081677, calibration_variable->cy(), 1.0e-3); +} + +TEST(Fixed3DLandmarkConstraint, OptimizationPoints) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are generated. + // Create the variables + auto position_variable = Position3DStamped::make_shared(ros::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(ros::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; + + auto calibration_variable = PinholeCameraFixed::make_shared(0); + calibration_variable->fx() = 638.34478759765620; + calibration_variable->fy() = 643.10717773437500; + calibration_variable->cx() = 310.29060457226840; + calibration_variable->cy() = 237.80861559081677; + + // Create an fixed position for the marker. + fuse_core::Vector7d mean; + mean << 0, 0, 10, 0.9238795, 0, -0.3826834, 0; + + // Create the 3D points + Eigen::Matrix pts3d; + pts3d << -1.50, -1.5, 0.0, // NOLINT + -1.50, 1.5, 0.0, // NOLINT + 1.50, -1.5, 0.0, // NOLINT + 1.50, 1.5, 0.0, // NOLINT + 3.24, -1.5, 0.0, // NOLINT + 3.24, 1.5, 0.0, // NOLINT + 1.74, -1.5, 0.0, // NOLINT + 1.74, 1.5, 0.0; // NOLINT + + // And observations... + Eigen::Matrix obs; + obs << 234.55045762290558, 129.89675764784400, // NOLINT + 234.55045762290558, 345.72047353378950, // NOLINT + 371.50457352525080, 150.59313756704000, // NOLINT + 371.50457352525080, 325.02409361459354, // NOLINT + 429.27697088295537, 159.32364907215157, // NOLINT + 429.27697088295537, 316.29358210948200, // NOLINT + 380.22578098989925, 151.91107841060833, // NOLINT + 380.22578098989925, 323.70615277102520; // NOLINT + + // Define Pose Covariance + fuse_core::Matrix6d cov; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, // NOLINT + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, // NOLINT + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, // NOLINT + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, // NOLINT + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, // NOLINT + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; // NOLINT + + auto constraint = Fixed3DLandmarkConstraint::make_shared("test", *position_variable, *orientation_variable, + *calibration_variable, pts3d, obs, 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(), + position_variable->localParameterization()); + problem.AddParameterBlock(orientation_variable->data(), orientation_variable->size(), + orientation_variable->localParameterization()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_variable->data()); + parameter_blocks.push_back(orientation_variable->data()); + parameter_blocks.push_back(calibration_variable->data()); + + if (calibration_variable->holdConstant()) + { + problem.SetParameterBlockConstant(calibration_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(0.0, position_variable->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_variable->y(), 1.0e-5); + EXPECT_NEAR(0.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); + + EXPECT_NEAR(638.34478759765620, calibration_variable->fx(), 1.0e-3); + EXPECT_NEAR(643.10717773437500, calibration_variable->fy(), 1.0e-3); + EXPECT_NEAR(310.29060457226840, calibration_variable->cx(), 1.0e-3); + EXPECT_NEAR(237.80861559081677, calibration_variable->cy(), 1.0e-3); +} + +TEST(Fixed3DLandmarkConstraint, MultiViewOptimization) +{ + // Optimize a single pose and single constraint, verify the expected value and covariance are generated. + // Create the variables + + auto calibration_variable = PinholeCameraFixed::make_shared(0); + calibration_variable->fx() = 638.34478759765620; + calibration_variable->fy() = 643.10717773437500; + calibration_variable->cx() = 310.29060457226840; + calibration_variable->cy() = 237.80861559081677; + + // Define Marker Size + double marker_size = 1.0; + + // Build the problem + ceres::Problem::Options problem_options; + problem_options.loss_function_ownership = fuse_core::Loss::Ownership; + ceres::Problem problem(problem_options); + + // Create N Constraints + uint N = 5; + std::vector > obs(5); + + obs[0] << 124.33479102109460, 30.196035559968568, // NOLINT + 124.33479102109460, 168.60442224720070, // NOLINT + 233.20987206846505, 57.617872261057050, // NOLINT + 233.20987206846505, 177.74503448089686; // NOLINT + + obs[1] << 193.02650778349710, 99.400228903584630, // NOLINT + 193.02650778349710, 237.80861559081677, // NOLINT + 292.82866541025334, 117.68145337097695, // NOLINT + 292.82866541025334, 237.80861559081677; // NOLINT + + obs[2] << 373.42853736463960, 168.46718090279768, // NOLINT + 373.42853736463960, 307.15005027883580, // NOLINT + 466.82773058187524, 176.09986812497370, // NOLINT + 466.82773058187524, 299.51736305665980; // NOLINT + + obs[3] << 442.25647916056020, 237.80861559081674, // NOLINT + 442.25647916056020, 376.49148496685490, // NOLINT + 528.07950735845330, 237.80861559081677, // NOLINT + 528.07950735845330, 361.22611052250290; // NOLINT + + obs[4] << 511.08442095648064, 307.15005027883580, // NOLINT + 511.08442095648064, 445.83291965487400, // NOLINT + 589.33128413503120, 299.51736305665980, // NOLINT + 589.33128413503120, 422.93485798834600; // NOLINT + + std::vector position_vars(N); + std::vector orientation_vars(N); + + for (uint i = 0; i < N; i++) + { + position_vars[i] = Position3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + position_vars[i]->x() = 0.0; + position_vars[i]->y() = 0.0; + position_vars[i]->z() = 0.0; + + orientation_vars[i] = Orientation3DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("spra")); + orientation_vars[i]->w() = 0.952; + orientation_vars[i]->x() = 0.038; + orientation_vars[i]->y() = -0.189; + orientation_vars[i]->z() = 0.239; + + problem.AddParameterBlock(position_vars[i]->data(), position_vars[i]->size(), + position_vars[i]->localParameterization()); + problem.AddParameterBlock(orientation_vars[i]->data(), orientation_vars[i]->size(), + orientation_vars[i]->localParameterization()); + problem.AddParameterBlock(calibration_variable->data(), calibration_variable->size(), + calibration_variable->localParameterization()); + + std::vector parameter_blocks; + parameter_blocks.push_back(position_vars[i]->data()); + parameter_blocks.push_back(orientation_vars[i]->data()); + parameter_blocks.push_back(calibration_variable->data()); + + // Create an fixed position for the marker. + fuse_core::Vector7d mean; + mean << 0, 0, 10, 0.9238795, 0, -0.3826834, 0; + + fuse_core::Matrix6d cov; + cov << 1.0, 0.1, 0.2, 0.3, 0.4, 0.5, // NOLINT + 0.1, 2.0, 0.6, 0.5, 0.4, 0.3, // NOLINT + 0.2, 0.6, 3.0, 0.2, 0.1, 0.2, // NOLINT + 0.3, 0.5, 0.2, 4.0, 0.3, 0.4, // NOLINT + 0.4, 0.4, 0.1, 0.3, 5.0, 0.5, // NOLINT + 0.5, 0.3, 0.2, 0.4, 0.5, 6.0; // NOLINT + + auto constraint = Fixed3DLandmarkConstraint::make_shared("test", *position_vars[i], *orientation_vars[i], + *calibration_variable, marker_size, obs[i], mean, cov); + + problem.AddResidualBlock(constraint->costFunction(), constraint->lossFunction(), parameter_blocks); + } + + if (calibration_variable->holdConstant()) + { + problem.SetParameterBlockConstant(calibration_variable->data()); + } + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(-2.0, position_vars[0]->x(), 1.0e-5); + EXPECT_NEAR(-2.0, position_vars[0]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[0]->z(), 1.0e-5); + + EXPECT_NEAR(1.0, orientation_vars[0]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[0]->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[0]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[0]->z(), 1.0e-3); + + EXPECT_NEAR(-1.0, position_vars[1]->x(), 1.0e-5); + EXPECT_NEAR(-1.0, position_vars[1]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[1]->z(), 1.0e-5); + + EXPECT_NEAR(1.0, orientation_vars[1]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[1]->x(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[1]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[1]->z(), 1.0e-3); + + EXPECT_NEAR(0.0, position_vars[2]->x(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[2]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[2]->z(), 1.0e-5); + + EXPECT_NEAR(0.9961947, orientation_vars[2]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[2]->x(), 1.0e-3); + EXPECT_NEAR(0.0871557, orientation_vars[2]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[2]->z(), 1.0e-3); + + EXPECT_NEAR(1.0, position_vars[3]->x(), 1.0e-5); + EXPECT_NEAR(1.0, position_vars[3]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[3]->z(), 1.0e-5); + + EXPECT_NEAR(0.9961947, orientation_vars[3]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[3]->x(), 1.0e-3); + EXPECT_NEAR(0.0871557, orientation_vars[3]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[3]->z(), 1.0e-3); + + EXPECT_NEAR(2.0, position_vars[4]->x(), 1.0e-5); + EXPECT_NEAR(2.0, position_vars[4]->y(), 1.0e-5); + EXPECT_NEAR(0.0, position_vars[4]->z(), 1.0e-5); + + EXPECT_NEAR(0.9961947, orientation_vars[4]->w(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[4]->x(), 1.0e-3); + EXPECT_NEAR(0.0871557, orientation_vars[4]->y(), 1.0e-3); + EXPECT_NEAR(0.0, orientation_vars[4]->z(), 1.0e-3); + + EXPECT_NEAR(638.34478759765620, calibration_variable->fx(), 1.0e-3); + EXPECT_NEAR(643.10717773437500, calibration_variable->fy(), 1.0e-3); + EXPECT_NEAR(310.29060457226840, calibration_variable->cx(), 1.0e-3); + EXPECT_NEAR(237.80861559081677, calibration_variable->cy(), 1.0e-3); +} + +TEST(Fixed3DLandmarkConstraint, Serialization) +{ + // Construct a constraint + Position3DStamped position_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + Orientation3DStamped orientation_variable(ros::Time(1234, 5678), fuse_core::uuid::generate("walle")); + + PinholeCameraFixed calibration_variable(0); + calibration_variable.fx() = 638.34478759765620; + calibration_variable.fy() = 643.10717773437500; + calibration_variable.cx() = 310.29060457226840; + calibration_variable.cy() = 237.80861559081677; + + double marker_size = 1.0; + + fuse_core::Vector7d mean; + mean << 1.0, 2.0, 3.0, 1.0, 0.0, 0.0, 0.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; + cov << 2.0847236144069, 1.10752598122138, 1.02943174290333, 1.96120532313878, 1.96735470687891, 1.5153042667951, + 1.10752598122138, 1.39176289439125, 0.643422499737987, 1.35471905449013, 1.18353784377297, 1.28979625492894, + 1.02943174290333, 0.643422499737987, 1.26701658550187, 1.23641771365403, 1.55169301761377, 1.34706781598061, + 1.96120532313878, 1.35471905449013, 1.23641771365403, 2.39750866789926, 2.06887486311147, 2.04350823837035, + 1.96735470687891, 1.18353784377297, 1.55169301761377, 2.06887486311147, 2.503913946461, 1.73844731158092, + 1.5153042667951, 1.28979625492894, 1.34706781598061, 2.04350823837035, 1.73844731158092, 2.15326088526198; + + // And observations... + Eigen::Matrix obs; + obs << 261.71822455, 168.60442225, 261.71822455, 307.01280893, 352.44745875, 177.74503448, 352.44745875, 297.87219670; + + Fixed3DLandmarkConstraint expected("test", position_variable, orientation_variable, calibration_variable, marker_size, + obs, 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 + Fixed3DLandmarkConstraint 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.pts3d(), actual.pts3d()); + EXPECT_MATRIX_EQ(expected.sqrtInformation(), actual.sqrtInformation()); + EXPECT_MATRIX_EQ(expected.observations(), actual.observations()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/fuse_constraints/test/test_marginalize_variables.cpp b/fuse_constraints/test/test_marginalize_variables.cpp index 42f40b212..f2e26be5c 100644 --- a/fuse_constraints/test/test_marginalize_variables.cpp +++ b/fuse_constraints/test/test_marginalize_variables.cpp @@ -38,7 +38,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/fuse_constraints/test/test_relative_constraint.cpp b/fuse_constraints/test/test_relative_constraint.cpp index ffbf78954..444658613 100644 --- a/fuse_constraints/test/test_relative_constraint.cpp +++ b/fuse_constraints/test/test_relative_constraint.cpp @@ -410,11 +410,11 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) auto x1 = fuse_variables::Orientation2DStamped::make_shared( ros::Time(1234, 5678), fuse_core::uuid::generate("t800")); - x1->yaw() = 0.7; + x1->setYaw(0.7); auto x2 = fuse_variables::Orientation2DStamped::make_shared( ros::Time(1235, 5678), fuse_core::uuid::generate("t800")); - x2->yaw() = -2.2; + x2->setYaw(-2.2); // Create an absolute constraint fuse_core::Vector1d mean; mean << 1.0; @@ -466,8 +466,8 @@ TEST(RelativeConstraint, RelativeOrientation2DOptimization) ceres::Solver::Summary summary; ceres::Solve(options, &problem, &summary); // Check - EXPECT_NEAR(1.0, x1->yaw(), 1.0e-5); - EXPECT_NEAR(1.1, x2->yaw(), 1.0e-5); + EXPECT_NEAR(1.0, x1->getYaw(), 1.0e-5); + EXPECT_NEAR(1.1, x2->getYaw(), 1.0e-5); // Compute the covariance std::vector > covariance_blocks; covariance_blocks.emplace_back(x1->data(), x1->data()); diff --git a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp index f6db71e91..c596dd172 100644 --- a/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp +++ b/fuse_constraints/test/test_relative_pose_2d_stamped_constraint.cpp @@ -105,12 +105,12 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) // Verify the expected poses and covariances are generated. // Create two poses auto orientation1 = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); - orientation1->yaw() = 0.8; + orientation1->setYaw(0.8); auto position1 = Position2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); position1->x() = 1.5; position1->y() = -3.0; auto orientation2 = Orientation2DStamped::make_shared(ros::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); - orientation2->yaw() = -2.7; + orientation2->setYaw(-2.7); auto position2 = Position2DStamped::make_shared(ros::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); position2->x() = 3.7; position2->y() = 1.2; @@ -181,10 +181,10 @@ TEST(RelativePose2DStampedConstraint, OptimizationFull) // Check EXPECT_NEAR(0.0, position1->x(), 1.0e-5); EXPECT_NEAR(0.0, position1->y(), 1.0e-5); - EXPECT_NEAR(0.0, orientation1->yaw(), 1.0e-5); + EXPECT_NEAR(0.0, orientation1->getYaw(), 1.0e-5); EXPECT_NEAR(1.0, position2->x(), 1.0e-5); EXPECT_NEAR(0.0, position2->y(), 1.0e-5); - EXPECT_NEAR(0.0, orientation2->yaw(), 1.0e-5); + EXPECT_NEAR(0.0, orientation2->getYaw(), 1.0e-5); // Compute the marginal covariance for pose1 { std::vector > covariance_blocks; @@ -252,13 +252,13 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) // Verify the expected poses and covariances are generated. // Create two poses auto orientation1 = Orientation2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); - orientation1->yaw() = 0.8; + orientation1->setYaw(0.8); auto position1 = Position2DStamped::make_shared(ros::Time(1, 0), fuse_core::uuid::generate("3b6ra7")); position1->x() = 1.5; position1->y() = -3.0; auto orientation2 = Orientation2DStamped::make_shared(ros::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); - orientation2->yaw() = -2.7; + orientation2->setYaw(-2.7); auto position2 = Position2DStamped::make_shared(ros::Time(2, 0), fuse_core::uuid::generate("3b6ra7")); position2->x() = 3.7; position2->y() = 1.2; @@ -362,10 +362,10 @@ TEST(RelativePose2DStampedConstraint, OptimizationPartial) // Check EXPECT_NEAR(0.0, position1->x(), 1.0e-5); EXPECT_NEAR(0.0, position1->y(), 1.0e-5); - EXPECT_NEAR(0.0, orientation1->yaw(), 1.0e-5); + EXPECT_NEAR(0.0, orientation1->getYaw(), 1.0e-5); EXPECT_NEAR(1.0, position2->x(), 1.0e-5); EXPECT_NEAR(0.0, position2->y(), 1.0e-5); - EXPECT_NEAR(0.0, orientation2->yaw(), 1.0e-5); + EXPECT_NEAR(0.0, orientation2->getYaw(), 1.0e-5); // Compute the marginal covariance for pose1 { diff --git a/fuse_core/CHANGELOG.rst b/fuse_core/CHANGELOG.rst index b0e152fa3..c9636d038 100644 --- a/fuse_core/CHANGELOG.rst +++ b/fuse_core/CHANGELOG.rst @@ -55,6 +55,51 @@ Changelog for package fuse_core * Added a time-limited optimization option to the Graph class (#234) * Contributors: Enrique Fernandez Perdomo, Gary Servin, Stephen Williams, Tom Moore +0.7.0 (2023-09-25) +------------------ +* [RST-7809] Fix optimization errors when the orientation is initialized at +PI (#334) + * Add some unit tests for the 2D orientation constraints; Create getters/setters for the 2D orientation variable is preparation for a fix. + * Force the 2D orientation value to be is minimum phase +* Add missed geometry_msgs package (#272) + * Add geometry_msgs as a test_depend. + * Make geometry_msgs includable. + --------- + Co-authored-by: Ivor Wanders +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* Fix Ceres 2.0.0 API support (#273) + * Pass kNumResiduals to the internal AutoDiff function. + Ceres added this argument in https://github.com/ceres-solver/ceres-solver/commit/e7a30359ee754057f9bd7b349c98c291138d91f4 we need to pass it else template substitution fails. + * Pass kLocalSize instead of kGlobalSize + Upstream commit made me assume kGlobalSize, but that threw at runtime when the tests ran. + This seems to work, also put a using statement there to make roslint happy. + Co-authored-by: Ivor Wanders +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* [RST-4455] Fix C++17 compile issue and simplify matrix serialization at the same time (#244) +* Adding doxygen to all packages (#241) +* Add unstamped 3D point variable (#233) (#239) + * Add unstamped 3D landmark variable + * Add landmark test and new uuid generator + Co-authored-by: Stephen Williams + Co-authored-by: Jake McLaughlin +* [RST-3474] Created a getConstraintCosts() method + * Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints +* [RST-2831] Support for optimization bounds (#235) +* Added a time-limited optimization option to the Graph class (#234) +* Contributors: Enrique Fernandez Perdomo, Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_core/CMakeLists.txt b/fuse_core/CMakeLists.txt index fa18c8452..e24f9d9ff 100644 --- a/fuse_core/CMakeLists.txt +++ b/fuse_core/CMakeLists.txt @@ -72,7 +72,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -94,7 +94,7 @@ target_link_libraries(fuse_echo ) set_target_properties(fuse_echo PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -126,6 +126,23 @@ install( if(CATKIN_ENABLE_TESTING) find_package(roslint REQUIRED) find_package(rostest REQUIRED) + find_package(geometry_msgs REQUIRED) + + include(ExternalProject) + find_package(Git REQUIRED) + + externalproject_add( + covariance_geometry_cpp + PREFIX ${CMAKE_CURRENT_BINARY_DIR}/covariance_geometry_cpp + GIT_REPOSITORY https://github.com/ARTI-Robots/covariance_geometry.git + GIT_TAG master + TIMEOUT 10 + UPDATE_COMMAND ${GIT_EXECUTABLE} pull + CMAKE_ARGS -DCMAKE_INSTALL_PREFIX=${CMAKE_CURRENT_BINARY_DIR}/covariance_geometry_cpp/src + LOG_DOWNLOAD ON + ) + + add_definitions(${EIGEN3_DEFINITIONS}) # Lint tests set(ROSLINT_CPP_OPTS "--filter=-build/c++11,-runtime/references") @@ -154,7 +171,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_async_motion_model PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -180,7 +197,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_async_publisher PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -206,7 +223,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_async_sensor_model PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -231,7 +248,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_callback_wrapper PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -256,7 +273,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_constraint PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -281,7 +298,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_eigen PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -298,7 +315,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_local_parameterization PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -323,7 +340,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -347,7 +364,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_message_buffer PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -373,7 +390,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_timestamp_manager PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -399,7 +416,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_transaction PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -424,7 +441,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_parameter PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -438,9 +455,15 @@ if(CATKIN_ENABLE_TESTING) ${PROJECT_NAME} ${catkin_LIBRARIES} ) + target_include_directories(test_throttled_callback + PRIVATE + include + ${catkin_INCLUDE_DIRS} + ${geometry_msgs_INCLUDE_DIRS} + ) set_target_properties(test_throttled_callback PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -450,6 +473,7 @@ if(CATKIN_ENABLE_TESTING) ) add_dependencies(test_util ${catkin_EXPORTED_TARGETS} + covariance_geometry_cpp ) target_include_directories(test_util PRIVATE @@ -458,13 +482,15 @@ if(CATKIN_ENABLE_TESTING) ${catkin_INCLUDE_DIRS} ${CERES_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} + ${CMAKE_CURRENT_BINARY_DIR}/covariance_geometry_cpp/src/include ) target_link_libraries(test_util ${catkin_LIBRARIES} + ${CMAKE_CURRENT_BINARY_DIR}/covariance_geometry_cpp/src/lib/libcovariance_geometry_cpp.so ) set_target_properties(test_util PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -489,7 +515,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_uuid PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -514,7 +540,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_variable PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_core/include/fuse_core/eigen.h b/fuse_core/include/fuse_core/eigen.h index 1b0f61448..af812ec2b 100644 --- a/fuse_core/include/fuse_core/eigen.h +++ b/fuse_core/include/fuse_core/eigen.h @@ -55,6 +55,16 @@ using Vector6d = Eigen::Matrix; using Vector7d = Eigen::Matrix; using Vector8d = Eigen::Matrix; using Vector9d = Eigen::Matrix; +using Vector10d = Eigen::Matrix; +using Vector11d = Eigen::Matrix; +using Vector12d = Eigen::Matrix; +using Vector13d = Eigen::Matrix; +using Vector14d = Eigen::Matrix; +using Vector15d = Eigen::Matrix; +using Vector16d = Eigen::Matrix; +using Vector17d = Eigen::Matrix; +using Vector18d = Eigen::Matrix; +using Vector19d = Eigen::Matrix; using MatrixXd = Eigen::Matrix; using Matrix1d = Eigen::Matrix; @@ -66,6 +76,16 @@ using Matrix6d = Eigen::Matrix; using Matrix7d = Eigen::Matrix; using Matrix8d = Eigen::Matrix; using Matrix9d = Eigen::Matrix; +using Matrix10d = Eigen::Matrix; +using Matrix11d = Eigen::Matrix; +using Matrix12d = Eigen::Matrix; +using Matrix13d = Eigen::Matrix; +using Matrix14d = Eigen::Matrix; +using Matrix15d = Eigen::Matrix; +using Matrix16d = Eigen::Matrix; +using Matrix17d = Eigen::Matrix; +using Matrix18d = Eigen::Matrix; +using Matrix19d = Eigen::Matrix; template using Matrix = Eigen::Matrix; diff --git a/fuse_core/include/fuse_core/graph_deserializer.h b/fuse_core/include/fuse_core/graph_deserializer.h index d4ef1969f..04d0a6073 100644 --- a/fuse_core/include/fuse_core/graph_deserializer.h +++ b/fuse_core/include/fuse_core/graph_deserializer.h @@ -38,7 +38,7 @@ #include #include #include -#include +#include namespace fuse_core diff --git a/fuse_core/include/fuse_core/loss_loader.h b/fuse_core/include/fuse_core/loss_loader.h index 69899d6db..b1d0dc326 100644 --- a/fuse_core/include/fuse_core/loss_loader.h +++ b/fuse_core/include/fuse_core/loss_loader.h @@ -35,7 +35,7 @@ #define FUSE_CORE_LOSS_LOADER_H #include -#include +#include #include diff --git a/fuse_core/include/fuse_core/transaction_deserializer.h b/fuse_core/include/fuse_core/transaction_deserializer.h index c478f0c3b..ec105e029 100644 --- a/fuse_core/include/fuse_core/transaction_deserializer.h +++ b/fuse_core/include/fuse_core/transaction_deserializer.h @@ -38,7 +38,7 @@ #include #include #include -#include +#include namespace fuse_core diff --git a/fuse_core/include/fuse_core/util.h b/fuse_core/include/fuse_core/util.h index 2e8a7f98e..089e01baa 100644 --- a/fuse_core/include/fuse_core/util.h +++ b/fuse_core/include/fuse_core/util.h @@ -38,10 +38,13 @@ #include #include +#include #include #include +#include + namespace fuse_core { @@ -51,8 +54,8 @@ namespace fuse_core * * @param[in] w The quaternion real-valued component * @param[in] x The quaternion x-axis component - * @param[in] y The quaternion x-axis component - * @param[in] z The quaternion x-axis component + * @param[in] y The quaternion y-axis component + * @param[in] z The quaternion z-axis component * @return The quaternion's Euler pitch angle component */ template @@ -76,8 +79,8 @@ static inline T getPitch(const T w, const T x, const T y, const T z) * * @param[in] w The quaternion real-valued component * @param[in] x The quaternion x-axis component - * @param[in] y The quaternion x-axis component - * @param[in] z The quaternion x-axis component + * @param[in] y The quaternion y-axis component + * @param[in] z The quaternion z-axis component * @return The quaternion's Euler roll angle component */ template @@ -92,10 +95,12 @@ static inline T getRoll(const T w, const T x, const T y, const T z) /** * @brief Returns the Euler yaw angle from a quaternion * + * Returned angle is in the range [-Pi, +Pi] + * * @param[in] w The quaternion real-valued component * @param[in] x The quaternion x-axis component - * @param[in] y The quaternion x-axis component - * @param[in] z The quaternion x-axis component + * @param[in] y The quaternion y-axis component + * @param[in] z The quaternion z-axis component * @return The quaternion's Euler yaw angle component */ template @@ -153,6 +158,501 @@ 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 (order w, x, y, z)) + * @param[out] rpy Pointer to the roll, pitch, yaw array (3x1) + * @param[out] 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; + jacobian_map.setZero(); + + if (discr > 0.49999) + { + // pitch = 90 deg + 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 < -0.49999) + { + // pitch = -90 deg + 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 a quaternion from roll, pitch, and yaw + * + * @param[in] rpy Pointer to the roll, pitch, yaw array (3x1) + * @param[out] q Pointer to the quaternion array (4x1 (order w, x, y, z)) + * @param[out] jacobian Pointer to the jacobian matrix (4x3, optional) + */ +static inline void rpy2quaternion(const double * rpy, double * q, double * jacobian = nullptr) +{ + const double ccc = cos(rpy[0] / 2.) * cos(rpy[1] / 2.) * cos(rpy[2] / 2.); + const double ccs = cos(rpy[0] / 2.) * cos(rpy[1] / 2.) * sin(rpy[2] / 2.); + const double csc = cos(rpy[0] / 2.) * sin(rpy[1] / 2.) * cos(rpy[2] / 2.); + const double scc = sin(rpy[0] / 2.) * cos(rpy[1] / 2.) * cos(rpy[2] / 2.); + const double ssc = sin(rpy[0] / 2.) * sin(rpy[1] / 2.) * cos(rpy[2] / 2.); + const double sss = sin(rpy[0] / 2.) * sin(rpy[1] / 2.) * sin(rpy[2] / 2.); + const double scs = sin(rpy[0] / 2.) * cos(rpy[1] / 2.) * sin(rpy[2] / 2.); + const double css = cos(rpy[0] / 2.) * sin(rpy[1] / 2.) * sin(rpy[2] / 2.); + + q[0] = ccc + sss; + q[1] = scc - css; + q[2] = csc + scs; + q[3] = ccs - ssc; + + if (jacobian) + { + Eigen::Map> jacobian_map(jacobian); + + // dqw/d(rpy) + jacobian_map.row(0) << + (css - scc) / 2., // droll + (scs - csc) / 2., // dpitch + (ssc - ccs) / 2.; // dyaw + + // dqx/d(rpy) + jacobian_map.row(1) << + (ccc + sss) / 2., // droll + -(ssc + ccs) / 2., // dpitch + -(csc + scs) / 2.; // dyaw + + // dqy/d(rpy) + jacobian_map.row(2) << + (ccs - ssc) / 2., // droll + (ccc - sss) / 2., // dpitch + (scc - css) / 2.; // dyaw + + // dqz/d(rpy) + jacobian_map.row(3) << + -(csc + scs) / 2., // droll + -(css + scc) / 2., // dpitch + (ccc + sss) / 2.; // dyaw + } +} + +/** + * @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 (order w, x, y, z)) + * @param[in] w Pointer to the second quaternion array (4x1 (order w, x, y, z)) + * @param[in] z Pointer to the first quaternion array (4x1 (order w, x, y, z)) + * @param[in] jacobian Pointer to the jacobian matrix (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 (order w, x, y, z)) + * @param[in] angle_axis Pointer to the angle_axis array (3x1) + * @param[in] jacobian Pointer to the jacobian matrix (3x4, optional) + */ +static inline void quaternionToAngleAxis(const double * q, double * angle_axis, double * jacobian = nullptr) +{ + ceres::QuaternionToAngleAxis(q, angle_axis); + if (jacobian) + { + Eigen::Map> jacobian_map(jacobian); + const double & q0 = q[0]; + const double & q1 = q[1]; + const double & q2 = q[2]; + const double & q3 = q[3]; + const double q_sum2 = q0 * q0 + q1 * q1 + q2 * q2 + q3 * q3; + const double sin_theta2 = q1 * q1 + q2 * q2 + q3 * q3; + const double sin_theta = std::sqrt(sin_theta2); + const double cos_theta = q0; + + if (std::fpclassify(sin_theta) != FP_ZERO) + { + const double two_theta = 2.0 * + (cos_theta < 0.0 ? std::atan2(-sin_theta, -cos_theta) : std::atan2(sin_theta, cos_theta)); + jacobian_map(0, 0) = -2.0 * q1 / q_sum2; + jacobian_map(0, 1) = + two_theta / sin_theta + + (2.0 * q0 * q1 * q1) / (sin_theta2 * q_sum2) - + (q1 * q1 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(0, 2) = + (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - + (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(0, 3) = + (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - + (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + + jacobian_map(1, 0) = -2.0 * q2 / q_sum2; + jacobian_map(1, 1) = + (2.0 * q0 * q1 * q2) / (sin_theta2 * q_sum2) - + (q1 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(1, 2) = + two_theta / sin_theta + + (2.0 * q0 * q2 * q2) / (sin_theta2 * q_sum2) - + (q2 * q2 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(1, 3) = + (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - + (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + + jacobian_map(2, 0) = -2.0 * q3 / q_sum2; + jacobian_map(2, 1) = + (2.0 * q0 * q1 * q3) / (sin_theta2 * q_sum2) - + (q1 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(2, 2) = + (2.0 * q0 * q2 * q3) / (sin_theta2 * q_sum2) - + (q2 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + jacobian_map(2, 3) = + two_theta / sin_theta + + (2.0 * q0 * q3 * q3) / (sin_theta2 * q_sum2) - + (q3 * q3 * two_theta) / std::pow(sin_theta2, 1.5); + } + else + { + jacobian_map.setZero(); + jacobian_map(1, 1) = 2.0; + jacobian_map(2, 2) = 2.0; + jacobian_map(3, 3) = 2.0; + } + } +} + +/** + * @brief Compute the jacobian of a quaternion normalization + * + * @param[in] q The quaternion + * @return The jacobian of the quaternion normalization + */ +static inline fuse_core::Matrix4d jacobianQuatNormalization(Eigen::Quaterniond q) +{ + fuse_core::Matrix4d ret; + ret << + q.x() * q.x() + q.y() * q.y() + q.z() * q.z(), -q.w() * q.x(), -q.w() * q.y(), -q.w() * q.z(), + -q.x() * q.w(), q.w() * q.w() + q.y() * q.y() + q.z() * q.z(), -q.x() * q.y(), -q.x() * q.z(), + -q.y() * q.w(), -q.y() * q.x(), q.w() * q.w() + q.x() * q.x() + q.z() * q.z(), -q.y() * q.z(), + -q.z() * q.w(), -q.z() * q.x(), -q.z() * q.y(), q.w() * q.w() + q.x() * q.x() + q.y() * q.y(); + + ret /= std::pow(q.norm(), 3.); + return ret; +} + +/** + * @brief Convert a pose covariance in x, y, z, roll, pitch, yaw to a pose covariance in x, y, z, qw, qx, qy, qz + * + * @param[in] pose The pose which covariance should be converted + * @param[in] cov_rpy The covariance in x, y, z, roll, pitch, yaw + * @return The pose covariance in x, y, z, qw, qx, qy, qz + */ +static inline fuse_core::Matrix7d convertToPoseQuatCovariance( + const Eigen::Isometry3d& pose, const fuse_core::Matrix6d& cov_rpy) +{ + double rpy[3]; + Eigen::Quaterniond q(pose.rotation()); + double q_array[4] = { // NOLINT(whitespace/braces) + q.w(), + q.x(), + q.y(), + q.z() + }; + quaternion2rpy(q_array, rpy); + + double J_rpy2quat[12]; + rpy2quaternion(rpy, q_array, J_rpy2quat); + + Eigen::Map> J_rpy2quat_map(J_rpy2quat); + + Eigen::Matrix J; + J.topLeftCorner<3, 3>().setIdentity(); + J.topRightCorner<3, 3>().setZero(); + J.bottomLeftCorner<4, 3>().setZero(); + J.bottomRightCorner<4, 3>() = J_rpy2quat_map; + + return J * cov_rpy * J.transpose(); +} + +/** + * @brief Convert a pose covariance in x, y, z, qw, qx, qy, qz to a pose covariance in x, y, z, roll, pitch, yaw + * + * @param[in] pose The pose which covariance should be converted + * @param[in] cov_rpy The covariance in x, y, z, qw, qx, qy, qz + * @return The pose covariance in x, y, z, roll, pitch, yaw + */ +static inline fuse_core::Matrix6d convertToPoseRPYCovariance( + const Eigen::Isometry3d& pose, const fuse_core::Matrix7d& cov_quat) +{ + Eigen::Quaterniond q(pose.rotation()); + double q_array[4] = { // NOLINT(whitespace/braces) + q.w(), + q.x(), + q.y(), + q.z() + }; + double rpy[3]; + double J_quat2rpy[12]; + quaternion2rpy(q_array, rpy, J_quat2rpy); + Eigen::Map> J_quat2rpy_map(J_quat2rpy); + Eigen::Matrix J; + J.topLeftCorner<3, 3>().setIdentity(); + J.topRightCorner<3, 4>().setZero(); + J.bottomLeftCorner<3, 3>().setZero(); + J.bottomRightCorner<3, 4>() = J_quat2rpy_map * jacobianQuatNormalization(q); + + return J * cov_quat * J.transpose(); +} + +/** + * @brief Compute the pose covariance of an inverted pose + * + * @param[in] pose The pose which will be inverted + * @param[in] cov The covariance of the pose which will be inverted (order: x, y, z, roll, pitch, yaw) + * @return The pose covariance of the inverted pose (order: x, y, z, roll, pitch, yaw) + */ +static inline fuse_core::Matrix6d invertPoseCovariance( + const Eigen::Isometry3d& pose, const fuse_core::Matrix6d& cov) +{ + // convert the covariances from 3D + roll-pitch-yaw to 3D + quaternion + auto cov_quat = convertToPoseQuatCovariance(pose, cov); + + // compute the inverse pose covariance with 3D + quaternion + const Eigen::Quaterniond q(pose.rotation()); + + const double dx = (0 - pose.translation().x()); + const double dy = (0 - pose.translation().y()); + const double dz = (0 - pose.translation().z()); + Eigen::Matrix fqrir; + fqrir << + -q.y() * dz + q.z() * dy, q.y() * dy + q.z() * dz, q.x() * dy - 2 * q.y() * dx - q.w() * dz, q.x() * dz + q.w() * dy - 2 * q.z() * dx, // NOLINT(whitespace/line_length) + q.x() * dz - q.z() * dx, q.y() * dx - 2 * q.x() * dy + q.w() * dz, q.x() * dx + q.z() * dz, -q.w() * dx - 2 * q.z() * dy + q.y() * dz, // NOLINT(whitespace/line_length) + q.y() * dx - q.x() * dy, q.z() * dx - q.w() * dy - 2 * q.x() * dz, q.z() * dy + q.w() * dx - 2 * q.y() * dz, q.x() * dx + q.y() * dy; // NOLINT(whitespace/line_length) + fqrir *= 2; + fqrir.applyOnTheRight(jacobianQuatNormalization(q)); + + + Eigen::Matrix fqri; + fqri.leftCols<3>() << + 2 * q.y() * q.y() + 2 * q.z() * q.z() - 1, -2 * q.w() * q.z() - 2 * q.x() * q.y(), 2 * q.w() * q.y() - 2 * q.x() * q.z(), // NOLINT(whitespace/line_length) + 2 * q.w() * q.z() - 2 * q.x() * q.y(), 2 * q.x() * q.x() + 2 * q.z() * q.z() - 1, -2 * q.w() * q.x() - 2 * q.y() * q.z(), // NOLINT(whitespace/line_length) + -2 * q.w() * q.y() - 2 * q.x() * q.z(), 2 * q.w() * q.x() - 2 * q.y() * q.z(), 2 * q.x() * q.x() + 2 * q.y() * q.y() - 1; // NOLINT(whitespace/line_length) + fqri.rightCols<4>() = fqrir; + + fuse_core::Matrix7d fqi; + fqi.topRows<3>() = fqri; + fqi.bottomLeftCorner<4, 3>().setZero(); + fqi.bottomRightCorner<4, 4>() << + 1, 0, 0, 0, + 0, -1, 0, 0, + 0, 0, -1, 0, + 0, 0, 0, -1; + fqi.bottomRightCorner<4, 4>().applyOnTheRight(jacobianQuatNormalization(q)); + + fuse_core::Matrix7d cov_inverse_quat = fqi * cov_quat * fqi.transpose(); + + // convert back to 3D + roll-pitch-yaw + const auto pose_inverse = pose.inverse(); + return convertToPoseRPYCovariance(pose_inverse, cov_inverse_quat); +} + +/** + * @brief Compute the jacobian of a pose composition wrt pose 1 (composition: pose3 = pose1 + pose2) + * + * @param[in] pose1 The pose 1 of the pose composition + * @param[in] pose2 The pose 2 of the pose composition + * @return The jacobian of the pose composition wrt pose 1 (order: x, y, z, qw, qx, qy, qz) + */ +static inline fuse_core::Matrix7d jacobianPosePoseCompositionA( + const Eigen::Isometry3d& pose1, const Eigen::Isometry3d& pose2) +{ + const Eigen::Quaterniond q1(pose1.rotation()); + const Eigen::Quaterniond q2(pose2.rotation()); + const Eigen::Vector3d a(pose2.translation()); + Eigen::Matrix fqr_pose1; + fqr_pose1.leftCols<3>().setIdentity(); + + fqr_pose1(0, 3) = -q1.z() * a.y() + q1.y() * a.z(); + fqr_pose1(0, 4) = q1.y() * a.y() + q1.z() * a.z(); + fqr_pose1(0, 5) = -2 * q1.y() * a.x() + q1.x() * a.y() + q1.w() * a.z(); + fqr_pose1(0, 6) = -2 * q1.z() * a.x() - q1.w() * a.y() + q1.x() * a.z(); + + fqr_pose1(1, 3) = q1.z() * a.x() - q1.x() * a.z(); + fqr_pose1(1, 4) = q1.y() * a.x() - 2 * q1.x() * a.y() - q1.w() * a.z(); + fqr_pose1(1, 5) = q1.x() * a.x() + q1.z() * a.z(); + fqr_pose1(1, 6) = q1.w() * a.x() - 2 * q1.z() * a.y() + q1.y() * a.z(); + + fqr_pose1(2, 3) = -q1.y() * a.x() + q1.x() * a.y(); + fqr_pose1(2, 4) = q1.z() * a.x() + q1.w() * a.y() - 2 * q1.x() * a.z(); + fqr_pose1(2, 5) = -q1.w() * a.x() + q1.z() * a.y() - 2 * q1.y() * a.z(); + fqr_pose1(2, 6) = q1.x() * a.x() + q1.y() * a.y(); + + fqr_pose1.rightCols<4>() *= 2; + + fqr_pose1.rightCols<4>().applyOnTheRight(jacobianQuatNormalization(q1)); + + fuse_core::Matrix7d fqc_pose1; + fqc_pose1.topRows<3>() = fqr_pose1; + fqc_pose1.bottomLeftCorner<4, 3>().setZero(); + fqc_pose1.bottomRightCorner<4, 4>() << + q2.w(), -q2.x(), -q2.y(), -q2.z(), + q2.x(), q2.w(), q2.z(), -q2.y(), + q2.y(), -q2.z(), q2.w(), q2.x(), + q2.z(), q2.y(), -q2.x(), q2.w(); + + return fqc_pose1; +} + +/** + * @brief Compute the jacobian of a pose composition wrt pose 2 (composition: pose3 = pose1 + pose2) + * + * @param[in] pose1 The pose 1 of the pose composition + * @return The jacobian of the pose composition wrt pose 2 (order: x, y, z, qw, qx, qy, qz) + */ +static inline fuse_core::Matrix7d jacobianPosePoseCompositionB( + const Eigen::Isometry3d& pose1) +{ + const Eigen::Quaterniond q1(pose1.rotation()); + + fuse_core::Matrix3d fqr_position2; + fqr_position2 << + 0.5 - q1.y() * q1.y() - q1.z() * q1.z(), q1.x() * q1.y() - q1.w() * q1.z(), q1.w() * q1.y() + q1.x() * q1.z(), + q1.w() * q1.z() + q1.x() * q1.y(), 0.5 - q1.x() * q1.x() - q1.z() * q1.z(), q1.y() * q1.z() - q1.w() * q1.x(), + q1.x() * q1.z() - q1.w() * q1.y(), q1.w() * q1.x() + q1.y() * q1.z(), 0.5 - q1.x() * q1.x() - q1.y() * q1.y(); + fqr_position2 *= 2; + + fuse_core::Matrix7d fqc_pose2; + fqc_pose2.topLeftCorner<3, 3>() = fqr_position2; + fqc_pose2.topRightCorner<3, 4>().setZero(); + fqc_pose2.bottomLeftCorner<4, 3>().setZero(); + fqc_pose2.bottomRightCorner<4, 4>() << + q1.w(), -q1.x(), -q1.y(), -q1.z(), + q1.x(), q1.w(), -q1.z(), q1.y(), + q1.y(), q1.z(), q1.w(), -q1.x(), + q1.z(), -q1.y(), q1.x(), q1.w(); + + return fqc_pose2; +} + +/** + * @brief Compute the covariance after a pose composition (composition: pose3 = pose1 + pose2) + * + * @param[in] pose1 The pose 1 of the pose composition + * @param[in] cov1 The covariance of pose 1 of the pose composition (order: x, y, z, roll, pitch, yaw) + * @param[in] pose2 The pose 2 of the pose composition + * @param[in] cov2 The covariance of pose 2 of the pose composition (order: x, y, z, roll, pitch, yaw) + * @return The covariance after the pose composition (order: x, y, z, roll, pitch, yaw) + */ +static inline fuse_core::Matrix6d composePoseCovariance( + const Eigen::Isometry3d& pose1, const fuse_core::Matrix6d& cov1, + const Eigen::Isometry3d& pose2, const fuse_core::Matrix6d& cov2) +{ + const auto pose3 = pose1 * pose2; + const Eigen::Quaterniond q3(pose3.rotation()); + + // first, convert the covariances from 3D + roll-pitch-yaw to 3D + quaternion + const auto cov1_quat = convertToPoseQuatCovariance(pose1, cov1); + const auto cov2_quat = convertToPoseQuatCovariance(pose2, cov2); + + // now compose the two covariances + const Eigen::Quaterniond q1(pose1.rotation()); + const Eigen::Quaterniond q2(pose2.rotation()); + const Eigen::Vector3d a(pose2.translation()); + + fuse_core::Matrix7d fqc_pose1 = jacobianPosePoseCompositionA(pose1, pose2); + + fuse_core::Matrix7d fqc_pose2 = jacobianPosePoseCompositionB(pose1); + + const auto fqn_pose3 = jacobianQuatNormalization(q3); + + fuse_core::Matrix7d fqc_pose1_including_fqn = fqc_pose1; + fqc_pose1_including_fqn.bottomRightCorner<4, 4>().applyOnTheLeft(fqn_pose3); + + fuse_core::Matrix7d fqc_pose2_including_fqn = fqc_pose2; + fqc_pose2_including_fqn.bottomRightCorner<4, 4>().applyOnTheLeft(fqn_pose3); + + const fuse_core::Matrix7d cov3_quat = + fqc_pose1_including_fqn * cov1_quat * fqc_pose1_including_fqn.transpose() + + fqc_pose2_including_fqn * cov2_quat * fqc_pose2_including_fqn.transpose(); + + // convert back to 3D + roll-pitch-yaw + return convertToPoseRPYCovariance(pose3, cov3_quat); +} + } // namespace fuse_core #endif // FUSE_CORE_UTIL_H diff --git a/fuse_core/include/fuse_core/variable.h b/fuse_core/include/fuse_core/variable.h index 2b7252b14..22ee9b4ab 100644 --- a/fuse_core/include/fuse_core/variable.h +++ b/fuse_core/include/fuse_core/variable.h @@ -325,7 +325,7 @@ class Variable * @param[in] index The variable dimension of interest * @return The lower bound for the requested variable dimension */ - virtual double lowerBound(size_t index) const + virtual double lowerBound(size_t /* index */) const { return std::numeric_limits::lowest(); } @@ -338,7 +338,7 @@ class Variable * @param[in] index The variable dimension of interest * @return The upper bound for the requested variable dimension */ - virtual double upperBound(size_t index) const + virtual double upperBound(size_t /* index */) const { return std::numeric_limits::max(); } diff --git a/fuse_core/package.xml b/fuse_core/package.xml index 239733db4..31a2e4d60 100644 --- a/fuse_core/package.xml +++ b/fuse_core/package.xml @@ -1,7 +1,7 @@ fuse_core - 0.6.0 + 0.7.0 The fuse_core package provides the base class interfaces for the various fuse components. Concrete implementations of these interfaces are provided in other packages. @@ -20,6 +20,7 @@ rosconsole roslint rostest + geometry_msgs diff --git a/fuse_core/test/test_util.cpp b/fuse_core/test/test_util.cpp index 30a8c5234..83649fa83 100644 --- a/fuse_core/test/test_util.cpp +++ b/fuse_core/test/test_util.cpp @@ -35,9 +35,14 @@ #include #include +#include +#include -#include -#include +#include + +#include +#include +#include TEST(Util, wrapAngle2D) { @@ -72,6 +77,189 @@ TEST(Util, wrapAngle2D) } } +TEST(Util, jacobianRpyQuatYawRotation) +{ + const double rpy[3] = { // NOLINT(whitespace/braces) + 0.0, + 0.0, + 90 * M_PI / 180. + }; + + double q_computed[4]; + double rpy_computed[3]; + + double J_rpy2q[12]; + double J_q2rpy[12]; + + fuse_core::rpy2quaternion(rpy, q_computed, J_rpy2q); + + fuse_core::quaternion2rpy(q_computed, rpy_computed, J_q2rpy); + + Eigen::Map> j_rpy2q_map(J_rpy2q); + Eigen::Map> j_q2rpy_map(J_q2rpy); + + EXPECT_NEAR(q_computed[0], 0.7071068, 1e-6); + EXPECT_NEAR(q_computed[1], 0, 1e-6); + EXPECT_NEAR(q_computed[2], 0, 1e-6); + EXPECT_NEAR(q_computed[3], 0.7071068, 1e-6); + + EXPECT_NEAR(rpy[0], rpy_computed[0], 1e-15); + EXPECT_NEAR(rpy[1], rpy_computed[1], 1e-15); + EXPECT_NEAR(rpy[2], rpy_computed[2], 1e-15); +} + +TEST(Util, jacobianRpyQuatFullRotation) +{ + const double rpy[3] = { // NOLINT(whitespace/braces) + 0.1, + 0.2, + 0.3 + }; + + double q_computed[4]; + double rpy_computed[3]; + + double J_rpy2q[12]; + double J_q2rpy[12]; + + fuse_core::rpy2quaternion(rpy, q_computed, J_rpy2q); + + fuse_core::quaternion2rpy(q_computed, rpy_computed, J_q2rpy); + + Eigen::Map> j_rpy2q_map(J_rpy2q); + Eigen::Map> j_q2rpy_map(J_q2rpy); + + EXPECT_NEAR(rpy[0], rpy_computed[0], 1e-15); + EXPECT_NEAR(rpy[1], rpy_computed[1], 1e-15); + EXPECT_NEAR(rpy[2], rpy_computed[2], 1e-15); +} + +TEST(Util, convertToPoseQuatCovariance) +{ + Eigen::Isometry3d p1 = Eigen::Isometry3d::Identity(); + p1.translation() = Eigen::Vector3d(1, 2, 3); + p1.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + fuse_core::Vector6d cov1_diagonal; + cov1_diagonal << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002; + + fuse_core::Matrix6d cov1(cov1_diagonal.asDiagonal()); + + // fuse's implementation + const auto cov1_quat = fuse_core::convertToPoseQuatCovariance(p1, cov1); + + const auto cov1_quat_to_rpy = fuse_core::convertToPoseRPYCovariance(p1, cov1_quat); + + // covariance_geometry as reference + // NOTE THAT AT COVARIANCE_GEOMETRY THE ORDER AT THE ORIENTATION COVARIANCE IS (X, Y, Z, W) + // COMPARED TO (W, X, Y, Z) AT FUSE! + + fuse_core::Matrix7d cov1_quat_in_cg_format = cov1_quat; + cov1_quat_in_cg_format.block<3, 3>(3, 3) = cov1_quat.block<3, 3>(4, 4); + cov1_quat_in_cg_format(6, 6) = cov1_quat(3, 3); + cov1_quat_in_cg_format.block<3, 1>(3, 6) = cov1_quat.block<3, 1>(4, 3); + cov1_quat_in_cg_format.block<1, 3>(6, 3) = cov1_quat.block<1, 3>(3, 4); + + covariance_geometry::PoseQuaternionCovarianceRPY cg_p1; + cg_p1.first.first = Eigen::Vector3d(p1.translation()); + cg_p1.first.second = Eigen::Quaterniond(p1.rotation()); + cg_p1.second = cov1; + + covariance_geometry::PoseQuaternionCovariance cg_p1_q; + covariance_geometry::Pose3DQuaternionCovarianceRPYTo3DQuaternionCovariance(cg_p1, cg_p1_q); + + covariance_geometry::PoseQuaternionCovariance cg_p1_q_initialized_from_fuse; + cg_p1_q_initialized_from_fuse.first.first = Eigen::Vector3d(p1.translation()); + cg_p1_q_initialized_from_fuse.first.second = Eigen::Quaterniond(p1.rotation()); + cg_p1_q_initialized_from_fuse.second = cov1_quat_in_cg_format; + + covariance_geometry::PoseQuaternionCovarianceRPY cg_p1_q_initialized_from_fuse_to_rpy; + covariance_geometry::Pose3DQuaternionCovarianceTo3DQuaternionCovarianceRPY( + cg_p1_q_initialized_from_fuse, cg_p1_q_initialized_from_fuse_to_rpy); + + EXPECT_MATRIX_NEAR(p1.translation(), cg_p1_q.first.first, 1e-15); + EXPECT_MATRIX_NEAR(Eigen::Quaterniond(p1.rotation()).coeffs(), cg_p1_q.first.second.coeffs(), 1e-15); + + EXPECT_MATRIX_NEAR(cov1_quat_in_cg_format, cg_p1_q.second, 1e-15); + EXPECT_MATRIX_NEAR(cov1_quat_to_rpy, cg_p1_q_initialized_from_fuse_to_rpy.second, 1e-15); + EXPECT_MATRIX_NEAR(cov1, cov1_quat_to_rpy, 1e-15); +} + +TEST(Util, invertPoseCovariance) +{ + Eigen::Isometry3d p1 = Eigen::Isometry3d::Identity(); + p1.translation() = Eigen::Vector3d(1, 2, 3); + p1.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + fuse_core::Vector6d cov1_diagonal; + cov1_diagonal << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002; + + fuse_core::Matrix6d cov1(cov1_diagonal.asDiagonal()); + + // fuse's implementation + const auto p1_inverse = p1.inverse(); + const auto cov1_inverse = fuse_core::invertPoseCovariance(p1, cov1); + + // covariance_geometry as reference + + covariance_geometry::PoseQuaternionCovarianceRPY cg_p1; + cg_p1.first.first = Eigen::Vector3d(p1.translation()); + cg_p1.first.second = Eigen::Quaterniond(p1.rotation()); + cg_p1.second = cov1; + + auto cg_p1_inverted = covariance_geometry::InversePose3DQuaternionCovarianceRPY(cg_p1); + + EXPECT_MATRIX_NEAR(p1_inverse.translation(), cg_p1_inverted.first.first, 1e-15); + EXPECT_MATRIX_NEAR(Eigen::Quaterniond(p1_inverse.rotation()).coeffs(), cg_p1_inverted.first.second.coeffs(), 1e-15); + EXPECT_MATRIX_NEAR(cov1_inverse, cg_p1_inverted.second, 1e-15); +} + +TEST(Util, composePoseCovariance) +{ + Eigen::Isometry3d p1 = Eigen::Isometry3d::Identity(); + p1.translation() = Eigen::Vector3d(1, 2, 3); + p1.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitZ())); + + fuse_core::Vector6d cov1_diagonal; + cov1_diagonal << 0.1, 0.01, 0.001, 0.01, 0.2, 0.002; + + fuse_core::Matrix6d cov1(cov1_diagonal.asDiagonal()); + + Eigen::Isometry3d p2 = Eigen::Isometry3d::Identity(); + p2.translation() = Eigen::Vector3d(4, 5, 6); + p2.rotate(Eigen::AngleAxisd(M_PI / 4, Eigen::Vector3d::UnitY())); + + fuse_core::Vector6d cov2_diagonal; + cov2_diagonal << 0.2, 0.02, 0.002, 0.02, 0.4, 0.004; + + fuse_core::Matrix6d cov2(cov2_diagonal.asDiagonal()); + + // fuse's implementation + const auto cov3 = fuse_core::composePoseCovariance( + p1, cov1, + p2, cov2); + const auto p3 = p1 * p2; + + // covariance_geometry as reference + covariance_geometry::PoseQuaternionCovarianceRPY cg_p1; + cg_p1.first.first = Eigen::Vector3d(p1.translation()); + cg_p1.first.second = Eigen::Quaterniond(p1.rotation()); + cg_p1.second = cov1; + + covariance_geometry::PoseQuaternionCovarianceRPY cg_p2; + cg_p2.first.first = Eigen::Vector3d(p2.translation()); + cg_p2.first.second = Eigen::Quaterniond(p2.rotation()); + cg_p2.second = cov2; + + covariance_geometry::PoseQuaternionCovarianceRPY cg_p3; + + covariance_geometry::ComposePoseQuaternionCovarianceRPY(cg_p1, cg_p2, cg_p3); + + EXPECT_MATRIX_NEAR(p3.translation(), cg_p3.first.first, 1e-12); + EXPECT_MATRIX_NEAR(Eigen::Quaterniond(p3.rotation()).coeffs(), cg_p3.first.second.coeffs(), 1e-12); + EXPECT_MATRIX_NEAR(cov3, cg_p3.second, 1e-12); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/fuse_doc/CHANGELOG.rst b/fuse_doc/CHANGELOG.rst index dcc5a286d..9b2552651 100644 --- a/fuse_doc/CHANGELOG.rst +++ b/fuse_doc/CHANGELOG.rst @@ -14,6 +14,15 @@ Changelog for package fuse_doc * Adding doxygen to all packages (#241) * Contributors: Gary Servin, Tom Moore +0.7.0 (2023-09-25) +------------------ +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* Adding doxygen to all packages (#241) +* Contributors: Gary Servin, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_doc/package.xml b/fuse_doc/package.xml index 49eedf614..0e955d84c 100644 --- a/fuse_doc/package.xml +++ b/fuse_doc/package.xml @@ -1,7 +1,7 @@ fuse_doc - 0.6.0 + 0.7.0 The fuse_doc package provides documentation and examples for the fuse package. diff --git a/fuse_graphs/CHANGELOG.rst b/fuse_graphs/CHANGELOG.rst index 8a1f93197..44b4742bd 100644 --- a/fuse_graphs/CHANGELOG.rst +++ b/fuse_graphs/CHANGELOG.rst @@ -36,6 +36,30 @@ Changelog for package fuse_graphs * Added a time-limited optimization option to the Graph class (#234) * Contributors: Gary Servin, Stephen Williams, Tom Moore +0.7.0 (2023-09-25) +------------------ +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* Adding doxygen to all packages (#241) +* [RST-3474] Created a getConstraintCosts() method + * Created a getConstraintCosts() method for reporting the costs and residuals of individual constraints +* [RST-2831] Support for optimization bounds (#235) +* Added a time-limited optimization option to the Graph class (#234) +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_graphs/CMakeLists.txt b/fuse_graphs/CMakeLists.txt index 7bcbea337..b81f46d5f 100644 --- a/fuse_graphs/CMakeLists.txt +++ b/fuse_graphs/CMakeLists.txt @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -109,7 +109,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_hash_graph PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -135,7 +135,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(benchmark_create_problem PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_graphs/package.xml b/fuse_graphs/package.xml index cc8d3231c..0f736bfea 100644 --- a/fuse_graphs/package.xml +++ b/fuse_graphs/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_graphs - 0.6.0 + 0.7.0 The fuse_graphs package provides some concrete implementations of the fuse_core::Graph interface. diff --git a/fuse_graphs/src/hash_graph.cpp b/fuse_graphs/src/hash_graph.cpp index 810ca9652..b5d421cbc 100644 --- a/fuse_graphs/src/hash_graph.cpp +++ b/fuse_graphs/src/hash_graph.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/CHANGELOG.rst b/fuse_loss/CHANGELOG.rst index 531d473e7..e322da5d3 100644 --- a/fuse_loss/CHANGELOG.rst +++ b/fuse_loss/CHANGELOG.rst @@ -14,6 +14,19 @@ Changelog for package fuse_loss * Adding doxygen to all packages (#241) * Contributors: Gary Servin, Tom Moore +0.7.0 (2023-09-25) +------------------ +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* Adding doxygen to all packages (#241) +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_loss/CMakeLists.txt b/fuse_loss/CMakeLists.txt index f71d4c341..edbb52ef2 100644 --- a/fuse_loss/CMakeLists.txt +++ b/fuse_loss/CMakeLists.txt @@ -58,7 +58,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -123,7 +123,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_loss_function PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -147,7 +147,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_composed_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -171,7 +171,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_huber_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -195,7 +195,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_tukey_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -227,7 +227,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_qwt_loss_plot PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -257,7 +257,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_scaled_loss PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_loss/package.xml b/fuse_loss/package.xml index e83d34e88..a6014e85b 100644 --- a/fuse_loss/package.xml +++ b/fuse_loss/package.xml @@ -1,7 +1,7 @@ fuse_loss - 0.6.0 + 0.7.0 The fuse_loss package provides a set of commonly used loss functions, such as the basic ones provided by Ceres. diff --git a/fuse_loss/src/arctan_loss.cpp b/fuse_loss/src/arctan_loss.cpp index 47520d783..9a275439c 100644 --- a/fuse_loss/src/arctan_loss.cpp +++ b/fuse_loss/src/arctan_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/cauchy_loss.cpp b/fuse_loss/src/cauchy_loss.cpp index e27261b6a..3b2d027c8 100644 --- a/fuse_loss/src/cauchy_loss.cpp +++ b/fuse_loss/src/cauchy_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/composed_loss.cpp b/fuse_loss/src/composed_loss.cpp index 28a69a5bb..554503a21 100644 --- a/fuse_loss/src/composed_loss.cpp +++ b/fuse_loss/src/composed_loss.cpp @@ -35,7 +35,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/dcs_loss.cpp b/fuse_loss/src/dcs_loss.cpp index 8d24cd672..3bd51c2fa 100644 --- a/fuse_loss/src/dcs_loss.cpp +++ b/fuse_loss/src/dcs_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/fair_loss.cpp b/fuse_loss/src/fair_loss.cpp index bebfe95b7..0ab13738c 100644 --- a/fuse_loss/src/fair_loss.cpp +++ b/fuse_loss/src/fair_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/geman_mcclure_loss.cpp b/fuse_loss/src/geman_mcclure_loss.cpp index f34d8ef3c..6924ad3ea 100644 --- a/fuse_loss/src/geman_mcclure_loss.cpp +++ b/fuse_loss/src/geman_mcclure_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/huber_loss.cpp b/fuse_loss/src/huber_loss.cpp index db68d3c7a..b2d3a7877 100644 --- a/fuse_loss/src/huber_loss.cpp +++ b/fuse_loss/src/huber_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/scaled_loss.cpp b/fuse_loss/src/scaled_loss.cpp index f38d5d3ac..f70d33baf 100644 --- a/fuse_loss/src/scaled_loss.cpp +++ b/fuse_loss/src/scaled_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_loss/src/softlone_loss.cpp b/fuse_loss/src/softlone_loss.cpp index 8a4fcbf22..fdf7f2909 100644 --- a/fuse_loss/src/softlone_loss.cpp +++ b/fuse_loss/src/softlone_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/tolerant_loss.cpp b/fuse_loss/src/tolerant_loss.cpp index d57277e82..426a2fc4a 100644 --- a/fuse_loss/src/tolerant_loss.cpp +++ b/fuse_loss/src/tolerant_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/trivial_loss.cpp b/fuse_loss/src/trivial_loss.cpp index b21defaa6..63877209f 100644 --- a/fuse_loss/src/trivial_loss.cpp +++ b/fuse_loss/src/trivial_loss.cpp @@ -33,7 +33,7 @@ */ #include -#include +#include #include #include diff --git a/fuse_loss/src/tukey_loss.cpp b/fuse_loss/src/tukey_loss.cpp index b9f74fc8d..d12753615 100644 --- a/fuse_loss/src/tukey_loss.cpp +++ b/fuse_loss/src/tukey_loss.cpp @@ -35,7 +35,7 @@ #include -#include +#include #include #include diff --git a/fuse_loss/src/welsch_loss.cpp b/fuse_loss/src/welsch_loss.cpp index fb5c72105..6780c30b9 100644 --- a/fuse_loss/src/welsch_loss.cpp +++ b/fuse_loss/src/welsch_loss.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_models/CHANGELOG.rst b/fuse_models/CHANGELOG.rst index fbe149c8b..5bbd38ef6 100644 --- a/fuse_models/CHANGELOG.rst +++ b/fuse_models/CHANGELOG.rst @@ -22,6 +22,31 @@ Changelog for package fuse_models * [RST-3451] Delay some transform warnings so startup is less chatty * Contributors: Gary Servin, Stephen Williams, Tom Moore +0.7.0 (2023-09-25) +------------------ +* [RST-7809] Fix optimization errors when the orientation is initialized at +PI (#334) + * Add some unit tests for the 2D orientation constraints; Create getters/setters for the 2D orientation variable is preparation for a fix. + * Force the 2D orientation value to be is minimum phase +* add missing tf timeout at differential mode of IMU, odometry, and pose sensor model (#322) +* Minor header fixes (#266) + * Use fuse_macros.h instead of deprecated macros.h + * Add missed header + * Sort headers +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Adding doxygen to all packages (#241) +* [RST-3451] Delay some transform warnings so startup is less chatty +* Contributors: Enrique Fernandez Perdomo, Gary Servin, Stephen Williams, Tom Moore, fabianhirmann + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index 4df78e450..fb282155c 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -86,14 +86,20 @@ 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_3d.cpp src/odometry_2d_publisher.cpp + src/odometry_3d_publisher.cpp src/pose_2d.cpp src/transaction.cpp src/twist_2d.cpp src/unicycle_2d.cpp src/unicycle_2d_ignition.cpp src/unicycle_2d_state_kinematic_constraint.cpp + src/unicycle_3d.cpp + src/unicycle_3d_ignition.cpp + src/unicycle_3d_state_kinematic_constraint.cpp ) add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} @@ -113,7 +119,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -165,7 +171,22 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_unicycle_2d PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + + add_rostest_gtest( + test_unicycle_3d + test/unicycle_3d.test + test/test_unicycle_3d.cpp + ) + target_link_libraries(test_unicycle_3d + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ) + set_target_properties(test_unicycle_3d + PROPERTIES + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -181,7 +202,22 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_unicycle_2d_predict PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + + catkin_add_gtest( + test_unicycle_3d_predict + test/test_unicycle_3d_predict.cpp + ) + target_link_libraries(test_unicycle_3d_predict + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ) + set_target_properties(test_unicycle_3d_predict + PROPERTIES + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -198,7 +234,23 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_unicycle_2d_state_cost_function PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + + catkin_add_gtest( + test_unicycle_3d_state_cost_function + test/test_unicycle_3d_state_cost_function.cpp + ) + target_link_libraries( + test_unicycle_3d_state_cost_function + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ) + set_target_properties(test_unicycle_3d_state_cost_function + PROPERTIES + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -224,7 +276,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_graph_ignition PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -240,7 +292,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_unicycle_2d_ignition PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -255,7 +307,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_sensor_proc PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -276,7 +328,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(benchmark_unicycle_2d_state_cost_function PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_models/fuse_plugins.xml b/fuse_models/fuse_plugins.xml index 6a6da558a..a97fe8a0a 100644 --- a/fuse_models/fuse_plugins.xml +++ b/fuse_models/fuse_plugins.xml @@ -12,6 +12,13 @@ + + + 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::Odometry message and broadcasts a tf transform for optimized 2D @@ -20,6 +27,14 @@ + + + Publisher plugin that publishes a nav_msgs::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 +55,24 @@ acceleration constraints from IMU sensor data published by another node + + + An adapter-type sensor that produces 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 +95,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 unicycle 3D motion model. + + diff --git a/fuse_models/include/fuse_models/common/sensor_config.h b/fuse_models/include/fuse_models/common/sensor_config.h index ad6968c73..0ead8ad6f 100644 --- a/fuse_models/include/fuse_models/common/sensor_config.h +++ b/fuse_models/include/fuse_models/common/sensor_config.h @@ -42,6 +42,11 @@ #include #include #include +#include +#include +#include +#include +#include #include #include @@ -90,6 +95,28 @@ std::enable_if_t::value, size_t> toIndex(const std::string& dime return 0u; } +/** + * @brief Method that converts from 3D linear axis dimension names to index values + * + * This method is enabled only for variables that contain _only_ 3D linear quantities + * + * @param[in] dimension - The dimension name to convert + * @return the index of the enumerated dimension for that type + * @throws runtime_error if the dimension name is invalid + */ +template +std::enable_if_t::value, size_t> toIndex(const std::string& dimension) +{ + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "x") return static_cast(T::X); + if (lower_dim == "y") return static_cast(T::Y); + if (lower_dim == "z") return static_cast(T::Z); + + throwDimensionError(dimension); + + return 0u; +} + /** * @brief Method that converts from 2D angular axis dimension names to index values * @@ -110,6 +137,60 @@ std::enable_if_t::value, size_t> toIndex(const std::string& dim return 0u; } +/** + * @brief Method that converts from 3D angular axis dimension names to index values + * + * This method is enabled only for variables that contain _only_ 3D angular quantities + * + * @param[in] dimension - The dimension name to convert + * @return the index of the enumerated dimension for that type + * @throws runtime_error if the dimension name is invalid + */ +template +std::enable_if_t::value && !is_orientation::value, size_t> toIndex(const std::string & dimension) +{ + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "roll" || lower_dim == "x") + { + return static_cast(T::ROLL); + } + if (lower_dim == "pitch" || lower_dim == "y") + { + return static_cast(T::PITCH); + } + if (lower_dim == "yaw" || lower_dim == "z") + { + return static_cast(T::YAW); + } + + throwDimensionError(dimension); + + return 0u; +} + +template +std::enable_if_t::value && is_orientation::value, size_t> toIndex(const std::string & dimension) +{ + // Trick to get roll, pitch, yaw indexes as 0, 1, 2 + auto lower_dim = boost::algorithm::to_lower_copy(dimension); + if (lower_dim == "roll" || lower_dim == "x") + { + return static_cast(fuse_variables::Orientation3DStamped::Euler::ROLL) - 4U; + } + if (lower_dim == "pitch" || lower_dim == "y") + { + return static_cast(fuse_variables::Orientation3DStamped::Euler::PITCH) - 4U; + } + if (lower_dim == "yaw" || lower_dim == "z") + { + return static_cast(fuse_variables::Orientation3DStamped::Euler::YAW) - 4U; + } + + throwDimensionError(dimension); + + return 0u; +} + /** * @brief Utility method to convert a vector of dimension names to a vector of dimension indices * @@ -139,6 +220,28 @@ std::vector getDimensionIndices(const std::vector& dimensio return indices; } +/** + * @brief Utility method to convert a vector of (integer) indices to a vector of Euler enum values + * + * Effectively this converts the integer indices 0,1,2 to Euler::ROLL, Euler::PITCH, Euler::YAW, respectively + * + * @param[in] indices - The indices vector to convert + * @return a vector of Euler enum values + */ +template +std::vector toEulerEnumVector(const std::vector& indices) +{ + using Euler = fuse_variables::Orientation3DStamped::Euler; + + std::vector euler_indices; + euler_indices.reserve(indices.size()); + + std::transform(indices.begin(), indices.end(), std::back_inserter(euler_indices), + [](int value) { return static_cast(value + 4U); }); + + return euler_indices; +} + } // namespace common } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/common/sensor_proc.h b/fuse_models/include/fuse_models/common/sensor_proc.h index b985038d9..6996177e5 100644 --- a/fuse_models/include/fuse_models/common/sensor_proc.h +++ b/fuse_models/include/fuse_models/common/sensor_proc.h @@ -35,7 +35,9 @@ #define FUSE_MODELS_COMMON_SENSOR_PROC_H #include +#include #include +#include #include #include #include @@ -43,10 +45,13 @@ #include #include #include +#include #include +#include #include #include #include +#include #include #include @@ -59,6 +64,7 @@ #include #include #include +#include #include @@ -313,7 +319,7 @@ inline bool processAbsolutePoseWithCovariance( auto orientation = fuse_variables::Orientation2DStamped::make_shared(pose.header.stamp, device_id); position->x() = absolute_pose_2d.x(); position->y() = absolute_pose_2d.y(); - orientation->yaw() = absolute_pose_2d.yaw(); + orientation->setYaw(absolute_pose_2d.yaw()); // Create the pose for the constraint fuse_core::Vector3d pose_mean; @@ -374,6 +380,120 @@ 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] 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::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 ros::Duration& tf_timeout = ros::Duration(0, 0)) +{ + if (position_indices.empty() && orientation_indices.empty()) + { + return false; + } + + geometry_msgs::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)) + { + ROS_WARN_STREAM_DELAYED_THROTTLE( + 10.0, + "Failed to transform pose message with stamp " << pose.header.stamp << ". Cannot create constraint."); + return false; + } + } + + // Create the pose variable + auto position = fuse_variables::Position3DStamped::make_shared(pose.header.stamp, device_id); + auto orientation = fuse_variables::Orientation3DStamped::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; + 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; + + // Create the pose for the constraint + fuse_core::Vector6d pose_mean; + pose_mean << position->x(), position->y(), position->z(), + orientation->roll(), orientation->pitch(), orientation->yaw(); + + // Create the covariance for the constraint + Eigen::Map pose_covariance(transformed_message.pose.covariance.data()); + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd pose_mean_partial(position_indices.size() + orientation_indices.size()); + fuse_core::MatrixXd pose_covariance_partial(pose_mean_partial.rows(), pose_mean_partial.rows()); + + const auto indices = mergeIndices(position_indices, orientation_indices, position->size()); + + populatePartialMeasurement(pose_mean, pose_covariance, indices, pose_mean_partial, pose_covariance_partial); + + if (validate) + { + try + { + validatePartialMeasurement(pose_mean_partial, pose_covariance_partial); + } + catch (const std::runtime_error& ex) + { + ROS_ERROR_STREAM_THROTTLE(10.0, "Invalid partial absolute pose measurement from '" << source + << "' source: " << ex.what()); + return false; + } + } + + // Create an absolute pose constraint + auto constraint = fuse_constraints::AbsolutePose3DStampedEulerConstraint::make_shared( + source, + *position, + *orientation, + pose_mean_partial, + pose_covariance_partial, + position_indices, + toEulerEnumVector(orientation_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 * @@ -432,13 +552,13 @@ inline bool processDifferentialPoseWithCovariance( fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); position1->x() = pose1_2d.x(); position1->y() = pose1_2d.y(); - orientation1->yaw() = pose1_2d.yaw(); + orientation1->setYaw(pose1_2d.yaw()); auto position2 = fuse_variables::Position2DStamped::make_shared(pose2.header.stamp, device_id); auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(pose2.header.stamp, device_id); position2->x() = pose2_2d.x(); position2->y() = pose2_2d.y(); - orientation2->yaw() = pose2_2d.yaw(); + orientation2->setYaw(pose2_2d.yaw()); // Create the delta for the constraint const double sy = ::sin(-pose1_2d.yaw()); @@ -721,6 +841,215 @@ 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 indepent 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] 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::PoseWithCovarianceStamped& pose1, + const geometry_msgs::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) +{ + if (position_indices.empty() && orientation_indices.empty()) + { + return false; + } + + // 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->w() = pose1.pose.pose.orientation.w; + orientation1->x() = pose1.pose.pose.orientation.x; + orientation1->y() = pose1.pose.pose.orientation.y; + orientation1->z() = pose1.pose.pose.orientation.z; + + 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->w() = pose2.pose.pose.orientation.w; + orientation2->x() = pose2.pose.pose.orientation.x; + orientation2->y() = pose2.pose.pose.orientation.y; + orientation2->z() = pose2.pose.pose.orientation.z; + + // Method adapted from https://github.com/locusrobotics/fuse/blob/90ded4ba333b426ad56d25eddf575c9b6027dfa7/fuse_models/include/fuse_models/common/sensor_proc.hpp#L1003 NOLINT(whitespace/line_length) + + // Convert from ROS msg to Eigen + Eigen::Isometry3d p1, p2; + tf2::fromMsg(pose1.pose.pose, p1); + const Eigen::Map> cov1(pose1.pose.covariance.data()); + tf2::fromMsg(pose2.pose.pose, p2); + const Eigen::Map> cov2(pose2.pose.covariance.data()); + + Eigen::Isometry3d p12 = p1.inverse() * p2; + fuse_core::Matrix6d cov12; + + if (independent) + { + const auto cov1_inverse = fuse_core::invertPoseCovariance(p1, cov1); + + cov12 = fuse_core::composePoseCovariance( + p1.inverse(), cov1_inverse, + p2, cov2); + } + 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 (cov1.isApprox(cov2, 1e-9)) + { + cov12.setZero(); + } + else + { + // Here we assume that poses are computed incrementally, so: p2 = p1 * p12. + // We know cov1 and cov2 and we should subtract 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) + fuse_core::Matrix7d cov1_q = fuse_core::convertToPoseQuatCovariance(p1, cov1); + fuse_core::Matrix7d cov2_q = fuse_core::convertToPoseQuatCovariance(p2, cov2); + + // Now we have to compute pose composition jacobians so we can rotate covariances + const auto j_qn = fuse_core::jacobianQuatNormalization(Eigen::Quaterniond(p12.rotation())); + + auto j_p1 = fuse_core::jacobianPosePoseCompositionA(p1, p12); + j_p1.block<4, 4>(3, 3).applyOnTheLeft(j_qn); + j_p1.block<4, 3>(3, 0).setZero(); + + auto j_p12 = fuse_core::jacobianPosePoseCompositionB(p1); + 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 + const auto j_p12_inv = j_p12.colPivHouseholderQr().solve(fuse_core::Matrix7d::Identity()); + const auto cov12_q = j_p12_inv * (cov2_q - j_p1 * cov1_q * j_p1.transpose()) * j_p12_inv.transpose(); + + // Now again convert the delta pose covariance back to RPY(6x6) + cov12 = fuse_core::convertToPoseRPYCovariance(p12, cov12_q); + } + } + + Eigen::Quaterniond delta_q(p12.rotation()); + double delta_q_array[4] = {delta_q.w(), // NOLINT(whitespace/braces) + delta_q.x(), + delta_q.y(), + delta_q.z()}; // NOLINT(whitespace/braces) + + double delta_rpy[3]; + fuse_core::quaternion2rpy(delta_q_array, delta_rpy); + + fuse_core::Vector6d pose_relative_mean; + pose_relative_mean << + p12.translation().x(), p12.translation().y(), p12.translation().z(), + delta_rpy[0], delta_rpy[1], delta_rpy[2]; + + fuse_core::Matrix6d pose_relative_covariance = cov12 + minimum_pose_relative_covariance; + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd pose_relative_mean_partial(position_indices.size() + orientation_indices.size()); + fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), + pose_relative_mean_partial.rows()); + + const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); + + populatePartialMeasurement( + pose_relative_mean, + pose_relative_covariance, + indices, + pose_relative_mean_partial, + pose_relative_covariance_partial); + + if (validate) + { + try + { + validatePartialMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-6); + } + catch (const std::runtime_error& ex) + { + ROS_ERROR_STREAM_THROTTLE(10.0, "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, + position_indices, + toEulerEnumVector(orientation_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 * @@ -780,13 +1109,13 @@ inline bool processDifferentialPoseWithTwistCovariance( fuse_variables::Orientation2DStamped::make_shared(pose1.header.stamp, device_id); position1->x() = pose1_2d.x(); position1->y() = pose1_2d.y(); - orientation1->yaw() = pose1_2d.yaw(); + orientation1->setYaw(pose1_2d.yaw()); auto position2 = fuse_variables::Position2DStamped::make_shared(pose2.header.stamp, device_id); auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(pose2.header.stamp, device_id); position2->x() = pose2_2d.x(); position2->y() = pose2_2d.y(); - orientation2->yaw() = pose2_2d.yaw(); + orientation2->setYaw(pose2_2d.yaw()); // Create the delta for the constraint const auto delta = pose1_2d.inverseTimes(pose2_2d); @@ -910,51 +1239,246 @@ 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. The pose delta is calculated as + * + * pose_relative = pose_absolute1^-1 * pose_absolute2 + * + * Additionally, the twist covariance of the last message is used to compute the relative pose covariance using the time + * difference between the pose_absolute2 and pose_absolute1 time stamps. This assumes the pose measurements are + * dependent. A small minimum relative covariance is added to avoid getting a zero or ill-conditioned covariance. This + * could happen if the twist covariance is very small, e.g. when the twist is zero. * * @param[in] source - The name of the sensor or motion model that generated this constraint * @param[in] device_id - The UUID of the machine - * @param[in] twist - The TwistWithCovarianceStamped message from which we will extract the twist data - * @param[in] linear_velocity_loss - The loss function for the 2D linear velocity constraint generated - * @param[in] angular_velocity_loss - The loss function for the 2D angular velocity constraint generated - * @param[in] target_frame - The frame ID into which the twist data will be transformed before it is used - * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] pose1 - The first (and temporally earlier) PoseWithCovarianceStamped message + * @param[in] pose2 - The second (and temporally later) PoseWithCovarianceStamped message + * @param[in] twist - The second (and temporally later) TwistWithCovarianceStamped message + * @param[in] minimum_pose_relative_covariance - The minimum pose relative covariance that is always added to the + * resulting pose relative covariance + * @param[in] twist_covariance_offset - The twist covariance offset that was added to the twist covariance and must be + * substracted from it before computing the pose relative covariance from it + * @param[in] loss - The loss function for the 3D pose constraint generated * @param[in] validate - Whether to validate the measurements or not. If the validation fails no constraint is added * @param[out] transaction - The generated variables and constraints are added to this transaction * @return true if any constraints were added, false otherwise */ -inline bool processTwistWithCovariance( +inline bool processDifferentialPoseWithTwist3DCovariance( const std::string& source, const fuse_core::UUID& device_id, + const geometry_msgs::PoseWithCovarianceStamped& pose1, + const geometry_msgs::PoseWithCovarianceStamped& pose2, const geometry_msgs::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 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, - const ros::Duration& tf_timeout = ros::Duration(0, 0)) + fuse_core::Transaction& transaction) { - // Make sure we actually have work to do - if (linear_indices.empty() && angular_indices.empty()) + if (position_indices.empty() && orientation_indices.empty()) { return false; } - geometry_msgs::TwistWithCovarianceStamped transformed_message; - if (target_frame.empty()) - { - transformed_message = twist; - } - else - { - transformed_message.header.frame_id = target_frame; + // 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->w() = pose1.pose.pose.orientation.w; + orientation1->x() = pose1.pose.pose.orientation.x; + orientation1->y() = pose1.pose.pose.orientation.y; + orientation1->z() = pose1.pose.pose.orientation.z; + + 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->w() = pose2.pose.pose.orientation.w; + orientation2->x() = pose2.pose.pose.orientation.x; + orientation2->y() = pose2.pose.pose.orientation.y; + orientation2->z() = pose2.pose.pose.orientation.z; + + // Create the delta for the constraint + tf2::Transform pose1_tf; + tf2::fromMsg(pose1.pose.pose, pose1_tf); + tf2::Transform pose2_tf; + tf2::fromMsg(pose2.pose.pose, pose2_tf); + + const auto delta = pose1_tf.inverseTimes(pose2_tf); + + double delta_quat[4] = {delta.getRotation().w(), // NOLINT(whitespace/braces) + delta.getRotation().x(), + delta.getRotation().y(), + delta.getRotation().z()}; // NOLINT(whitespace/braces) + + double delta_rpy[3]; + fuse_core::quaternion2rpy(delta_quat, delta_rpy); + + fuse_core::Vector6d pose_relative_mean; + pose_relative_mean << + delta.getOrigin().x(), delta.getOrigin().y(), delta.getOrigin().z(), + delta_rpy[0], delta_rpy[1], delta_rpy[2]; + + // Create the covariance components for the constraint + Eigen::Map cov(twist.twist.covariance.data()); + + // For dependent pose measurements p1 and p2, we assume they're computed as: + // + // p2 = p1 * p12 [1] + // + // where p12 is the relative pose between p1 and p2, which is computed here as: + // + // p12 = p1^-1 * p2 + // + // Note that the twist t12 is computed as: + // + // t12 = p12 / dt + // + // where dt = t2 - t1, for t1 and t2 being the p1 and p2 timestamps, respectively. + // + // Therefore, the relative pose p12 is computed as follows given the twist t12: + // + // p12 = t12 * dt + // + // The covariance propagation of this equation is: + // + // C12 = J_t12 * T12 * J_t12^T [2] + // + // where T12 is the twist covariance and J_t12 is the jacobian of the equation wrt to t12. + // + // The jacobian wrt t12 is: + // + // J_t12 = dt * Id + // + // where Id is a 3x3 Identity matrix. + // + // In some cases the twist covariance T12 is very small and it could yield to an ill-conditioned C12 covariance. For + // that reason a minimum covariance is added to [2]. + // + // It is also common that for the same reason, the twist covariance T12 already has a minimum covariance offset added + // to it by the publisher, so we have to remove it before using it. + const auto dt = (pose2.header.stamp - pose1.header.stamp).toSec(); + + if (dt < 1e-6) + { + ROS_ERROR_STREAM_THROTTLE(10.0, "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; + + // Build the sub-vector and sub-matrices based on the requested indices + fuse_core::VectorXd pose_relative_mean_partial(position_indices.size() + orientation_indices.size()); + fuse_core::MatrixXd pose_relative_covariance_partial(pose_relative_mean_partial.rows(), + pose_relative_mean_partial.rows()); + + const auto indices = mergeIndices(position_indices, orientation_indices, position1->size()); + + populatePartialMeasurement( + pose_relative_mean, + pose_relative_covariance, + indices, + pose_relative_mean_partial, + pose_relative_covariance_partial); + + if (validate) + { + try + { + validatePartialMeasurement(pose_relative_mean_partial, pose_relative_covariance_partial, 1e-6); + } + catch (const std::runtime_error& ex) + { + ROS_ERROR_STREAM_THROTTLE(10.0, "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, + position_indices, + toEulerEnumVector(orientation_indices)); + + constraint->loss(loss); + + transaction.addVariable(position1); + transaction.addVariable(orientation1); + transaction.addVariable(position2); + transaction.addVariable(orientation2); + transaction.addConstraint(constraint); + transaction.addInvolvedStamp(pose1.header.stamp); + transaction.addInvolvedStamp(pose2.header.stamp); + + return true; +} + +/** + * @brief Extracts velocity data from a TwistWithCovarianceStamped and adds that data to a fuse Transaction + * + * This method effectively adds two variables (2D linear velocity and 2D angular velocity) and their respective + * constraints to the given \p transaction. The velocity data is extracted from the \p twist message. Only 2D data is + * used. The data will be automatically transformed into the \p target_frame before it is used. + * + * @param[in] source - The name of the sensor or motion model that generated this constraint + * @param[in] device_id - The UUID of the machine + * @param[in] twist - The TwistWithCovarianceStamped message from which we will extract the twist data + * @param[in] linear_velocity_loss - The loss function for the 2D linear velocity constraint generated + * @param[in] angular_velocity_loss - The loss function for the 2D angular velocity constraint generated + * @param[in] target_frame - The frame ID into which the twist data will be transformed before it is used + * @param[in] tf_buffer - The transform buffer with which we will lookup the required transform + * @param[in] validate - Whether to validate the measurements or not. If the validation fails no constraint is added + * @param[out] transaction - The generated variables and constraints are added to this transaction + * @return true if any constraints were added, false otherwise + */ +inline bool processTwistWithCovariance( + const std::string& source, + const fuse_core::UUID& device_id, + const geometry_msgs::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 ros::Duration& tf_timeout = ros::Duration(0, 0)) +{ + // Make sure we actually have work to do + if (linear_indices.empty() && angular_indices.empty()) + { + return false; + } + + geometry_msgs::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)) { @@ -1077,6 +1601,195 @@ 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] 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::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 ros::Duration& tf_timeout = ros::Duration(0, 0)) +{ + // Make sure we actually have work to do + if (linear_indices.empty() && angular_indices.empty()) + { + return false; + } + + geometry_msgs::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)) + { + ROS_WARN_STREAM_DELAYED_THROTTLE( + 10.0, + "Failed to transform twist message with stamp " << twist.header.stamp << ". 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 covariances for the constraints + fuse_core::Matrix3d linear_vel_covariance; + linear_vel_covariance << + transformed_message.twist.covariance[0], transformed_message.twist.covariance[1], transformed_message.twist.covariance[2], // NOLINT + transformed_message.twist.covariance[6], transformed_message.twist.covariance[7], transformed_message.twist.covariance[8], // NOLINT + transformed_message.twist.covariance[12], transformed_message.twist.covariance[13], transformed_message.twist.covariance[14]; // NOLINT + + // 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, + 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); + } + catch (const std::runtime_error& ex) + { + ROS_ERROR_STREAM_THROTTLE(10.0, "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; + + fuse_core::Matrix3d angular_vel_covariance; + angular_vel_covariance << + transformed_message.twist.covariance[21], transformed_message.twist.covariance[22], transformed_message.twist.covariance[23], // NOLINT + transformed_message.twist.covariance[27], transformed_message.twist.covariance[28], transformed_message.twist.covariance[29], // NOLINT + transformed_message.twist.covariance[33], transformed_message.twist.covariance[34], transformed_message.twist.covariance[35]; // NOLINT + + // 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_covariance, + 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); + } + catch (const std::runtime_error& ex) + { + ROS_ERROR_STREAM_THROTTLE(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 * @@ -1185,6 +1898,116 @@ 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] 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::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 ros::Duration& tf_timeout = ros::Duration(0, 0)) +{ + // Make sure we actually have work to do + if (indices.empty()) + { + return false; + } + + geometry_msgs::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)) + { + ROS_WARN_STREAM_DELAYED_THROTTLE( + 10.0, + "Failed to transform acceleration message with stamp " << acceleration.header.stamp + << ". Cannot create constraint."); + 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 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; + + fuse_core::Matrix3d accel_covariance; + accel_covariance << + transformed_message.accel.covariance[0], transformed_message.accel.covariance[1], transformed_message.accel.covariance[2], // NOLINT + transformed_message.accel.covariance[6], transformed_message.accel.covariance[7], transformed_message.accel.covariance[8], // NOLINT + transformed_message.accel.covariance[12], transformed_message.accel.covariance[13], transformed_message.accel.covariance[14]; // NOLINT + + // 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, indices, accel_mean_partial, accel_covariance_partial); + + if (validate) + { + try + { + validatePartialMeasurement(accel_mean_partial, accel_covariance_partial); + } + catch (const std::runtime_error& ex) + { + ROS_ERROR_STREAM_THROTTLE(10.0, "Invalid partial linear acceleration measurement from '" + << source << "' source: " << ex.what()); + return false; + } + } + + // 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 * @@ -1220,6 +2043,46 @@ inline void scaleProcessNoiseCovariance(fuse_core::Matrix8d& process_noise_covar velocity * process_noise_covariance.topLeftCorner<3, 3>() * velocity.transpose(); } +/** + * @brief Scales the process noise covariance pose by the norm of the velocity + * + * @param[in, out] process_noise_covariance - The process noise covariance to scale. Only the pose components (x, y, + * z, roll, pitch, yaw) are scaled, and they are assumed to be in the top + * left 6x6 corner + * @param[in] velocity_linear - The linear velocity + * @param[in] velocity_angular - The angular velocity + * @param[in] velocity_norm_min - The minimum velocity norm + */ +inline void scaleProcessNoiseCovariance(fuse_core::Matrix15d& process_noise_covariance, + const tf2::Vector3& velocity_linear, const tf2::Vector3& velocity_angular, + const double velocity_norm_min) +{ + // A more principled approach would be to get the current velocity from the state, make a diagonal matrix from it, + // and then rotate it to be in the world frame (i.e., the same frame as the pose data). We could then use this + // rotated velocity matrix to scale the process noise covariance for the pose variables as + // rotatedVelocityMatrix * poseCovariance * rotatedVelocityMatrix' + // However, this presents trouble for robots that may incur rotational error as a result of linear motion (and + // vice-versa). Instead, we create a diagonal matrix whose diagonal values are the vector norm of the state's + // velocity. We use that to scale the process noise covariance. + // + // The comment above has been taken from: + // https://github.com/cra-ros-pkg/robot_localization/blob/melodic-devel/src/filter_base.cpp#L138-L144 + // + // We also need to make sure the norm is not zero, because otherwise the resulting process noise covariance for the + // pose becomes zero and we get NaN when we compute the inverse to obtain the information + fuse_core::Matrix6d velocity; + velocity.setIdentity(); + + fuse_core::Vector6d velocity_values; + velocity_values << velocity_linear.x(), velocity_linear.y(), velocity_linear.z(), + velocity_angular.x(), velocity_angular.y(), velocity_angular.z(); + + velocity.diagonal() *= std::max(velocity_norm_min, velocity_values.norm()); + + process_noise_covariance.topLeftCorner<6, 6>() = + velocity * process_noise_covariance.topLeftCorner<6, 6>() * velocity.transpose(); +} + } // namespace common } // namespace fuse_models diff --git a/fuse_models/include/fuse_models/common/variable_traits.h b/fuse_models/include/fuse_models/common/variable_traits.h index 338b8cf9e..60754b995 100644 --- a/fuse_models/include/fuse_models/common/variable_traits.h +++ b/fuse_models/include/fuse_models/common/variable_traits.h @@ -39,6 +39,11 @@ #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,42 @@ 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_2d.h b/fuse_models/include/fuse_models/imu_2d.h index d9257df91..7a3671b19 100644 --- a/fuse_models/include/fuse_models/imu_2d.h +++ b/fuse_models/include/fuse_models/imu_2d.h @@ -34,13 +34,14 @@ #ifndef FUSE_MODELS_IMU_2D_H #define FUSE_MODELS_IMU_2D_H -#include #include +#include #include #include #include +#include #include #include #include diff --git a/fuse_models/include/fuse_models/imu_3d.h b/fuse_models/include/fuse_models/imu_3d.h new file mode 100644 index 000000000..c829e10f3 --- /dev/null +++ b/fuse_models/include/fuse_models/imu_3d.h @@ -0,0 +1,159 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_MODELS_IMU_3D_H + +#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::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::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 Callback for pose messages + * @param[in] msg - The IMU message to process + */ + void process(const sensor_msgs::Imu::ConstPtr& 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::PoseWithCovarianceStamped& pose, + const geometry_msgs::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + ParameterType params_; + + std::unique_ptr previous_pose_; + + tf2_ros::Buffer tf_buffer_; + + tf2_ros::TransformListener tf_listener_; + + ros::Subscriber subscriber_; + + using ImuThrottledCallback = fuse_core::ThrottledMessageCallback; + ImuThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS_IMU_3D_H diff --git a/fuse_models/include/fuse_models/odometry_3d.h b/fuse_models/include/fuse_models/odometry_3d.h new file mode 100644 index 000000000..dd55c5cfb --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d.h @@ -0,0 +1,155 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_MODELS_ODOMETRY_3D_H + +#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::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::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 Callback for pose messages + * @param[in] msg - The pose message to process + */ + void process(const nav_msgs::Odometry::ConstPtr& 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::PoseWithCovarianceStamped& pose, + const geometry_msgs::TwistWithCovarianceStamped& twist, const bool validate, + fuse_core::Transaction& transaction); + + ParameterType params_; + + std::unique_ptr previous_pose_; + + tf2_ros::Buffer tf_buffer_; + + tf2_ros::TransformListener tf_listener_; + + ros::Subscriber subscriber_; + + using OdometryThrottledCallback = fuse_core::ThrottledMessageCallback; + OdometryThrottledCallback throttled_callback_; +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS_ODOMETRY_3D_H diff --git a/fuse_models/include/fuse_models/odometry_3d_publisher.h b/fuse_models/include/fuse_models/odometry_3d_publisher.h new file mode 100644 index 000000000..40662ae21 --- /dev/null +++ b/fuse_models/include/fuse_models/odometry_3d_publisher.h @@ -0,0 +1,230 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_MODELS_ODOMETRY_3D_PUBLISHER_H + +#include + +#include +#include +#include +#include +#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::Odometry message and broadcasts a tf transform for optimized 3D + * state data (combination of Position3DStamped, Orientation3DStamped, VelocityLinear3DStamped, and + * VelocityAngular3DStamped, 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 unicycle 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::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::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; + +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 ros::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::Odometry& odometry, + geometry_msgs::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(const ros::TimerEvent& event); + + /** + * @brief Object that searches for the most recent common timestamp for a set of variables + */ + using Synchronizer = fuse_publishers::StampedVariableSynchronizer; + + fuse_core::UUID device_id_; //!< The UUID of this device + + ParameterType params_; + + ros::Time latest_stamp_; + + ros::Time latest_covariance_stamp_; + + bool latest_covariance_valid_{ false }; //!< Whether the latest covariance computed is valid or not + + nav_msgs::Odometry odom_output_; + + geometry_msgs::AccelWithCovarianceStamped acceleration_output_; + + Synchronizer synchronizer_; //!< Object that tracks the latest common timestamp of multiple variables + + std::unique_ptr tf_buffer_; + + ros::Publisher odom_pub_; + + ros::Publisher acceleration_pub_; + + tf2_ros::TransformBroadcaster tf_broadcaster_; + + 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 + + ros::Timer publish_timer_; + + ros::CallbackQueue publish_timer_callback_queue_; //!< A dedicated callback queue for the publish timer + ros::NodeHandle publish_timer_node_handle_; //!< A dedicated node handle for the publish timer, so it can use + //!< its own callback queue + ros::AsyncSpinner publish_timer_spinner_; //!< A dedicated async spinner for the publish timer that manages + //!< its callback queue with a dedicated thread + + 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_H diff --git a/fuse_models/include/fuse_models/parameters/imu_3d_params.h b/fuse_models/include/fuse_models/parameters/imu_3d_params.h new file mode 100644 index 000000000..03128e3f6 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/imu_3d_params.h @@ -0,0 +1,154 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_MODELS_PARAMETERS_IMU_3D_PARAMS_H + +#include + +#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] nh - The ROS node handle with which to load parameters + */ + void loadFromROS(const ros::NodeHandle& nh) final + { + angular_velocity_indices = + loadSensorConfig(nh, "angular_velocity_dimensions"); + linear_acceleration_indices = + loadSensorConfig(nh, "linear_acceleration_dimensions"); + orientation_indices = loadSensorConfig(nh, "orientation_dimensions"); + + nh.getParam("differential", differential); + nh.getParam("disable_checks", disable_checks); + nh.getParam("queue_size", queue_size); + nh.getParam("tcp_no_delay", tcp_no_delay); + fuse_core::getPositiveParam(nh, "tf_timeout", tf_timeout, false); + + fuse_core::getPositiveParam(nh, "throttle_period", throttle_period, false); + nh.getParam("throttle_use_wall_time", throttle_use_wall_time); + + nh.getParam("remove_gravitational_acceleration", remove_gravitational_acceleration); + nh.getParam("gravitational_acceleration", gravitational_acceleration); + fuse_core::getParamRequired(nh, "topic", topic); + + if (differential) + { + nh.getParam("independent", independent); + nh.getParam("use_twist_covariance", use_twist_covariance); + + minimum_pose_relative_covariance = + fuse_core::getCovarianceDiagonalParam<6>(nh, "minimum_pose_relative_covariance_diagonal", 0.0); + twist_covariance_offset = + fuse_core::getCovarianceDiagonalParam<6>(nh, "twist_covariance_offset_diagonal", 0.0); + } + + minimum_linear_acceleration_covariance = + fuse_core::getCovarianceDiagonalParam<3>(nh, "minimum_linear_acceleration_covariance_diagonal", 0.0); + + minimum_angular_velocity_covariance = + fuse_core::getCovarianceDiagonalParam<3>(nh, "minimum_angular_velocity_covariance_diagonal", 0.0); + + minimum_orientation_covariance = + fuse_core::getCovarianceDiagonalParam<3>(nh, "minimum_orientation_covariance_diagonal", 0.0); + + nh.getParam("acceleration_target_frame", acceleration_target_frame); + nh.getParam("orientation_target_frame", orientation_target_frame); + nh.getParam("twist_target_frame", twist_target_frame); + + pose_loss = fuse_core::loadLossConfig(nh, "pose_loss"); + angular_velocity_loss = fuse_core::loadLossConfig(nh, "angular_velocity_loss"); + linear_acceleration_loss = fuse_core::loadLossConfig(nh, "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 + //!< subtracted in order to recover the raw values + fuse_core::Matrix3d minimum_linear_acceleration_covariance; //!< Minimum linear acceleration covariance matrix + fuse_core::Matrix3d minimum_angular_velocity_covariance; //!< Minimum angular velocity covariance matrix + fuse_core::Matrix3d minimum_orientation_covariance; //!< Minimum orientation covariance matrix + bool remove_gravitational_acceleration { false }; + int queue_size { 10 }; + bool tcp_no_delay { false }; //!< Whether to use TCP_NODELAY, i.e. disable Nagle's algorithm, in the subscriber + //!< socket or not. TCP_NODELAY forces a socket to send the data in its buffer, + //!< whatever the packet size. This reduces delay at the cost of network congestion, + //!< specially if the payload of a packet is smaller than the TCP header data. This is + //!< true for small ROS messages like geometry_msgs::AccelWithCovarianceStamped + ros::Duration tf_timeout { 0.0 }; //!< The maximum time to wait for a transform to become available + ros::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_H diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_params.h b/fuse_models/include/fuse_models/parameters/odometry_3d_params.h new file mode 100644 index 000000000..2004acce0 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_params.h @@ -0,0 +1,139 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_MODELS_PARAMETERS_ODOMETRY_3D_PARAMS_H + +#include + +#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] nh - The ROS node handle with which to load parameters + */ + void loadFromROS(const ros::NodeHandle& nh) final + { + position_indices = loadSensorConfig(nh, "position_dimensions"); + orientation_indices = loadSensorConfig(nh, "orientation_dimensions"); + linear_velocity_indices = + loadSensorConfig(nh, "linear_velocity_dimensions"); + angular_velocity_indices = + loadSensorConfig(nh, "angular_velocity_dimensions"); + + nh.getParam("differential", differential); + nh.getParam("disable_checks", disable_checks); + nh.getParam("queue_size", queue_size); + nh.getParam("tcp_no_delay", tcp_no_delay); + fuse_core::getPositiveParam(nh, "tf_timeout", tf_timeout, false); + + fuse_core::getPositiveParam(nh, "throttle_period", throttle_period, false); + nh.getParam("throttle_use_wall_time", throttle_use_wall_time); + + fuse_core::getParamRequired(nh, "topic", topic); + + nh.getParam("twist_target_frame", twist_target_frame); + nh.getParam("pose_target_frame", pose_target_frame); + + if (differential) + { + nh.getParam("independent", independent); + nh.getParam("use_twist_covariance", use_twist_covariance); + + minimum_pose_relative_covariance = + fuse_core::getCovarianceDiagonalParam<6>(nh, "minimum_pose_relative_covariance_diagonal", 0.0); + twist_covariance_offset = + fuse_core::getCovarianceDiagonalParam<6>(nh, "twist_covariance_offset_diagonal", 0.0); + } + + pose_loss = fuse_core::loadLossConfig(nh, "pose_loss"); + linear_velocity_loss = fuse_core::loadLossConfig(nh, "linear_velocity_loss"); + angular_velocity_loss = fuse_core::loadLossConfig(nh, "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 + //!< subtracted in order to recover the raw values + int queue_size { 10 }; + bool tcp_no_delay { false }; //!< Whether to use TCP_NODELAY, i.e. disable Nagle's algorithm, in the subscriber + //!< socket or not. TCP_NODELAY forces a socket to send the data in its buffer, + //!< whatever the packet size. This reduces delay at the cost of network congestion, + //!< specially if the payload of a packet is smaller than the TCP header data. This is + //!< true for small ROS messages like geometry_msgs::AccelWithCovarianceStamped + ros::Duration tf_timeout { 0.0 }; //!< The maximum time to wait for a transform to become available + ros::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_H diff --git a/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.h b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.h new file mode 100644 index 000000000..9021abc94 --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/odometry_3d_publisher_params.h @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE 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_H +#define FUSE_MODELS_PARAMETERS_ODOMETRY_3D_PUBLISHER_PARAMS_H + +#include + +#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] nh - The ROS node handle with which to load parameters + */ + void loadFromROS(const ros::NodeHandle& nh) final + { + nh.getParam("publish_tf", publish_tf); + nh.getParam("invert_tf", invert_tf); + nh.getParam("predict_to_current_time", predict_to_current_time); + nh.getParam("predict_with_acceleration", predict_with_acceleration); + nh.getParam("publish_frequency", publish_frequency); + + process_noise_covariance = fuse_core::getCovarianceDiagonalParam<15>(nh, "process_noise_diagonal", 0.0); + nh.param("scale_process_noise", scale_process_noise, scale_process_noise); + nh.param("velocity_norm_min", velocity_norm_min, velocity_norm_min); + + fuse_core::getPositiveParam(nh, "covariance_throttle_period", covariance_throttle_period, false); + + fuse_core::getPositiveParam(nh, "tf_cache_time", tf_cache_time, false); + fuse_core::getPositiveParam(nh, "tf_timeout", tf_timeout, false); + + nh.getParam("queue_size", queue_size); + + nh.getParam("map_frame_id", map_frame_id); + nh.getParam("odom_frame_id", odom_frame_id); + nh.getParam("base_link_frame_id", base_link_frame_id); + nh.param("base_link_output_frame_id", base_link_output_frame_id, base_link_frame_id); + nh.param("world_frame_id", world_frame_id, odom_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) + { + ROS_FATAL_STREAM("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); + } + + nh.getParam("topic", topic); + nh.getParam("acceleration_topic", acceleration_topic); + + fuse_core::loadCovarianceOptionsFromROS(ros::NodeHandle(nh, "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_norm_min{ 1e-3 }; + ros::Duration covariance_throttle_period { 0.0 }; //!< The throttle period duration in seconds to compute the + //!< covariance + ros::Duration tf_cache_time { 10.0 }; + ros::Duration tf_timeout { 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_H diff --git a/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.h b/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.h new file mode 100644 index 000000000..6e6a870ba --- /dev/null +++ b/fuse_models/include/fuse_models/parameters/unicycle_3d_ignition_params.h @@ -0,0 +1,183 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS_PARAMETERS_UNICYCLE_3D_IGNITION_PARAMS_H +#define FUSE_MODELS_PARAMETERS_UNICYCLE_3D_IGNITION_PARAMS_H + +#include + +#include +#include +#include + +#include +#include +#include +#include + + +namespace fuse_models +{ + +namespace parameters +{ + +/** + * @brief Defines the set of parameters required by the Unicycle3DIgnition class + */ +struct Unicycle3DIgnitionParams : public ParameterBase +{ + public: + /** + * @brief Method for loading parameter values from ROS. + * + * @param[in] nh - The ROS node handle with which to load parameters + */ + void loadFromROS(const ros::NodeHandle& nh) final + { + nh.getParam("publish_on_startup", publish_on_startup); + nh.getParam("queue_size", queue_size); + nh.getParam("reset_service", reset_service); + nh.getParam("set_pose_service", set_pose_service); + nh.getParam("set_pose_deprecated_service", set_pose_deprecated_service); + nh.getParam("topic", topic); + + std::vector sigma_vector; + if (nh.getParam("initial_sigma", sigma_vector)) + { + 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; + if (nh.getParam("initial_state", state_vector)) + { + 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(nh, "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, // NOLINT(whitespace/braces) + 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 }; // NOLINT(whitespace/braces) + + /** + * @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, // NOLINT(whitespace/braces) + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0 }; // NOLINT(whitespace/braces) + + /** + * @brief Loss function + */ + fuse_core::Loss::SharedPtr loss; +}; + +} // namespace parameters + +} // namespace fuse_models + +#endif // FUSE_MODELS_PARAMETERS_UNICYCLE_3D_IGNITION_PARAMS_H diff --git a/fuse_models/include/fuse_models/unicycle_3d.h b/fuse_models/include/fuse_models/unicycle_3d.h new file mode 100644 index 000000000..88e881bda --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d.h @@ -0,0 +1,205 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS_UNICYCLE_3D_H +#define FUSE_MODELS_UNICYCLE_3D_H + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + + +namespace fuse_models +{ + +/** + * @brief A fuse_models 3D kinematic model that generates kinematic constraints between provided time stamps, and adds + * those constraints to the fuse graph. + * + * This class uses a unicycle kinematic model for the robot. It is equivalent to the motion model in the + * robot_localization state estimation nodes. + * + * Parameters: + * - ~device_id (uuid string, default: 00000000-0000-0000-0000-000000000000) The device/robot ID to publish + * - ~device_name (string) Used to generate the device/robot ID if the device_id is not provided + * - ~buffer_length (double) The length of the graph state buffer and state history, in seconds + * - ~process_noise_diagonal (vector of doubles) An 15-dimensional vector containing the diagonal values for the + * process noise covariance matrix. Variable order is (x, y, z, + * roll, pitch, yaw, x_vel, y_vel, z_vel, roll_vel, pitch_vel, yaw_vel, + * x_acc, y_acc, z_acc). + */ +class Unicycle3D : public fuse_core::AsyncMotionModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS_WITH_EIGEN(Unicycle3D); + + /** + * @brief Default constructor + * + * All plugins are required to have a constructor that accepts no arguments + */ + Unicycle3D(); + + /** + * @brief Destructor + */ + ~Unicycle3D() = default; + + 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 + tf2::Transform pose; //!< Map-frame pose + tf2::Vector3 velocity_linear; //!< Body-frame linear velocity + tf2::Vector3 velocity_angular; //!< Body-frame yaw velocity + tf2::Vector3 acceleration_linear; //!< Body-frame linear acceleration + + void print(std::ostream& stream = std::cout) const; + + /** + * @brief Validate the state components: pose, linear velocity, yaw velocity and linear acceleration. + * + * 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 ros::Time& beginning_stamp, + const ros::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 ros::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 rotated, scaled and multiplied by dt + */ + static void validateMotionModel(const StateHistoryElement& state1, const StateHistoryElement& state2, + const fuse_core::Matrix15d& process_noise_covariance); + + ros::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_norm_min_{ 1e-3 }; //!< The minimum velocity/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 Unicycle3D& unicycle_3d); + +} // namespace fuse_models + +#endif // FUSE_MODELS_UNICYCLE_3D_H diff --git a/fuse_models/include/fuse_models/unicycle_3d_ignition.h b/fuse_models/include/fuse_models/unicycle_3d_ignition.h new file mode 100644 index 000000000..ba0659815 --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_ignition.h @@ -0,0 +1,180 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS_UNICYCLE_3D_IGNITION_H +#define FUSE_MODELS_UNICYCLE_3D_IGNITION_H + +#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 unicycle 3D motion model. + * + * This class publishes a transaction that contains a prior on each state subvariable used in the unicycle 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 and 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 (x_vel, y_vel, z_vel, + * roll_vel, pitch_vel, yaw_vel, x_acc, y_acc and z_acc) 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 Unicycle3DIgnition : public fuse_core::AsyncSensorModel +{ +public: + FUSE_SMART_PTR_DEFINITIONS(Unicycle3DIgnition); + using ParameterType = parameters::Unicycle3DIgnitionParams; + + /** + * @brief Default constructor + * + * All plugins are required to have a constructor that accepts no arguments + */ + Unicycle3DIgnition(); + + /** + * @brief Destructor + */ + ~Unicycle3DIgnition() = default; + + /** + * @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 Unicycle3DIgnition 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 Unicycle3DIgnition 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::PoseWithCovarianceStamped::ConstPtr& msg); + + /** + * @brief Triggers the publication of a new prior transaction at the supplied pose + */ + bool setPoseServiceCallback(fuse_models::SetPose::Request& req, fuse_models::SetPose::Response& res); + + /** + * @brief Triggers the publication of a new prior transaction at the supplied pose + */ + bool setPoseDeprecatedServiceCallback( + fuse_models::SetPoseDeprecated::Request& req, + fuse_models::SetPoseDeprecated::Response&); + +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, yaw) + */ + void process(const geometry_msgs::PoseWithCovarianceStamped& pose); + + /** + * @brief Create and send a prior transaction based on the supplied pose + * + * The unicycle 3d state members not included in the pose message (x_vel, y_vel, yaw_vel, x_acc, y_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, yaw) + */ + void sendPrior(const geometry_msgs::PoseWithCovarianceStamped& pose); + + 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 + + ParameterType params_; //!< Object containing all of the configuration parameters + + ros::ServiceClient reset_client_; //!< Service client used to call the "reset" service on the optimizer + + ros::ServiceServer set_pose_service_; //!< ROS service server that receives SetPose requests + + ros::ServiceServer set_pose_deprecated_service_; //!< ROS service server that receives SetPoseDeprecated requests + + ros::Subscriber subscriber_; //!< ROS subscriber that receives PoseWithCovarianceStamped messages +}; + +} // namespace fuse_models + +#endif // FUSE_MODELS_UNICYCLE_3D_IGNITION_H diff --git a/fuse_models/include/fuse_models/unicycle_3d_predict.h b/fuse_models/include/fuse_models/unicycle_3d_predict.h new file mode 100644 index 000000000..aabf2e5ad --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_predict.h @@ -0,0 +1,806 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS_UNICYCLE_3D_PREDICT_H +#define FUSE_MODELS_UNICYCLE_3D_PREDICT_H + +#include +#include +#include +#include + +#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] roll1 - First roll orientation + * @param[in] pitch1 - First pitch orientation + * @param[in] yaw1 - 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_roll1 - First roll velocity + * @param[in] vel_pitch1 - First pitch velocity + * @param[in] vel_yaw1 - 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] roll2 - Second roll orientation + * @param[out] pitch2 - Second pitch orientation + * @param[out] yaw2 - 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_roll2 - Second roll velocity + * @param[out] vel_pitch2 - Second pitch velocity + * @param[out] vel_yaw2 - 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 roll1, + const T pitch1, + const T yaw1, + const T vel_linear1_x, + const T vel_linear1_y, + const T vel_linear1_z, + const T vel_roll1, + const T vel_pitch1, + const T vel_yaw1, + 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& roll2, + T& pitch2, + T& yaw2, + T& vel_linear2_x, + T& vel_linear2_y, + T& vel_linear2_z, + T& vel_roll2, + T& vel_pitch2, + T& vel_yaw2, + T& acc_linear2_x, + T& acc_linear2_y, + T& acc_linear2_z) +{ + // There are better models for this projection, but this matches the one used by r_l. + T sr = ceres::sin(roll1); + T cr = ceres::cos(roll1); + + T sp = ceres::sin(pitch1); + T cp = ceres::cos(pitch1); + T cpi = T(1.0) / cp; + T tp = sp * cpi; + + T sy = ceres::sin(yaw1); + T cy = ceres::cos(yaw1); + + position2_x = position1_x + vel_linear1_x * cy * cp * dt + + vel_linear1_y * (cy * sp * sr - sy * cr) * dt + + vel_linear1_z * (cy * sp * cr + sy * sr) * dt + + acc_linear1_x * T(0.5) * cy * cp * dt * dt + + acc_linear1_y * T(0.5) * (cy * sp * sr - sy * cr) * dt * dt + + acc_linear1_z * T(0.5) * (cy * sp * cr + sy * sr) * dt * dt; + + position2_y = position1_y + vel_linear1_x * sy * cp * dt + + vel_linear1_y * (sy * sp * sr + cy * cr) * dt + + vel_linear1_z * (sy * sp * cr - cy * sr) * dt + + acc_linear1_x * T(0.5) * sy * cp * dt * dt + + acc_linear1_y * T(0.5) * (sy * sp * sr + cy * cr) * dt * dt + + acc_linear1_z * T(0.5) * (sy * sp * cr - cy * sr) * dt * dt; + + position2_z = position1_z + vel_linear1_x * (-sp) * dt + + vel_linear1_y * cp * sr * dt + + vel_linear1_z * cp * cr * dt + + acc_linear1_x * T(0.5) * (-sp) * dt * dt + + acc_linear1_y * T(0.5) * cp * sr * dt * dt + + acc_linear1_z * T(0.5) * cp * cr * dt * dt; + + roll2 = roll1 + vel_roll1 * dt + + vel_pitch1 * sr * tp * dt + + vel_yaw1 * cr * tp * dt; + + pitch2 = pitch1 + vel_pitch1 * cr * dt + + vel_yaw1 * (-sr) * dt; + + yaw2 = yaw1 + vel_pitch1 * sr * cpi * dt + + vel_yaw1 * cr * cpi * 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_roll2 = vel_roll1; + vel_pitch2 = vel_pitch1; + vel_yaw2 = vel_yaw1; + + acc_linear2_x = acc_linear1_x; + acc_linear2_y = acc_linear1_y; + acc_linear2_z = acc_linear1_z; + + fuse_core::wrapAngle2D(roll2); + fuse_core::wrapAngle2D(pitch2); + fuse_core::wrapAngle2D(yaw2); +} + +/** + * @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] roll1 - First roll orientation + * @param[in] pitch1 - First pitch orientation + * @param[in] yaw1 - 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_roll1 - First roll velocity + * @param[in] vel_pitch1 - First pitch velocity + * @param[in] vel_yaw1 - 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] roll2 - Second roll orientation + * @param[out] pitch2 - Second pitch orientation + * @param[out] yaw2 - 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_roll2 - Second roll velocity + * @param[out] vel_pitch2 - Second pitch velocity + * @param[out] vel_yaw2 - 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 roll1, + const double pitch1, + const double yaw1, + const double vel_linear1_x, + const double vel_linear1_y, + const double vel_linear1_z, + const double vel_roll1, + const double vel_pitch1, + const double vel_yaw1, + 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& roll2, + double& pitch2, + double& yaw2, + double& vel_linear2_x, + double& vel_linear2_y, + double& vel_linear2_z, + double& vel_roll2, + double& vel_pitch2, + double& vel_yaw2, + double& acc_linear2_x, + double& acc_linear2_y, + double& acc_linear2_z, + double** jacobians) +{ + predict(position1_x, + position1_y, + position1_z, + roll1, + pitch1, + yaw1, + vel_linear1_x, + vel_linear1_y, + vel_linear1_z, + vel_roll1, + vel_pitch1, + vel_yaw1, + acc_linear1_x, + acc_linear1_y, + acc_linear1_z, + dt, + position2_x, + position2_y, + position2_z, + roll2, + pitch2, + yaw2, + vel_linear2_x, + vel_linear2_y, + vel_linear2_z, + vel_roll2, + vel_pitch2, + vel_yaw2, + acc_linear2_x, + acc_linear2_y, + acc_linear2_z); + + if (jacobians) + { + const double sr = ceres::sin(roll1); + const double cr = ceres::cos(roll1); + + const double sp = ceres::sin(pitch1); + const double cp = ceres::cos(pitch1); + const double cpi = 1.0 / cp; + const double tp = sp * cpi; + + const double sy = ceres::sin(yaw1); + const double cy = ceres::cos(yaw1); + + // Jacobian wrt position1 + if (jacobians[0]) + { + Eigen::Map> jacobian(jacobians[0]); + jacobian << 1, 0, 0, + 0, 1, 0, + 0, 0, 1, + 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, + 0, 0, 0, + 0, 0, 0; + } + + // Jacobian wrt orientation1 in roll, pitch, yaw (compared to quaternion components which are used from ceres) + if (jacobians[1]) + { + const auto dposx_droll = vel_linear1_y * (cy * sp * cr - sy * (-sr)) * dt + + vel_linear1_z * (cy * sp * (-sr) + sy * cr) * dt + + acc_linear1_y * 0.5 * (cy * sp * cr - sy * (-sr)) * dt * dt + + acc_linear1_z * 0.5 * (cy * sp * (-sr) + sy * cr) * dt * dt; + + const auto dposy_droll = vel_linear1_y * (sy * sp * cr + cy * (-sr)) * dt + + vel_linear1_z * (sy * sp * (-sr) - cy * cr) * dt + + acc_linear1_y * 0.5 * (sy * sp * cr + cy * (-sr)) * dt * dt + + acc_linear1_z * 0.5 * (sy * sp * (-sr) - cy * cr) * dt * dt; + + const auto dposz_droll = vel_linear1_y * cp * cr * dt + + vel_linear1_z * cp * (-sr) * dt + + acc_linear1_y * 0.5 * cp * cr * dt * dt + + acc_linear1_z * 0.5 * cp * (-sr) * dt * dt; + + const auto dposx_dpitch = vel_linear1_x * cy * (-sp) * dt + + vel_linear1_y * cy * cp * sr * dt + + vel_linear1_z * cy * cp * cr * dt + + acc_linear1_x * 0.5 * cy * (-sp) * dt * dt + + acc_linear1_y * 0.5 * cy * cp * sr * dt * dt + + acc_linear1_z * 0.5 * cy * cp * cr * dt * dt; + + const auto dposy_dpitch = vel_linear1_x * sy * (-sp) * dt + + vel_linear1_y * sy * cp * sr * dt + + vel_linear1_z * sy * cp * cr * dt + + acc_linear1_x * 0.5 * sy * (-sp) * dt * dt + + acc_linear1_y * 0.5 * sy * cp * sr * dt * dt + + acc_linear1_z * 0.5 * sy * cp * cr * dt * dt; + + const auto dposz_dpitch = vel_linear1_x * (-cp) * dt + + vel_linear1_y * (-sp) * sr * dt + + vel_linear1_z * (-sp) * cr * dt + + acc_linear1_x * 0.5 * (-cp) * dt * dt + + acc_linear1_y * 0.5 * (-sp) * sr * dt * dt + + acc_linear1_z * 0.5 * (-sp) * cr * dt * dt; + + const auto dposx_dyaw = vel_linear1_x * (-sy) * cp * dt + + vel_linear1_y * ((-sy) * sp * sr - cy * cr) * dt + + vel_linear1_z * ((-sy) * sp * cr + cy * sr) * dt + + acc_linear1_x * 0.5 * (-sy) * cp * dt * dt + + acc_linear1_y * 0.5 * ((-sy) * sp * sr - cy * cr) * dt * dt + + acc_linear1_z * 0.5 * ((-sy) * sp * cr + cy * sr) * dt * dt; + + const auto dposy_dyaw = vel_linear1_x * cy * cp * dt + + vel_linear1_y * (cy * sp * sr + (-sy) * cr) * dt + + vel_linear1_z * (cy * sp * cr - (-sy) * sr) * dt + + acc_linear1_x * 0.5 * cy * cp * dt * dt + + acc_linear1_y * 0.5 * (cy * sp * sr + (-sy) * cr) * dt * dt + + acc_linear1_z * 0.5 * (cy * sp * cr - (-sy) * sr) * dt * dt; + + const auto dposz_dyaw = 0.0; + + const auto droll_droll = 1.0 + + vel_pitch1 * cr * tp * dt + + vel_yaw1 * (-sr) * tp * dt; + + const auto dpitch_droll = vel_pitch1 * (-sr) * dt + + vel_yaw1 * (-cr) * dt; + + const auto dyaw_droll = vel_pitch1 * cr * cpi * dt + + vel_yaw1 * (-sr) * cpi * dt; + + const auto droll_dpitch = vel_pitch1 * sr * (1.0 / (cp * cp)) * dt + + vel_yaw1 * cr * (1.0 / (cp * cp)) * dt; + + const auto dpitch_dpitch = 1.0; + + const auto dyaw_dpitch = vel_pitch1 * sr * (sp / (cp * cp)) * dt + + vel_yaw1 * cr * (sp / (cp * cp)) * dt; + + const auto droll_dyaw = 0.0; + + const auto dpitch_dyaw = 0.0; + + const auto dyaw_dyaw = 1.0; + + Eigen::Map> jacobian(jacobians[1]); + + jacobian << dposx_droll, dposx_dpitch, dposx_dyaw, + dposy_droll, dposy_dpitch, dposy_dyaw, + dposz_droll, dposz_dpitch, dposz_dyaw, + droll_droll, droll_dpitch, droll_dyaw, + dpitch_droll, dpitch_dpitch, dpitch_dyaw, + dyaw_droll, dyaw_dpitch, dyaw_dyaw, + 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; + } + + // Jacobian wrt vel_linear1 + if (jacobians[2]) + { + const double dposx_dvel_linear_1_x = cy * cp * dt; + const double dposx_dvel_linear_1_y = (cy * sp * sr - sy * cr) * dt; + const double dposx_dvel_linear_1_z = (cy * sp * cr + sy * sr) * dt; + + const double dposy_dvel_linear_1_x = sy * cp * dt; + const double dposy_dvel_linear_1_y = (sy * sp * sr + cy * cr) * dt; + const double dposy_dvel_linear_1_z = (sy * sp * cr - cy * sr) * dt; + + const double dposz_dvel_linear_1_x = (-sp) * dt; + const double dposz_dvel_linear_1_y = cp * sr * dt; + const double dposz_dvel_linear_1_z = cp * cr * dt; + + Eigen::Map> jacobian(jacobians[2]); + jacobian << dposx_dvel_linear_1_x, dposx_dvel_linear_1_y, dposx_dvel_linear_1_z, + dposy_dvel_linear_1_x, dposy_dvel_linear_1_y, dposy_dvel_linear_1_z, + dposz_dvel_linear_1_x, dposz_dvel_linear_1_y, dposz_dvel_linear_1_z, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0; + } + + // Jacobian wrt vel_orientation1 + if (jacobians[3]) + { + Eigen::Map> jacobian(jacobians[3]); + jacobian << 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + dt, sr * tp * dt, cr * tp * dt, + 0, cr * dt, (-sr) * dt, + 0, sr * cpi * dt, cr * cpi * dt, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0; + } + + // Jacobian wrt acc_linear1 + if (jacobians[4]) + { + const double dposx_dacc_linear_1_x = 0.5 * cy * cp * dt * dt; + const double dposx_dacc_linear_1_y = 0.5 * (cy * sp * sr - sy * cr) * dt * dt; + const double dposx_dacc_linear_1_z = 0.5 * (cy * sp * cr + sy * sr) * dt * dt; + + const double dposy_dacc_linear_1_x = 0.5 * sy * cp * dt * dt; + const double dposy_dacc_linear_1_y = 0.5 * (sy * sp * sr + cy * cr) * dt * dt; + const double dposy_dacc_linear_1_z = 0.5 * (sy * sp * cr - cy * sr) * dt * dt; + + const double dposz_dacc_linear_1_x = 0.5 * (-sp) * dt * dt; + const double dposz_dacc_linear_1_y = 0.5 * cp * sr * dt * dt; + const double dposz_dacc_linear_1_z = 0.5 * cp * cr * dt * dt; + + Eigen::Map> jacobian(jacobians[4]); + jacobian << dposx_dacc_linear_1_x, dposx_dacc_linear_1_y, dposx_dacc_linear_1_z, + dposy_dacc_linear_1_x, dposy_dacc_linear_1_y, dposy_dacc_linear_1_z, + dposz_dacc_linear_1_x, dposz_dacc_linear_1_y, dposz_dacc_linear_1_z, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + dt, 0, 0, + 0, dt, 0, + 0, 0, dt, + 0, 0, 0, + 0, 0, 0, + 0, 0, 0, + 1, 0, 0, + 0, 1, 0, + 0, 0, 1; + } + } +} + +/** + * @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, qz at index 3) + * @param[in] vel_linear1 - First velocity (array with x at index 0, y at index 1, z at index 2) + * @param[in] vel_angular1 - First angular velocity (array with roll at index 0, pitch at index 1, yaw 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 angular velocity (array with roll at index 0, pitch at index 1, yaw 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] pose1 - The first 3D pose + * @param[in] vel_linear_1 - The first linear velocity + * @param[in] vel_angular1 - The first angular velocity + * @param[in] acc_linear1 - The first linear acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[in] pose2 - The second 3D pose + * @param[in] vel_linear_2 - The second linear velocity + * @param[in] vel_angular2 - The second angular velocity + * @param[in] acc_linear2 - The second linear acceleration + * @param[in] jacobian - The jacobian wrt the state + */ +inline void predict( + const tf2::Transform& pose1, + const tf2::Vector3& vel_linear1, + const tf2::Vector3& vel_angular1, + const tf2::Vector3& acc_linear1, + const double dt, + tf2::Transform& pose2, + tf2::Vector3& vel_linear2, + tf2::Vector3& vel_angular2, + tf2::Vector3& acc_linear2, + fuse_core::Matrix15d& jacobian) +{ + double x_pred {}; + double y_pred {}; + double z_pred {}; + double roll_pred {}; + double pitch_pred {}; + double yaw_pred {}; + double vel_linear_x_pred {}; + double vel_linear_y_pred {}; + double vel_linear_z_pred {}; + double vel_roll_pred {}; + double vel_pitch_pred {}; + double vel_yaw_pred {}; + double acc_linear_x_pred {}; + double acc_linear_y_pred {}; + double acc_linear_z_pred {}; + + // 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: 3 (because of roll, pitch, yaw instead of quaternion components), + // vel_linear1: 3, + // vel_orientation1: 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, 3, 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(); + } + + double quat[4] = {pose1.getRotation().w(), // NOLINT(whitespace/braces) + pose1.getRotation().x(), + pose1.getRotation().y(), + pose1.getRotation().z()}; // NOLINT(whitespace/braces) + + double rpy[3]; + fuse_core::quaternion2rpy(quat, rpy); + + predict( + pose1.getOrigin().x(), + pose1.getOrigin().y(), + pose1.getOrigin().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, + x_pred, + y_pred, + z_pred, + roll_pred, + pitch_pred, + yaw_pred, + vel_linear_x_pred, + vel_linear_y_pred, + vel_linear_z_pred, + vel_roll_pred, + vel_pitch_pred, + vel_yaw_pred, + acc_linear_x_pred, + acc_linear_y_pred, + acc_linear_z_pred, + jacobians.data()); + + pose2.setOrigin(tf2::Vector3{x_pred, y_pred, z_pred}); + + Eigen::Quaterniond orientation2 = Eigen::AngleAxisd(yaw_pred, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(pitch_pred, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(roll_pred, Eigen::Vector3d::UnitX()); + + pose2.setRotation({orientation2.x(), orientation2.y(), orientation2.z(), orientation2.w()}); + + vel_linear2.setX(vel_linear_x_pred); + vel_linear2.setY(vel_linear_y_pred); + vel_linear2.setZ(vel_linear_z_pred); + + vel_angular2.setX(vel_roll_pred); + vel_angular2.setY(vel_pitch_pred); + vel_angular2.setZ(vel_yaw_pred); + + acc_linear2.setX(acc_linear_x_pred); + acc_linear2.setY(acc_linear_y_pred); + acc_linear2.setZ(acc_linear_z_pred); + + jacobian << J[0], J[1], J[2], J[3], J[4]; +} + +/** + * @brief Given a state and time delta, predicts a new state + * @param[in] pose1 - The first 3D pose + * @param[in] vel_linear_1 - The first linear velocity + * @param[in] vel_angular1 - The first angular velocity + * @param[in] acc_linear1 - The first linear acceleration + * @param[in] dt - The time delta across which to predict the state + * @param[in] pose2 - The second 3D pose + * @param[in] vel_linear_2 - The second linear velocity + * @param[in] vel_angular2 - The second angular velocity + * @param[in] acc_linear2 - The second linear acceleration + */ +inline void predict( + const tf2::Transform& pose1, + const tf2::Vector3& vel_linear1, + const tf2::Vector3& vel_angular1, + const tf2::Vector3& acc_linear1, + const double dt, + tf2::Transform& pose2, + tf2::Vector3& vel_linear2, + tf2::Vector3& vel_angular2, + tf2::Vector3& acc_linear2) +{ + double x_pred {}; + double y_pred {}; + double z_pred {}; + double roll_pred {}; + double pitch_pred {}; + double yaw_pred {}; + double vel_linear_x_pred {}; + double vel_linear_y_pred {}; + double vel_linear_z_pred {}; + double vel_roll_pred {}; + double vel_pitch_pred {}; + double vel_yaw_pred {}; + double acc_linear_x_pred {}; + double acc_linear_y_pred {}; + double acc_linear_z_pred {}; + + const double roll1 = fuse_core::getRoll(pose1.getRotation().w(), + pose1.getRotation().x(), + pose1.getRotation().y(), + pose1.getRotation().z()); + + const double pitch1 = fuse_core::getPitch(pose1.getRotation().w(), + pose1.getRotation().x(), + pose1.getRotation().y(), + pose1.getRotation().z()); + + const double yaw1 = fuse_core::getYaw(pose1.getRotation().w(), + pose1.getRotation().x(), + pose1.getRotation().y(), + pose1.getRotation().z()); + + predict( + pose1.getOrigin().x(), + pose1.getOrigin().y(), + pose1.getOrigin().z(), + roll1, + pitch1, + yaw1, + 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, + x_pred, + y_pred, + z_pred, + roll_pred, + pitch_pred, + yaw_pred, + vel_linear_x_pred, + vel_linear_y_pred, + vel_linear_z_pred, + vel_roll_pred, + vel_pitch_pred, + vel_yaw_pred, + acc_linear_x_pred, + acc_linear_y_pred, + acc_linear_z_pred); + + pose2.setOrigin(tf2::Vector3{x_pred, y_pred, z_pred}); + + Eigen::Quaterniond orientation_pred_q = + Eigen::AngleAxisd(yaw_pred, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(pitch_pred, Eigen::Vector3d::UnitY()) * + Eigen::AngleAxisd(roll_pred, Eigen::Vector3d::UnitX()); + + pose2.setRotation(tf2::Quaternion{ orientation_pred_q.x(), // NOLINT(whitespace/braces) + orientation_pred_q.y(), + orientation_pred_q.z(), + orientation_pred_q.w()}); // NOLINT(whitespace/braces) + + vel_linear2.setX(vel_linear_x_pred); + vel_linear2.setY(vel_linear_y_pred); + vel_linear2.setZ(vel_linear_z_pred); + + vel_angular2.setX(vel_roll_pred); + vel_angular2.setY(vel_pitch_pred); + vel_angular2.setZ(vel_yaw_pred); + + acc_linear2.setX(acc_linear_x_pred); + acc_linear2.setY(acc_linear_y_pred); + acc_linear2.setZ(acc_linear_z_pred); +} + +} // namespace fuse_models + +#endif // FUSE_MODELS_UNICYCLE_3D_PREDICT_H diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.h b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.h new file mode 100644 index 000000000..989f81a5a --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_function.h @@ -0,0 +1,358 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS_UNICYCLE_3D_STATE_COST_FUNCTION_H +#define FUSE_MODELS_UNICYCLE_3D_STATE_COST_FUNCTION_H + +#include + +#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: + * x position + * y position + * z position + * roll (rotation about the x axis) + * pitch (rotation about the y axis) + * yaw (rotation about the z axis) + * x velocity + * y velocity + * z velocity + * roll velocity + * pitch velocity + * yaw velocity + * x acceleration + * y acceleration + * z acceleration + * + * 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 + * cost(x) = || [ y_t2 - proj(y_t1) ] || + * || [ z_t2 - proj(z_t1) ] || + * || [ roll_t2 - proj(roll_t1) ] || + * || [ pitch_t2 - proj(pitch_t1) ] || + * || [ yaw_t2 - proj(yaw_t1) ] || + * ||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 Unicycle3DStateCostFunction : 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) + */ + Unicycle3DStateCostFunction(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) + * 3 : vel_angular1 - First angular velocity (array with roll at index 0, pitch at index 1, yaw 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 roll at index 0, pitch at index 1, yaw at index 2) + * 9 : acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1, z at index 2) + * @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], // roll1 + orientation1_rpy[1], // pitch1 + orientation1_rpy[2], // yaw1 + parameters[2][0], // vel_linear1_x + parameters[2][1], // vel_linear1_y + parameters[2][2], // vel_linear1_z + parameters[3][0], // vel_angular1 + parameters[3][1], // vel_angular1 + parameters[3][2], // vel_angular1 + 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); + + // Because the above function computes the jacobian wrt rpy orientation, it needs to be converted to + // quaternion orientation. + // This is still the case even although local parameterization is used because the jacobian matrix is the global + // size (so 15x4) and then later for optimization the jacobian internally gets updated with the jacobian of the + // transformation function from the global to the local size. + // See ceres 2.0.0: internal/ceres/residual_block.cc::143 + // Also see: https://github.com/ceres-solver/ceres-solver/issues/387 + if (jacobians && jacobians[1]) + { + // Note that this only works without an out-of-bounds memory access because jacobians[1] is originally 15x4 and + // the rpy jacobian is smaller (15x3) while vice versa a segfault might occur because of an out-of-bounds access + Eigen::Map> jacobian_orientation_rpy(jacobians[1]); + Eigen::Map> jacobian_orientation_q(jacobians[1]); + + Eigen::Map> j_quat2rpy_map(j1_quat2rpy); + + jacobian_orientation_q = jacobian_orientation_rpy * j_quat2rpy_map; + } + + 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) + { + // It might be possible to simplify the code below implementing something like this but using compile-time + // template recursion. + // + // // state1: (position1, orientation1, vel_linear1, vel_orientation1, acc_linear1) + // for (size_t i = 0; i < 5; ++i) + // { + // if (jacobians[i]) + // { + // Eigen::Map> jacobian(jacobians[i]); + // jacobian.applyOnTheLeft(-A_); + // } + // } + + // Update jacobian wrt position1 + if (jacobians[0]) + { + Eigen::Map> jacobian(jacobians[0]); + jacobian.applyOnTheLeft(-A_); + } + + // Update jacobian wrt orientation1 + if (jacobians[1]) + { + // Although local parameterization is used, the jacobian matrix is still the global size (so 15x4) and + // then later for optimization the jacobian internally gets updated with the jacobian of the transformation + // function from the global to the local size (see ceres 2.0.0: internal/ceres/residual_block.cc::143) + // Also see: https://github.com/ceres-solver/ceres-solver/issues/387 + + 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_orientation1 + 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_); + } + + // It might be possible to simplify the code below implementing something like this but using compile-time + // template recursion. + // + // // state2: (position2, orientation2, vel_linear2, vel_orientation2, acc_linear2) + // for (size_t i = 5, offset = 0; i < ParameterDims::kNumParameterBlocks; ++i) + // { + // constexpr auto dim = ParameterDims::GetDim(i); + + // if (jacobians[i]) + // { + // Eigen::Map> jacobian(jacobians[i]); + // jacobian = A_.block<15, dim>(0, offset); + // } + + // offset += dim; + // } + + // Jacobian wrt position2 + if (jacobians[5]) + { + Eigen::Map> jacobian(jacobians[5]); + jacobian = A_.block<15, 3>(0, 0); + } + + // Jacobian wrt orientation2 + if (jacobians[6]) + { + // Although local parameterization is used, the jacobian matrix is still the global size (so 15x4) and + // then later for optimization the jacobian internally gets updated with the jacobian of the transformation + // function from the global to the local size (see ceres 2.0.0: internal/ceres/residual_block.cc::143) + // Also see: https://github.com/ceres-solver/ceres-solver/issues/387 + + 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_orientation2 + 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 +}; + +Unicycle3DStateCostFunction::Unicycle3DStateCostFunction(const double dt, const fuse_core::Matrix15d& A) : + dt_(dt), + A_(A) +{ +} + +} // namespace fuse_models + +#endif // FUSE_MODELS_UNICYCLE_3D_STATE_COST_FUNCTION_H diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.h b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.h new file mode 100644 index 000000000..691913b2f --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_state_cost_functor.h @@ -0,0 +1,231 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS_UNICYCLE_3D_STATE_COST_FUNCTOR_H +#define FUSE_MODELS_UNICYCLE_3D_STATE_COST_FUNCTOR_H + +#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: + * x position + * y position + * z position + * roll (rotation about the x axis) + * pitch (rotation about the y axis) + * yaw (rotation about the z axis) + * x velocity + * y velocity + * z velocity + * roll velocity + * pitch velocity + * yaw velocity + * x acceleration + * y acceleration + * z acceleration + * + * 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 + * cost(x) = || [ y_t2 - proj(y_t1) ] || + * || [ z_t2 - proj(z_t1) ] || + * || [ roll_t2 - proj(roll_t1) ] || + * || [ pitch_t2 - proj(pitch_t1) ] || + * || [ yaw_t2 - proj(yaw_t1) ] || + * ||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 Unicycle3DStateCostFunctor +{ +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) + */ + Unicycle3DStateCostFunctor(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) + * @param[in] orientation1 - First orientation + * @param[in] vel_linear1 - First linear velocity (array with x at index 0, y at index 1) + * @param[in] vel_angular1 - First angular velocity + * @param[in] acc_linear1 - First linear acceleration (array with x at index 0, y at index 1) + * @param[in] position2 - Second position (array with x at index 0, y at index 1) + * @param[in] orientation2 - Second orientation + * @param[in] vel_linear2 - Second linear velocity (array with x at index 0, y at index 1) + * @param[in] vel_angular2 - Second angular velocity + * @param[in] acc_linear2 - Second linear acceleration (array with x at index 0, y at index 1) + * @param[out] residual - The computed residual (error) + */ + 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 +}; + +Unicycle3DStateCostFunctor::Unicycle3DStateCostFunctor(const double dt, const fuse_core::Matrix15d& A) : + dt_(dt), + A_(A) +{ +} + +template +bool Unicycle3DStateCostFunctor::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] { // NOLINT(whitespace/braces) + 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] { // NOLINT(whitespace/braces) + 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]; + + fuse_core::wrapAngle2D(residuals_map(3)); + fuse_core::wrapAngle2D(residuals_map(4)); + fuse_core::wrapAngle2D(residuals_map(5)); + + 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]; + + // Scale the residuals by the square root information matrix to account for + // the measurement uncertainty. + residuals_map.applyOnTheLeft(A_.template cast()); + + return true; +} + +} // namespace fuse_models + +#endif // FUSE_MODELS_UNICYCLE_3D_STATE_COST_FUNCTOR_H diff --git a/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.h b/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.h new file mode 100644 index 000000000..d0d782715 --- /dev/null +++ b/fuse_models/include/fuse_models/unicycle_3d_state_kinematic_constraint.h @@ -0,0 +1,181 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_MODELS_UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_H +#define FUSE_MODELS_UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_H + +#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 Unicycle3DStateKinematicConstraint : public fuse_core::Constraint +{ +public: + FUSE_CONSTRAINT_DEFINITIONS_WITH_EIGEN(Unicycle3DStateKinematicConstraint); + + /** + * @brief Default constructor + */ + Unicycle3DStateKinematicConstraint() = 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] linear_velocity1 Linear velocity component variable of the first state + * @param[in] angular_velocity1 Agnular velocity component variable of the first state + * @param[in] linear_acceleration1 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] linear_velocity2 Linear velocity component variable of the second state + * @param[in] angular_velocity2 Angular velocity component variable of the second state + * @param[in] linear_acceleration2 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) + */ + Unicycle3DStateKinematicConstraint( + const std::string& source, + const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::VelocityLinear3DStamped& linear_velocity1, + const fuse_variables::VelocityAngular3DStamped& angular_velocity1, + const fuse_variables::AccelerationLinear3DStamped& linear_acceleration1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_variables::VelocityLinear3DStamped& linear_velocity2, + const fuse_variables::VelocityAngular3DStamped& angular_velocity2, + const fuse_variables::AccelerationLinear3DStamped& linear_acceleration2, + const fuse_core::Matrix15d& covariance); + + /** + * @brief Destructor + */ + virtual ~Unicycle3DStateKinematicConstraint() = 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::Unicycle3DStateKinematicConstraint); + +#endif // FUSE_MODELS_UNICYCLE_3D_STATE_KINEMATIC_CONSTRAINT_H diff --git a/fuse_models/package.xml b/fuse_models/package.xml index 98a3b842e..2c32acfce 100644 --- a/fuse_models/package.xml +++ b/fuse_models/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_models - 0.6.0 + 0.7.0 fuse plugins that implement various kinematic and sensor models Tom Moore diff --git a/fuse_models/src/acceleration_2d.cpp b/fuse_models/src/acceleration_2d.cpp index 46019e89b..63e314325 100644 --- a/fuse_models/src/acceleration_2d.cpp +++ b/fuse_models/src/acceleration_2d.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include diff --git a/fuse_models/src/graph_ignition.cpp b/fuse_models/src/graph_ignition.cpp index 5977b34ae..80d5b56f1 100644 --- a/fuse_models/src/graph_ignition.cpp +++ b/fuse_models/src/graph_ignition.cpp @@ -36,7 +36,7 @@ #include -#include +#include #include #include diff --git a/fuse_models/src/imu_2d.cpp b/fuse_models/src/imu_2d.cpp index ed5b20f10..24a4ae0e1 100644 --- a/fuse_models/src/imu_2d.cpp +++ b/fuse_models/src/imu_2d.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_models/src/imu_3d.cpp b/fuse_models/src/imu_3d.cpp new file mode 100644 index 000000000..280934262 --- /dev/null +++ b/fuse_models/src/imu_3d.cpp @@ -0,0 +1,282 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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), + tf_listener_(tf_buffer_), + throttled_callback_(std::bind(&Imu3D::process, this, std::placeholders::_1)) +{ +} + +void Imu3D::onInit() +{ + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + + params_.loadFromROS(private_node_handle_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + throttled_callback_.setUseWallTime(params_.throttle_use_wall_time); + + if (params_.orientation_indices.empty() && + params_.linear_acceleration_indices.empty() && + params_.angular_velocity_indices.empty()) + { + ROS_WARN_STREAM("No dimensions were specified. Data from topic " << ros::names::resolve(params_.topic) << + " will be ignored."); + } +} + +void Imu3D::onStart() +{ + if (!params_.orientation_indices.empty() || + !params_.linear_acceleration_indices.empty() || + !params_.angular_velocity_indices.empty()) + { + previous_pose_.reset(); + subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, + &ImuThrottledCallback::callback, &throttled_callback_, + ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); + } +} + +void Imu3D::onStop() +{ + subscriber_.shutdown(); +} + +void Imu3D::process(const sensor_msgs::Imu::ConstPtr& 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; + + Eigen::Map orientation_covariance(msg->orientation_covariance.data()); + + Eigen::Map> + pose_msg_covariance(pose->pose.covariance.data() + 21); + + pose_msg_covariance = orientation_covariance.cwiseMax(params_.minimum_orientation_covariance); + + // Preprocess the twist data + geometry_msgs::TwistWithCovarianceStamped twist; + twist.header = msg->header; + twist.twist.twist.angular = msg->angular_velocity; + + Eigen::Map angular_velocity_covariance(msg->angular_velocity_covariance.data()); + + Eigen::Map> + twist_msg_covariance(twist.twist.covariance.data() + 21); + + twist_msg_covariance = angular_velocity_covariance.cwiseMax(params_.minimum_angular_velocity_covariance); + + 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::AccelWithCovarianceStamped accel; + accel.header = msg->header; + accel.accel.accel.linear = msg->linear_acceleration; + + Eigen::Map linear_acceleration_covariance(msg->linear_acceleration_covariance.data()); + + Eigen::Map> + acceleration_msg_covariance(accel.accel.covariance.data()); + + acceleration_msg_covariance = linear_acceleration_covariance.cwiseMax(params_.minimum_linear_acceleration_covariance); + + // Optionally remove the acceleration due to gravity + if (params_.remove_gravitational_acceleration) + { + geometry_msgs::Vector3 accel_gravity; + accel_gravity.z = params_.gravitational_acceleration; + geometry_msgs::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::PoseWithCovarianceStamped& pose, + const geometry_msgs::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)) + { + ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform pose message with stamp " << pose.header.stamp + << " 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::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, params_.tf_timeout)) + { + ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform twist message with stamp " << twist.header.stamp + << " to twist target frame " + << params_.twist_target_frame); + } + else + { + common::processDifferentialPoseWithTwist3DCovariance( + 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_2d.cpp b/fuse_models/src/odometry_2d.cpp index 663631c70..73bc06d18 100644 --- a/fuse_models/src/odometry_2d.cpp +++ b/fuse_models/src/odometry_2d.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_models/src/odometry_2d_publisher.cpp b/fuse_models/src/odometry_2d_publisher.cpp index 28d744f09..f8af5e04c 100644 --- a/fuse_models/src/odometry_2d_publisher.cpp +++ b/fuse_models/src/odometry_2d_publisher.cpp @@ -41,7 +41,7 @@ #include #include -#include +#include #include #include #include @@ -288,7 +288,7 @@ bool Odometry2DPublisher::getState( odometry.pose.pose.position.x = position_variable.x(); odometry.pose.pose.position.y = position_variable.y(); odometry.pose.pose.position.z = 0.0; - odometry.pose.pose.orientation = tf2::toMsg(tf2_2d::Rotation(orientation_variable.yaw())); + odometry.pose.pose.orientation = tf2::toMsg(tf2_2d::Rotation(orientation_variable.getYaw())); odometry.twist.twist.linear.x = velocity_linear_variable.x(); odometry.twist.twist.linear.y = velocity_linear_variable.y(); odometry.twist.twist.linear.z = 0.0; diff --git a/fuse_models/src/odometry_3d.cpp b/fuse_models/src/odometry_3d.cpp new file mode 100644 index 000000000..aec404ca2 --- /dev/null +++ b/fuse_models/src/odometry_3d.cpp @@ -0,0 +1,229 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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), + tf_listener_(tf_buffer_), + throttled_callback_(std::bind(&Odometry3D::process, this, std::placeholders::_1)) +{ +} + +void Odometry3D::onInit() +{ + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + + params_.loadFromROS(private_node_handle_); + + throttled_callback_.setThrottlePeriod(params_.throttle_period); + throttled_callback_.setUseWallTime(params_.throttle_use_wall_time); + + if (params_.position_indices.empty() && + params_.orientation_indices.empty() && + params_.linear_velocity_indices.empty() && + params_.angular_velocity_indices.empty()) + { + ROS_WARN_STREAM("No dimensions were specified. Data from topic " << ros::names::resolve(params_.topic) << + " will be ignored."); + } +} + +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(); + subscriber_ = node_handle_.subscribe(ros::names::resolve(params_.topic), params_.queue_size, + &OdometryThrottledCallback::callback, &throttled_callback_, + ros::TransportHints().tcpNoDelay(params_.tcp_no_delay)); + } +} + +void Odometry3D::onStop() +{ + subscriber_.shutdown(); +} + +void Odometry3D::process(const nav_msgs::Odometry::ConstPtr& 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::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::PoseWithCovarianceStamped& pose, + const geometry_msgs::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, params_.tf_timeout)) + { + ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform pose message with stamp " + << pose.header.stamp << " 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::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, params_.tf_timeout)) + { + ROS_WARN_STREAM_THROTTLE(5.0, "Cannot transform twist message with stamp " << twist.header.stamp + << " to twist target frame " + << params_.twist_target_frame); + } + else + { + common::processDifferentialPoseWithTwist3DCovariance( + 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..a1ab2b694 --- /dev/null +++ b/fuse_models/src/odometry_3d_publisher.cpp @@ -0,0 +1,686 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 + +// 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), + latest_stamp_(Synchronizer::TIME_ZERO), + latest_covariance_stamp_(Synchronizer::TIME_ZERO), + publish_timer_spinner_(1, &publish_timer_callback_queue_) +{ +} + +void Odometry3DPublisher::onInit() +{ + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + + params_.loadFromROS(private_node_handle_); + + if (!params_.invert_tf && params_.world_frame_id == params_.map_frame_id) + { + tf_buffer_ = std::make_unique(params_.tf_cache_time); + tf_listener_ = std::make_unique(*tf_buffer_, node_handle_); + } + + odom_pub_ = node_handle_.advertise(ros::names::resolve(params_.topic), params_.queue_size); + acceleration_pub_ = node_handle_.advertise( + ros::names::resolve(params_.acceleration_topic), params_.queue_size); + + publish_timer_node_handle_.setCallbackQueue(&publish_timer_callback_queue_); + + publish_timer_ = publish_timer_node_handle_.createTimer( + ros::Duration(1.0 / params_.publish_frequency), + &Odometry3DPublisher::publishTimerCallback, + this, + false, + false); + + publish_timer_spinner_.start(); +} + +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 (latest_stamp == Synchronizer::TIME_ZERO) + { + { + std::lock_guard lock(mutex_); + latest_stamp_ = latest_stamp; + } + + ROS_WARN_STREAM_THROTTLE( + 10.0, "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::Odometry odom_output; + geometry_msgs::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 + ros::Time latest_covariance_stamp = latest_covariance_stamp_; + bool latest_covariance_valid = latest_covariance_valid_; + if (odom_pub_.getNumSubscribers() > 0 || acceleration_pub_.getNumSubscribers() > 0) + { + // Throttle covariance computation + if (params_.covariance_throttle_period.isZero() || + 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); + + // position + { + size_t covariance_matrix_counter = 0; + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 0; col_i < 3; ++col_i) + { + odom_output.pose.covariance[col_i * 6 + row_i] = covariance_matrices[0][covariance_matrix_counter]; + ++covariance_matrix_counter; + } + } + } + + // position <-> orientation + { + size_t covariance_matrix_counter = 0; + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 3; col_i < 6; ++col_i) + { + odom_output.pose.covariance[col_i * 6 + row_i] = covariance_matrices[1][covariance_matrix_counter]; + ++covariance_matrix_counter; + } + } + } + + // orientation <-> position + { + size_t covariance_matrix_counter = 0; + for (size_t col_i = 0; col_i < 3; ++col_i) + { + for (size_t row_i = 3; row_i < 6; ++row_i) + { + odom_output.pose.covariance[col_i * 6 + row_i] = covariance_matrices[1][covariance_matrix_counter]; + ++covariance_matrix_counter; + } + } + } + + // orientation + { + size_t covariance_matrix_counter = 0; + for (size_t row_i = 3; row_i < 6; ++row_i) + { + for (size_t col_i = 3; col_i < 6; ++col_i) + { + odom_output.pose.covariance[col_i * 6 + row_i] = covariance_matrices[2][covariance_matrix_counter]; + ++covariance_matrix_counter; + } + } + } + + // linear velocity + { + size_t covariance_matrix_counter = 0; + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 0; col_i < 3; ++col_i) + { + odom_output.twist.covariance[col_i * 6 + row_i] = covariance_matrices[3][covariance_matrix_counter]; + ++covariance_matrix_counter; + } + } + } + + // linear velocity <-> angular velocity + { + size_t covariance_matrix_counter = 0; + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 3; col_i < 6; ++col_i) + { + odom_output.twist.covariance[col_i * 6 + row_i] = covariance_matrices[4][covariance_matrix_counter]; + ++covariance_matrix_counter; + } + } + } + + // angular velocity <-> linear velocity + { + size_t covariance_matrix_counter = 0; + for (size_t col_i = 0; col_i < 3; ++col_i) + { + for (size_t row_i = 3; row_i < 6; ++row_i) + { + odom_output.twist.covariance[col_i * 6 + row_i] = covariance_matrices[4][covariance_matrix_counter]; + ++covariance_matrix_counter; + } + } + } + + // angular velocity + { + size_t covariance_matrix_counter = 0; + for (size_t row_i = 3; row_i < 6; ++row_i) + { + for (size_t col_i = 3; col_i < 6; ++col_i) + { + odom_output.twist.covariance[col_i * 6 + row_i] = covariance_matrices[5][covariance_matrix_counter]; + ++covariance_matrix_counter; + } + } + } + + // linear acceleration + { + size_t covariance_matrix_counter = 0; + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 0; col_i < 3; ++col_i) + { + acceleration_output.accel.covariance[col_i * 6 + row_i] = + covariance_matrices[6][covariance_matrix_counter]; + ++covariance_matrix_counter; + } + } + } + + latest_covariance_valid = true; + } + catch (const std::exception& e) + { + ROS_WARN_STREAM("An error occurred computing the covariance information for " << latest_stamp << ". " + "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_ = Synchronizer::TIME_ZERO; + latest_covariance_valid_ = false; + odom_output_ = nav_msgs::Odometry(); + acceleration_output_ = geometry_msgs::AccelWithCovarianceStamped(); + publish_timer_.start(); + delayed_throttle_filter_.reset(); +} + +void Odometry3DPublisher::onStop() +{ + publish_timer_.stop(); +} + +bool Odometry3DPublisher::getState( + const fuse_core::Graph& graph, + const ros::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::Odometry& odometry, + geometry_msgs::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 = + tf2::toMsg(tf2::Quaternion{ orientation_variable.x(), // NOLINT(whitespace/braces) + orientation_variable.y(), + orientation_variable.z(), + orientation_variable.w() }); // NOLINT(whitespace/braces) + 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; + + ROS_DEBUG_STREAM("Odometry3DPublisher::getState: " << std::endl << + "position_variable: " << position_variable << std::endl << + "orientation_variable: " << position_variable << std::endl << + "velocity_linear_variable: " << position_variable << std::endl << + "velocity_angular_variable: " << position_variable << std::endl << + "acceleration_linear_variable: " << position_variable << std::endl << + "position_variable: " << position_variable << std::endl << + "odometry: " << odometry << std::endl << + "acceleration: " << acceleration); + } + catch (const std::exception& e) + { + ROS_WARN_STREAM_THROTTLE(10.0, "Failed to find a state at time " << stamp << ". Error: " << e.what()); + return false; + } + catch (...) + { + ROS_WARN_STREAM_THROTTLE(10.0, "Failed to find a state at time " << stamp << ". Error: unknown"); + return false; + } + + return true; +} + +void Odometry3DPublisher::publishTimerCallback(const ros::TimerEvent& event) +{ + ros::Time latest_stamp; + ros::Time latest_covariance_stamp; + bool latest_covariance_valid; + nav_msgs::Odometry odom_output; + geometry_msgs::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 (latest_stamp == Synchronizer::TIME_ZERO) + { + ROS_WARN_STREAM_FILTER(&delayed_throttle_filter_, "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) + { + tf2::Vector3 velocity_linear; + tf2::fromMsg(odom_output.twist.twist.linear, velocity_linear); + + tf2::Vector3 velocity_angular; + tf2::fromMsg(odom_output.twist.twist.angular, velocity_angular); + + const double dt = event.current_real.toSec() - odom_output.header.stamp.toSec(); + + fuse_core::Matrix15d jacobian; + + tf2::Vector3 acceleration_linear; + if (params_.predict_with_acceleration) + { + tf2::fromMsg(acceleration_output.accel.accel.linear, acceleration_linear); + } + else + { + acceleration_linear.setZero(); + } + + predict( + pose, + velocity_linear, + velocity_angular, + acceleration_linear, + dt, + pose, + velocity_linear, + velocity_angular, + acceleration_linear, + jacobian); + + tf2::toMsg(pose, odom_output.pose.pose); + + odom_output.twist.twist.linear = tf2::toMsg(velocity_linear); + odom_output.twist.twist.angular = tf2::toMsg(velocity_angular); + + if (params_.predict_with_acceleration) + { + acceleration_output.accel.accel.linear = tf2::toMsg(acceleration_linear); + } + + odom_output.header.stamp = event.current_real; + acceleration_output.header.stamp = event.current_real; + + // Either the last covariance computation was skipped because there was no subscriber, + // or it failed + if (latest_covariance_valid) + { + fuse_core::Matrix15d covariance; + + // position + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 0; col_i < 3; ++col_i) + { + covariance(row_i, col_i) = odom_output.pose.covariance[col_i * 6 + row_i]; + } + } + + // orientation + for (size_t row_i = 3; row_i < 6; ++row_i) + { + for (size_t col_i = 3; col_i < 6; ++col_i) + { + covariance(row_i, col_i) = odom_output.pose.covariance[col_i * 6 + row_i]; + } + } + + // linear velocity + size_t covariance_matrix_offset = 6; + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 0; col_i < 3; ++col_i) + { + covariance(covariance_matrix_offset + row_i, covariance_matrix_offset + col_i) = + odom_output.twist.covariance[col_i * 6 + row_i]; + } + } + + // angular velocity + covariance_matrix_offset = 6; + for (size_t row_i = 3; row_i < 6; ++row_i) + { + for (size_t col_i = 3; col_i < 6; ++col_i) + { + covariance(covariance_matrix_offset + row_i, covariance_matrix_offset + col_i) = + odom_output.twist.covariance[col_i * 6 + row_i]; + } + } + + // linear acceleration + covariance_matrix_offset = 12; + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 0; col_i < 3; ++col_i) + { + covariance(covariance_matrix_offset + row_i, covariance_matrix_offset + col_i) = + acceleration_output.accel.covariance[col_i * 6 + row_i]; + } + } + + // TODO(efernandez) for now we leave the out-of-diagonal blocks with the correlations between pose, twist + // and acceleration zero, but we could cache them in another attribute when we retrieve the covariance from + // the ceres problem + covariance.topRightCorner<3, 12>().setZero(); + covariance.bottomLeftCorner<12, 3>().setZero(); + + covariance.block<3, 9>(3, 6).setZero(); + covariance.block<9, 3>(6, 3).setZero(); + + covariance.block<3, 6>(6, 9).setZero(); + covariance.block<6, 3>(9, 6).setZero(); + + covariance.block<3, 3>(9, 12).setZero(); + covariance.block<3, 3>(12, 9).setZero(); + + 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_norm_min); + } + + covariance.noalias() += dt * process_noise_covariance; + + // position + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 0; col_i < 3; ++col_i) + { + odom_output.pose.covariance[col_i * 6 + row_i] = covariance(row_i, col_i); + } + } + + // orientation + for (size_t row_i = 3; row_i < 6; ++row_i) + { + for (size_t col_i = 3; col_i < 6; ++col_i) + { + odom_output.pose.covariance[col_i * 6 + row_i] = covariance(row_i, col_i); + } + } + + // linear velocity + covariance_matrix_offset = 6; + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 0; col_i < 3; ++col_i) + { + odom_output.twist.covariance[col_i * 6 + row_i] = + covariance(covariance_matrix_offset + row_i, covariance_matrix_offset + col_i); + } + } + + // angular velocity + covariance_matrix_offset = 6; + for (size_t row_i = 3; row_i < 6; ++row_i) + { + for (size_t col_i = 3; col_i < 6; ++col_i) + { + odom_output.twist.covariance[col_i * 6 + row_i] = + covariance(covariance_matrix_offset + row_i, covariance_matrix_offset + col_i); + } + } + + // linear acceleration + covariance_matrix_offset = 12; + for (size_t row_i = 0; row_i < 3; ++row_i) + { + for (size_t col_i = 0; col_i < 3; ++col_i) + { + acceleration_output.accel.covariance[col_i * 6 + row_i] = + covariance(covariance_matrix_offset + row_i, covariance_matrix_offset + col_i); + } + } + } + } + + 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::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::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) + { + ROS_WARN_STREAM_THROTTLE(5.0, "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/pose_2d.cpp b/fuse_models/src/pose_2d.cpp index 14c7030e3..66fb2e1a1 100644 --- a/fuse_models/src/pose_2d.cpp +++ b/fuse_models/src/pose_2d.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_models/src/transaction.cpp b/fuse_models/src/transaction.cpp index fa4b80d16..e81d55fb2 100644 --- a/fuse_models/src/transaction.cpp +++ b/fuse_models/src/transaction.cpp @@ -34,7 +34,7 @@ #include -#include +#include #include // Register this sensor model with ROS as a plugin. diff --git a/fuse_models/src/twist_2d.cpp b/fuse_models/src/twist_2d.cpp index 964165a4f..5acf674db 100644 --- a/fuse_models/src/twist_2d.cpp +++ b/fuse_models/src/twist_2d.cpp @@ -38,7 +38,7 @@ #include #include -#include +#include #include diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 9a88b7dd0..cd68fd7dc 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -48,7 +48,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_models/src/unicycle_2d_ignition.cpp b/fuse_models/src/unicycle_2d_ignition.cpp index 914518196..5f162ab40 100644 --- a/fuse_models/src/unicycle_2d_ignition.cpp +++ b/fuse_models/src/unicycle_2d_ignition.cpp @@ -50,7 +50,7 @@ #include #include -#include +#include #include #include #include @@ -256,10 +256,10 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::PoseWithCovarianceStampe position->x() = pose.pose.pose.position.x; position->y() = pose.pose.pose.position.y; auto orientation = fuse_variables::Orientation2DStamped::make_shared(stamp, device_id_); - orientation->yaw() = fuse_core::getYaw(pose.pose.pose.orientation.w, - pose.pose.pose.orientation.x, - pose.pose.pose.orientation.y, - pose.pose.pose.orientation.z); + orientation->setYaw(fuse_core::getYaw(pose.pose.pose.orientation.w, + pose.pose.pose.orientation.x, + pose.pose.pose.orientation.y, + pose.pose.pose.orientation.z)); auto linear_velocity = fuse_variables::VelocityLinear2DStamped::make_shared(stamp, device_id_); linear_velocity->x() = params_.initial_state[3]; linear_velocity->y() = params_.initial_state[4]; @@ -295,7 +295,7 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::PoseWithCovarianceStampe auto orientation_constraint = fuse_constraints::AbsoluteOrientation2DStampedConstraint::make_shared( name(), *orientation, - fuse_core::Vector1d(orientation->yaw()), + fuse_core::Vector1d(orientation->getYaw()), orientation_cov); auto linear_velocity_constraint = fuse_constraints::AbsoluteVelocityLinear2DStampedConstraint::make_shared( name(), @@ -332,7 +332,7 @@ void Unicycle2DIgnition::sendPrior(const geometry_msgs::PoseWithCovarianceStampe sendTransaction(transaction); ROS_INFO_STREAM("Received a set_pose request (stamp: " << stamp << ", x: " << position->x() << ", y: " << - position->y() << ", yaw: " << orientation->yaw() << ")"); + position->y() << ", yaw: " << orientation->getYaw() << ")"); } } // namespace fuse_models diff --git a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp index 6d4d30e6e..1f9505a46 100644 --- a/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp +++ b/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp @@ -39,7 +39,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_models/src/unicycle_3d.cpp b/fuse_models/src/unicycle_3d.cpp new file mode 100644 index 000000000..a13b65f66 --- /dev/null +++ b/fuse_models/src/unicycle_3d.cpp @@ -0,0 +1,589 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 + + +// Register this motion model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle3D, fuse_core::MotionModel) + +namespace std +{ + +inline bool isfinite(const tf2::Vector3& vector) +{ + return std::isfinite(vector.x()) && std::isfinite(vector.y()) && std::isfinite(vector.z()); +} + +inline bool isfinite(const tf2::Transform& transform) +{ + double q[4]; + q[0] = transform.getRotation().w(); + q[1] = transform.getRotation().x(); + q[2] = transform.getRotation().y(); + q[3] = transform.getRotation().z(); + double rpy[3]; + fuse_core::quaternion2rpy(q, rpy); + + return std::isfinite(transform.getOrigin().x()) && + std::isfinite(transform.getOrigin().y()) && + std::isfinite(transform.getOrigin().z()) && + std::isfinite(rpy[0]) && + std::isfinite(rpy[1]) && + std::isfinite(rpy[2]); +} + +std::string to_string(const tf2::Vector3& vector) +{ + std::ostringstream oss; + oss << vector; + return oss.str(); +} + +std::string to_string(const tf2::Transform& transform) +{ + double q[4]; + q[0] = transform.getRotation().w(); + q[1] = transform.getRotation().x(); + q[2] = transform.getRotation().y(); + q[3] = transform.getRotation().z(); + double rpy[3]; + fuse_core::quaternion2rpy(q, rpy); + + std::ostringstream oss; + oss << "x: " << transform.getOrigin().x() << ", "; + oss << "y: " << transform.getOrigin().y() << ", "; + oss << "z: " << transform.getOrigin().z() << ", "; + oss << "roll: " << rpy[0] << ", "; + oss << "pitch: " << rpy[1] << ", "; + oss << "yaw: " << rpy[2]; + 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 +{ + +Unicycle3D::Unicycle3D() : + fuse_core::AsyncMotionModel(1), + buffer_length_(ros::DURATION_MAX), + device_id_(fuse_core::uuid::NIL), + timestamp_manager_(&Unicycle3D::generateMotionModel, this, ros::DURATION_MAX) +{ +} + +void Unicycle3D::print(std::ostream& stream) const +{ + stream << "state history:\n"; + for (const auto& state : state_history_) + { + stream << "- stamp: " << state.first << "\n"; + state.second.print(stream); + } +} + +void Unicycle3D::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" + << " pose: " << std::to_string(pose) << "\n" + << " velocity linear: " << velocity_linear << "\n" + << " velocity angular: " << velocity_angular << "\n" + << " acceleration linear: " << acceleration_linear << "\n"; +} + +void Unicycle3D::StateHistoryElement::validate() const +{ + if (!std::isfinite(pose)) + { + throw std::runtime_error("Invalid pose " + std::to_string(pose)); + } + + if (!std::isfinite(velocity_linear)) + { + throw std::runtime_error("Invalid linear velocity " + std::to_string(velocity_linear)); + } + + if (!std::isfinite(velocity_angular)) + { + throw std::runtime_error("Invalid angular velocity " + std::to_string(velocity_angular)); + } + + if (!std::isfinite(acceleration_linear)) + { + throw std::runtime_error("Invalid linear acceleration " + std::to_string(acceleration_linear)); + } +} + +bool Unicycle3D::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) + { + ROS_ERROR_STREAM_THROTTLE(10.0, "An error occurred while completing the motion model query. Error: " << e.what()); + return false; + } + return true; +} + +void Unicycle3D::onGraphUpdate(fuse_core::Graph::ConstSharedPtr graph) +{ + updateStateHistoryEstimates(*graph, state_history_, buffer_length_); +} + +void Unicycle3D::onInit() +{ + std::vector process_noise_diagonal; + private_node_handle_.param("process_noise_diagonal", 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(); + + private_node_handle_.param("scale_process_noise", scale_process_noise_, scale_process_noise_); + private_node_handle_.param("velocity_norm_min", velocity_norm_min_, velocity_norm_min_); + private_node_handle_.param("disable_checks", disable_checks_, disable_checks_); + + double buffer_length = 3.0; + private_node_handle_.param("buffer_length", 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) ? ros::DURATION_MAX : ros::Duration(buffer_length); + timestamp_manager_.bufferLength(buffer_length_); + + device_id_ = fuse_variables::loadDeviceId(private_node_handle_); +} + +void Unicycle3D::onStart() +{ + timestamp_manager_.clear(); + state_history_.clear(); +} + +void Unicycle3D::generateMotionModel( + const ros::Time& beginning_stamp, + const ros::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) +{ + assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history_.empty())); + + StateHistoryElement base_state; + ros::Time base_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()) + { + ROS_WARN_STREAM_COND_NAMED(!state_history_.empty(), "UnicycleModel", "Unable to locate a state in this history " + "with stamp <= " << beginning_stamp << ". 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.pose, + base_state.velocity_linear, + base_state.velocity_angular, + base_state.acceleration_linear, + (beginning_stamp - base_time).toSec(), + state1.pose, + state1.velocity_linear, + state1.velocity_angular, + state1.acceleration_linear); + } + else + { + state1 = base_state; + } + + // If dt is zero, we only need to update the state history: + const double dt = (ending_stamp - beginning_stamp).toSec(); + + 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.pose, + state1.velocity_linear, + state1.velocity_angular, + state1.acceleration_linear, + dt, + state2.pose, + state2.velocity_linear, + state2.velocity_angular, + state2.acceleration_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.pose.getOrigin().x(); + position1->data()[fuse_variables::Position3DStamped::Y] = state1.pose.getOrigin().y(); + position1->data()[fuse_variables::Position3DStamped::Z] = state1.pose.getOrigin().z(); + orientation1->data()[fuse_variables::Orientation3DStamped::W] = state1.pose.getRotation().w(); + orientation1->data()[fuse_variables::Orientation3DStamped::X] = state1.pose.getRotation().x(); + orientation1->data()[fuse_variables::Orientation3DStamped::Y] = state1.pose.getRotation().y(); + orientation1->data()[fuse_variables::Orientation3DStamped::Z] = state1.pose.getRotation().z(); + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::X] = state1.velocity_linear.x(); + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Y] = state1.velocity_linear.y(); + velocity_linear1->data()[fuse_variables::VelocityLinear3DStamped::Z] = state1.velocity_linear.z(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state1.velocity_angular.x(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state1.velocity_angular.y(); + velocity_angular1->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state1.velocity_angular.z(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::X] = state1.acceleration_linear.x(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Y] = state1.acceleration_linear.y(); + acceleration_linear1->data()[fuse_variables::AccelerationLinear3DStamped::Z] = state1.acceleration_linear.z(); + + position2->data()[fuse_variables::Position3DStamped::X] = state2.pose.getOrigin().x(); + position2->data()[fuse_variables::Position3DStamped::Y] = state2.pose.getOrigin().y(); + position2->data()[fuse_variables::Position3DStamped::Z] = state2.pose.getOrigin().z(); + orientation2->data()[fuse_variables::Orientation3DStamped::W] = state2.pose.getRotation().w(); + orientation2->data()[fuse_variables::Orientation3DStamped::X] = state2.pose.getRotation().x(); + orientation2->data()[fuse_variables::Orientation3DStamped::Y] = state2.pose.getRotation().y(); + orientation2->data()[fuse_variables::Orientation3DStamped::Z] = state2.pose.getRotation().z(); + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::X] = state2.velocity_linear.x(); + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Y] = state2.velocity_linear.y(); + velocity_linear2->data()[fuse_variables::VelocityLinear3DStamped::Z] = state2.velocity_linear.z(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::ROLL] = state2.velocity_angular.x(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::PITCH] = state2.velocity_angular.y(); + velocity_angular2->data()[fuse_variables::VelocityAngular3DStamped::YAW] = state2.velocity_angular.z(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::X] = state2.acceleration_linear.x(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Y] = state2.acceleration_linear.y(); + acceleration_linear2->data()[fuse_variables::AccelerationLinear3DStamped::Z] = state2.acceleration_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)); + + auto process_noise_covariance = process_noise_covariance_; + + // Rotate the process noise covariance with the angle of the current state orientation + Eigen::Matrix3d rotation_matrix; + for (int i = 0; i < 3; ++i) + { + for (int j = 0; j < 3; ++j) + { + rotation_matrix(i, j) = state1.pose.getBasis()[i][j]; + } + } + + // Apply only to x, y and z position as the other state variables are already along the + // current state orientation + process_noise_covariance.topLeftCorner<3, 3>() = + rotation_matrix * process_noise_covariance.topLeftCorner<3, 3>() * rotation_matrix.transpose(); + + // Scale process noise covariance pose by the norm of the current state twist + if (scale_process_noise_) + { + common::scaleProcessNoiseCovariance(process_noise_covariance, state1.velocity_linear, state1.velocity_angular, + velocity_norm_min_); + } + + // Validate + process_noise_covariance *= dt; + + if (!disable_checks_) + { + try + { + validateMotionModel(state1, state2, process_noise_covariance); + } + catch (const std::runtime_error& ex) + { + ROS_ERROR_STREAM_THROTTLE(10.0, "Invalid '" << name() << "' motion model: " << ex.what()); + return; + } + } + + // Create the constraints for this motion model segment + auto constraint = fuse_models::Unicycle3DStateKinematicConstraint::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 Unicycle3D::updateStateHistoryEstimates( + const fuse_core::Graph& graph, + StateHistory& state_history, + const ros::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; + auto expiration_time = + ending_stamp.toSec() > buffer_length.toSec() ? ending_stamp - buffer_length : ros::Time(0, 0); + + // 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.pose.setOrigin( + tf2::Vector3{ position.data()[fuse_variables::Position3DStamped::X], // NOLINT(whitespace/braces) + position.data()[fuse_variables::Position3DStamped::Y], + position.data()[fuse_variables::Position3DStamped::Z] }); // NOLINT(whitespace/braces) + + current_state.pose.setRotation( + tf2::Quaternion{ orientation.data()[fuse_variables::Orientation3DStamped::X], // NOLINT(whitespace/braces) + orientation.data()[fuse_variables::Orientation3DStamped::Y], + orientation.data()[fuse_variables::Orientation3DStamped::Z], + orientation.data()[fuse_variables::Orientation3DStamped::W] }); // NOLINT(whitespace/braces) + + current_state.velocity_linear.setX(vel_linear.data()[fuse_variables::VelocityLinear3DStamped::X]); + current_state.velocity_linear.setY(vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Y]); + current_state.velocity_linear.setZ(vel_linear.data()[fuse_variables::VelocityLinear3DStamped::Z]); + current_state.velocity_angular.setX(vel_angular.data()[fuse_variables::VelocityAngular3DStamped::ROLL]); + current_state.velocity_angular.setY(vel_angular.data()[fuse_variables::VelocityAngular3DStamped::PITCH]); + current_state.velocity_angular.setZ(vel_angular.data()[fuse_variables::VelocityAngular3DStamped::YAW]); + current_state.acceleration_linear.setX(acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::X]); + current_state.acceleration_linear.setY(acc_linear.data()[fuse_variables::AccelerationLinear3DStamped::Y]); + current_state.acceleration_linear.setZ(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.pose, + previous_state.velocity_linear, + previous_state.velocity_angular, + previous_state.acceleration_linear, + (current_stamp - previous_stamp).toSec(), + current_state.pose, + current_state.velocity_linear, + current_state.velocity_angular, + current_state.acceleration_linear); + } + } +} + +void Unicycle3D::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 Unicycle3D& unicycle_3d) +{ + unicycle_3d.print(stream); + return stream; +} + +} // namespace fuse_models diff --git a/fuse_models/src/unicycle_3d_ignition.cpp b/fuse_models/src/unicycle_3d_ignition.cpp new file mode 100644 index 000000000..bb4184c8c --- /dev/null +++ b/fuse_models/src/unicycle_3d_ignition.cpp @@ -0,0 +1,373 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 +#include + + +// Register this motion model with ROS as a plugin. +PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle3DIgnition, fuse_core::SensorModel); + +namespace fuse_models +{ + +Unicycle3DIgnition::Unicycle3DIgnition() : + fuse_core::AsyncSensorModel(1), + started_(false), + initial_transaction_sent_(false), + device_id_(fuse_core::uuid::NIL) +{ +} + +void Unicycle3DIgnition::onInit() +{ + // Read settings from the parameter sever + device_id_ = fuse_variables::loadDeviceId(private_node_handle_); + + params_.loadFromROS(private_node_handle_); + + // Connect to the reset service + if (!params_.reset_service.empty()) + { + reset_client_ = node_handle_.serviceClient(ros::names::resolve(params_.reset_service)); + } + + // Advertise + subscriber_ = node_handle_.subscribe( + ros::names::resolve(params_.topic), + params_.queue_size, + &Unicycle3DIgnition::subscriberCallback, + this); + set_pose_service_ = node_handle_.advertiseService( + ros::names::resolve(params_.set_pose_service), + &Unicycle3DIgnition::setPoseServiceCallback, + this); + set_pose_deprecated_service_ = node_handle_.advertiseService( + ros::names::resolve(params_.set_pose_deprecated_service), + &Unicycle3DIgnition::setPoseDeprecatedServiceCallback, + this); +} + +void Unicycle3DIgnition::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::PoseWithCovarianceStamped(); + pose.header.stamp = ros::Time::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]; + tf2::Quaternion q_orientation; + q_orientation.setEuler(params_.initial_state[3], params_.initial_state[4], params_.initial_state[5]); + pose.pose.pose.orientation = tf2::toMsg(q_orientation); + pose.pose.covariance[0] = params_.initial_sigma[0] * params_.initial_sigma[0]; + pose.pose.covariance[6 * 1 + 1] = params_.initial_sigma[1] * params_.initial_sigma[1]; + pose.pose.covariance[6 * 2 + 2] = params_.initial_sigma[2] * params_.initial_sigma[2]; + pose.pose.covariance[6 * 3 + 3] = params_.initial_sigma[3] * params_.initial_sigma[3]; + pose.pose.covariance[6 * 4 + 4] = params_.initial_sigma[4] * params_.initial_sigma[4]; + pose.pose.covariance[6 * 5 + 5] = params_.initial_sigma[5] * params_.initial_sigma[5]; + sendPrior(pose); + initial_transaction_sent_ = true; + } +} + +void Unicycle3DIgnition::stop() +{ + started_ = false; +} + +void Unicycle3DIgnition::subscriberCallback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr& msg) +{ + try + { + process(*msg); + } + catch (const std::exception& e) + { + ROS_ERROR_STREAM(e.what() << " Ignoring message."); + } +} + +bool Unicycle3DIgnition::setPoseServiceCallback(fuse_models::SetPose::Request& req, fuse_models::SetPose::Response& res) +{ + try + { + process(req.pose); + res.success = true; + } + catch (const std::exception& e) + { + res.success = false; + res.message = e.what(); + ROS_ERROR_STREAM(e.what() << " Ignoring request."); + } + return true; +} + +bool Unicycle3DIgnition::setPoseDeprecatedServiceCallback( + fuse_models::SetPoseDeprecated::Request& req, + fuse_models::SetPoseDeprecated::Response&) +{ + try + { + process(req.pose); + return true; + } + catch (const std::exception& e) + { + ROS_ERROR_STREAM(e.what() << " Ignoring request."); + return false; + } +} + +void Unicycle3DIgnition::process(const geometry_msgs::PoseWithCovarianceStamped& pose) +{ + // 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) + ")."); + } + 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(); + 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 matri\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(); + 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::isPositiveDefinite(orientation_cov)) + { + throw std::invalid_argument("Attempting to set the pose with a non-positive-definite orientation covariance " + "matrix " + 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_.waitForExistence(ros::Duration(10.0)) && ros::ok()) + { + ROS_WARN_STREAM("Waiting for '" << reset_client_.getService() << "' service to become avaiable."); + } + + auto srv = std_srvs::Empty(); + if (!reset_client_.call(srv)) + { + // The reset() service failed. Propagate that failure to the caller of this service. + throw std::runtime_error("Failed to call the '" + reset_client_.getService() + "' service."); + } + } + + // Now that the pose has been validated and the optimizer has been reset, actually send the initial state constraints + // to the optimizer + sendPrior(pose); +} + +void Unicycle3DIgnition::sendPrior(const geometry_msgs::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(); + 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]; + auto orientation_cov = fuse_core::Matrix3d(); + 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]; + auto linear_velocity_cov = fuse_core::Matrix3d(); + 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]; + auto angular_velocity_cov = fuse_core::Matrix3d(); + 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]; + auto linear_acceleration_cov = fuse_core::Matrix3d(); + 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::AbsoluteOrientation3DStampedEulerConstraint::make_shared( + name(), + *orientation, + fuse_core::Vector3d(orientation->roll(), orientation->pitch(), orientation->yaw()), + orientation_cov, + std::vector{ // NOLINT(whitespace/braces) + fuse_variables::Orientation3DStamped::Euler::ROLL, + fuse_variables::Orientation3DStamped::Euler::PITCH, + fuse_variables::Orientation3DStamped::Euler::YAW}); // NOLINT(whitespace/braces) + 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); + + ROS_INFO_STREAM("Received a set_pose request (" + "stamp: " << stamp << ", " + "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/unicycle_3d_state_kinematic_constraint.cpp b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp new file mode 100644 index 000000000..d3d2819a4 --- /dev/null +++ b/fuse_models/src/unicycle_3d_state_kinematic_constraint.cpp @@ -0,0 +1,121 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 +{ + +Unicycle3DStateKinematicConstraint::Unicycle3DStateKinematicConstraint( + const std::string& source, + const fuse_variables::Position3DStamped& position1, + const fuse_variables::Orientation3DStamped& orientation1, + const fuse_variables::VelocityLinear3DStamped& linear_velocity1, + const fuse_variables::VelocityAngular3DStamped& angular_velocity1, + const fuse_variables::AccelerationLinear3DStamped& linear_acceleration1, + const fuse_variables::Position3DStamped& position2, + const fuse_variables::Orientation3DStamped& orientation2, + const fuse_variables::VelocityLinear3DStamped& linear_velocity2, + const fuse_variables::VelocityAngular3DStamped& angular_velocity2, + const fuse_variables::AccelerationLinear3DStamped& linear_acceleration2, + const fuse_core::Matrix15d& covariance) : + fuse_core::Constraint( + source, + {position1.uuid(), + orientation1.uuid(), + linear_velocity1.uuid(), + angular_velocity1.uuid(), + linear_acceleration1.uuid(), + position2.uuid(), + orientation2.uuid(), + linear_velocity2.uuid(), + angular_velocity2.uuid(), + linear_acceleration2.uuid()}), // NOLINT + dt_((position2.stamp() - position1.stamp()).toSec()), + sqrt_information_(covariance.inverse().llt().matrixU()) +{ +} + +void Unicycle3DStateKinematicConstraint::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* Unicycle3DStateKinematicConstraint::costFunction() const +{ + return new Unicycle3DStateCostFunction(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 Unicycle3DStateCostFunctor(dt_, sqrt_information_)); + // + // which requires: + // + // #include +} + +} // namespace fuse_models + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_models::Unicycle3DStateKinematicConstraint); +PLUGINLIB_EXPORT_CLASS(fuse_models::Unicycle3DStateKinematicConstraint, fuse_core::Constraint); diff --git a/fuse_models/test/test_unicycle_2d.cpp b/fuse_models/test/test_unicycle_2d.cpp index 51dae33b1..de6f04e6b 100644 --- a/fuse_models/test/test_unicycle_2d.cpp +++ b/fuse_models/test/test_unicycle_2d.cpp @@ -63,7 +63,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto linear_acceleration1 = fuse_variables::AccelerationLinear2DStamped::make_shared(ros::Time(1, 0)); position1->x() = 1.1; position1->y() = 2.1; - yaw1->yaw() = 3.1; + yaw1->setYaw(3.1); linear_velocity1->x() = 1.0; linear_velocity1->y() = 0.0; yaw_velocity1->yaw() = 0.0; @@ -76,7 +76,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto linear_acceleration2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ros::Time(2, 0)); position2->x() = 1.2; position2->y() = 2.2; - yaw2->yaw() = M_PI / 2.0; + yaw2->setYaw(M_PI / 2.0); linear_velocity2->x() = 0.0; linear_velocity2->y() = 1.0; yaw_velocity2->yaw() = 0.0; @@ -89,7 +89,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto linear_acceleration3 = fuse_variables::AccelerationLinear2DStamped::make_shared(ros::Time(3, 0)); position3->x() = 1.3; position3->y() = 2.3; - yaw3->yaw() = 3.3; + yaw3->setYaw(3.3); linear_velocity3->x() = 4.3; linear_velocity3->y() = 5.3; yaw_velocity3->yaw() = 6.3; @@ -102,7 +102,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto linear_acceleration4 = fuse_variables::AccelerationLinear2DStamped::make_shared(ros::Time(4, 0)); position4->x() = 1.4; position4->y() = 2.4; - yaw4->yaw() = 3.4; + yaw4->setYaw(3.4); linear_velocity4->x() = 4.4; linear_velocity4->y() = 5.4; yaw_velocity4->yaw() = 6.4; @@ -115,7 +115,7 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) auto linear_acceleration5 = fuse_variables::AccelerationLinear2DStamped::make_shared(ros::Time(5, 0)); position5->x() = 1.5; position5->y() = 2.5; - yaw5->yaw() = 3.5; + yaw5->setYaw(3.5); linear_velocity5->x() = 4.5; linear_velocity5->y() = 5.5; yaw_velocity5->yaw() = 6.5; @@ -326,7 +326,7 @@ TEST(Unicycle2D, generateMotionModel) auto linear_acceleration1 = fuse_variables::AccelerationLinear2DStamped::make_shared(ros::Time(1, 0)); position1->x() = 1.1; position1->y() = 2.1; - yaw1->yaw() = 1.2; + yaw1->setYaw(1.2); linear_velocity1->x() = 1.0; linear_velocity1->y() = 0.0; yaw_velocity1->yaw() = 0.0; @@ -345,7 +345,7 @@ TEST(Unicycle2D, generateMotionModel) linear_velocity1->uuid(), yaw_velocity1->uuid(), linear_acceleration1->uuid(), - tf2_2d::Transform(position1->x(), position1->y(), yaw1->yaw()), + tf2_2d::Transform(position1->x(), position1->y(), yaw1->getYaw()), tf2_2d::Vector2(linear_velocity1->x(), linear_velocity1->y()), yaw_velocity1->yaw(), tf2_2d::Vector2(linear_acceleration1->x(), linear_acceleration1->y())}); // NOLINT(whitespace/braces) @@ -380,14 +380,14 @@ TEST(Unicycle2D, generateMotionModel) auto velocity_yaw = std::static_pointer_cast(variables.at(3)); auto acceleration_linear = std::static_pointer_cast(variables.at(4)); - auto expected_pose = tf2_2d::Transform(position1->x(), position1->y(), yaw1->yaw()); + auto expected_pose = tf2_2d::Transform(position1->x(), position1->y(), yaw1->getYaw()); auto expected_linear_velocity = tf2_2d::Vector2(linear_velocity1->x(), linear_velocity1->y()); auto expected_yaw_velocity = yaw_velocity1->yaw(); auto expected_linear_acceleration = tf2_2d::Vector2(linear_acceleration1->x(), linear_acceleration1->y()); EXPECT_EQ(position->x(), expected_pose.x()); EXPECT_EQ(position->y(), expected_pose.y()); - EXPECT_EQ(orientation->yaw(), expected_pose.yaw()); + EXPECT_EQ(orientation->getYaw(), expected_pose.getYaw()); EXPECT_EQ(velocity_linear->x(), expected_linear_velocity.x()); EXPECT_EQ(velocity_linear->y(), expected_linear_velocity.y()); @@ -414,7 +414,7 @@ TEST(Unicycle2D, generateMotionModel) EXPECT_NEAR(position->x(), expected_pose.x(), 1.0e-9); EXPECT_NEAR(position->y(), expected_pose.y(), 1.0e-9); - EXPECT_NEAR(orientation->yaw(), expected_pose.yaw(), 1.0e-9); + EXPECT_NEAR(orientation->getYaw(), expected_pose.getYaw(), 1.0e-9); EXPECT_NEAR(velocity_linear->x(), expected_linear_velocity.x(), 1.0e-9); EXPECT_NEAR(velocity_linear->y(), expected_linear_velocity.y(), 1.0e-9); diff --git a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp index 0a8c549ec..059b3eeb7 100644 --- a/fuse_models/test/test_unicycle_2d_state_cost_function.cpp +++ b/fuse_models/test/test_unicycle_2d_state_cost_function.cpp @@ -103,9 +103,7 @@ TEST(CostFunction, evaluateCostFunction) // 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; + 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 diff --git a/fuse_models/test/test_unicycle_3d.cpp b/fuse_models/test/test_unicycle_3d.cpp new file mode 100644 index 000000000..bd9a4962a --- /dev/null +++ b/fuse_models/test/test_unicycle_3d.cpp @@ -0,0 +1,598 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 + +/** + * @brief Derived class used in unit tests to expose protected/private functions and variables + */ +class Unicycle3DModelTest : public fuse_models::Unicycle3D +{ +public: + using fuse_models::Unicycle3D::updateStateHistoryEstimates; + using fuse_models::Unicycle3D::StateHistoryElement; + using fuse_models::Unicycle3D::StateHistory; + using fuse_models::Unicycle3D::generateMotionModel; + + void setProcessNoiseCovariance(const fuse_core::Matrix15d& process_noise_covariance) + { + process_noise_covariance_ = process_noise_covariance; + } + + void setStateHistory(const StateHistory& state_history) + { + state_history_ = state_history; + } + + void setScaleProcessNoise(const bool& scale_process_noise) + { + scale_process_noise_ = scale_process_noise; + } + + void setDisableChecks(const bool& disable_checks) + { + disable_checks_ = disable_checks; + } +}; + +TEST(Unicycle3D, UpdateStateHistoryEstimates) +{ + // Create some variables + auto position1 = fuse_variables::Position3DStamped::make_shared(ros::Time(1, 0)); + auto orientation1 = fuse_variables::Orientation3DStamped::make_shared(ros::Time(1, 0)); + auto linear_velocity1 = fuse_variables::VelocityLinear3DStamped::make_shared(ros::Time(1, 0)); + auto angular_velocity1 = fuse_variables::VelocityAngular3DStamped::make_shared(ros::Time(1, 0)); + auto linear_acceleration1 = + fuse_variables::AccelerationLinear3DStamped::make_shared(ros::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(ros::Time(2, 0)); + auto orientation2 = fuse_variables::Orientation3DStamped::make_shared(ros::Time(2, 0)); + auto linear_velocity2 = fuse_variables::VelocityLinear3DStamped::make_shared(ros::Time(2, 0)); + auto angular_velocity2 = fuse_variables::VelocityAngular3DStamped::make_shared(ros::Time(2, 0)); + auto linear_acceleration2 = + fuse_variables::AccelerationLinear3DStamped::make_shared(ros::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(ros::Time(3, 0)); + auto orientation3 = fuse_variables::Orientation3DStamped::make_shared(ros::Time(3, 0)); + auto linear_velocity3 = fuse_variables::VelocityLinear3DStamped::make_shared(ros::Time(3, 0)); + auto angular_velocity3 = fuse_variables::VelocityAngular3DStamped::make_shared(ros::Time(3, 0)); + auto linear_acceleration3 = + fuse_variables::AccelerationLinear3DStamped::make_shared(ros::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(ros::Time(4, 0)); + auto orientation4 = fuse_variables::Orientation3DStamped::make_shared(ros::Time(4, 0)); + auto linear_velocity4 = fuse_variables::VelocityLinear3DStamped::make_shared(ros::Time(4, 0)); + auto angular_velocity4 = fuse_variables::VelocityAngular3DStamped::make_shared(ros::Time(4, 0)); + auto linear_acceleration4 = + fuse_variables::AccelerationLinear3DStamped::make_shared(ros::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(ros::Time(5, 0)); + auto orientation5 = fuse_variables::Orientation3DStamped::make_shared(ros::Time(5, 0)); + auto linear_velocity5 = fuse_variables::VelocityLinear3DStamped::make_shared(ros::Time(5, 0)); + auto angular_velocity5 = fuse_variables::VelocityAngular3DStamped::make_shared(ros::Time(5, 0)); + auto linear_acceleration5 = + fuse_variables::AccelerationLinear3DStamped::make_shared(ros::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 + Unicycle3DModelTest::StateHistory state_history; + state_history.emplace( + position1->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position1->uuid(), + orientation1->uuid(), + linear_velocity1->uuid(), + angular_velocity1->uuid(), + linear_acceleration1->uuid(), + tf2::Transform{{0.0, 0.0, 0.0, 1.0}, {1.0, 0.0, 0.0}}, + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position2->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position2->uuid(), + orientation2->uuid(), + linear_velocity2->uuid(), + angular_velocity2->uuid(), + linear_acceleration2->uuid(), + tf2::Transform{{0.0, 0.0, 0.0, 1.0}, {2.0, 0.0, 0.0}}, + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position3->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position3->uuid(), + orientation3->uuid(), + linear_velocity3->uuid(), + angular_velocity3->uuid(), + linear_acceleration3->uuid(), + tf2::Transform{{0.0, 0.0, 0.0, 1.0}, {3.0, 0.0, 0.0}}, + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position4->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position4->uuid(), + orientation4->uuid(), + linear_velocity4->uuid(), + angular_velocity4->uuid(), + linear_acceleration4->uuid(), + tf2::Transform{{0.0, 0.0, 0.0, 1.0}, {4.0, 0.0, 0.0}}, + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + state_history.emplace( + position5->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position5->uuid(), + orientation5->uuid(), + linear_velocity5->uuid(), + angular_velocity5->uuid(), + linear_acceleration5->uuid(), + tf2::Transform{{0.0, 0.0, 0.0, 1.0}, {5.0, 0.0, 0.0}}, + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0), + tf2::Vector3(0.0, 0.0, 0.0)}); // NOLINT(whitespace/braces) + + // Update the state history + Unicycle3DModelTest::updateStateHistoryEstimates( + graph, state_history, ros::Duration(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[ros::Time(1, 0)].pose.getOrigin(); + auto actual_orientation = state_history[ros::Time(1, 0)].pose.getRotation(); + 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[ros::Time(1, 0)].velocity_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[ros::Time(1, 0)].velocity_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[ros::Time(1, 0)].acceleration_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[ros::Time(2, 0)].pose.getOrigin(); + auto actual_orientation = state_history[ros::Time(2, 0)].pose.getRotation(); + 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[ros::Time(2, 0)].velocity_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[ros::Time(2, 0)].velocity_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[ros::Time(2, 0)].acceleration_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[ros::Time(3, 0)].pose.getOrigin(); + auto actual_orientation = state_history[ros::Time(3, 0)].pose.getRotation(); + 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[ros::Time(3, 0)].velocity_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[ros::Time(3, 0)].velocity_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[ros::Time(3, 0)].acceleration_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[ros::Time(4, 0)].pose.getOrigin(); + auto actual_orientation = state_history[ros::Time(4, 0)].pose.getRotation(); + 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[ros::Time(4, 0)].velocity_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[ros::Time(4, 0)].velocity_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[ros::Time(4, 0)].acceleration_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[ros::Time(5, 0)].pose.getOrigin(); + auto actual_orientation = state_history[ros::Time(5, 0)].pose.getRotation(); + 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[ros::Time(5, 0)].velocity_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[ros::Time(5, 0)].velocity_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[ros::Time(5, 0)].acceleration_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); + } +} + +// TODO(fhirmann): adapt test copied from test_unicycle2D.cpp to 3D +/* TEST(Unicycle3D, generateMotionModel) +{ + // Create some variables + auto position1 = fuse_variables::Position3DStamped::make_shared(ros::Time(1, 0)); + auto yaw1 = fuse_variables::Orientation3DStamped::make_shared(ros::Time(1, 0)); + auto linear_velocity1 = fuse_variables::VelocityLinear3DStamped::make_shared(ros::Time(1, 0)); + auto yaw_velocity1 = fuse_variables::VelocityAngular3DStamped::make_shared(ros::Time(1, 0)); + auto linear_acceleration1 = fuse_variables::AccelerationLinear3DStamped::make_shared(ros::Time(1, 0)); + position1->x() = 1.1; + position1->y() = 2.1; + yaw1->setYaw(1.2); + linear_velocity1->x() = 1.0; + linear_velocity1->y() = 0.0; + yaw_velocity1->yaw() = 0.0; + linear_acceleration1->x() = 1.0; + linear_acceleration1->y() = 0.0; + + const ros::Time beginning_stamp = position1->stamp(); + const ros::Time ending_stamp = beginning_stamp + ros::Duration{1.0}; + + Unicycle3DModelTest::StateHistory state_history; + state_history.emplace( + position1->stamp(), + Unicycle3DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position1->uuid(), + yaw1->uuid(), + linear_velocity1->uuid(), + yaw_velocity1->uuid(), + linear_acceleration1->uuid(), + tf2::Transform(position1->x(), position1->y(), yaw1->getYaw()), + tf2::Vector2(linear_velocity1->x(), linear_velocity1->y()), + yaw_velocity1->yaw(), + tf2::Vector2(linear_acceleration1->x(), linear_acceleration1->y())}); // NOLINT(whitespace/braces) + + Unicycle3DModelTest unicycle_3d_model_test; + + unicycle_3d_model_test.setStateHistory(state_history); + + std::vector process_noise_diagonal{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; + unicycle_3d_model_test.setProcessNoiseCovariance(fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal()); + + unicycle_3d_model_test.setScaleProcessNoise(false); + unicycle_3d_model_test.setDisableChecks(false); + + std::vector constraints; + std::vector variables; + + // Generate the motion model + unicycle_3d_model_test.generateMotionModel( + beginning_stamp, + ending_stamp, + constraints, + variables); + + // Check first the created variables + { + // check the first entry (the original value before prediction) + + auto position = std::static_pointer_cast(variables.at(0)); + auto orientation = std::static_pointer_cast(variables.at(1)); + auto velocity_linear = std::static_pointer_cast(variables.at(2)); + auto velocity_yaw = std::static_pointer_cast(variables.at(3)); + auto acceleration_linear = std::static_pointer_cast(variables.at(4)); + + auto expected_pose = tf2::Transform(position1->x(), position1->y(), yaw1->getYaw()); + auto expected_linear_velocity = tf2::Vector2(linear_velocity1->x(), linear_velocity1->y()); + auto expected_yaw_velocity = yaw_velocity1->yaw(); + auto expected_linear_acceleration = tf2::Vector2(linear_acceleration1->x(), linear_acceleration1->y()); + + EXPECT_EQ(position->x(), expected_pose.x()); + EXPECT_EQ(position->y(), expected_pose.y()); + EXPECT_EQ(orientation->getYaw(), expected_pose.getYaw()); + + EXPECT_EQ(velocity_linear->x(), expected_linear_velocity.x()); + EXPECT_EQ(velocity_linear->y(), expected_linear_velocity.y()); + + EXPECT_EQ(velocity_yaw->yaw(), expected_yaw_velocity); + + EXPECT_EQ(acceleration_linear->x(), expected_linear_acceleration.x()); + EXPECT_EQ(acceleration_linear->y(), expected_linear_acceleration.y()); + } + + { + // check the second entry (the prediction) + + auto position = std::static_pointer_cast(variables.at(5)); + auto orientation = std::static_pointer_cast(variables.at(6)); + auto velocity_linear = std::static_pointer_cast(variables.at(7)); + auto velocity_yaw = std::static_pointer_cast(variables.at(8)); + auto acceleration_linear = std::static_pointer_cast(variables.at(9)); + + auto expected_pose = tf2::Transform(1.643536632, 3.498058629, 1.2); + auto expected_linear_velocity = tf2::Vector2(2.0, 0.0); + auto expected_yaw_velocity = 0.0; + auto expected_linear_acceleration = tf2::Vector2(1.0, 0.0); + + EXPECT_NEAR(position->x(), expected_pose.x(), 1.0e-9); + EXPECT_NEAR(position->y(), expected_pose.y(), 1.0e-9); + EXPECT_NEAR(orientation->getYaw(), expected_pose.getYaw(), 1.0e-9); + + EXPECT_NEAR(velocity_linear->x(), expected_linear_velocity.x(), 1.0e-9); + EXPECT_NEAR(velocity_linear->y(), expected_linear_velocity.y(), 1.0e-9); + + EXPECT_NEAR(velocity_yaw->yaw(), expected_yaw_velocity, 1.0e-9); + + EXPECT_NEAR(acceleration_linear->x(), expected_linear_acceleration.x(), 1.0e-9); + EXPECT_NEAR(acceleration_linear->y(), expected_linear_acceleration.y(), 1.0e-9); + } + + // Now check the created constraint + { + auto new_constraint = + std::static_pointer_cast(constraints.back()); + + // As based on the yaw angle we are mainly moving upwards and the process noise covariance + // is defined in the robot frame (original: cov_xx=1.0, cov_yy=2.0) it is expected that the final + // process noise covariance in the world frame (so the frame where optimization happens) is higher + // at cov_xx than in cov_yy. + // Additionally, cov_xy and cov_yx are also no more zero (so no more independent between x and y) because + // again it is moving simultaneously in the x- and y-direction. + fuse_core::Matrix8d expected_cov; + expected_cov << 1.868696858, -0.33773159, 0, 0, 0, 0, 0, 0, + -0.33773159, 1.131303142, 0, 0, 0, 0, 0, 0, + 0, 0, 3, 0, 0, 0, 0, 0, + 0, 0, 0, 4, 0, 0, 0, 0, + 0, 0, 0, 0, 5, 0, 0, 0, + 0, 0, 0, 0, 0, 6, 0, 0, + 0, 0, 0, 0, 0, 0, 7, 0, + 0, 0, 0, 0, 0, 0, 0, 8; + + EXPECT_MATRIX_NEAR(expected_cov, new_constraint->covariance(), 1.0e-9); + } +} */ + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "unicycle_3d_test"); + auto spinner = ros::AsyncSpinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; +} diff --git a/fuse_models/test/test_unicycle_3d_predict.cpp b/fuse_models/test/test_unicycle_3d_predict.cpp new file mode 100644 index 000000000..bddabe6f0 --- /dev/null +++ b/fuse_models/test/test_unicycle_3d_predict.cpp @@ -0,0 +1,655 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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) +{ + tf2::Transform pose1 { tf2::Quaternion{ 0.0, 0.0, 0.0, 1.0}, + tf2::Vector3{0.0, 0.0, 0.0} }; + tf2::Vector3 vel_linear1 { 1.0, 0.0, 0.0}; + tf2::Vector3 vel_angular1 { 0.0, 0.0, 1.570796327 }; + tf2::Vector3 acc_linear1 {1.0, 0.0, 0.0 }; + double dt = 0.1; + tf2::Transform pose2; + tf2::Vector3 vel_linear2; + tf2::Vector3 vel_angular2; + tf2::Vector3 acc_linear2; + + fuse_models::predict( + pose1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + pose2, + 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, pose2.getOrigin().x()); + EXPECT_DOUBLE_EQ(0.0, pose2.getOrigin().y()); + EXPECT_DOUBLE_EQ(0.0, pose2.getOrigin().z()); + EXPECT_DOUBLE_EQ(q.w(), pose2.getRotation().w()); + EXPECT_DOUBLE_EQ(q.x(), pose2.getRotation().x()); + EXPECT_DOUBLE_EQ(q.y(), pose2.getRotation().y()); + EXPECT_DOUBLE_EQ(q.z(), pose2.getRotation().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( + pose2, + vel_linear2, + vel_angular2, + acc_linear2, + dt, + pose2, + 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, pose2.getOrigin().x()); + EXPECT_DOUBLE_EQ(0.017989963481956205, pose2.getOrigin().y()); + EXPECT_DOUBLE_EQ(0.0, pose2.getOrigin().z()); + EXPECT_DOUBLE_EQ(q.w(), pose2.getRotation().w()); + EXPECT_DOUBLE_EQ(q.x(), pose2.getRotation().x()); + EXPECT_DOUBLE_EQ(q.y(), pose2.getRotation().y()); + EXPECT_DOUBLE_EQ(q.z(), pose2.getRotation().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.setY(-1.0); + vel_angular1.setZ(-1.570796327); + acc_linear1.setY(-1.0); + + fuse_models::predict( + pose1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + pose2, + 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()); + + Eigen::Quaterniond q_2{ pose2.getRotation().getW(), // NOLINT(whitespace/braces) + pose2.getRotation().getX(), + pose2.getRotation().getY(), + pose2.getRotation().getZ()}; // NOLINT(whitespace/braces) + + EXPECT_DOUBLE_EQ(0.105, pose2.getOrigin().x()); + EXPECT_DOUBLE_EQ(-0.105, pose2.getOrigin().y()); + EXPECT_DOUBLE_EQ(0.0, pose2.getOrigin().z()); + EXPECT_TRUE(q.isApprox(q_2)); + 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 + pose1.setOrigin({0.0, 0.0, 0.0}); + pose1.setRotation({ 0.0, 0.0, 0.0, 1.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( + pose1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + pose2, + vel_linear2, + vel_angular2, + acc_linear2 + ); // NOLINT(whitespace/parens) + + EXPECT_DOUBLE_EQ(0.0, pose2.getOrigin().x()); + EXPECT_DOUBLE_EQ(0.0, pose2.getOrigin().y()); + EXPECT_DOUBLE_EQ(0.015, pose2.getOrigin().z()); + EXPECT_DOUBLE_EQ(0.99691733373232339, pose2.getRotation().w()); + EXPECT_DOUBLE_EQ(0.078459095738068516, pose2.getRotation().x()); + EXPECT_DOUBLE_EQ(0.0, pose2.getRotation().y()); + EXPECT_DOUBLE_EQ(0.0, pose2.getRotation().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) + pose1.setOrigin({0.0, 0.0, 0.0}); + pose1.setRotation({-0.003, -0.943, 0.314, 0.110}); // 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( + pose1, + vel_linear1, + vel_angular1, + acc_linear1, + dt, + pose2, + vel_linear2, + vel_angular2, + acc_linear2 + ); // NOLINT(whitespace/parens) + + // TODO(fhirmann): The resulting quaternion might have all components negated but effectively + // this is the same rotation but this is not checked in the tests + EXPECT_NEAR(-0.012044123300410431, pose2.getOrigin().x(), 1e-4); + EXPECT_NEAR(0.011755776496514461, pose2.getOrigin().y(), 1e-4); + EXPECT_NEAR(-0.024959783911094033, pose2.getOrigin().z(), 1e-4); + EXPECT_NEAR(-0.20388993714859482, pose2.getRotation().w(), 1e-4); + EXPECT_NEAR(-0.061993007799788086, pose2.getRotation().x(), 1e-4); + EXPECT_NEAR(0.90147820778463239, pose2.getRotation().y(), 1e-4); + EXPECT_NEAR(-0.3767264277999153, pose2.getRotation().z(), 1e-4); + 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 + ); // NOLINT(whitespace/parens) + + 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 + ); // NOLINT(whitespace/parens) + + 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 + ); // NOLINT(whitespace/parens) + + 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 + ); // NOLINT(whitespace/parens) + + 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); +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/fuse_models/test/test_unicycle_3d_state_cost_function.cpp b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp new file mode 100644 index 000000000..82fcf9003 --- /dev/null +++ b/fuse_models/test/test_unicycle_3d_state_cost_function.cpp @@ -0,0 +1,152 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, ARTI - Autonomous Robot Technology GmbH + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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, 2e-3, 3e-3, // NOLINT(whitespace/braces) + 4e-3, 5e-3, 6e-3, + 7e-3, 8e-3, 9e-3, + 10e-3, 11e-3, 12e-3, + 13e-3, 14e-3, 15e-3 }; // NOLINT(whitespace/braces) + 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::Unicycle3DStateCostFunction cost_function{ dt, sqrt_information }; + + // Evaluate cost function + const double position1[3] = {1.0, 2.0, 3.0}; + const double orientation1[4] = {1.0, 0.0, 0.0, 0.0}; + const double vel_linear1[3] = {1.0, 2.0, 3.0}; + const double vel_angular1[3] = {1.570796327, 2.570796327, 3.570796327}; + const double acc_linear1[3] = {1.0, 2.0, 3.0}; + + const double position2[3] = {1.105, 2.21, 3.315}; + Eigen::Quaterniond q2 = Eigen::AngleAxisd(0.3570796327, Eigen::Vector3d::UnitZ()) * + Eigen::AngleAxisd(0.2570796327, 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, 2.2, 3.3}; + const double vel_angular2[3] = {1.570796327, 2.570796327, 3.570796327}; + const double acc_linear2[3] = {1.0, 2.0, 3.0}; + + const double* parameters[] = + { + 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 + EXPECT_MATRIX_NEAR(fuse_core::Vector15d::Zero(), residuals, 1e-15); + + // Check jacobians are correct using a gradient checker + ceres::NumericDiffOptions numeric_diff_options; + ceres::GradientChecker gradient_checker(&cost_function, NULL, numeric_diff_options); + + // We cannot use std::numeric_limits::epsilon() tolerance because the worst relative error is 5.26356e-10 + ceres::GradientChecker::ProbeResults probe_results; + 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::Unicycle3DStateCostFunctor(dt, sqrt_information)); + + // Evaluate cost function that uses automatic differentiation + fuse_core::Vector15d residuals_autodiff; + 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, block_sizes[i]); + jacobians_autodiff[i] = J_autodiff[i].data(); + } + + EXPECT_TRUE(cost_function_autodiff.Evaluate(parameters, residuals_autodiff.data(), jacobians_autodiff.data())); + + // We cannot use std::numeric_limits::epsilon() tolerance because with the expected state2 above the residuals + // are not zero + EXPECT_MATRIX_NEAR(residuals_autodiff, residuals, 1e-15); + + 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-12) + << "Autodiff Jacobian[" << i << "] =\n" << J_autodiff[i].format(HeavyFmt) + << "\nAnalytic Jacobian[" << i << "] =\n" << J[i].format(HeavyFmt); + } +} + +int main(int argc, char **argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/fuse_models/test/unicycle_3d.test b/fuse_models/test/unicycle_3d.test new file mode 100644 index 000000000..1e72479f0 --- /dev/null +++ b/fuse_models/test/unicycle_3d.test @@ -0,0 +1,4 @@ + + + + diff --git a/fuse_msgs/CHANGELOG.rst b/fuse_msgs/CHANGELOG.rst index cb1ade376..5624eb4a0 100644 --- a/fuse_msgs/CHANGELOG.rst +++ b/fuse_msgs/CHANGELOG.rst @@ -11,6 +11,14 @@ Changelog for package fuse_msgs * Update changelogs * Contributors: Gary Servin +0.7.0 (2023-09-25) +------------------ +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* Contributors: Gary Servin + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_msgs/package.xml b/fuse_msgs/package.xml index a7ef19dba..cbfea4589 100644 --- a/fuse_msgs/package.xml +++ b/fuse_msgs/package.xml @@ -1,7 +1,7 @@ fuse_msgs - 0.6.0 + 0.7.0 The fuse_msgs package contains messages capable of holding serialized fuse objects diff --git a/fuse_optimizers/CHANGELOG.rst b/fuse_optimizers/CHANGELOG.rst index 54562a3c7..5655a658b 100644 --- a/fuse_optimizers/CHANGELOG.rst +++ b/fuse_optimizers/CHANGELOG.rst @@ -24,6 +24,25 @@ Changelog for package fuse_optimizers * [RST-3451] Cleaned up condition variable usage * Contributors: Gary Servin, Stephen Williams, Tom Moore +0.7.0 (2023-09-25) +------------------ +* Print graph & transaction on optimization failure (#321) +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Adding doxygen to all packages (#241) +* [RST-3240] Fix how the variables to be marginalized are selected. +* [RST-3451] Cleaned up condition variable usage +* Contributors: Enrique Fernandez Perdomo, Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_optimizers/CMakeLists.txt b/fuse_optimizers/CMakeLists.txt index 798787394..3bb31f6f7 100644 --- a/fuse_optimizers/CMakeLists.txt +++ b/fuse_optimizers/CMakeLists.txt @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -72,7 +72,7 @@ target_link_libraries(batch_optimizer_node ) set_target_properties(batch_optimizer_node PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -94,7 +94,7 @@ target_link_libraries(fixed_lag_smoother_node ) set_target_properties(fixed_lag_smoother_node PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -153,7 +153,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_variable_stamp_index PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -177,7 +177,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_optimizer PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -209,7 +209,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_fixed_lag_ignition PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_optimizers/include/fuse_optimizers/optimizer.h b/fuse_optimizers/include/fuse_optimizers/optimizer.h index 20f16f736..0fa6928a2 100644 --- a/fuse_optimizers/include/fuse_optimizers/optimizer.h +++ b/fuse_optimizers/include/fuse_optimizers/optimizer.h @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_optimizers/package.xml b/fuse_optimizers/package.xml index e2cadf6ab..e5c953d38 100644 --- a/fuse_optimizers/package.xml +++ b/fuse_optimizers/package.xml @@ -1,7 +1,7 @@ fuse_optimizers - 0.6.0 + 0.7.0 The fuse_optimizers package provides a set of optimizer implementations. An optimizer is the object responsible for coordinating the sensors and motion model inputs, computing the optimal state values, and providing access to diff --git a/fuse_optimizers/src/fixed_lag_smoother.cpp b/fuse_optimizers/src/fixed_lag_smoother.cpp index a91850825..dad9058c6 100644 --- a/fuse_optimizers/src/fixed_lag_smoother.cpp +++ b/fuse_optimizers/src/fixed_lag_smoother.cpp @@ -223,20 +223,26 @@ void FixedLagSmoother::optimizationLoop() // Optimize the entire graph summary_ = graph_->optimize(params_.solver_options); - // Optimization is complete. Notify all the things about the graph changes. - const auto new_transaction_stamp = new_transaction->stamp(); - notify(std::move(new_transaction), graph_->clone()); - // Abort if optimization failed. Not converging is not a failure because the solution found is usable. if (!summary_.IsSolutionUsable()) { + std::ostringstream oss; + oss << "Graph:\n"; + graph_->print(oss); + oss << "\nTransaction:\n"; + new_transaction->print(oss); + ROS_FATAL_STREAM("Optimization failed after updating the graph with the transaction with timestamp " - << new_transaction_stamp << ". Leaving optimization loop and requesting node shutdown..."); + << new_transaction->stamp() << ". Leaving optimization loop and requesting node shutdown...\n" + << oss.str()); ROS_INFO_STREAM(summary_.FullReport()); ros::requestShutdown(); break; } + // Optimization is complete and succeeded. Notify all the things about the graph changes. + notify(std::move(new_transaction), graph_->clone()); + // Compute a transaction that marginalizes out those variables. lag_expiration_ = computeLagExpirationTime(); marginal_transaction_ = fuse_constraints::marginalizeVariables( diff --git a/fuse_publishers/CHANGELOG.rst b/fuse_publishers/CHANGELOG.rst index 2daef5d34..5c42a7c85 100644 --- a/fuse_publishers/CHANGELOG.rst +++ b/fuse_publishers/CHANGELOG.rst @@ -20,6 +20,25 @@ Changelog for package fuse_publishers * Adding doxygen to all packages (#241) * Contributors: Gary Servin, Stephen Williams, Tom Moore +0.7.0 (2023-09-25) +------------------ +* [RST-7809] Fix optimization errors when the orientation is initialized at +PI (#334) + * Add some unit tests for the 2D orientation constraints; Create getters/setters for the 2D orientation variable is preparation for a fix. + * Force the 2D orientation value to be is minimum phase +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Adding doxygen to all packages (#241) +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_publishers/CMakeLists.txt b/fuse_publishers/CMakeLists.txt index 68885d310..a2f78f010 100644 --- a/fuse_publishers/CMakeLists.txt +++ b/fuse_publishers/CMakeLists.txt @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -114,7 +114,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_path_2d_publisher PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -137,7 +137,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_pose_2d_publisher PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -158,7 +158,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_stamped_variable_synchronizer PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_publishers/package.xml b/fuse_publishers/package.xml index caccb0fbc..19efb0ec3 100644 --- a/fuse_publishers/package.xml +++ b/fuse_publishers/package.xml @@ -1,7 +1,7 @@ fuse_publishers - 0.6.0 + 0.7.0 The fuse_publishers package provides a set of common publisher plugins. diff --git a/fuse_publishers/src/path_2d_publisher.cpp b/fuse_publishers/src/path_2d_publisher.cpp index f1903285f..ef70e0fd4 100644 --- a/fuse_publishers/src/path_2d_publisher.cpp +++ b/fuse_publishers/src/path_2d_publisher.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include #include #include @@ -114,7 +114,7 @@ void Path2DPublisher::notifyCallback( pose.pose.position.x = position->x(); pose.pose.position.y = position->y(); pose.pose.position.z = 0.0; - pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation->yaw())); + pose.pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation->getYaw())); poses.push_back(std::move(pose)); } } diff --git a/fuse_publishers/src/pose_2d_publisher.cpp b/fuse_publishers/src/pose_2d_publisher.cpp index 6c99b228d..200ff292d 100644 --- a/fuse_publishers/src/pose_2d_publisher.cpp +++ b/fuse_publishers/src/pose_2d_publisher.cpp @@ -42,7 +42,7 @@ #include #include #include -#include +#include #include #include #include @@ -80,7 +80,7 @@ bool findPose( pose.position.x = position_variable.x(); pose.position.y = position_variable.y(); pose.position.z = 0.0; - pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation_variable.yaw())); + pose.orientation = tf2::toMsg(tf2::Quaternion(tf2::Vector3(0, 0, 1), orientation_variable.getYaw())); } catch (const std::exception& e) { diff --git a/fuse_publishers/src/serialized_publisher.cpp b/fuse_publishers/src/serialized_publisher.cpp index 85ecb9f44..db1e20bc8 100644 --- a/fuse_publishers/src/serialized_publisher.cpp +++ b/fuse_publishers/src/serialized_publisher.cpp @@ -41,7 +41,7 @@ #include #include #include -#include +#include #include diff --git a/fuse_publishers/test/test_path_2d_publisher.cpp b/fuse_publishers/test/test_path_2d_publisher.cpp index e8192ec45..23a1d4a87 100644 --- a/fuse_publishers/test/test_path_2d_publisher.cpp +++ b/fuse_publishers/test/test_path_2d_publisher.cpp @@ -71,24 +71,24 @@ class Path2DPublisherTestFixture : public ::testing::Test position1->x() = 1.01; position1->y() = 2.01; auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1234, 10)); - orientation1->yaw() = 3.01; + orientation1->setYaw(3.01); auto position2 = fuse_variables::Position2DStamped::make_shared(ros::Time(1235, 10)); position2->x() = 1.02; position2->y() = 2.02; auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1235, 10)); - orientation2->yaw() = 3.02; + orientation2->setYaw(3.02); auto position3 = fuse_variables::Position2DStamped::make_shared(ros::Time(1235, 9)); position3->x() = 1.03; position3->y() = 2.03; auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1235, 9)); - orientation3->yaw() = 3.03; + orientation3->setYaw(3.03); auto position4 = fuse_variables::Position2DStamped::make_shared(ros::Time(1235, 11), fuse_core::uuid::generate("kitt")); position4->x() = 1.04; position4->y() = 2.04; auto orientation4 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1235, 11), fuse_core::uuid::generate("kitt")); - orientation4->yaw() = 3.04; + orientation4->setYaw(3.04); transaction_->addInvolvedStamp(position1->stamp()); transaction_->addInvolvedStamp(orientation1->stamp()); diff --git a/fuse_publishers/test/test_pose_2d_publisher.cpp b/fuse_publishers/test/test_pose_2d_publisher.cpp index 26194c3bd..b6a49616e 100644 --- a/fuse_publishers/test/test_pose_2d_publisher.cpp +++ b/fuse_publishers/test/test_pose_2d_publisher.cpp @@ -73,24 +73,24 @@ class Pose2DPublisherTestFixture : public ::testing::Test position1->x() = 1.01; position1->y() = 2.01; auto orientation1 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1234, 10)); - orientation1->yaw() = 3.01; + orientation1->setYaw(3.01); auto position2 = fuse_variables::Position2DStamped::make_shared(ros::Time(1235, 10)); position2->x() = 1.02; position2->y() = 2.02; auto orientation2 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1235, 10)); - orientation2->yaw() = 3.02; + orientation2->setYaw(3.02); auto position3 = fuse_variables::Position2DStamped::make_shared(ros::Time(1235, 9)); position3->x() = 1.03; position3->y() = 2.03; auto orientation3 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1235, 9)); - orientation3->yaw() = 3.03; + orientation3->setYaw(3.03); auto position4 = fuse_variables::Position2DStamped::make_shared(ros::Time(1235, 11), fuse_core::uuid::generate("kitt")); position4->x() = 1.04; position4->y() = 2.04; auto orientation4 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1235, 11), fuse_core::uuid::generate("kitt")); - orientation4->yaw() = 3.04; + orientation4->setYaw(3.04); transaction_->addInvolvedStamp(position1->stamp()); transaction_->addInvolvedStamp(orientation1->stamp()); diff --git a/fuse_tutorials/CHANGELOG.rst b/fuse_tutorials/CHANGELOG.rst index ed2a0787b..1bdd65be6 100644 --- a/fuse_tutorials/CHANGELOG.rst +++ b/fuse_tutorials/CHANGELOG.rst @@ -27,5 +27,25 @@ Changelog for package fuse_tutorials * Create a new sensor type with a non-trivial measurement function, a new publisher to visualize the results, and a simplistic robot simulator to demonstrate the sensor in action. * Contributors: Gary Servin, Paul Bovbel, Stephen Williams +0.7.0 (2023-09-25) +------------------ +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* Fix changelog +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Fix install space for fuse_tutorials (#264) +* Added simple tutorial files from the S3 bucket (#253) +* Sensor tutorial (#251) + * Create a new sensor type with a non-trivial measurement function, a new publisher to visualize the results, and a simplistic robot simulator to demonstrate the sensor in action. +* Contributors: Gary Servin, Paul Bovbel, Stephen Williams + 0.0.1 (2018-07-05) ------------------ diff --git a/fuse_tutorials/CMakeLists.txt b/fuse_tutorials/CMakeLists.txt index 269c8f32f..57da1db9d 100644 --- a/fuse_tutorials/CMakeLists.txt +++ b/fuse_tutorials/CMakeLists.txt @@ -50,7 +50,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -72,7 +72,7 @@ target_link_libraries(range_sensor_simulator ) set_target_properties(range_sensor_simulator PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) diff --git a/fuse_tutorials/package.xml b/fuse_tutorials/package.xml index ce2cbed30..d0c731350 100644 --- a/fuse_tutorials/package.xml +++ b/fuse_tutorials/package.xml @@ -4,7 +4,7 @@ schematypens="http://www.w3.org/2001/XMLSchema"?> fuse_tutorials - 0.6.0 + 0.7.0 Package containing source code for the fuse tutorials diff --git a/fuse_tutorials/src/beacon_publisher.cpp b/fuse_tutorials/src/beacon_publisher.cpp index a42e6a3b0..cbd800403 100644 --- a/fuse_tutorials/src/beacon_publisher.cpp +++ b/fuse_tutorials/src/beacon_publisher.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/fuse_tutorials/src/range_constraint.cpp b/fuse_tutorials/src/range_constraint.cpp index 885c8e0cd..42971c993 100644 --- a/fuse_tutorials/src/range_constraint.cpp +++ b/fuse_tutorials/src/range_constraint.cpp @@ -34,7 +34,7 @@ #include #include -#include +#include #include #include diff --git a/fuse_tutorials/src/range_sensor_model.cpp b/fuse_tutorials/src/range_sensor_model.cpp index 9e9e32030..1b0595412 100644 --- a/fuse_tutorials/src/range_sensor_model.cpp +++ b/fuse_tutorials/src/range_sensor_model.cpp @@ -40,7 +40,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/CHANGELOG.rst b/fuse_variables/CHANGELOG.rst index a78544f43..1d1b0cebc 100644 --- a/fuse_variables/CHANGELOG.rst +++ b/fuse_variables/CHANGELOG.rst @@ -40,6 +40,36 @@ Changelog for package fuse_variables Co-authored-by: Jake McLaughlin * Contributors: Gary Servin, Stephen Williams, Tom Moore +0.7.0 (2023-09-25) +------------------ +* [RST-7809] Fix optimization errors when the orientation is initialized at +PI (#334) + * Add some unit tests for the 2D orientation constraints; Create getters/setters for the 2D orientation variable is preparation for a fix. + * Force the 2D orientation value to be is minimum phase +* Derive the fixed landmarks from the standard landmarks (#259) +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* [RST-4186] Fix fuse macro names (#263) + * Namespace all macros with the FUSE\_ prefix. Mark original macros as deprecated. + * Update all fuse objects to use the new macro names +* Make 2D versions of the landmark variables (#250) +* [RST-4390] Allow variables to be held constant during optimization (#243) + * Add support for holding variables constant + * Create a 'fixed' landmark + * Added initial support for marginalizing constant variables +* Adding doxygen to all packages (#241) +* Add unstamped 3D point variable (#233) (#239) + * Add unstamped 3D landmark variable + * Add landmark test and new uuid generator + Co-authored-by: Stephen Williams + Co-authored-by: Jake McLaughlin +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_variables/CMakeLists.txt b/fuse_variables/CMakeLists.txt index fe74f100e..e09f5a868 100644 --- a/fuse_variables/CMakeLists.txt +++ b/fuse_variables/CMakeLists.txt @@ -34,6 +34,8 @@ add_library(${PROJECT_NAME} src/acceleration_angular_3d_stamped.cpp src/acceleration_linear_2d_stamped.cpp src/acceleration_linear_3d_stamped.cpp + src/pinhole_camera.cpp + src/pinhole_camera_fixed.cpp src/orientation_2d_stamped.cpp src/orientation_3d_stamped.cpp src/point_2d_fixed_landmark.cpp @@ -63,7 +65,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -120,7 +122,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_acceleration_angular_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -144,7 +146,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_acceleration_angular_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -168,7 +170,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_acceleration_linear_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -192,10 +194,58 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_acceleration_linear_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) + # Camera Intrinsics tests + catkin_add_gtest(test_pinhole_camera + test/test_pinhole_camera.cpp + ) + add_dependencies(test_pinhole_camera + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(test_pinhole_camera + PRIVATE + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ) + target_link_libraries(test_pinhole_camera + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ) + set_target_properties(test_pinhole_camera + PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + + # Camera Intrinsics tests + catkin_add_gtest(test_pinhole_camera_fixed + test/test_pinhole_camera_fixed.cpp + ) + add_dependencies(test_pinhole_camera_fixed + ${catkin_EXPORTED_TARGETS} + ) + target_include_directories(test_pinhole_camera_fixed + PRIVATE + include + ${catkin_INCLUDE_DIRS} + ${CERES_INCLUDE_DIRS} + ) + target_link_libraries(test_pinhole_camera_fixed + ${PROJECT_NAME} + ${catkin_LIBRARIES} + ${CERES_LIBRARIES} + ) + set_target_properties(test_pinhole_camera_fixed + PROPERTIES + CXX_STANDARD 17 + CXX_STANDARD_REQUIRED YES + ) + # Fixed Size Variable Tests catkin_add_gtest(test_fixed_size_variable test/test_fixed_size_variable.cpp @@ -215,7 +265,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_fixed_size_variable PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -239,7 +289,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_load_device_id PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -263,7 +313,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_orientation_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -287,7 +337,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_orientation_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -311,7 +361,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_point_2d_fixed_landmark PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -335,7 +385,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_point_2d_landmark PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -359,7 +409,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_point_3d_fixed_landmark PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -383,7 +433,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_point_3d_landmark PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -407,7 +457,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_position_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -431,7 +481,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_position_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -455,7 +505,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_velocity_angular_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -479,7 +529,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_velocity_angular_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -503,7 +553,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_velocity_linear_2d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) @@ -527,7 +577,7 @@ if(CATKIN_ENABLE_TESTING) ) set_target_properties(test_velocity_linear_3d_stamped PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) endif() diff --git a/fuse_variables/include/fuse_variables/orientation_2d_stamped.h b/fuse_variables/include/fuse_variables/orientation_2d_stamped.h index 8f0492a87..b0bd50e59 100644 --- a/fuse_variables/include/fuse_variables/orientation_2d_stamped.h +++ b/fuse_variables/include/fuse_variables/orientation_2d_stamped.h @@ -161,13 +161,25 @@ class Orientation2DStamped : public FixedSizeVariable<1>, public Stamped /** * @brief Read-write access to the heading angle. */ + [[deprecated("The yaw value must be in the range [-pi, pi). Use the setYaw(value) method to ensure minimum phase.")]] double& yaw() { return data_[YAW]; } /** * @brief Read-only access to the heading angle. */ + [[deprecated("Use the getYaw()/setYaw(value) methods to ensure const-correctness.")]] const double& yaw() const { return data_[YAW]; } + /** + * @brief Read-only access to the heading angle. + */ + const double& getYaw() const { return data_[YAW]; } + + /** + * @brief Write access to the heading angle. + */ + void setYaw(const double yaw) { data_[YAW] = fuse_core::wrapAngle2D(yaw); } + /** * @brief Print a human-readable description of the variable to the provided stream. * diff --git a/fuse_variables/include/fuse_variables/orientation_3d_stamped.h b/fuse_variables/include/fuse_variables/orientation_3d_stamped.h index 9d7af3858..43d9731d7 100644 --- a/fuse_variables/include/fuse_variables/orientation_3d_stamped.h +++ b/fuse_variables/include/fuse_variables/orientation_3d_stamped.h @@ -251,17 +251,17 @@ class Orientation3DStamped : public FixedSizeVariable<4>, public Stamped /** * @brief Read-only access to quaternion's Euler roll angle component */ - double roll() { return fuse_core::getRoll(w(), x(), y(), z()); } + double roll() const { return fuse_core::getRoll(w(), x(), y(), z()); } /** * @brief Read-only access to quaternion's Euler pitch angle component */ - double pitch() { return fuse_core::getPitch(w(), x(), y(), z()); } + double pitch() const { return fuse_core::getPitch(w(), x(), y(), z()); } /** * @brief Read-only access to quaternion's Euler yaw angle component */ - double yaw() { return fuse_core::getYaw(w(), x(), y(), z()); } + double yaw() const { return fuse_core::getYaw(w(), x(), y(), z()); } /** * @brief Print a human-readable description of the variable to the provided stream. diff --git a/fuse_variables/include/fuse_variables/pinhole_camera.h b/fuse_variables/include/fuse_variables/pinhole_camera.h new file mode 100644 index 000000000..c967cdff1 --- /dev/null +++ b/fuse_variables/include/fuse_variables/pinhole_camera.h @@ -0,0 +1,184 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_VARIABLES_PINHOLE_CAMERA_H +#define FUSE_VARIABLES_PINHOLE_CAMERA_H + +#include +#include +#include +#include + +#include +#include +#include + +#include + +namespace fuse_variables +{ +/** + * @brief Variable representing intrinsic parameters of a camera. + * + * The UUID of this class is constant after + * construction and dependent on a user input database id. As such, the database id cannot be altered after + * construction. + */ +class PinholeCamera : public FixedSizeVariable<4> +{ +public: + FUSE_VARIABLE_DEFINITIONS(PinholeCamera); + + /** + * @brief Can be used to directly index variables in the data array + */ + enum : size_t + { + FX = 0, + FY = 1, + CX = 2, + CY = 3 + }; + + /** + * @brief Default constructor + */ + PinholeCamera() = default; + + /** + * @brief Construct a pinhole camera variable given a camera id + * + * @param[in] camera_id The id associated to a camera + */ + explicit PinholeCamera(const uint64_t& camera_id); + + /** + * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters + * + * @param[in] camera_id The id associated to a camera + */ + explicit PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id, + const double& fx, const double& fy, + const double& cx, const double& cy); + + /** + * @brief Read-write access to the cx parameter. + */ + double& cx() { return data_[CX]; } + + /** + * @brief Read-only access to the cx parameter. + */ + const double& cx() const { return data_[CX]; } + + /** + * @brief Read-write access to the cy parameter. + */ + double& cy() { return data_[CY]; } + + /** + * @brief Read-only access to the cy parameter. + */ + const double& cy() const { return data_[CY]; } + + /** + * @brief Read-write access to the fx parameter. + */ + double& fx() { return data_[FX]; } + + /** + * @brief Read-only access to the fx parameter. + */ + const double& fx() const { return data_[FX]; } + + /** + * @brief Read-write access to the fy parameter. + */ + double& fy() { return data_[FY]; } + + /** + * @brief Read-only access to the fy parameter. + */ + const double& fy() const { return data_[FY]; } + + /** + * @brief Read-only access to the id + */ + const uint64_t& id() const { return id_; } + + /** + * @brief Print a human-readable description of the variable to the provided + * stream. + * + * @param[out] stream The stream to write to. Defaults to stdout. + */ + void print(std::ostream& stream = std::cout) const override; + +protected: + /** + * @brief Construct a point 3D variable given a landmarks id + * + * @param[in] landmark_id The id associated to a landmark + */ + PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& landmark_id); + +private: + // Allow Boost Serialization access to private methods + friend class boost::serialization::access; + uint64_t id_ { 0 }; + + /** + * @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& id_; + } +}; + +} // namespace fuse_variables + +BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCamera); + +#endif // FUSE_VARIABLES_PINHOLE_CAMERA_H diff --git a/fuse_variables/include/fuse_variables/pinhole_camera_fixed.h b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.h new file mode 100644 index 000000000..abc0c388a --- /dev/null +++ b/fuse_variables/include/fuse_variables/pinhole_camera_fixed.h @@ -0,0 +1,108 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#ifndef FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H +#define FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H + +#include +#include +#include + +#include +#include +#include + +namespace fuse_variables +{ +/** + * @brief Variable representing a pinhole camera that exists across time. + * + */ +class PinholeCameraFixed : public PinholeCamera +{ +public: + FUSE_VARIABLE_DEFINITIONS(PinholeCameraFixed); + + /** + * @brief Default constructor + */ + PinholeCameraFixed() = default; + + /** + * @brief Construct a point 2D variable given a landmarks id + * + * @param[in] camera_id The id associated to a camera + */ + explicit PinholeCameraFixed(const uint64_t& camera_id); + + /** + * @brief Construct a pinhole camera variable given a camera id and intrinsic parameters + * + * @param[in] camera_id The id associated to a camera + */ + explicit PinholeCameraFixed(const uint64_t& camera_id, + const double& fx, const double& fy, + const double& cx, const double& cy); + /** + * @brief Specifies if the value of the variable should not be changed during optimization + */ + bool holdConstant() const override { return true; } + +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); + } +}; + +} // namespace fuse_variables + +BOOST_CLASS_EXPORT_KEY(fuse_variables::PinholeCameraFixed); + +#endif // FUSE_VARIABLES_PINHOLE_CAMERA_FIXED_H diff --git a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h index 1a5967e16..4d5007512 100644 --- a/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h +++ b/fuse_variables/include/fuse_variables/point_2d_fixed_landmark.h @@ -36,37 +36,27 @@ #include #include -#include +#include #include #include #include -#include - namespace fuse_variables { /** * @brief Variable representing a 2D point landmark that exists across time. * - * This is commonly used to represent locations of visual features. The UUID of this class is constant after - * construction and dependent on a user input database id. As such, the database id cannot be altered after - * construction. + * This is commonly used to represent locations of visual features. This class differs from the Point2DLandmark in that + * the value of the landmark is held constant during optimization. This is appropriate if the landmark positions are + * known or were previously estimated to sufficient accuracy. The UUID of this class is constant after construction and + * dependent on a user input database id. As such, the database id cannot be altered after construction. */ -class Point2DFixedLandmark : public FixedSizeVariable<2> +class Point2DFixedLandmark : public Point2DLandmark { public: FUSE_VARIABLE_DEFINITIONS(Point2DFixedLandmark); - /** - * @brief Can be used to directly index variables in the data array - */ - enum : size_t - { - X = 0, - Y = 1 - }; - /** * @brief Default constructor */ @@ -79,48 +69,14 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> */ explicit Point2DFixedLandmark(const uint64_t& landmark_id); - /** - * @brief Read-write access to the X-axis position. - */ - double& x() { return data_[X]; } - - /** - * @brief Read-only access to the X-axis position. - */ - const double& x() const { return data_[X]; } - - /** - * @brief Read-write access to the Y-axis position. - */ - double& y() { return data_[Y]; } - - /** - * @brief Read-only access to the Y-axis position. - */ - const double& y() const { return data_[Y]; } - - /** - * @brief Read-only access to the id - */ - const uint64_t& id() const { return id_; } - - /** - * @brief Print a human-readable description of the variable 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 Specifies if the value of the variable should not be changed during optimization */ - bool holdConstant() const override; + bool holdConstant() const override { return true; } private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ { 0 }; /** * @brief The Boost Serialize method that serializes all of the data members @@ -134,8 +90,7 @@ class Point2DFixedLandmark : public FixedSizeVariable<2> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); - archive& id_; + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/point_2d_landmark.h b/fuse_variables/include/fuse_variables/point_2d_landmark.h index e6c7a6582..1ebd80d50 100644 --- a/fuse_variables/include/fuse_variables/point_2d_landmark.h +++ b/fuse_variables/include/fuse_variables/point_2d_landmark.h @@ -36,6 +36,7 @@ #include #include +#include #include #include @@ -112,6 +113,15 @@ class Point2DLandmark : public FixedSizeVariable<2> */ void print(std::ostream& stream = std::cout) const override; +protected: + /** + * @brief Construct a point 2D variable given a UUID and a landmarks id + * + * @param[in] uuid The UUID for this variable + * @param[in] landmark_id The id associated to a landmark + */ + Point2DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id); + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h index 89c8590ae..d690bf9ed 100644 --- a/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h +++ b/fuse_variables/include/fuse_variables/point_3d_fixed_landmark.h @@ -36,38 +36,27 @@ #include #include -#include +#include #include #include #include -#include - namespace fuse_variables { /** * @brief Variable representing a 3D point landmark that exists across time. * - * This is commonly used to represent locations of visual features. The UUID of this class is constant after - * construction and dependent on a user input database id. As such, the database id cannot be altered after - * construction. + * This is commonly used to represent locations of visual features. This class differs from the Point3DLandmark in that + * the value of the landmark is held constant during optimization. This is appropriate if the landmark positions are + * known or were previously estimated to sufficient accuracy. The UUID of this class is constant after construction and + * dependent on a user input database id. As such, the database id cannot be altered after construction. */ -class Point3DFixedLandmark : public FixedSizeVariable<3> +class Point3DFixedLandmark : public Point3DLandmark { public: FUSE_VARIABLE_DEFINITIONS(Point3DFixedLandmark); - /** - * @brief Can be used to directly index variables in the data array - */ - enum : size_t - { - X = 0, - Y = 1, - Z = 2 - }; - /** * @brief Default constructor */ @@ -80,58 +69,14 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> */ explicit Point3DFixedLandmark(const uint64_t& landmark_id); - /** - * @brief Read-write access to the X-axis position. - */ - double& x() { return data_[X]; } - - /** - * @brief Read-only access to the X-axis position. - */ - const double& x() const { return data_[X]; } - - /** - * @brief Read-write access to the Y-axis position. - */ - double& y() { return data_[Y]; } - - /** - * @brief Read-only access to the Y-axis position. - */ - const double& y() const { return data_[Y]; } - - /** - * @brief Read-write access to the Z-axis position. - */ - double& z() { return data_[Z]; } - - /** - * @brief Read-only access to the Z-axis position. - */ - const double& z() const { return data_[Z]; } - - /** - * @brief Read-only access to the id - */ - const uint64_t& id() const { return id_; } - - /** - * @brief Print a human-readable description of the variable 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 Specifies if the value of the variable should not be changed during optimization */ - bool holdConstant() const override; + bool holdConstant() const override { return true; } private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; - uint64_t id_ { 0 }; /** * @brief The Boost Serialize method that serializes all of the data members @@ -145,8 +90,7 @@ class Point3DFixedLandmark : public FixedSizeVariable<3> template void serialize(Archive& archive, const unsigned int /* version */) { - archive& boost::serialization::base_object>(*this); - archive& id_; + archive& boost::serialization::base_object(*this); } }; diff --git a/fuse_variables/include/fuse_variables/point_3d_landmark.h b/fuse_variables/include/fuse_variables/point_3d_landmark.h index 06cf2458b..c165a68d3 100644 --- a/fuse_variables/include/fuse_variables/point_3d_landmark.h +++ b/fuse_variables/include/fuse_variables/point_3d_landmark.h @@ -39,6 +39,7 @@ #include #include +#include #include #include @@ -126,6 +127,14 @@ class Point3DLandmark : public FixedSizeVariable<3> */ void print(std::ostream& stream = std::cout) const override; +protected: + /** + * @brief Construct a point 3D variable given a landmarks id + * + * @param[in] landmark_id The id associated to a landmark + */ + Point3DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id); + private: // Allow Boost Serialization access to private methods friend class boost::serialization::access; diff --git a/fuse_variables/package.xml b/fuse_variables/package.xml index 967bccd75..f2fb49934 100644 --- a/fuse_variables/package.xml +++ b/fuse_variables/package.xml @@ -1,7 +1,7 @@ fuse_variables - 0.6.0 + 0.7.0 The fuse_variables package provides a set of commonly used variable types, such as 2D and 3D positions, orientations, velocities, and accelerations. diff --git a/fuse_variables/src/acceleration_angular_2d_stamped.cpp b/fuse_variables/src/acceleration_angular_2d_stamped.cpp index 5d4bd545d..c65f1144b 100644 --- a/fuse_variables/src/acceleration_angular_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/acceleration_angular_3d_stamped.cpp b/fuse_variables/src/acceleration_angular_3d_stamped.cpp index 710be3098..37e80468d 100644 --- a/fuse_variables/src/acceleration_angular_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_angular_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/acceleration_linear_2d_stamped.cpp b/fuse_variables/src/acceleration_linear_2d_stamped.cpp index 089dcbdaa..037e562ef 100644 --- a/fuse_variables/src/acceleration_linear_2d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/acceleration_linear_3d_stamped.cpp b/fuse_variables/src/acceleration_linear_3d_stamped.cpp index 31297d1c7..e86a0f17a 100644 --- a/fuse_variables/src/acceleration_linear_3d_stamped.cpp +++ b/fuse_variables/src/acceleration_linear_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/orientation_2d_stamped.cpp b/fuse_variables/src/orientation_2d_stamped.cpp index 6abb54314..e4118af6b 100644 --- a/fuse_variables/src/orientation_2d_stamped.cpp +++ b/fuse_variables/src/orientation_2d_stamped.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include @@ -62,7 +62,7 @@ void Orientation2DStamped::print(std::ostream& stream) const << " device_id: " << deviceId() << "\n" << " size: " << size() << "\n" << " data:\n" - << " - yaw: " << yaw() << "\n"; + << " - yaw: " << getYaw() << "\n"; } fuse_core::LocalParameterization* Orientation2DStamped::localParameterization() const diff --git a/fuse_variables/src/orientation_3d_stamped.cpp b/fuse_variables/src/orientation_3d_stamped.cpp index 462eea00e..f5dea4a83 100644 --- a/fuse_variables/src/orientation_3d_stamped.cpp +++ b/fuse_variables/src/orientation_3d_stamped.cpp @@ -37,7 +37,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/pinhole_camera.cpp b/fuse_variables/src/pinhole_camera.cpp new file mode 100644 index 000000000..ec665c7af --- /dev/null +++ b/fuse_variables/src/pinhole_camera.cpp @@ -0,0 +1,87 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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 + +namespace fuse_variables +{ +PinholeCamera::PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id) + : FixedSizeVariable(uuid), id_(camera_id) +{ +} + +PinholeCamera::PinholeCamera(const uint64_t& camera_id) + : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) +{ +} + +PinholeCamera::PinholeCamera(const fuse_core::UUID& uuid, const uint64_t& camera_id, + const double& fx, const double& fy, + const double& cx, const double& cy) + : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) +{ + data_[FX] = fx; + data_[FY] = fy; + data_[CX] = cx; + data_[CY] = cy; +} + +void PinholeCamera::print(std::ostream& stream) const +{ + stream << type() << ":\n" + << " uuid: " << uuid() << "\n" + << " size: " << size() << "\n" + << " landmark id: " << id() << "\n" + << " data:\n" + << " - cx: " << cx() << "\n" + << " - cy: " << cy() << "\n" + << " - fx: " << fx() << "\n" + << " - fy: " << fy() << "\n"; +} + +} // namespace fuse_variables + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::PinholeCamera); +PLUGINLIB_EXPORT_CLASS(fuse_variables::PinholeCamera, fuse_core::Variable); diff --git a/fuse_variables/src/pinhole_camera_fixed.cpp b/fuse_variables/src/pinhole_camera_fixed.cpp new file mode 100644 index 000000000..472663388 --- /dev/null +++ b/fuse_variables/src/pinhole_camera_fixed.cpp @@ -0,0 +1,63 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2023, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (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_variables +{ +PinholeCameraFixed::PinholeCameraFixed(const uint64_t& camera_id) : + PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id) +{ +} + +PinholeCameraFixed::PinholeCameraFixed(const uint64_t& camera_id, + const double& fx, const double& fy, + const double& cx, const double& cy) + : PinholeCamera(fuse_core::uuid::generate(detail::type(), camera_id), camera_id, fx, fy, cx, cy) +{ +} + +} // namespace fuse_variables + +BOOST_CLASS_EXPORT_IMPLEMENT(fuse_variables::PinholeCameraFixed); +PLUGINLIB_EXPORT_CLASS(fuse_variables::PinholeCameraFixed, fuse_core::Variable); diff --git a/fuse_variables/src/point_2d_fixed_landmark.cpp b/fuse_variables/src/point_2d_fixed_landmark.cpp index ab00f94d3..05e102621 100644 --- a/fuse_variables/src/point_2d_fixed_landmark.cpp +++ b/fuse_variables/src/point_2d_fixed_landmark.cpp @@ -35,35 +35,16 @@ #include #include -#include -#include +#include +#include #include -#include - namespace fuse_variables { Point2DFixedLandmark::Point2DFixedLandmark(const uint64_t& landmark_id) : - FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) -{ -} - -void Point2DFixedLandmark::print(std::ostream& stream) const -{ - stream << type() << ":\n" - << " uuid: " << uuid() << "\n" - << " size: " << size() << "\n" - << " landmark id: " << id() << "\n" - << " data:\n" - << " - x: " << x() << "\n" - << " - y: " << y() << "\n"; -} - -bool Point2DFixedLandmark::holdConstant() const + Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) { - return true; } } // namespace fuse_variables diff --git a/fuse_variables/src/point_2d_landmark.cpp b/fuse_variables/src/point_2d_landmark.cpp index 603b46ed2..9d8b9fe2e 100644 --- a/fuse_variables/src/point_2d_landmark.cpp +++ b/fuse_variables/src/point_2d_landmark.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include @@ -44,12 +44,17 @@ namespace fuse_variables { -Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) : - FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), +Point2DLandmark::Point2DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) : + FixedSizeVariable(uuid), id_(landmark_id) { } +Point2DLandmark::Point2DLandmark(const uint64_t& landmark_id) : + Point2DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) +{ +} + void Point2DLandmark::print(std::ostream& stream) const { stream << type() << ":\n" diff --git a/fuse_variables/src/point_3d_fixed_landmark.cpp b/fuse_variables/src/point_3d_fixed_landmark.cpp index 7b92113f5..f3288bcb7 100644 --- a/fuse_variables/src/point_3d_fixed_landmark.cpp +++ b/fuse_variables/src/point_3d_fixed_landmark.cpp @@ -35,36 +35,16 @@ #include #include -#include -#include +#include +#include #include -#include - namespace fuse_variables { Point3DFixedLandmark::Point3DFixedLandmark(const uint64_t& landmark_id) : - FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), - id_(landmark_id) -{ -} - -void Point3DFixedLandmark::print(std::ostream& stream) const -{ - stream << type() << ":\n" - << " uuid: " << uuid() << "\n" - << " size: " << size() << "\n" - << " landmark id: " << id() << "\n" - << " data:\n" - << " - x: " << x() << "\n" - << " - y: " << y() << "\n" - << " - z: " << z() << "\n"; -} - -bool Point3DFixedLandmark::holdConstant() const + Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) { - return true; } } // namespace fuse_variables diff --git a/fuse_variables/src/point_3d_landmark.cpp b/fuse_variables/src/point_3d_landmark.cpp index 7743b13d4..4010f01db 100644 --- a/fuse_variables/src/point_3d_landmark.cpp +++ b/fuse_variables/src/point_3d_landmark.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include @@ -44,12 +44,17 @@ namespace fuse_variables { -Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) : - FixedSizeVariable(fuse_core::uuid::generate(detail::type(), landmark_id)), +Point3DLandmark::Point3DLandmark(const fuse_core::UUID& uuid, const uint64_t& landmark_id) : + FixedSizeVariable(uuid), id_(landmark_id) { } +Point3DLandmark::Point3DLandmark(const uint64_t& landmark_id) : + Point3DLandmark(fuse_core::uuid::generate(detail::type(), landmark_id), landmark_id) +{ +} + void Point3DLandmark::print(std::ostream& stream) const { stream << type() << ":\n" diff --git a/fuse_variables/src/position_2d_stamped.cpp b/fuse_variables/src/position_2d_stamped.cpp index 0f7411f45..e653937f7 100644 --- a/fuse_variables/src/position_2d_stamped.cpp +++ b/fuse_variables/src/position_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/position_3d_stamped.cpp b/fuse_variables/src/position_3d_stamped.cpp index 66efcccbb..65dd23a0e 100644 --- a/fuse_variables/src/position_3d_stamped.cpp +++ b/fuse_variables/src/position_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/velocity_angular_2d_stamped.cpp b/fuse_variables/src/velocity_angular_2d_stamped.cpp index 5da8cf624..176808256 100644 --- a/fuse_variables/src/velocity_angular_2d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/velocity_angular_3d_stamped.cpp b/fuse_variables/src/velocity_angular_3d_stamped.cpp index 6756716ff..5c565589e 100644 --- a/fuse_variables/src/velocity_angular_3d_stamped.cpp +++ b/fuse_variables/src/velocity_angular_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/velocity_linear_2d_stamped.cpp b/fuse_variables/src/velocity_linear_2d_stamped.cpp index 8dae97c31..505368111 100644 --- a/fuse_variables/src/velocity_linear_2d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_2d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/src/velocity_linear_3d_stamped.cpp b/fuse_variables/src/velocity_linear_3d_stamped.cpp index 901aaa0a4..8661c7ec6 100644 --- a/fuse_variables/src/velocity_linear_3d_stamped.cpp +++ b/fuse_variables/src/velocity_linear_3d_stamped.cpp @@ -36,7 +36,7 @@ #include #include #include -#include +#include #include #include diff --git a/fuse_variables/test/test_orientation_2d_stamped.cpp b/fuse_variables/test/test_orientation_2d_stamped.cpp index 499215aa8..5a2e6902e 100644 --- a/fuse_variables/test/test_orientation_2d_stamped.cpp +++ b/fuse_variables/test/test_orientation_2d_stamped.cpp @@ -240,7 +240,7 @@ TEST(Orientation2DStamped, Optimization) { // Create a Orientation2DStamped Orientation2DStamped orientation(ros::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); - orientation.yaw() = 1.5; + orientation.setYaw(1.5); // Create a simple a constraint ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); @@ -264,14 +264,14 @@ TEST(Orientation2DStamped, Optimization) ceres::Solve(options, &problem, &summary); // Check - EXPECT_NEAR(3.0, orientation.yaw(), 1.0e-5); + EXPECT_NEAR(3.0, orientation.getYaw(), 1.0e-5); } TEST(Orientation2DStamped, Serialization) { // Create a Orientation2DStamped Orientation2DStamped expected(ros::Time(12345678, 910111213), fuse_core::uuid::generate("hal9000")); - expected.yaw() = 1.5; + expected.setYaw(1.5); // Serialize the variable into an archive std::stringstream stream; @@ -290,7 +290,7 @@ TEST(Orientation2DStamped, Serialization) // Compare EXPECT_EQ(expected.deviceId(), actual.deviceId()); EXPECT_EQ(expected.stamp(), actual.stamp()); - EXPECT_EQ(expected.yaw(), actual.yaw()); + EXPECT_EQ(expected.getYaw(), actual.getYaw()); } int main(int argc, char **argv) diff --git a/fuse_variables/test/test_pinhole_camera.cpp b/fuse_variables/test/test_pinhole_camera.cpp new file mode 100644 index 000000000..5a0569ef3 --- /dev/null +++ b/fuse_variables/test/test_pinhole_camera.cpp @@ -0,0 +1,462 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2021, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +using fuse_variables::PinholeCamera; + +TEST(PinholeCamera, Type) +{ + PinholeCamera variable(0); + EXPECT_EQ("fuse_variables::PinholeCamera", variable.type()); +} + +TEST(PinholeCamera, UUID) +{ + // Verify two positions with the same landmark ids produce the same uuids + { + PinholeCamera variable1(0); + PinholeCamera variable2(0); + EXPECT_EQ(variable1.uuid(), variable2.uuid()); + } + + // Verify two positions with the different landmark ids produce different uuids + { + PinholeCamera variable1(0); + PinholeCamera variable2(1); + EXPECT_NE(variable1.uuid(), variable2.uuid()); + } +} + +struct CostFunctor +{ + CostFunctor() + { + } + + template + bool operator()(const T* const k, T* residual) const + { + residual[0] = k[0] - T(1.2); + residual[1] = k[1] + T(0.8); + residual[2] = k[2] - T(0.51); + residual[3] = k[3] - T(0.49); + return true; + } +}; + +TEST(PinholeCamera, Optimization) +{ + // Create a Point3DLandmark + PinholeCamera K(0); + K.fx() = 4.1; + K.fy() = 3.5; + K.cx() = 5; + K.cy() = 2.49; + + // Create a simple a constraint + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(K.data(), K.size()); + std::vector parameter_blocks; + parameter_blocks.push_back(K.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(1.2, K.fx(), 1.0e-5); + EXPECT_NEAR(-0.8, K.fy(), 1.0e-5); + EXPECT_NEAR(0.51, K.cx(), 1.0e-5); + EXPECT_NEAR(0.49, K.cy(), 1.0e-5); +} + +struct FuseProjectionCostFunctor +{ + Eigen::Matrix X; + Eigen::Matrix x; + + FuseProjectionCostFunctor() + { + // Define 3D Points + X << (-1.0), (-1.0), (9.0), // NOLINT + (-1.0), (-1.0), (11.0), // NOLINT + (-1.0), (1.0), (9.0), // NOLINT + (-1.0), (1.0), (11.0), // NOLINT + (1.0), (-1.0), (9.0), // NOLINT + (1.0), (-1.0), (11.0), // NOLINT + (1.0), (1.0), (9.0), // NOLINT + (1.0), (1.0), (11.0); // NOLINT + + // Define 2D Points + x << (239.36340595030663), (166.35226250921954), (1.0), // NOLINT + (252.25926024520876), (179.34432670587358), (1.0), // NOLINT + (239.36340595030663), (309.26496867241400), (1.0), // NOLINT + (252.25926024520876), (296.27290447575996), (1.0), // NOLINT + (381.21780319423020), (166.35226250921954), (1.0), // NOLINT + (368.32194889932810), (179.34432670587358), (1.0), // NOLINT + (381.21780319423020), (309.26496867241400), (1.0), // NOLINT + (368.32194889932810), (296.27290447575996), (1.0); // NOLINT + } + + /* FuseProjectionCostFunctor + * Projects a set of 3D poitns X to 2D points x using the equation: + * x = KX + * and optiimzes K using a reprojection error as residual + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Create Matrix + Eigen::Matrix K(3, 3); + K << k[0], T(0.0), k[2], // NOLINT + T(0.0), k[1], k[3], // NOLINT + T(0.0), T(0.0), T(1.0); // NOLINT + + Eigen::Matrix xp(8, 3); + xp = (K * X.cast().transpose()); + + // TODO(omendez): There is probably a better way to do this using hnormalized on operation above. + xp.transposeInPlace(); + xp = xp.array().colwise() / X.cast().col(2).array(); + + Eigen::Matrix d = x.cast() - xp; + + for (uint i = 0; i < 8; i++) + { + residual[i * 2] = T(d.row(i)[0]); + residual[i * 2 + 1] = T(d.row(i)[1]); + } + + return true; + } +}; + +TEST(PinholeCamera, FuseProjectionOptimization) +{ + // Create a Camera to Optimize + PinholeCamera K(0); + K.fx() = 640.0; // fx == w + K.fy() = 640.0; // fy == w + K.cx() = 320.0; // cx == w/2 + K.cy() = 240.0; // cy == h/2 + + // Expected Intrinsics + PinholeCamera expected(0); + expected.fx() = 638.34478759765620; + expected.fy() = 643.10717773437500; + expected.cx() = 310.29060457226840; + expected.cy() = 237.80861559081677; + + // Create a simple a constraint + // (NOTE: Separating X and Y residuals is significantly better than joint distance. + // Therefore, 16 residuals for 8 points.) + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new FuseProjectionCostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(K.data(), K.size()); + std::vector parameter_blocks; + parameter_blocks.push_back(K.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), K.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), K.fy(), 0.1); + EXPECT_NEAR(expected.cx(), K.cx(), 0.1); + EXPECT_NEAR(expected.cy(), K.cy(), 0.1); +} + +struct ProjectionCostFunctor +{ + double X[8][3]; // 3D Points (2x2 Cube at 0,0,10) + double x[8][2]; // 2D Points (Projection of Cube w/GT intrinsics) + ProjectionCostFunctor() + : // Define 3D Points (2x2 Cube at 0,0,10) + X // NOLINT + { + -1.0, -1.0, 9.0, // NOLINT + -1.0, -1.0, 11.0, // NOLINT + -1.0, 1.0, 9.0, // NOLINT + -1.0, 1.0, 11.0, // NOLINT + 1.0, -1.0, 9.0, // NOLINT + 1.0, -1.0, 11.0, // NOLINT + 1.0, 1.0, 9.0, // NOLINT + 1.0, 1.0, 11.0 // NOLINT + } + , + // Define 2D Points + x // NOLINT + { + 239.36340595030663, 166.35226250921954, // NOLINT + 252.25926024520876, 179.34432670587358, // NOLINT + 239.36340595030663, 309.26496867241400, // NOLINT + 252.25926024520876, 296.27290447575996, // NOLINT + 381.21780319423020, 166.35226250921954, // NOLINT + 368.32194889932810, 179.34432670587358, // NOLINT + 381.21780319423020, 309.26496867241400, // NOLINT + 368.32194889932810, 296.27290447575996 // NOLINT + } + { + } + + /* ProjectionCostFunctor + * Projects a set of 3D points X to 2D points x using the equation: + * x = (x*f_x + z * c_x)/z + * y = (y*f_y + z * c_y)/z + * and optiimzes f_x, f_y, c_x, c_y using a reprojection error as residual. + * (NOTE: Separating X and Y residuals is significantly better than joint distance. + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Do Projection Manually + T xp[8][2]; + for (uint i = 0; i < 8; i++) + { + xp[i][0] = (T(X[i][0]) * k[0] + T(X[i][2]) * k[2]) / T(X[i][2]); // x = (x*f_x + z * c_x)/z + xp[i][1] = (T(X[i][1]) * k[1] + T(X[i][2]) * k[3]) / T(X[i][2]); // y = (y*f_y + z * c_y)/z + + T xerr = xp[i][0] - T(x[i][0]); + T yerr = xp[i][1] - T(x[i][1]); + residual[i * 2] = xerr; + residual[i * 2 + 1] = yerr; + } + return true; + } +}; + +TEST(PinholeCamera, ProjectionOptimization) +{ + // Create a Camera to Optimize + PinholeCamera K(0); + K.fx() = 640.0; // fx == w + K.fy() = 640.0; // fy == w + K.cx() = 320.0; // cx == w/2 + K.cy() = 240.0; // cy == h/2 + + PinholeCamera expected(0); + expected.fx() = 638.34478759765620; + expected.fy() = 643.10717773437500; + expected.cx() = 310.29060457226840; + expected.cy() = 237.80861559081677; + + // Create a simple a constraint + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new ProjectionCostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(K.data(), K.size()); + std::vector parameter_blocks; + parameter_blocks.push_back(K.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), K.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), K.fy(), 0.1); + EXPECT_NEAR(expected.cx(), K.cx(), 0.1); + EXPECT_NEAR(expected.cy(), K.cy(), 0.1); +} + +struct PerPointProjectionCostFunctor +{ + double pt3d_x; + double pt3d_y; + double pt3d_z; + double observed_x; + double observed_y; + + PerPointProjectionCostFunctor(double pt3d_x, double pt3d_y, double pt3d_z, double observed_x, double observed_y) + : pt3d_x(pt3d_x), pt3d_y(pt3d_y), pt3d_z(pt3d_z), observed_x(observed_x), observed_y(observed_y) + { + } + + /* PerPointProjectionCostFunctor + * + * Projects a 3D point XYZ to 2D xy points using the equation: + * x = (X*f_x + Z * c_x)/Z + * y = (Y*f_y + Z * c_y)/Z + * and optiimzes f_x, f_y, c_x, c_y using a reprojection error as residual. + * (NOTE: Separating X and Y residuals is significantly better than joint distance. + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Do Projection Manually + T xp = (T(pt3d_x) * k[0] + T(pt3d_z) * k[2]) / T(pt3d_z); // x = (x*f_x + z * c_x)/z + T yp = (T(pt3d_y) * k[1] + T(pt3d_z) * k[3]) / T(pt3d_z); // y = (y*f_y + z * c_y)/z + + residual[0] = T(xp - T(observed_x)); + residual[1] = T(yp - T(observed_y)); + + return true; + } +}; + +TEST(PinholeCamera, PerPointProjectionCostFunctor) +{ + // Create a Camera to Optimize + PinholeCamera K(0); + K.fx() = 645.0; // fx == w + K.fy() = 635.0; // fy == w + K.cx() = 325.0; // cx == w/2 + K.cy() = 245.0; // cy == h/2 + + PinholeCamera expected(0); + expected.fx() = 638.34478759765620; + expected.fy() = 643.10717773437500; + expected.cx() = 310.29060457226840; + expected.cy() = 237.80861559081677; + + double X[8][3] = { -1.0, -1.0, 9.0, // NOLINT + -1.0, -1.0, 11.0, // NOLINT + -1.0, 1.0, 9.0, // NOLINT + -1.0, 1.0, 11.0, // NOLINT + 1.0, -1.0, 9.0, // NOLINT + 1.0, -1.0, 11.0, // NOLINT + 1.0, 1.0, 9.0, // NOLINT + 1.0, 1.0, 11.0 }; // NOLINT + + // Define 2D Points + double pts2d[8][2] = { 239.36340595030663, 166.35226250921954, // NOLINT + 252.25926024520876, 179.34432670587358, // NOLINT + 239.36340595030663, 309.26496867241400, // NOLINT + 252.25926024520876, 296.27290447575996, // NOLINT + 381.21780319423020, 166.35226250921954, // NOLINT + 368.32194889932810, 179.34432670587358, // NOLINT + 381.21780319423020, 309.26496867241400, // NOLINT + 368.32194889932810, 296.27290447575996 }; // NOLINT + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(K.data(), K.size()); + + std::vector camera_parameter_blocks; + camera_parameter_blocks.push_back(K.data()); + + for (uint i = 0; i < 8; i++) + { + // Create a simple a constraint + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction( + new PerPointProjectionCostFunctor(X[i][0], X[i][1], X[i][2], pts2d[i][0], pts2d[i][1])); + problem.AddResidualBlock(cost_function, nullptr, K.data()); + } + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), K.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), K.fy(), 0.1); + EXPECT_NEAR(expected.cx(), K.cx(), 0.1); + EXPECT_NEAR(expected.cy(), K.cy(), 0.1); +} + +TEST(Point3DLandmark, Serialization) +{ + // Create a Point3DLandmark + PinholeCamera expected(0); + expected.fx() = 640; + expected.fy() = 480; + expected.cx() = 320; + expected.cy() = 240; + + // Serialize the variable into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new variable from that same stream + PinholeCamera actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.id(), actual.id()); + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.fx(), actual.fx()); + EXPECT_EQ(expected.fy(), actual.fy()); + EXPECT_EQ(expected.cx(), actual.cx()); + EXPECT_EQ(expected.cy(), actual.cy()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/fuse_variables/test/test_pinhole_camera_fixed.cpp b/fuse_variables/test/test_pinhole_camera_fixed.cpp new file mode 100644 index 000000000..834b3ff2b --- /dev/null +++ b/fuse_variables/test/test_pinhole_camera_fixed.cpp @@ -0,0 +1,478 @@ +/* + * Software License Agreement (BSD License) + * + * Author: Oscar Mendez + * Created: 11.13.2023 + * + * Copyright (c) 2021, Locus Robotics + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of the copyright holder nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ +#include +#include +#include +#include + +#include +#include +#include +#include + +#include +#include + +using fuse_variables::PinholeCameraFixed; + +TEST(PinholeCameraFixed, Type) +{ + PinholeCameraFixed variable(0); + EXPECT_EQ("fuse_variables::PinholeCameraFixed", variable.type()); +} + +TEST(PinholeCameraFixed, UUID) +{ + // Verify two positions with the same landmark ids produce the same uuids + { + PinholeCameraFixed variable1(0); + PinholeCameraFixed variable2(0); + EXPECT_EQ(variable1.uuid(), variable2.uuid()); + } + + // Verify two positions with the different landmark ids produce different uuids + { + PinholeCameraFixed variable1(0); + PinholeCameraFixed variable2(1); + EXPECT_NE(variable1.uuid(), variable2.uuid()); + } +} + +struct CostFunctor +{ + CostFunctor() + { + } + + template + bool operator()(const T* const k, T* residual) const + { + residual[0] = k[0] - T(1.2); + residual[1] = k[1] + T(0.8); + residual[2] = k[2] - T(0.51); + residual[3] = k[3] - T(0.49); + return true; + } +}; + +TEST(PinholeCameraFixed, Optimization) +{ + // Create a Point3DLandmark + PinholeCameraFixed K(0); + K.fx() = 4.10; + K.fy() = 3.50; + K.cx() = 5.00; + K.cy() = 2.49; + + // Create a simple a constraint + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction(new CostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(K.data(), K.size()); + std::vector parameter_blocks; + parameter_blocks.push_back(K.data()); + + if (K.holdConstant()) + { + problem.SetParameterBlockConstant(K.data()); + } + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + // Run the solver + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(4.10, K.fx(), 1.0e-5); + EXPECT_NEAR(3.50, K.fy(), 1.0e-5); + EXPECT_NEAR(5.00, K.cx(), 1.0e-5); + EXPECT_NEAR(2.49, K.cy(), 1.0e-5); +} + +struct FuseProjectionCostFunctor +{ + Eigen::Matrix X; + Eigen::Matrix x; + + FuseProjectionCostFunctor() + { + // Define 3D Points + X << (-1.0), (-1.0), (9.0), // NOLINT + (-1.0), (-1.0), (11.0), // NOLINT + (-1.0), (1.0), (9.0), // NOLINT + (-1.0), (1.0), (11.0), // NOLINT + (1.0), (-1.0), (9.0), // NOLINT + (1.0), (-1.0), (11.0), // NOLINT + (1.0), (1.0), (9.0), // NOLINT + (1.0), (1.0), (11.0); // NOLINT + + // Define 2D Points + x << (239.36340595030663), (166.35226250921954), (1.0), // NOLINT + (252.25926024520876), (179.34432670587358), (1.0), // NOLINT + (239.36340595030663), (309.26496867241400), (1.0), // NOLINT + (252.25926024520876), (296.27290447575996), (1.0), // NOLINT + (381.21780319423020), (166.35226250921954), (1.0), // NOLINT + (368.32194889932810), (179.34432670587358), (1.0), // NOLINT + (381.21780319423020), (309.26496867241400), (1.0), // NOLINT + (368.32194889932810), (296.27290447575996), (1.0); // NOLINT + } + + /* FuseProjectionCostFunctor + * Projects a set of 3D poitns X to 2D points x using the equation: + * x = KX + * and optiimzes K using a reprojection error as residual + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Create Matrix + Eigen::Matrix K(3, 3); + K << k[0], T(0.0), k[2], // NOLINT + T(0.0), k[1], k[3], // NOLINT + T(0.0), T(0.0), T(1.0); // NOLINT + + Eigen::Matrix xp(8, 3); + xp = (K * X.cast().transpose()); + + // TODO(omendez): There is probably a better way to do this using hnormalized on operation above. + xp.transposeInPlace(); + xp = xp.array().colwise() / X.cast().col(2).array(); + + Eigen::Matrix d = x.cast() - xp; + + for (uint i = 0; i < 8; i++) + { + residual[i * 2] = T(d.row(i)[0]); + residual[i * 2 + 1] = T(d.row(i)[1]); + } + + return true; + } +}; + +TEST(PinholeCameraFixed, FuseProjectionOptimization) +{ + // Create a Camera to Optimize + PinholeCameraFixed K(0); + K.fx() = 640.0; // fx == w + K.fy() = 640.0; // fy == w + K.cx() = 320.0; // cx == w/2 + K.cy() = 240.0; // cy == h/2 + + // Expected Intrinsics + PinholeCameraFixed expected(0); + expected.fx() = 640.0; + expected.fy() = 640.0; + expected.cx() = 320.0; + expected.cy() = 240.0; + + // Create a simple a constraint + // (NOTE: Separating X and Y residuals is significantly better than joint distance. + // Therefore, 16 residuals for 8 points.) + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new FuseProjectionCostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(K.data(), K.size()); + std::vector parameter_blocks; + parameter_blocks.push_back(K.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + if (K.holdConstant()) + { + problem.SetParameterBlockConstant(K.data()); + } + + // Run the solver + ceres::Solver::Options options; + options.linear_solver_type = ceres::DENSE_SCHUR; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), K.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), K.fy(), 0.1); + EXPECT_NEAR(expected.cx(), K.cx(), 0.1); + EXPECT_NEAR(expected.cy(), K.cy(), 0.1); +} + +struct ProjectionCostFunctor +{ + double X[8][3]; // 3D Points (2x2 Cube at 0,0,10) + double x[8][2]; // 2D Points (Projection of Cube w/GT intrinsics) + ProjectionCostFunctor() + : // Define 3D Points (2x2 Cube at 0,0,10) + X // NOLINT + { + -1.0, -1.0, 9.0, // NOLINT + -1.0, -1.0, 11.0, // NOLINT + -1.0, 1.0, 9.0, // NOLINT + -1.0, 1.0, 11.0, // NOLINT + 1.0, -1.0, 9.0, // NOLINT + 1.0, -1.0, 11.0, // NOLINT + 1.0, 1.0, 9.0, // NOLINT + 1.0, 1.0, 11.0 // NOLINT + } + , + // Define 2D Points + x // NOLINT + { + 239.36340595030663, 166.35226250921954, // NOLINT + 252.25926024520876, 179.34432670587358, // NOLINT + 239.36340595030663, 309.26496867241400, // NOLINT + 252.25926024520876, 296.27290447575996, // NOLINT + 381.21780319423020, 166.35226250921954, // NOLINT + 368.32194889932810, 179.34432670587358, // NOLINT + 381.21780319423020, 309.26496867241400, // NOLINT + 368.32194889932810, 296.27290447575996 // NOLINT + } + { + } + + /* ProjectionCostFunctor + * Projects a set of 3D points X to 2D points x using the equation: + * x = (x*f_x + z * c_x)/z + * y = (y*f_y + z * c_y)/z + * and optiimzes f_x, f_y, c_x, c_y using a reprojection error as residual. + * (NOTE: Separating X and Y residuals is significantly better than joint distance. + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Do Projection Manually + T xp[8][2]; + for (uint i = 0; i < 8; i++) + { + xp[i][0] = (T(X[i][0]) * k[0] + T(X[i][2]) * k[2]) / T(X[i][2]); // x = (x*f_x + z * c_x)/z + xp[i][1] = (T(X[i][1]) * k[1] + T(X[i][2]) * k[3]) / T(X[i][2]); // y = (y*f_y + z * c_y)/z + + T xerr = xp[i][0] - T(x[i][0]); + T yerr = xp[i][1] - T(x[i][1]); + residual[i * 2] = xerr; + residual[i * 2 + 1] = yerr; + } + return true; + } +}; + +TEST(PinholeCameraFixed, ProjectionOptimization) +{ + // Create a Camera to Optimize + PinholeCameraFixed K(0, 640, 640, 320, 240); + + PinholeCameraFixed expected(0); + expected.fx() = 640.0; + expected.fy() = 640.0; + expected.cx() = 320.0; + expected.cy() = 240.0; + + // Create a simple a constraint + ceres::CostFunction* cost_function = + new ceres::AutoDiffCostFunction(new ProjectionCostFunctor()); + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(K.data(), K.size()); + std::vector parameter_blocks; + parameter_blocks.push_back(K.data()); + problem.AddResidualBlock(cost_function, nullptr, parameter_blocks); + + if (K.holdConstant()) + { + problem.SetParameterBlockConstant(K.data()); + } + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), K.fx(), 1e-5); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), K.fy(), 1e-5); + EXPECT_NEAR(expected.cx(), K.cx(), 1e-5); + EXPECT_NEAR(expected.cy(), K.cy(), 1e-5); +} + +struct PerPointProjectionCostFunctor +{ + double pt3d_x; + double pt3d_y; + double pt3d_z; + double observed_x; + double observed_y; + + PerPointProjectionCostFunctor(double pt3d_x, double pt3d_y, double pt3d_z, double observed_x, double observed_y) + : pt3d_x(pt3d_x), pt3d_y(pt3d_y), pt3d_z(pt3d_z), observed_x(observed_x), observed_y(observed_y) + { + } + + /* PerPointProjectionCostFunctor + * + * Projects a 3D point XYZ to 2D xy points using the equation: + * x = (X*f_x + Z * c_x)/Z + * y = (Y*f_y + Z * c_y)/Z + * and optiimzes f_x, f_y, c_x, c_y using a reprojection error as residual. + * (NOTE: Separating X and Y residuals is significantly better than joint distance. + * + */ + + template + bool operator()(const T* const k, T* residual) const + { + // Do Projection Manually + T xp = (T(pt3d_x) * k[0] + T(pt3d_z) * k[2]) / T(pt3d_z); // x = (x*f_x + z * c_x)/z + T yp = (T(pt3d_y) * k[1] + T(pt3d_z) * k[3]) / T(pt3d_z); // y = (y*f_y + z * c_y)/z + + residual[0] = T(xp - T(observed_x)); + residual[1] = T(yp - T(observed_y)); + + return true; + } +}; + +TEST(PinholeCameraFixed, PerPointProjectionCostFunctor) +{ + // Create a Camera to Optimize + PinholeCameraFixed K(0); + K.fx() = 645.0; // fx == w + K.fy() = 635.0; // fy == w + K.cx() = 325.0; // cx == w/2 + K.cy() = 245.0; // cy == h/2 + + PinholeCameraFixed expected(0); + expected.fx() = 645.0; + expected.fy() = 635.0; + expected.cx() = 325.0; + expected.cy() = 245.0; + + double X[8][3] = { -1.0, -1.0, 9.0, // NOLINT + -1.0, -1.0, 11.0, // NOLINT + -1.0, 1.0, 9.0, // NOLINT + -1.0, 1.0, 11.0, // NOLINT + 1.0, -1.0, 9.0, // NOLINT + 1.0, -1.0, 11.0, // NOLINT + 1.0, 1.0, 9.0, // NOLINT + 1.0, 1.0, 11.0 }; // NOLINT + + // Define 2D Points + double pts2d[8][2] = { 239.36340595030663, 166.35226250921954, // NOLINT + 252.25926024520876, 179.34432670587358, // NOLINT + 239.36340595030663, 309.26496867241400, // NOLINT + 252.25926024520876, 296.27290447575996, // NOLINT + 381.21780319423020, 166.35226250921954, // NOLINT + 368.32194889932810, 179.34432670587358, // NOLINT + 381.21780319423020, 309.26496867241400, // NOLINT + 368.32194889932810, 296.27290447575996 }; // NOLINT + + // Build the problem. + ceres::Problem problem; + problem.AddParameterBlock(K.data(), K.size()); + + std::vector camera_parameter_blocks; + camera_parameter_blocks.push_back(K.data()); + + if (K.holdConstant()) + { + problem.SetParameterBlockConstant(K.data()); + } + + for (uint i = 0; i < 8; i++) + { + // Create a simple a constraint + ceres::CostFunction* cost_function = new ceres::AutoDiffCostFunction( + new PerPointProjectionCostFunctor(X[i][0], X[i][1], X[i][2], pts2d[i][0], pts2d[i][1])); + problem.AddResidualBlock(cost_function, nullptr, K.data()); + } + + // Run the solver + ceres::Solver::Options options; + ceres::Solver::Summary summary; + ceres::Solve(options, &problem, &summary); + + // Check + EXPECT_NEAR(expected.fx(), K.fx(), 0.1); // 0.1 Pixel Error + EXPECT_NEAR(expected.fy(), K.fy(), 0.1); + EXPECT_NEAR(expected.cx(), K.cx(), 0.1); + EXPECT_NEAR(expected.cy(), K.cy(), 0.1); +} + +TEST(PinholeCameraFixed, Serialization) +{ + // Create a Point3DLandmark + PinholeCameraFixed expected(0); + expected.fx() = 640; + expected.fy() = 480; + expected.cx() = 320; + expected.cy() = 240; + + // Serialize the variable into an archive + std::stringstream stream; + { + fuse_core::TextOutputArchive archive(stream); + expected.serialize(archive); + } + + // Deserialize a new variable from that same stream + PinholeCameraFixed actual; + { + fuse_core::TextInputArchive archive(stream); + actual.deserialize(archive); + } + + // Compare + EXPECT_EQ(expected.id(), actual.id()); + EXPECT_EQ(expected.uuid(), actual.uuid()); + EXPECT_EQ(expected.fx(), actual.fx()); + EXPECT_EQ(expected.fy(), actual.fy()); + EXPECT_EQ(expected.cx(), actual.cx()); + EXPECT_EQ(expected.cy(), actual.cy()); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/fuse_viz/CHANGELOG.rst b/fuse_viz/CHANGELOG.rst index 5e9bb4ac0..7cbbe54f3 100644 --- a/fuse_viz/CHANGELOG.rst +++ b/fuse_viz/CHANGELOG.rst @@ -14,6 +14,22 @@ Changelog for package fuse_viz * Adding doxygen to all packages (#241) * Contributors: Gary Servin, Tom Moore +0.7.0 (2023-09-25) +------------------ +* [RST-7809] Fix optimization errors when the orientation is initialized at +PI (#334) + * Add some unit tests for the 2D orientation constraints; Create getters/setters for the 2D orientation variable is preparation for a fix. + * Force the 2D orientation value to be is minimum phase +* Update devel to build on Ubuntu Jammy (22.04) (#326) + * Update to C++17 for use with Ubuntu Jammy + * Include Rviz and Eigen as system includes, which supresses warnings within the included libraries + * use pluginlib and class_list_macros .hpp include instead of deprecated .h From: Lucas Walter +* 0.6.0 +* Update changelogs +* 0.5.0 +* Update changelogs +* Adding doxygen to all packages (#241) +* Contributors: Gary Servin, Stephen Williams, Tom Moore + 0.4.2 (2021-07-20) ------------------ * Adding roslint dependency to fuse_viz (`#231 `_) diff --git a/fuse_viz/CMakeLists.txt b/fuse_viz/CMakeLists.txt index aa9a0ec44..bde535755 100644 --- a/fuse_viz/CMakeLists.txt +++ b/fuse_viz/CMakeLists.txt @@ -73,6 +73,9 @@ add_dependencies(${PROJECT_NAME} target_include_directories(${PROJECT_NAME} PUBLIC include +) +target_include_directories(${PROJECT_NAME} + SYSTEM PUBLIC ${Boost_INCLUDE_DIRS} ${catkin_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} @@ -85,7 +88,7 @@ target_link_libraries(${PROJECT_NAME} ) set_target_properties(${PROJECT_NAME} PROPERTIES - CXX_STANDARD 14 + CXX_STANDARD 17 CXX_STANDARD_REQUIRED YES ) diff --git a/fuse_viz/include/fuse_viz/conversions.h b/fuse_viz/include/fuse_viz/conversions.h index 458082a4f..8f8d16b41 100644 --- a/fuse_viz/include/fuse_viz/conversions.h +++ b/fuse_viz/include/fuse_viz/conversions.h @@ -90,7 +90,7 @@ inline tf2::Vector3 toTF(const fuse_variables::Position2DStamped& position) inline tf2::Quaternion toTF(const fuse_variables::Orientation2DStamped& orientation) { - return { tf2::Vector3{ 0, 0, 1 }, orientation.yaw() }; + return { tf2::Vector3{ 0, 0, 1 }, orientation.getYaw() }; } inline tf2::Transform toTF(const fuse_variables::Position2DStamped& position, diff --git a/fuse_viz/package.xml b/fuse_viz/package.xml index 77c2e2e7e..1616d2dfc 100644 --- a/fuse_viz/package.xml +++ b/fuse_viz/package.xml @@ -1,7 +1,7 @@ fuse_viz - 0.6.0 + 0.7.0 The fuse_viz package provides visualization tools for fuse. diff --git a/fuse_viz/src/serialized_graph_display.cpp b/fuse_viz/src/serialized_graph_display.cpp index f95bbc8c6..90679f5e2 100644 --- a/fuse_viz/src/serialized_graph_display.cpp +++ b/fuse_viz/src/serialized_graph_display.cpp @@ -322,5 +322,5 @@ void SerializedGraphDisplay::processMessage(const fuse_msgs::SerializedGraph::Co } // namespace rviz -#include +#include PLUGINLIB_EXPORT_CLASS(rviz::SerializedGraphDisplay, rviz::Display)