Constant Velocity Constraint between NavStates #701
Conversation
|
auto is great. Taking a look now. |
| }; | ||
|
|
||
| if (H1) { | ||
| (*H1) = numericalDerivative21<gtsam::Vector, NavState, NavState>(evaluateErrorInner, x1, x2, 1e-5); |
There was a problem hiding this comment.
I think what is needed here is a call to
Vector9 NavState::localCoordinates(const NavState& g, OptionalJacobian<9, 9> H1, OptionalJacobian<9, 9> H2) const {
where you evaluate the difference between predicted and x2, as in:
NavState predicted = (something that yields predicted_H_x1)
Vector9 error = predicted.localCoordinates(x2, error_H_predicted, H2);
if (H1) *H1 = error_H_predicted * predicted_H_x1;
There was a problem hiding this comment.
Any reason I can't just leverage the navstate.update(body_accel,body_omega, dt) function here to get my predicted NavState? I will want to use those terms later (and build out a NavState++ that can store those states as well) so this would get me closer to that final goal.
There was a problem hiding this comment.
I took a pass at this style. Seems to behave, but let me know if I missunderstood something. Obviously I need to test the rotation cases and find a cleaner way to init a Vector9 to clean up the verbosity of the tests.
|
BTW Matias from Oxford just posted three amazing posts on https://gtsam.org/blog/ that can help with manifold concepts. Esp. part 2. |
|
|
I also noticed while looking at |
|
Yes, feel free to change to dP, and dv to dV. |
fixed. |
|
Sorry, got busy with IROS. Is this still WIP or ready for re-review? |
Yes please. More extensions can come in a later pr. |
| NavState predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); | ||
|
|
||
| Matrix error_H_predicted; | ||
| Vector9 error = predicted.localCoordinates(x2, error_H_predicted, H2); |
There was a problem hiding this comment.
Cool. But please use H1 ? &error_H_predicted : nullptr to avoid evaluation of derivative if H1 is not asked for.
| static const Vector3 b_omega{0.0, 0.0, 0.0}; | ||
|
|
||
| Matrix predicted_H_x1; | ||
| NavState predicted = x1.update(b_accel, b_omega, dt_, predicted_H_x1, {}, {}); |
There was a problem hiding this comment.
Cool. But please use H1 ? &predicted_H_x1 : nullptr to avoid evaluation of derivative if H1 is not asked for.
There was a problem hiding this comment.
I couldn't get this style to compile, so I went a little more verbose. Any thoughts on how to make this prettier would be appreciated, otherwise this accomplishes your request I believe.
There was a problem hiding this comment.
error: no viable conversion from 'gtsam::Matrix *' (aka 'Matrix<double, Dynamic, Dynamic> *') to 'OptionalJacobian<9, 9>
There was a problem hiding this comment.
Ah, yeah. This style should definitely compile, and I recommend you do use it. But the issue is you need a fixed size matrix. Which will also benefit performance, as it gets rid of a heap allocation. In example above:
Matrix99 predicted_H_x1;
There was a problem hiding this comment.
Ahh... got it. That compiles now. Should be done!
Here is a first pass at a Constant Velocity Constraint between two NavStates.
It is much cribbed from the VelocityConstraint in gtsam_unstable/dynamics. Hopefully along the way we can improve this to be usable in external state tracking problems.
I am using almost always auto style here, let me know if that is discouraged, or how else I can tailor this back to being more in line with the rest of the library.