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/include/fuse_models/unicycle_2d.h b/fuse_models/include/fuse_models/unicycle_2d.h index 1191bd34a..167f83d98 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); diff --git a/fuse_models/src/unicycle_2d.cpp b/fuse_models/src/unicycle_2d.cpp index 103459fc9..9a88b7dd0 100644 --- a/fuse_models/src/unicycle_2d.cpp +++ b/fuse_models/src/unicycle_2d.cpp @@ -205,7 +205,6 @@ void Unicycle2D::onInit() 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; @@ -348,8 +347,16 @@ 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 + 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_) { common::scaleProcessNoiseCovariance(process_noise_covariance, state1.velocity_linear, state1.velocity_yaw, @@ -367,7 +374,7 @@ 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; } } diff --git a/fuse_models/test/test_unicycle_2d.cpp b/fuse_models/test/test_unicycle_2d.cpp index 225a7241b..51dae33b1 100644 --- a/fuse_models/test/test_unicycle_2d.cpp +++ b/fuse_models/test/test_unicycle_2d.cpp @@ -5,6 +5,7 @@ ***************************************************************************/ #include +#include #include #include #include @@ -15,10 +16,13 @@ #include #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 { @@ -26,6 +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) @@ -291,8 +316,148 @@ 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) + + 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}; + 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); + + std::vector constraints; + std::vector variables; + + // Generate the motion model + unicycle_2d_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_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); - 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; } 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 @@ + + + +