Publish odometry with timer and allow to predict it #109

efernandez · 2019-10-15T15:38:10Z

Add predict_odom parameter that:
1. Disables publishing the odometry in the notifyCallback
2. Enables publishing the odometry in the tfPublishTimerCallback, predicting it into the current time.
  The Jacobian of the motion model used in the predict function is computed so the odometry covariance can be propagated.
  The odometry cached in the class is updated with the predicted pose, covariance and time stamp.
Add process_noise_diagonal parameter that defines the diagonal of the process noise covariance that is added to the propagated covariance computing after predicting the odometry into the current time.
The process noise covariance has 8x8 size, so it can represent all the state, although the acceleration is ignored at the moment.
Add new predict free functions to compute the Jacobian of the motion model wrt the state. Two versions are provided:
1. One that takes each single component and a plain array of double to
  the Jacobian.
2. Another that takes aggregate objects like tf2_2d::Transform and an fuse_core::Matrix8d for the Jacobian.
Cache the stamp of the latest covariance successfully computed in the notifyCallback, so it's possible to know if the covariance is valid and up to date in the tfPublishTimerCallback.
If the covariance is invalid or not up to date, no time is wasted propagating it.

svwilliams · 2019-10-16T12:14:27Z

Best I can tell, all of the CI tests have passed. I'll get the review done later today.

On Wed, Oct 16, 2019 at 12:00 AM Enrique Fernandez Perdomo < ***@***.***> wrote: @svwilliams <https://github.com/svwilliams> All roslint and tests are passing for fuse_models pkg locally for me. Could you share the test/s failure output here, please? — You are receiving this because you were mentioned. Reply to this email directly, view it on GitHub <#109?email_source=notifications&email_token=ABHNJUGTIYZ5IM6NW5KPZOTQO23YNA5CNFSM4JA6ORL2YY3PNVWWK3TUL52HS4DFVREXG43VMVBW63LNMVXHJKTDN5WW2ZLOORPWSZGOEBLLIOY#issuecomment-542553147>, or unsubscribe <https://github.com/notifications/unsubscribe-auth/ABHNJUASWPAGYXSEIVZWUZLQO23YNANCNFSM4JA6ORLQ> .

efernandez · 2019-10-16T13:28:50Z

nvm, I removed my comment about the CI test failures because I figured out the roslint warnings and I fixed them.

fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h

fuse_models/src/odometry_2d_publisher.cpp

fuse_models/include/fuse_models/unicycle_2d_predict.h

svwilliams · 2019-10-16T18:27:19Z

fuse_models/include/fuse_models/unicycle_2d_predict.h

+ * @param[in] acc_linear2_y - Second Y acceleration
+ * @param[in] jacobians - Jacobians wrt the state
+ */
+// TODO(efernandez) fuse this with the templated predict function so we can use the analytic Jacobian in the cost


I would really like to get these two functions merged.

This could be done by easily by defaulting the jacobian variable to null.

template<typename T> inline void predict( const T position1_x, ... T& acc_linear2_y, T* jacobian = nullptr { ... if (!jacobian) return; ... }

This would completely replace the existing templated function. The other two signatures (that involve tf2_2d objects) would call this function, providing a non-null jacobian when appropriate.

Another option that seems more C++/type-safe would be to use a boost::optional. I really wish std::optional was included in C++14.

template<typename T> inline void predict( const T position1_x, ... T& acc_linear2_y, boost::optional<Eigen::Matrix<T, 8, 8>&> jacobian { ... if (!jacobian) return; ... }

where I'm not exactly sure what Eigen type should be passed in. An EigenBase<T, 8, 8> maybe???? I'm just making things up now. In the case of using this function to compute Ceres cost function Jacobians, Ceres will provide a C-style double array. Within the cost function implementation, you could use Eigen::Map<> to convert the double array into an Eigen compatible type. I feel that should be possible, but don't know the exact type required off the top of my head.

Ultimately I'm fine with either approach. I just don't like having the duplicate code that exists now.

I tried that and it didn't work because predict is used inside a cost function and Ceres requires the jacobian to be a T** double pointer: http://ceres-solver.org/analytical_derivatives.html

Right. I guess what I was expecting was predict() would be wrapped in a derived ceres::CostFunction. The way Ceres populates jacobians is not directly compatible with the new predict() function anyway. So something vaguely along the lines of:

class Unicycle2DStateCostFunction : public ceres::CostFunction { public: bool Evaluate( double const* const* parameters, double* residuals, double** jacobians) const override { boost::optional<Eigen::Matrix8d> jacobian_matrix = boost::none; if (jacobians) { jacobian_matrix = Eigen::Matrix8d(); } predict( position1, yaw1, vel_linear1, vel_yaw1, acc_linear1, dt_, position_pred, yaw_pred, vel_linear_pred, vel_yaw_pred, acc_linear_pred, jacobian_matrix); // Only computes jacobians if Ceres wants them // Populate the Ceres-requested jacobian blocks // I'm having to think about what goes here too hard for a code review. // The predict() function computes State2 as a function of State1. // But the cost function computes (conceptually) "error = State2 - State1" // I'm sure there is a way to translate the predict() jacobian into a Ceres // jacobian. Regardless, Ceres wants the jacobian in blocks, not as one // giant matrix, so some copying will be required. if (jacobians) { if (jacobians[0]) { Eigen::Map<fuse_core::MatrixXd>(jacobians[0], 8, 2) = jacobian_matrix->block(0, 0, 8, 2); // Maybe??? } if (jacobians[1]) { ... } ... } }

Will do that next week. All other changes requested are in. I'm going to validate things still work as intended.

fuse_models/include/fuse_models/unicycle_2d_predict.h

ayrton04

LGTM, Stephen's feedback notwithstanding.

fuse_models/include/fuse_models/unicycle_2d_predict.h

fuse_models/src/odometry_2d_publisher.cpp

efernandez · 2019-10-19T19:18:03Z

I've addressed some of your review comments and replied to the others. I'm going to wait for your feedback before I continue implementing more changes on this PR.

Thanks for your review. This PR was a first iteration on this topic, so I was indeed looking for that kind of feedback. 😃

fuse_models/test/test_unicycle_2d_predict.cpp

efernandez · 2019-10-25T13:38:48Z

fuse_models/src/odometry_2d_publisher.cpp

  }

-  tf_broadcaster_.sendTransform(trans);
+  if (params_.publish_tf)


publish_tf wasn't used before. That was a bug I just discovered.

svwilliams

Changes look good. All that's left is to merge the predict functions in some usable way.

svwilliams · 2019-10-29T16:32:30Z

fuse_models/include/fuse_models/parameters/odometry_2d_publisher_params.h

+    }
+
+    if (std::any_of(process_noise_diagonal.begin(), process_noise_diagonal.end(),
+                    [](const auto& v) { return v < 0.0; }))  // NOLINT(whitespace/braces)


Suggested change

[](const auto& v) { return v < 0.0; })) // NOLINT(whitespace/braces)

[](const auto& v) { return v <= 0.0; })) // NOLINT(whitespace/braces)

You're probably duplicating code I wrote elsewhere, 0's really shouldn't be allowed either.

If I don't allow 0.0, then the default value for process_noise_diagonal can't be all 0.0s as it's now. Then the question is what value should I use in that case. Maybe something like 1e-6? But the diagonal represents different state components, with different units, so some should be smaller than others, like the velocities or accelerations. 🤔

The reason I allowed 0.0s in this case is because it's the process noise covariance, so if the user doesn't specify it, then it'd be like there's no process noise, that it's not recommended, but the math won't blow up. Maybe I can add a ROS_WARN saying that it's not recommended to have zeros in the process noise covariance diagonal.

Regarding the check, I can only find this in unicycle 2D motion model, but it doesn't perform the check I do here. Maybe it should also have a check similar to mine.

fuse/fuse_models/src/unicycle_2d.cpp

Lines 99 to 105 in 9933456

std::vector<double> process_noise_diagonal;

private_node_handle_.param("process_noise_diagonal", process_noise_diagonal, process_noise_diagonal);

if (process_noise_diagonal.size() != 8)

{

throw std::runtime_error("Process noise diagonal must be of length 8!");

}

I expect the math will blow up actually. Inside the motion model cost function, the error is normalized by the square root information matrix. If there are any zero terms in the process noise, the covariance cannot be inverted into an information matrix.

Although this is not that process noise, I guess.

Exactly, the process_noise_diagonal set here is NOT used in: https://github.com/locusrobotics/fuse/blob/devel/fuse_models/src/unicycle_2d.cpp#L246 and https://github.com/locusrobotics/fuse/blob/devel/fuse_models/src/unicycle_2d_state_kinematic_constraint.cpp#L81, where a covariance with zeros will certainly blow up.

This one is only used in https://github.com/locusrobotics/fuse/pull/109/files#diff-ec05e4b7290a6166c67ff2570f66529fR320, that will simply make that addition a nop.

That being said, if we agree on some initial values, or some WARN messages, I could add that, so the user is aware of a potentially bad configuration.

Ideally, the publisher should be using the same process noise covariance as the unicycle_2d motion model. Indeed, I think the publisher should allow the user to specify the motion model to use for prediction, but that would be another story, for another PR.

efernandez · 2019-11-01T18:40:17Z

fuse_models/src/odometry_2d_publisher.cpp


-      return;
-    }
+    odom_pub_.publish(odom_output_);


@svwilliams @ayrton04 I just fixed this. I have this instruction inside the if (params_.predict_to_current_time) block, so if that parameter was false, the odometry was never published. Now it's always published, either with the optimization or predicted value. Let me know if you think something is wrong with this.

I wrote the comment before pushing the changes, but you can simply search for publish( and se e that now is outside that if block.

refs CORE-14776

refs CORE-14776 * Don't publish the odometry in the notifyCallback method, and publish it in the publishTimerCallback (formerly tfPublishTimerCallback) method instead. * Add 'process_noise_diagonal' parameter that defines the diagonal of the process noise covariance that is added to the propagated covariance computing after predicting the odometry into the current time. The process noise covariance has 8x8 size, so it can represent all the state, although the acceleration is ignored at the moment. * Add new 'predict' free functions to compute the Jacobian of the motion model wrt the state. Two versions are provided: 1. One that takes each single component and a plain array of double to the Jacobian. 2. Another that takes aggregate objects like tf2_2d::Transform and an fuse_core::Matrix8d for the Jacobian. * Cache the stamp of the latest covariance successfully computed in the notifyCallback, so it's possible to know if the covariance is valid and up to date in the publishTimerCallback method. If the covariance is invalid or not up to date, no time is wasted propagating it.

refs CORE-14776 Check all entries are positive.

refs CORE-14776

refs CORE-14776 Now the publish_frequency ROS parameter (formerly tf_publish_frequency) controls both the filtered state publication and the TF broadcasting.

refs CORE-14776 When we lookup the base_to_odom transform we should use the same time stamp as trans, which is the same as the odom_output_, which can either be last_stamp_ if predict_to_current_time is false, or event.current_real if it's true.

efernandez · 2019-11-01T18:47:51Z

fuse_models/src/odometry_2d_publisher.cpp

+        auto base_to_odom = tf_buffer_->lookupTransform(
+          params_.base_link_frame_id,
+          params_.odom_frame_id,
+          trans.header.stamp,


@svwilliams @ayrton04 This was a bug! Before the time stamp passed to lookupTransform was latest_stamp_, but if predict_to_current_time is true, this should be event.current_real instead. Using trans.header.stamp we get the correct stamp in both cases.

Because of this, with a robot turning in place the pose estimation overshoots. This is easy to see looking at a LIDAR scan alignment wrt the map. After this fix, the scans aligned perfectly. 😃

Let me know if you want this on another PR, only with this change, as it looks like a big issue.

efernandez · 2019-11-01T18:51:08Z

@svwilliams I still need to merge the predict functions, but I also found a couple of issues. See my comments in the code. I've thoroughly tested this brunch in sim and real data. I'll continue doing so, but now I'm more confident on the code contributed here.

I'll try to get the predict functions merged asap, hopefully over the weekend. But maybe it's better that this PR gets merged and I create another one after, so it's easier to review.

svwilliams · 2019-11-05T04:04:21Z

Merging this PR, and expecting a second "predict" PR at some point in the future. :)

efernandez force-pushed the CORE-14776_predict_odom_upstream branch 2 times, most recently from e3388c0 to e982ccd Compare October 16, 2019 07:10

svwilliams requested review from svwilliams and ayrton04 October 16, 2019 12:11

svwilliams reviewed Oct 16, 2019

View reviewed changes

ayrton04 approved these changes Oct 17, 2019

View reviewed changes

fuse_models/include/fuse_models/unicycle_2d_predict.h Show resolved Hide resolved

fuse_models/src/odometry_2d_publisher.cpp Outdated Show resolved Hide resolved

efernandez force-pushed the CORE-14776_predict_odom_upstream branch from eb8cb18 to 7e97427 Compare October 19, 2019 18:34

svwilliams reviewed Oct 24, 2019

View reviewed changes

fuse_models/test/test_unicycle_2d_predict.cpp Outdated Show resolved Hide resolved

efernandez force-pushed the CORE-14776_predict_odom_upstream branch 2 times, most recently from 44c1185 to 836c7f0 Compare October 25, 2019 10:01

efernandez changed the title ~~Allow to predict odometry~~ Publish odometry with timer and allow to predict it Oct 25, 2019

efernandez commented Oct 25, 2019

View reviewed changes

svwilliams reviewed Oct 29, 2019

View reviewed changes

efernandez commented Nov 1, 2019

View reviewed changes

efernandez added 6 commits November 1, 2019 19:42

Add missing comment

2c5b431

refs CORE-14776

Check process_noise_diagonal is positive

db94427

refs CORE-14776 Check all entries are positive.

Add predictJacobians unit test

994f9ba

refs CORE-14776

Rename tf_publish_frequency to publish_frequency

8725f4c

refs CORE-14776 Now the publish_frequency ROS parameter (formerly tf_publish_frequency) controls both the filtered state publication and the TF broadcasting.

efernandez force-pushed the CORE-14776_predict_odom_upstream branch from 836c7f0 to cceb457 Compare November 1, 2019 18:42

efernandez commented Nov 1, 2019

View reviewed changes

svwilliams approved these changes Nov 5, 2019

View reviewed changes

svwilliams merged commit eaaf1f2 into locusrobotics:devel Nov 5, 2019

efernandez mentioned this pull request Nov 6, 2019

Predict jacobians per parameter block #115

Merged

efernandez deleted the CORE-14776_predict_odom_upstream branch March 11, 2020 09:59

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Publish odometry with timer and allow to predict it #109

Publish odometry with timer and allow to predict it #109

efernandez commented Oct 15, 2019

svwilliams commented Oct 16, 2019 via email

efernandez commented Oct 16, 2019

svwilliams Oct 16, 2019

efernandez Oct 19, 2019

svwilliams Oct 24, 2019

efernandez Oct 25, 2019

ayrton04 left a comment

efernandez commented Oct 19, 2019

efernandez Oct 25, 2019

svwilliams left a comment

svwilliams Oct 29, 2019

efernandez Nov 1, 2019

svwilliams Nov 1, 2019 •

edited

Loading

efernandez Nov 4, 2019

efernandez Nov 1, 2019

efernandez Nov 1, 2019

efernandez Nov 1, 2019

efernandez commented Nov 1, 2019

svwilliams commented Nov 5, 2019

	[](const auto& v) { return v < 0.0; })) // NOLINT(whitespace/braces)
	[](const auto& v) { return v <= 0.0; })) // NOLINT(whitespace/braces)

	std::vector<double> process_noise_diagonal;
	private_node_handle_.param("process_noise_diagonal", process_noise_diagonal, process_noise_diagonal);

	if (process_noise_diagonal.size() != 8)
	{
	throw std::runtime_error("Process noise diagonal must be of length 8!");
	}

Publish odometry with timer and allow to predict it #109

Publish odometry with timer and allow to predict it #109

Conversation

efernandez commented Oct 15, 2019

svwilliams commented Oct 16, 2019 via email

efernandez commented Oct 16, 2019

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

ayrton04 left a comment

Choose a reason for hiding this comment

efernandez commented Oct 19, 2019

Choose a reason for hiding this comment

svwilliams left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

svwilliams Nov 1, 2019 • edited Loading

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 commented Nov 1, 2019

svwilliams commented Nov 5, 2019

svwilliams Nov 1, 2019 •

edited

Loading