From 66cd2124e428363dafb744b1557023bd3907fc3c Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Tue, 9 May 2023 11:26:57 +0200 Subject: [PATCH 1/8] unicycle motion model: add option to rotate the process noise covariance to the current state orientation using its yaw angle --- fuse_models/include/fuse_models/unicycle_2d.h | 5 +++- fuse_models/src/unicycle_2d.cpp | 25 ++++++++++++++++++- 2 files changed, 28 insertions(+), 2 deletions(-) diff --git a/fuse_models/include/fuse_models/unicycle_2d.h b/fuse_models/include/fuse_models/unicycle_2d.h index 1191bd34a..3d4778dce 100644 --- a/fuse_models/include/fuse_models/unicycle_2d.h +++ b/fuse_models/include/fuse_models/unicycle_2d.h @@ -178,7 +178,7 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * * @param[in] state1 The first/oldest state * @param[in] state2 The second/newest state - * @param[in] process_noise_covariance The process noise covariance, after it is scaled and multiplied by dt + * @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::Matrix8d& process_noise_covariance); @@ -189,6 +189,9 @@ class Unicycle2D : public fuse_core::AsyncMotionModel fuse_core::Matrix8d 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 + bool rotate_process_noise_covariance_to_state_orientation_{ false }; //!< Whether to rotate the process noise + //!< covariance to the orientation of the + //!< current state using its yaw angle 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, diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 103459fc9..8154cef15 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -204,6 +204,10 @@ void Unicycle2D::onInit() process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal(); private_node_handle_.param("scale_process_noise", scale_process_noise_, scale_process_noise_); + private_node_handle_.param("rotate_process_noise_covariance_to_state_orientation", + rotate_process_noise_covariance_to_state_orientation_, + rotate_process_noise_covariance_to_state_orientation_); + private_node_handle_.param("velocity_norm_min", velocity_norm_min_, velocity_norm_min_); private_node_handle_.param("disable_checks", disable_checks_, disable_checks_); @@ -348,8 +352,27 @@ void Unicycle2D::generateMotionModel( state_history_.emplace(beginning_stamp, std::move(state1)); state_history_.emplace(ending_stamp, std::move(state2)); - // Scale process noise covariance pose by the norm of the current state twist + auto process_noise_covariance = process_noise_covariance_; + + // Rotate the process noise covariance with the yaw angle of the current state orientation + if (rotate_process_noise_covariance_to_state_orientation_) + { + auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix(); + // apply to x and y position + process_noise_covariance.topLeftCorner<2, 2>() = + rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose(); + + // apply to x and y velocity + process_noise_covariance.block<2,2>(3,3) = rotation_matrix * process_noise_covariance.block<2,2>(3,3) * + rotation_matrix.transpose(); + + // apply to x and y acceleration + process_noise_covariance.block<2,2>(6,6) = rotation_matrix * process_noise_covariance.block<2,2>(6,6) * + 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_yaw, From 7238f0dc8a52035b0824c483a932452d1772e11e Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Tue, 9 May 2023 13:07:55 +0200 Subject: [PATCH 2/8] fix linter errors for unicycle motion model: rotate process noise covariance to state orientation --- fuse_models/src/unicycle_2d.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 8154cef15..347b2a7d5 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -364,11 +364,11 @@ void Unicycle2D::generateMotionModel( rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose(); // apply to x and y velocity - process_noise_covariance.block<2,2>(3,3) = rotation_matrix * process_noise_covariance.block<2,2>(3,3) * + process_noise_covariance.block<2, 2>(3, 3) = rotation_matrix * process_noise_covariance.block<2, 2>(3, 3) * rotation_matrix.transpose(); // apply to x and y acceleration - process_noise_covariance.block<2,2>(6,6) = rotation_matrix * process_noise_covariance.block<2,2>(6,6) * + process_noise_covariance.block<2, 2>(6, 6) = rotation_matrix * process_noise_covariance.block<2, 2>(6, 6) * rotation_matrix.transpose(); } From 05abcacef2f1c3a780d5a17352a88fb1f893c2c5 Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Wed, 10 May 2023 11:59:15 +0200 Subject: [PATCH 3/8] unicycle motion model: apply rotation of process noise covariance only to x and y position as the other state variables are already along the current state orientation --- fuse_models/src/unicycle_2d.cpp | 11 ++--------- 1 file changed, 2 insertions(+), 9 deletions(-) diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 347b2a7d5..2b410283c 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -359,17 +359,10 @@ void Unicycle2D::generateMotionModel( if (rotate_process_noise_covariance_to_state_orientation_) { auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix(); - // apply to x and y position + // apply only to x and y position as the other state variables are already along the + // current state orientation process_noise_covariance.topLeftCorner<2, 2>() = rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose(); - - // apply to x and y velocity - process_noise_covariance.block<2, 2>(3, 3) = rotation_matrix * process_noise_covariance.block<2, 2>(3, 3) * - rotation_matrix.transpose(); - - // apply to x and y acceleration - process_noise_covariance.block<2, 2>(6, 6) = rotation_matrix * process_noise_covariance.block<2, 2>(6, 6) * - rotation_matrix.transpose(); } // Scale process noise covariance pose by the norm of the current state twist From 555ac04618136165404bfe98ca2cfb35ced12720 Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Wed, 10 May 2023 13:17:51 +0200 Subject: [PATCH 4/8] unicycle motion model: rotate process noise with covariance: fixes for better coding style --- fuse_models/src/unicycle_2d.cpp | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 2b410283c..2b26028dc 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -205,8 +205,8 @@ void Unicycle2D::onInit() private_node_handle_.param("scale_process_noise", scale_process_noise_, scale_process_noise_); private_node_handle_.param("rotate_process_noise_covariance_to_state_orientation", - rotate_process_noise_covariance_to_state_orientation_, - rotate_process_noise_covariance_to_state_orientation_); + rotate_process_noise_covariance_to_state_orientation_, + rotate_process_noise_covariance_to_state_orientation_); private_node_handle_.param("velocity_norm_min", velocity_norm_min_, velocity_norm_min_); @@ -352,13 +352,12 @@ void Unicycle2D::generateMotionModel( 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 yaw angle of the current state orientation if (rotate_process_noise_covariance_to_state_orientation_) { - auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix(); + const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix(); // apply only to x and y position as the other state variables are already along the // current state orientation process_noise_covariance.topLeftCorner<2, 2>() = From 04592ef229a014b65e3c45ac00b66314c905fe57 Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Wed, 10 May 2023 13:24:37 +0200 Subject: [PATCH 5/8] unicycle motion model: rotate process noise covariance with state orientation: remove of option to enable/disable it by a ROS parameter and instead always rotate it --- fuse_models/include/fuse_models/unicycle_2d.h | 3 --- fuse_models/src/unicycle_2d.cpp | 16 +++++----------- 2 files changed, 5 insertions(+), 14 deletions(-) diff --git a/fuse_models/include/fuse_models/unicycle_2d.h b/fuse_models/include/fuse_models/unicycle_2d.h index 3d4778dce..167f83d98 100644 --- a/fuse_models/include/fuse_models/unicycle_2d.h +++ b/fuse_models/include/fuse_models/unicycle_2d.h @@ -189,9 +189,6 @@ class Unicycle2D : public fuse_core::AsyncMotionModel fuse_core::Matrix8d 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 - bool rotate_process_noise_covariance_to_state_orientation_{ false }; //!< Whether to rotate the process noise - //!< covariance to the orientation of the - //!< current state using its yaw angle 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, diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 2b26028dc..481c29f52 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -204,9 +204,6 @@ void Unicycle2D::onInit() process_noise_covariance_ = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal(); private_node_handle_.param("scale_process_noise", scale_process_noise_, scale_process_noise_); - private_node_handle_.param("rotate_process_noise_covariance_to_state_orientation", - rotate_process_noise_covariance_to_state_orientation_, - rotate_process_noise_covariance_to_state_orientation_); private_node_handle_.param("velocity_norm_min", velocity_norm_min_, velocity_norm_min_); @@ -355,14 +352,11 @@ void Unicycle2D::generateMotionModel( auto process_noise_covariance = process_noise_covariance_; // Rotate the process noise covariance with the yaw angle of the current state orientation - if (rotate_process_noise_covariance_to_state_orientation_) - { - const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix(); - // apply only to x and y position as the other state variables are already along the - // current state orientation - process_noise_covariance.topLeftCorner<2, 2>() = - rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose(); - } + const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix(); + // Apply only to x and y position as the other state variables are already along the + // current state orientation + process_noise_covariance.topLeftCorner<2, 2>() = + rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose(); // Scale process noise covariance pose by the norm of the current state twist if (scale_process_noise_) From 7f8b97a5dea42a67a25dc0f54460f4f2e79b9a84 Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Mon, 15 May 2023 11:09:53 +0200 Subject: [PATCH 6/8] unicycle motion model: create unit test for generateMotionModel and necessary rewrite in code to make creation of the unit test easier and separated --- fuse_models/include/fuse_models/unicycle_2d.h | 39 ++++- fuse_models/src/unicycle_2d.cpp | 76 +++++---- fuse_models/test/test_unicycle_2d.cpp | 148 +++++++++++++++++- 3 files changed, 231 insertions(+), 32 deletions(-) diff --git a/fuse_models/include/fuse_models/unicycle_2d.h b/fuse_models/include/fuse_models/unicycle_2d.h index 167f83d98..91494a71a 100644 --- a/fuse_models/include/fuse_models/unicycle_2d.h +++ b/fuse_models/include/fuse_models/unicycle_2d.h @@ -123,6 +123,26 @@ class Unicycle2D : public fuse_core::AsyncMotionModel */ bool applyCallback(fuse_core::Transaction& transaction) override; + /** + * @brief Generator function for providing to the \c TimestampManager to create a single motion model segment + * between the specified timestamps. + * + * This function is effectively using \c generateMotionModel. + * + * @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 generateMotionModelGenerator( + const ros::Time& beginning_stamp, + const ros::Time& ending_stamp, + std::vector& constraints, + std::vector& variables); + /** * @brief Generate a single motion model segment between the specified timestamps. * @@ -133,13 +153,30 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * \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[in] state_history The state history object to be updated + * @param[in] device_id The UUID of the device to be published + * @param[in] name The unique name for this motion model instance + * @param[in] process_noise_covariance Process noise covariance matrix + * @param[in] scale_process_noise Whether to scale the process noise covariance pose by the norm + of the current state twist. + * @param[in] velocity_norm_min The minimum velocity/twist norm allowed when scaling the + process noise covariance + * @param[in] disable_checks 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. * @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( + static void generateMotionModel( const ros::Time& beginning_stamp, const ros::Time& ending_stamp, + StateHistory& state_history, + const fuse_core::UUID& device_id, + const std::string name, + fuse_core::Matrix8d process_noise_covariance, + const bool scale_process_noise, + const double velocity_norm_min, + const bool disable_checks, std::vector& constraints, std::vector& variables); diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 481c29f52..4ad0dd5bf 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -119,7 +119,7 @@ Unicycle2D::Unicycle2D() : fuse_core::AsyncMotionModel(1), buffer_length_(ros::DURATION_MAX), device_id_(fuse_core::uuid::NIL), - timestamp_manager_(&Unicycle2D::generateMotionModel, this, ros::DURATION_MAX) + timestamp_manager_(&Unicycle2D::generateMotionModelGenerator, this, ros::DURATION_MAX) { } @@ -229,23 +229,41 @@ void Unicycle2D::onStart() state_history_.clear(); } +void Unicycle2D::generateMotionModelGenerator( + const ros::Time& beginning_stamp, + const ros::Time& ending_stamp, + std::vector& constraints, + std::vector& variables) +{ + generateMotionModel(beginning_stamp, ending_stamp, state_history_, device_id_, name(), + process_noise_covariance_, scale_process_noise_, velocity_norm_min_, disable_checks_, + constraints, variables); +} + void Unicycle2D::generateMotionModel( const ros::Time& beginning_stamp, const ros::Time& ending_stamp, + StateHistory& state_history, + const fuse_core::UUID& device_id, + const std::string name, + fuse_core::Matrix8d process_noise_covariance, + const bool scale_process_noise, + const double velocity_norm_min, + const bool disable_checks, std::vector& constraints, std::vector& variables) { - assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history_.empty())); + assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history.empty())); StateHistoryElement base_state; 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()) + 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 " + 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; } @@ -282,13 +300,13 @@ void Unicycle2D::generateMotionModel( if (dt == 0.0) { - state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id_).uuid(); - state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id_).uuid(); - state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id_).uuid(); - state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id_).uuid(); + state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id).uuid(); + state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id).uuid(); + state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id).uuid(); + state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id).uuid(); + state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id).uuid(); - state_history_.emplace(beginning_stamp, std::move(state1)); + state_history.emplace(beginning_stamp, std::move(state1)); return; } @@ -307,16 +325,16 @@ void Unicycle2D::generateMotionModel( state2.acceleration_linear); // Define the fuse variables required for this constraint - auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id_); - auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id_); - auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id_); - auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id_); - auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id_); - auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id_); - auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id_); - auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id_); - auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id_); - auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id_); + auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id); + auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id); + auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id); + auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id); + auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id); + auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id); + auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id); + auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id); + auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id); + auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id); position1->data()[fuse_variables::Position2DStamped::X] = state1.pose.x(); position1->data()[fuse_variables::Position2DStamped::Y] = state1.pose.y(); @@ -346,10 +364,8 @@ void Unicycle2D::generateMotionModel( state2.vel_yaw_uuid = velocity_yaw2->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_; + state_history.emplace(beginning_stamp, std::move(state1)); + state_history.emplace(ending_stamp, std::move(state2)); // Rotate the process noise covariance with the yaw angle of the current state orientation const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix(); @@ -359,16 +375,16 @@ void Unicycle2D::generateMotionModel( rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose(); // Scale process noise covariance pose by the norm of the current state twist - if (scale_process_noise_) + if (scale_process_noise) { common::scaleProcessNoiseCovariance(process_noise_covariance, state1.velocity_linear, state1.velocity_yaw, - velocity_norm_min_); + velocity_norm_min); } // Validate process_noise_covariance *= dt; - if (!disable_checks_) + if (!disable_checks) { try { @@ -376,14 +392,14 @@ void Unicycle2D::generateMotionModel( } catch (const std::runtime_error& ex) { - ROS_ERROR_STREAM_THROTTLE(10.0, "Invalid '" << name_ << "' motion model: " << ex.what()); + 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::Unicycle2DStateKinematicConstraint::make_shared( - name(), + name, *position1, *yaw1, *velocity_linear1, diff --git a/fuse_models/test/test_unicycle_2d.cpp b/fuse_models/test/test_unicycle_2d.cpp index 225a7241b..6b3f9672c 100644 --- a/fuse_models/test/test_unicycle_2d.cpp +++ b/fuse_models/test/test_unicycle_2d.cpp @@ -5,6 +5,10 @@ ***************************************************************************/ #include +#include +#include + +#include #include #include #include @@ -15,7 +19,7 @@ #include #include - +#include /** * @brief Derived class used in unit tests to expose protected functions @@ -26,6 +30,8 @@ class Unicycle2DModelTest : public fuse_models::Unicycle2D using fuse_models::Unicycle2D::updateStateHistoryEstimates; using fuse_models::Unicycle2D::StateHistoryElement; using fuse_models::Unicycle2D::StateHistory; + + using fuse_models::Unicycle2D::generateMotionModel; }; TEST(Unicycle2D, UpdateStateHistoryEstimates) @@ -291,6 +297,146 @@ TEST(Unicycle2D, UpdateStateHistoryEstimates) } } +TEST(Unicycle2D, generateMotionModel) +{ + // Create some variables + auto position1 = fuse_variables::Position2DStamped::make_shared(ros::Time(1, 0)); + auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(ros::Time(1, 0)); + auto linear_velocity1 = fuse_variables::VelocityLinear2DStamped::make_shared(ros::Time(1, 0)); + auto yaw_velocity1 = fuse_variables::VelocityAngular2DStamped::make_shared(ros::Time(1, 0)); + auto linear_acceleration1 = fuse_variables::AccelerationLinear2DStamped::make_shared(ros::Time(1, 0)); + position1->x() = 1.1; + position1->y() = 2.1; + yaw1->yaw() = 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}; + + Unicycle2DModelTest::StateHistory state_history; + state_history.emplace( + position1->stamp(), + Unicycle2DModelTest::StateHistoryElement{ // NOLINT(whitespace/braces) + position1->uuid(), + yaw1->uuid(), + linear_velocity1->uuid(), + yaw_velocity1->uuid(), + linear_acceleration1->uuid(), + tf2_2d::Transform(position1->x(), position1->y(), yaw1->yaw()), + tf2_2d::Vector2(linear_velocity1->x(), linear_velocity1->y()), + yaw_velocity1->yaw(), + tf2_2d::Vector2(linear_acceleration1->x(), linear_acceleration1->y())}); // NOLINT(whitespace/braces) + + const fuse_core::UUID device_id{{0}}; + const std::string name = ""; + + std::vector process_noise_diagonal{1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0}; + fuse_core::Matrix8d process_noise_covariance = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal(); + + const bool scale_process_noise = false; + const double velocity_norm_min = 0.0; + const bool disable_checks = false; + std::vector constraints; + std::vector variables; + + // Generate the motion model + Unicycle2DModelTest::generateMotionModel( + beginning_stamp, + ending_stamp, + state_history, + device_id, + name, + process_noise_covariance, + scale_process_noise, + velocity_norm_min, + disable_checks, + 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_2d::Transform(position1->x(), position1->y(), yaw1->yaw()); + 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(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_2d::Transform(1.643536632, 3.498058629, 1.2); + auto expected_linear_velocity = tf2_2d::Vector2(2.0, 0.0); + auto expected_yaw_velocity = 0.0; + auto expected_linear_acceleration = tf2_2d::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->yaw(), expected_pose.yaw(), 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); From ecb11a018ef72fcd830397210c39980cb3fae369 Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Tue, 16 May 2023 15:14:50 +0200 Subject: [PATCH 7/8] unicycle motion model: revert previous changes which created a separate static function Unicycle2D::generateMotionModel and make changes to unit test to test that now again non-static protected member function which needs setting private member variables before --- fuse_models/include/fuse_models/unicycle_2d.h | 39 +--------- fuse_models/src/unicycle_2d.cpp | 78 +++++++------------ fuse_models/test/test_unicycle_2d.cpp | 59 +++++++++----- 3 files changed, 70 insertions(+), 106 deletions(-) diff --git a/fuse_models/include/fuse_models/unicycle_2d.h b/fuse_models/include/fuse_models/unicycle_2d.h index 91494a71a..167f83d98 100644 --- a/fuse_models/include/fuse_models/unicycle_2d.h +++ b/fuse_models/include/fuse_models/unicycle_2d.h @@ -123,26 +123,6 @@ class Unicycle2D : public fuse_core::AsyncMotionModel */ bool applyCallback(fuse_core::Transaction& transaction) override; - /** - * @brief Generator function for providing to the \c TimestampManager to create a single motion model segment - * between the specified timestamps. - * - * This function is effectively using \c generateMotionModel. - * - * @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 generateMotionModelGenerator( - const ros::Time& beginning_stamp, - const ros::Time& ending_stamp, - std::vector& constraints, - std::vector& variables); - /** * @brief Generate a single motion model segment between the specified timestamps. * @@ -153,30 +133,13 @@ class Unicycle2D : public fuse_core::AsyncMotionModel * \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[in] state_history The state history object to be updated - * @param[in] device_id The UUID of the device to be published - * @param[in] name The unique name for this motion model instance - * @param[in] process_noise_covariance Process noise covariance matrix - * @param[in] scale_process_noise Whether to scale the process noise covariance pose by the norm - of the current state twist. - * @param[in] velocity_norm_min The minimum velocity/twist norm allowed when scaling the - process noise covariance - * @param[in] disable_checks 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. * @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. */ - static void generateMotionModel( + void generateMotionModel( const ros::Time& beginning_stamp, const ros::Time& ending_stamp, - StateHistory& state_history, - const fuse_core::UUID& device_id, - const std::string name, - fuse_core::Matrix8d process_noise_covariance, - const bool scale_process_noise, - const double velocity_norm_min, - const bool disable_checks, std::vector& constraints, std::vector& variables); diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 4ad0dd5bf..9a88b7dd0 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -119,7 +119,7 @@ Unicycle2D::Unicycle2D() : fuse_core::AsyncMotionModel(1), buffer_length_(ros::DURATION_MAX), device_id_(fuse_core::uuid::NIL), - timestamp_manager_(&Unicycle2D::generateMotionModelGenerator, this, ros::DURATION_MAX) + timestamp_manager_(&Unicycle2D::generateMotionModel, this, ros::DURATION_MAX) { } @@ -204,9 +204,7 @@ void Unicycle2D::onInit() process_noise_covariance_ = fuse_core::Vector8d(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; @@ -229,41 +227,23 @@ void Unicycle2D::onStart() state_history_.clear(); } -void Unicycle2D::generateMotionModelGenerator( - const ros::Time& beginning_stamp, - const ros::Time& ending_stamp, - std::vector& constraints, - std::vector& variables) -{ - generateMotionModel(beginning_stamp, ending_stamp, state_history_, device_id_, name(), - process_noise_covariance_, scale_process_noise_, velocity_norm_min_, disable_checks_, - constraints, variables); -} - void Unicycle2D::generateMotionModel( const ros::Time& beginning_stamp, const ros::Time& ending_stamp, - StateHistory& state_history, - const fuse_core::UUID& device_id, - const std::string name, - fuse_core::Matrix8d process_noise_covariance, - const bool scale_process_noise, - const double velocity_norm_min, - const bool disable_checks, std::vector& constraints, std::vector& variables) { - assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history.empty())); + assert(beginning_stamp < ending_stamp || (beginning_stamp == ending_stamp && state_history_.empty())); StateHistoryElement base_state; 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()) + 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 " + 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; } @@ -300,13 +280,13 @@ void Unicycle2D::generateMotionModel( if (dt == 0.0) { - state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id).uuid(); - state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id).uuid(); - state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id).uuid(); - state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id).uuid(); - state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id).uuid(); + state1.position_uuid = fuse_variables::Position2DStamped(beginning_stamp, device_id_).uuid(); + state1.yaw_uuid = fuse_variables::Orientation2DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_linear_uuid = fuse_variables::VelocityLinear2DStamped(beginning_stamp, device_id_).uuid(); + state1.vel_yaw_uuid = fuse_variables::VelocityAngular2DStamped(beginning_stamp, device_id_).uuid(); + state1.acc_linear_uuid = fuse_variables::AccelerationLinear2DStamped(beginning_stamp, device_id_).uuid(); - state_history.emplace(beginning_stamp, std::move(state1)); + state_history_.emplace(beginning_stamp, std::move(state1)); return; } @@ -325,16 +305,16 @@ void Unicycle2D::generateMotionModel( state2.acceleration_linear); // Define the fuse variables required for this constraint - auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id); - auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id); - auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id); - auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id); - auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id); - auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id); - auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id); - auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id); - auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id); - auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id); + auto position1 = fuse_variables::Position2DStamped::make_shared(beginning_stamp, device_id_); + auto yaw1 = fuse_variables::Orientation2DStamped::make_shared(beginning_stamp, device_id_); + auto velocity_linear1 = fuse_variables::VelocityLinear2DStamped::make_shared(beginning_stamp, device_id_); + auto velocity_yaw1 = fuse_variables::VelocityAngular2DStamped::make_shared(beginning_stamp, device_id_); + auto acceleration_linear1 = fuse_variables::AccelerationLinear2DStamped::make_shared(beginning_stamp, device_id_); + auto position2 = fuse_variables::Position2DStamped::make_shared(ending_stamp, device_id_); + auto yaw2 = fuse_variables::Orientation2DStamped::make_shared(ending_stamp, device_id_); + auto velocity_linear2 = fuse_variables::VelocityLinear2DStamped::make_shared(ending_stamp, device_id_); + auto velocity_yaw2 = fuse_variables::VelocityAngular2DStamped::make_shared(ending_stamp, device_id_); + auto acceleration_linear2 = fuse_variables::AccelerationLinear2DStamped::make_shared(ending_stamp, device_id_); position1->data()[fuse_variables::Position2DStamped::X] = state1.pose.x(); position1->data()[fuse_variables::Position2DStamped::Y] = state1.pose.y(); @@ -364,8 +344,10 @@ void Unicycle2D::generateMotionModel( state2.vel_yaw_uuid = velocity_yaw2->uuid(); state2.acc_linear_uuid = acceleration_linear2->uuid(); - state_history.emplace(beginning_stamp, std::move(state1)); - state_history.emplace(ending_stamp, std::move(state2)); + 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 yaw angle of the current state orientation const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix(); @@ -375,16 +357,16 @@ void Unicycle2D::generateMotionModel( rotation_matrix * process_noise_covariance.topLeftCorner<2, 2>() * rotation_matrix.transpose(); // Scale process noise covariance pose by the norm of the current state twist - if (scale_process_noise) + if (scale_process_noise_) { common::scaleProcessNoiseCovariance(process_noise_covariance, state1.velocity_linear, state1.velocity_yaw, - velocity_norm_min); + velocity_norm_min_); } // Validate process_noise_covariance *= dt; - if (!disable_checks) + if (!disable_checks_) { try { @@ -392,14 +374,14 @@ void Unicycle2D::generateMotionModel( } catch (const std::runtime_error& ex) { - ROS_ERROR_STREAM_THROTTLE(10.0, "Invalid '" << name << "' motion model: " << ex.what()); + 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::Unicycle2DStateKinematicConstraint::make_shared( - name, + name(), *position1, *yaw1, *velocity_linear1, diff --git a/fuse_models/test/test_unicycle_2d.cpp b/fuse_models/test/test_unicycle_2d.cpp index 6b3f9672c..51dae33b1 100644 --- a/fuse_models/test/test_unicycle_2d.cpp +++ b/fuse_models/test/test_unicycle_2d.cpp @@ -5,9 +5,6 @@ ***************************************************************************/ #include -#include -#include - #include #include #include @@ -21,8 +18,11 @@ #include #include +#include +#include + /** - * @brief Derived class used in unit tests to expose protected functions + * @brief Derived class used in unit tests to expose protected/private functions and variables */ class Unicycle2DModelTest : public fuse_models::Unicycle2D { @@ -30,8 +30,27 @@ class Unicycle2DModelTest : public fuse_models::Unicycle2D using fuse_models::Unicycle2D::updateStateHistoryEstimates; using fuse_models::Unicycle2D::StateHistoryElement; using fuse_models::Unicycle2D::StateHistory; - using fuse_models::Unicycle2D::generateMotionModel; + + void setProcessNoiseCovariance(const fuse_core::Matrix8d& 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(Unicycle2D, UpdateStateHistoryEstimates) @@ -331,29 +350,23 @@ TEST(Unicycle2D, generateMotionModel) yaw_velocity1->yaw(), tf2_2d::Vector2(linear_acceleration1->x(), linear_acceleration1->y())}); // NOLINT(whitespace/braces) - const fuse_core::UUID device_id{{0}}; - const std::string name = ""; + Unicycle2DModelTest unicycle_2d_model_test; + + unicycle_2d_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}; - fuse_core::Matrix8d process_noise_covariance = fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal(); + unicycle_2d_model_test.setProcessNoiseCovariance(fuse_core::Vector8d(process_noise_diagonal.data()).asDiagonal()); + + unicycle_2d_model_test.setScaleProcessNoise(false); + unicycle_2d_model_test.setDisableChecks(false); - const bool scale_process_noise = false; - const double velocity_norm_min = 0.0; - const bool disable_checks = false; std::vector constraints; std::vector variables; // Generate the motion model - Unicycle2DModelTest::generateMotionModel( + unicycle_2d_model_test.generateMotionModel( beginning_stamp, ending_stamp, - state_history, - device_id, - name, - process_noise_covariance, - scale_process_noise, - velocity_norm_min, - disable_checks, constraints, variables); @@ -440,5 +453,11 @@ TEST(Unicycle2D, generateMotionModel) int main(int argc, char **argv) { testing::InitGoogleTest(&argc, argv); - return RUN_ALL_TESTS(); + ros::init(argc, argv, "unicycle_2d_test"); + auto spinner = ros::AsyncSpinner(1); + spinner.start(); + int ret = RUN_ALL_TESTS(); + spinner.stop(); + ros::shutdown(); + return ret; } From 39bbb990125d3ed959b01595c9264e26f113d128 Mon Sep 17 00:00:00 2001 From: Fabian Hirmann Date: Thu, 13 Jul 2023 10:57:44 +0200 Subject: [PATCH 8/8] Update test_unicycle_2d to rostest instead of pure gtest after now roscore is required to run it --- fuse_models/CMakeLists.txt | 5 +++-- fuse_models/test/unicycle_2d.test | 4 ++++ 2 files changed, 7 insertions(+), 2 deletions(-) create mode 100644 fuse_models/test/unicycle_2d.test diff --git a/fuse_models/CMakeLists.txt b/fuse_models/CMakeLists.txt index 597706e0a..4df78e450 100644 --- a/fuse_models/CMakeLists.txt +++ b/fuse_models/CMakeLists.txt @@ -154,13 +154,14 @@ if(CATKIN_ENABLE_TESTING) roslint_add_test() # Model tests - catkin_add_gtest(test_unicycle_2d + add_rostest_gtest( + test_unicycle_2d + test/unicycle_2d.test test/test_unicycle_2d.cpp ) target_link_libraries(test_unicycle_2d ${PROJECT_NAME} ${catkin_LIBRARIES} - ${CERES_LIBRARIES} ) set_target_properties(test_unicycle_2d PROPERTIES diff --git a/fuse_models/test/unicycle_2d.test b/fuse_models/test/unicycle_2d.test new file mode 100644 index 000000000..6a50620b7 --- /dev/null +++ b/fuse_models/test/unicycle_2d.test @@ -0,0 +1,4 @@ + + + +