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

fabianhirmann · 2023-05-09T09:48:26Z

This PR adds an option to rotate the process noise covariance of the unicycle motion model to the current state orientation using its yaw angle.

I noticed that this does not exist yet when I set the process noise covariance to a non-symmetric value for x- and y-coordinates which then had the unexpected effect that the resulted covariance was indeed having (in this case) less covariance in the y-values (y-position, y-velocity, y-acceleration) but always in the world frame (in this case map frame) which was not intuitive for me because I would have expected that the unicycle motion model applies the process noise covariance according to the current state orientation meaning that process noise covariance's x-values are along the longitudinal forward direction and y-values along lateral left direction (in the same way as the base link is mostly defined).

It would be great if @svwilliams, @efernandez, @ayrton04 or @carlos-m159 could have a look on it and provide feedback such that this can be integrated in the fuse main repository.

Thank you,
Fabian

…nce to the current state orientation using its yaw angle

…ariance to state orientation

efernandez · 2023-05-09T19:02:24Z

IMHO if this is the tright thing to do, we should always do it instead of having a parameter. I'm not sure if that's the case though. I'd have to look into the math, but I don't have time atm.

…y to x and y position as the other state variables are already along the current state orientation

efernandez · 2023-05-10T10:56:13Z

fuse_models/src/unicycle_2d.cpp

@@ -348,8 +352,20 @@ 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
+


Remove extra empty line:

Suggested change

efernandez · 2023-05-10T10:56:34Z

fuse_models/src/unicycle_2d.cpp

+  // 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();


This can be const:

Suggested change

auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix();

const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix();

efernandez · 2023-05-10T10:57:05Z

fuse_models/src/unicycle_2d.cpp

+                            rotate_process_noise_covariance_to_state_orientation_,
+                            rotate_process_noise_covariance_to_state_orientation_);


Indentation seems to be missing one space for each line here:

Suggested change

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

efernandez · 2023-05-10T10:58:11Z

fuse_models/include/fuse_models/unicycle_2d.h

@@ -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


As I said before, I wonder if this should always be done. I sounds like it's probably the right thing to do. In that case, I think we should always do it, and we wouldn't need a ROS param at all. 🤔

I also believe that this should always be done and would enable it for all our robots. That's why I now also removed the ROS parameter with the latest commit.
The only reason why I created a ROS parameter before and set it to false by default was that existing applications using fuse are not changed and remain backwards compatibility. If you agree that this is no issue, then I would be happy to have it always enabled. Otherwise I will just revert the last commit and I'm fine with a ROS parameter too.

If the math is wrong, then I would consider it a bug. I see no need for an option to retain the wrong math either.

How can we confirm it was a bug?

Is it something we can clearly see from the math?

@fabianhirmann Would you be able to provide a unit test that only passes with your changes but didn't before? 🤔

I just pushed a unit test for it that does not pass before my changes but only with my changes. There I also wrote an explanation why I expect that the final process noise covariance should be rotated. Summarized my perspective is that the process noise covariance is defined in the robot frame so when the position is defined in the world frame, we need to rotate the process noise covariance to the world frame (using the yaw angle that defines the orientation relation between the robot frame and world frame).
An example, imagine we are defining the process noise covariance with cov_robot_xx = 2.0 and cov_robot_yy = 1.0 (assuming that the process noise covariance is in the robot frame there is then less process noise covariance in the lateral direction than the longitudinal direction). Then, when having yaw angle = 90°, I would expect that the final process noise covariance applied to the position in the world frame would have cov_world_xx = 1.0 and cov_world_yy = 2.0 because the orientation of the robot is upwards and the covariance must then get 90° rotated.

…r better coding style

…entation: remove of option to enable/disable it by a ROS parameter and instead always rotate it

…ecessary rewrite in code to make creation of the unit test easier and separated

efernandez

Some minor comments, but LGTM.

Thanks for adding a unit test.

I didn't have time to give this a try or do the math myself, but what you're saying and doing makes perfect sense to me.

I'll try to give this a try offline with some bagged data to sanity check the changes.

efernandez · 2023-05-16T09:35:49Z

fuse_models/include/fuse_models/unicycle_2d.h

   * @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(


I wonder if this breaks existing code because it might break the API for children classes. 🤔

Why did you have to make this static?

I made this function static because I am honestly not that familiar with gtest and setting the private variables process_noise_covariance_, state_history_, etc. seemed complicated otherwise. Also Unicycle2D::onInit() gets some ROS parameters that again seemed more complicated to handle. Defining that function as static which I can then directly use seemed easier for me.
However, I just thought about it again and with another way I came around this issue and there is no need to set this static and make many changes in the class itself (and also many comments from you are no more relevant then). If you feel like having a static function is still better, I can just revert my changes again. I look forward to your feedback on it.

efernandez · 2023-05-16T09:37:25Z

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


I think this needs to be reworded, maybe something like:

Suggested change

* @brief Generator function for providing to the \c TimestampManager to create a single motion model segment

* @brief Generator function for providing to the \c TimestampManager a way to create a single motion model segment

efernandez · 2023-05-16T09:40:13Z

fuse_models/src/unicycle_2d.cpp

@@ -204,6 +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_);
+


I think the extra lines for the others are only because they need extra work other than just calling param, so I wouldn't add a new line here:

Suggested change

efernandez · 2023-05-16T09:44:28Z

fuse_models/src/unicycle_2d.cpp

+  const std::string name,
+  fuse_core::Matrix8d process_noise_covariance,


Do you really need to copy here? Why don't you do the following?:

Suggested change

const std::string name,

fuse_core::Matrix8d process_noise_covariance,

const std::string& name,

const fuse_core::Matrix8d& process_noise_covariance,

It looks like you're relying on making a copy of the process noise covariance, so the input doesn't change. I believe it's cleaner to just create a local copy below because there are cases where we return even before using the process nosie covariance, so there's no need to copy always it.

efernandez · 2023-05-16T09:44:45Z

fuse_models/test/test_unicycle_2d.cpp

+
+  using fuse_models::Unicycle2D::generateMotionModel;


Suggested change

using fuse_models::Unicycle2D::generateMotionModel;

using fuse_models::Unicycle2D::generateMotionModel;

efernandez · 2023-05-16T09:45:28Z

fuse_models/test/test_unicycle_2d.cpp

+#include <string>
+#include <vector>
+


Not sure, but I think more general headers are usually at the end in the fuse repo.

efernandez

Some minor comments, but LGTM.

Thanks for adding a unit test.

I didn't have time to give this a try or do the math myself, but what you're saying and doing makes perfect sense to me.

I'll try to give this a try offline with some bagged data to sanity check the changes.

efernandez

Some minor comments, but LGTM.

Thanks for adding a unit test.

I didn't have time to give this a try or do the math myself, but what you're saying and doing makes perfect sense to me.

I'll try to give this a try offline with some bagged data to sanity check the changes.

…te 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

efernandez

This looks a lot better IMO. Thanks for doing it!

Just one more comment about using ROS node. Not sure why that is required. 🤔

Please remove if not needed.

efernandez · 2023-05-16T22:03:45Z

fuse_models/test/test_unicycle_2d.cpp

 int main(int argc, char **argv)
 {
  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+  ros::init(argc, argv, "unicycle_2d_test");


Why do you need ROS?

We need ROS because the Unicycle2D class is derived from the fuse_core::AsyncMotionModel which contains variables of ros::NodeHandle and ros::AsyncSpinner and if we don't initialize ROS, there are the following errors shown:

[FATAL ${node} ros.roscpp 1684306444.156373179]: You must call ros::init() before creating the first NodeHandle [ERROR ${node} ros.roscpp 1684306444.167166713]: [registerPublisher] Failed to contact master at [:0]. Retrying...

I see. Before the unit tests didn't create a Unicycle2DTest object, but now you create one.

I don't think there's any workaround to avoid this, so I think it's fine.

efernandez

LGTM

But you'd need @svwilliams or @ayrton04 , or someone else to review and merge this.

efernandez · 2023-05-17T09:20:18Z

fuse_models/test/test_unicycle_2d.cpp

 int main(int argc, char **argv)
 {
  testing::InitGoogleTest(&argc, argv);
-  return RUN_ALL_TESTS();
+  ros::init(argc, argv, "unicycle_2d_test");


I see. Before the unit tests didn't create a Unicycle2DTest object, but now you create one.

I don't think there's any workaround to avoid this, so I think it's fine.

efernandez · 2023-06-16T18:41:51Z

https://build.ros.org/job/Mpr__fuse__ubuntu_bionic_amd64/70/ and https://build.ros.org/job/Npr__fuse__ubuntu_focal_amd64/72/ seem to have failed due to CI infrastructure issues.

@ayrton04 @svwilliams I wonder if there's a way to make those checks run again.

Either way, the other one is also failing. I don't have access to that one.

ayrton04 · 2023-06-26T07:07:00Z

I think we can just close/open the PR to kick the job off again.

svwilliams · 2023-07-10T02:01:48Z

Closing and re-opening to rerun CI tests

efernandez · 2023-07-10T14:32:42Z

@svwilliams Based on https://build.ros.org/job/Mpr__fuse__ubuntu_bionic_amd64/71/console

09:46:37 Build timed out (after 120 minutes). Marking the build as failed.
11:07:30 Build was aborted

the test build timeout should be increased.

svwilliams · 2023-07-10T20:31:38Z

I was looking at this over the weekend. Now that this test requires a roscore running, it needs to be changed from a gtest to a rostest. The test case is waiting for a rosmaster to be available.

…score is required to run it

fabianhirmann · 2023-07-13T10:09:13Z

@svwilliams Thank you very much for that hint! I changed it now to a rostest and the CI tests indeed run successful.

jakemclaughlin6 · 2024-04-15T19:17:19Z

fuse_models/src/unicycle_2d.cpp

+  // 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();


I'm a little confused with this. The actual residual that is computed for the unicycle constraints is just the position difference. That difference is expressed in the world/map frame here, so rotating the process noise into the frame doesn't make sense to me. This would make sense if that residual is also expressed in the first poses frame.

@svwilliams Has your team had a chance to validate or test this?

I looked at some point, but haven't had time to actually fix it.
From what I remember, I agree with the assessment that the current covariance is not represented in the same frame as the error. However, I believe the correct thing to do is change the error calculation to be in the frame of Pose1 rather than to spin the covariance to be in the global frame.

Yeah that makes more sense to me. I can work on a PR that does that

fabianhirmann added 2 commits May 9, 2023 11:26

unicycle motion model: add option to rotate the process noise covaria…

66cd212

…nce to the current state orientation using its yaw angle

fix linter errors for unicycle motion model: rotate process noise cov…

7238f0d

…ariance to state orientation

unicycle motion model: apply rotation of process noise covariance onl…

05abcac

…y to x and y position as the other state variables are already along the current state orientation

efernandez suggested changes May 10, 2023

View reviewed changes

fabianhirmann added 3 commits May 10, 2023 13:17

unicycle motion model: rotate process noise with covariance: fixes fo…

555ac04

…r better coding style

unicycle motion model: rotate process noise covariance with state ori…

04592ef

…entation: remove of option to enable/disable it by a ROS parameter and instead always rotate it

unicycle motion model: create unit test for generateMotionModel and n…

7f8b97a

…ecessary rewrite in code to make creation of the unit test easier and separated

efernandez suggested changes May 16, 2023

View reviewed changes

unicycle motion model: revert previous changes which created a separa…

ecb11a0

…te 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

fabianhirmann force-pushed the feature/unicycle_motion_model_rotate_process_noise_with_state_orientation branch from 0c2c81e to ecb11a0 Compare May 16, 2023 13:15

efernandez reviewed May 16, 2023

View reviewed changes

efernandez approved these changes May 17, 2023

View reviewed changes

fabianhirmann mentioned this pull request Jun 2, 2023

unicycle motion model: add option to rotate the process noise covariance to the current state orientation ARTI-Robots/fuse#2

Merged

ayrton04 closed this Jun 26, 2023

ayrton04 reopened this Jun 26, 2023

svwilliams closed this Jul 10, 2023

svwilliams reopened this Jul 10, 2023

Update test_unicycle_2d to rostest instead of pure gtest after now ro…

39bbb99

…score is required to run it

efernandez approved these changes Jul 13, 2023

View reviewed changes

jakemclaughlin6 reviewed Apr 15, 2024

View reviewed changes

jakemclaughlin6 mentioned this pull request May 15, 2024

Rotate position in motion model constraint #365

Open

	auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix();
	const auto rotation_matrix = Eigen::Rotation2Dd(state1.pose.yaw()).toRotationMatrix();

		rotate_process_noise_covariance_to_state_orientation_,
		rotate_process_noise_covariance_to_state_orientation_);

	* @brief Generator function for providing to the \c TimestampManager to create a single motion model segment
	* @brief Generator function for providing to the \c TimestampManager a way to create a single motion model segment

		@@ -204,6 +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_);

		const std::string name,
		fuse_core::Matrix8d process_noise_covariance,


	using fuse_models::Unicycle2D::generateMotionModel;
	using fuse_models::Unicycle2D::generateMotionModel;

		#include <string>
		#include <vector>

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

Are you sure you want to change the base?

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

Conversation

fabianhirmann commented May 9, 2023

efernandez commented May 9, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

efernandez left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

efernandez left a comment

Choose a reason for hiding this comment

efernandez left a comment

Choose a reason for hiding this comment

efernandez left a comment • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

fabianhirmann May 17, 2023 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

efernandez left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

efernandez commented Jun 16, 2023

ayrton04 commented Jun 26, 2023

svwilliams commented Jul 10, 2023

efernandez commented Jul 10, 2023

svwilliams commented Jul 10, 2023

fabianhirmann commented Jul 13, 2023

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

efernandez left a comment •

edited

fabianhirmann May 17, 2023 •

edited