-
Notifications
You must be signed in to change notification settings - Fork 613
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
[wpimath] Add 3d odometry and pose estimation #7119
base: main
Are you sure you want to change the base?
Conversation
Timing summary is below. I'm skeptical of the results, though, given the large disparity between C++ debug and Java debug, and the very low difference between release and debug. If someone knows of any pitfalls in running tests in release vs debug mode in cmake, please let me know so I can try to run the tests more accurately:
Below are snippets from running tests on 6a13f34:
|
The |
Ran out of stack space on Windows when doing the warmup
Looks good- Linux CMake was just an internet issue, wpiformat was the previously mentioned bug with |
Does this need the reset constructor call that was recently added to the 2D pose estimators? |
Yes, Also, prior to merging all of the timing-related code needs to be removed. |
* Returns the position of the robot on the field. | ||
* @return The pose of the robot. | ||
*/ | ||
const Pose2d& GetPose() const { return m_pose2d; } |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Should Java also cache the 2D pose?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Probably. The main point of the cache is really so that the reference is valid, but it wouldn't hurt to make the implementations more consistent and save on allocations. There also shouldn't be any wasted work in updating the cache, since it's likely that every call to update will also want the 2d pose.
By the way, I'm at school, so sometimes I'll be unresponsive for an hour or two.
Opening mostly just to get the code out there.
Resolves #4933.
Still needs to be timed to determine whether the performance is good enough to always use 3D or if we need to maintain a separate 2D implementation to not affect performance for the 2D case too much. Local accuracy tests show very similar results, though, on the order of 1e-12 meters and 1e-12 radians. (Local accuracy tests were done by adding some logging code to
testFollowTrajectory()
inSwerveDrivePoseEstimatorTest.cpp
, copying intoSwerveDrivePoseEstimator3dTest.cpp
, replacingSwerveDrivePoseEstimator
withSwerveDrivePoseEstimator3d
, and then running tests. Hopefully in the next few weeks I can document the process more precisely.)Also needs 3D-specific tests added at some point.