unicycle motion model: add option to rotate the process noise covariance to the current state orientation

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

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);

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;
     }
   }

fuse_models/test/test_unicycle_2d.cpp

@@ -5,6 +5,7 @@
  ***************************************************************************/
 #include <fuse_models/unicycle_2d.h>
 
+#include <fuse_models/unicycle_2d_state_kinematic_constraint.h>
 #include <fuse_graphs/hash_graph.h>
 #include <fuse_variables/acceleration_linear_2d_stamped.h>
 #include <fuse_variables/orientation_2d_stamped.h>
@@ -15,17 +16,41 @@
 #include <ros/time.h>
 
 #include <gtest/gtest.h>
+#include <fuse_core/eigen_gtest.h>
 
+#include <string>
+#include <vector>
 
 /**
- * @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
 {
 public:
   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<double> 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<fuse_core::Constraint::SharedPtr> constraints;
+  std::vector<fuse_core::Variable::SharedPtr> 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<fuse_variables::Position2DStamped>(variables.at(0));
+    auto orientation = std::static_pointer_cast<fuse_variables::Orientation2DStamped>(variables.at(1));
+    auto velocity_linear = std::static_pointer_cast<fuse_variables::VelocityLinear2DStamped>(variables.at(2));
+    auto velocity_yaw = std::static_pointer_cast<fuse_variables::VelocityAngular2DStamped>(variables.at(3));
+    auto acceleration_linear = std::static_pointer_cast<fuse_variables::AccelerationLinear2DStamped>(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<fuse_variables::Position2DStamped>(variables.at(5));
+    auto orientation = std::static_pointer_cast<fuse_variables::Orientation2DStamped>(variables.at(6));
+    auto velocity_linear = std::static_pointer_cast<fuse_variables::VelocityLinear2DStamped>(variables.at(7));
+    auto velocity_yaw = std::static_pointer_cast<fuse_variables::VelocityAngular2DStamped>(variables.at(8));
+    auto acceleration_linear = std::static_pointer_cast<fuse_variables::AccelerationLinear2DStamped>(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<fuse_models::Unicycle2DStateKinematicConstraint>(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;
 }

fuse_models/test/unicycle_2d.test

@@ -0,0 +1,4 @@
+<?xml version="1.0"?>
+<launch>
+  <test test-name="unicycle_2d_test" pkg="fuse_models" type="test_unicycle_2d" />
+</launch>