Made JNI modifications to expose the faster function, made the API use
the typesafe Matrix API, and synchronized the documentation with C++.
Sped up C++ LTV diff drive test from 20 ms to 15 ms.
Sped up C++ LTV unicycle test from 15 ms to 10 ms.
Both seem to work, but the SDA algorithm is specifically recommended for
solving DAREs as opposed to P-DAREs.
The QR decomposition was replaced with a partial pivoting LU
decomposition at the recommendation of section 2.4 of the paper.
More tests and a separate JNI function for each DARE solver variant were
added.
This avoids allocation overhead on construction. times() was also
rewritten to not allocate any temporary objects.
Getter calls in the C++ Quaternion class were modified for parity.
15 m/s is about 50 ft/s, which is way above what FRC robots should be
able to achieve. This limit lets us catch user errors from bad unit
conversions immediately instead of the LUT generation in the LTV
controllers hanging for a really long time.
Fixes#5027.
I timed the DARE unit tests, and the new solver is 0 to 100% faster in
all cases (that is, it's at least as fast as Drake's and up to 2x faster
in some cases).
The new solver is also much simpler, takes less time to compile, and
drops the libwpimath.so size from 325 MB to 301 MB.
I think most of the compilation time is coming from the eigenvalue
decompositions used to enforce argument preconditions.
This makes it possible to mock the timestamp for wpimath without affecting the rest of the library.
Co-authored-by: Peter Johnson <johnson.peter@gmail.com>
The ComputerVisionUtil class was added before AprilTag support was
announced. Now that it has, the functions for estimating a pose from
range and yaw are no longer needed; it's just better to get the pose
directly from the AprilTag.
The coordinate system on some function arguments was confusing or didn't
match the NWU convention the rest of the library uses. It's easier to
remove the functions now and add them back after they're fixed since the
fixes aren't trivial.
The range function was removed because it uses pitch and yaw in the
camera's spherical coordinate system, which is obsoleted by AprilTags.
AprilTags give you a 6DOF pose directly, so range can be obtained via
Pose2d.getTranslation().getDistance().
Fixes#4757.
This effectively replaces the Unscented Kalman Filter used for Pose Estimation with the Odometry model, and uses a recalculable Kalman gain matrix to update pose estimations whenever a vision measurement is added.
Notably, this change removes the need for the confusing generics used in Java, and the C++ implementation got quite a bit less complex as well.
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Comparison operators which compared against every class member variable
now use C++20's default comparison operators.
Also remove operator!= that in C++20 is now auto-generated from operator==.
This is an API for looking up a Pose3d from a tag id, and includes functionality to load that map from a JSON file.
This also adds JSON support to Pose3d, Rotation3d. Translation3d, and Quaternion.
Co-authored-by: Tyler Veness <calcmogul@gmail.com>
Co-authored-by: AMereBagatelle <themerebagatelle@gmail.com>
