WPIcal: Field Calibration Tool

[ElliotScher]

WPIcal

WPIcal is a tool used to “calibrate” or empirically measure the position and orientation of the Apriltags on FRC fields. This tool was inspired by team 1538, The Holy Cows, who created a command-line tool to perform this field calibration: https://github.com/TheHolyCows/cowlibration-field. WPIcal aims to streamline field calibration by combining the needed camera calibration with the field calibration process into a user-friendly application.

Overview

WPIcal performs camera calibration using a video of either a ChArUco board (using OpenCV calibration) or a chessboard (using MRcal). Users can also upload their own camera intrinsics JSON if they so choose. The tool then measures the positions of the Apriltags on the field relative to each other. By selecting a tag to “pin” or use the default position for, WPIcal will create a field map, in which the other tags on the field will be moved to their measured positions relative to the pinned tag.

Proposal

Integrating this tool into WPILib will ensure that all teams have access to a good calibration technique for:

• Field calibration at events
• Calibration of practice fields (which are very often imperfect)
• Apriltag "sanity check" for FTAs (tag position readout is provided with both position and rotation deltas)

Dependencies

WPIcal uses some dependencies that will need to be added to WPILib:

• mrcal-java is a wrapper for the mrcal library
  • mrcal is a robust camera calibration library
  • libdogleg is a optimization library that mrcal depends on (there may be license issues with this library)
  • SuiteSparse (this library is not explicitly used by WPIcal but is a dependency of mrcal-java)
• Ceres is a numerical optimization library used to solve for the locations of each apriltag

Tasks

WPIcal still has a couple of things that need to happen before it is ready:

• Possible rename. WPIcal was the working title and suggestions are always appreciated!
• Tool icon
• Tool documentation

working version of WPIcal: https://github.com/ElliotScher/wpical

[rzblue]

Re: nlohmann json- we already have this added. It's accessible through wpi/json.h and uses the wpi namespace instead of nlohmann

[mcm001]

I'm not super proud of the native code in mrcal-java lol. But that's just because mrcal is one huge C function with a million arguments.

[sciencewhiz]

Since mrcal-java is GPLv3, this tool would have to be GPLv3, vs the rest of WPIlib which is BSD.

[PeterJohnson]

This is a C++ tool; what does this actually use from mrcal-java? Would the authors be willing to relicense those pieces?

[ElliotScher]

This tool only uses the native code, it doesn't use any of the JNI stuff. Specifically, WPIcal only uses mrcal_wrapper.cpp.

[mcm001]

Mrcal wrapper is a crime against humanity. Y’all can have that licensed under the WTFPL lmao.

[spacey-sooty]

It feels like Ceres serves a similar purpose to Sleipnir which we already depend on. since we already have that as a dependency could this be migrated to Sleipnir?

[spacey-sooty]

Shouldn't resources like images be marked as binaries not as generated files in git? If they are in fact generated they should be placed in a directory with generated in the path as such.

[PeterJohnson]

Shouldn't resources like images be marked as binaries not as generated files in git? If they are in fact generated they should be placed in a directory with generated in the path as such.

I don't see where they are marked as generated files in git? .gitattributes has not been modified. If you're talking about .styleguide, this is consistent with how all the other tools do it currently.

[calcmogul]

It feels like Ceres serves a similar purpose to Sleipnir which we already depend on. since we already have that as a dependency could this be migrated to Sleipnir?

I haven't tried using Sleipnir for manifold-based optimization, so I don't know how robust it is. Instantiating Eigen::Quaternion<sleipnir::Variable> should work at least, after I move the NumTraits specialization into Variable.hpp (#6924).

[ElliotScher]

Would the WPIlib community prefer to use Sleipnir to Ceres in this application?

[Gold856]

It would be nice if we could use Sleipnir instead of Ceres since Sleipnir is already a part of the build, so using Sleipnir means less work for us.

Also, Ceres pulls in abseil as a dependency, and we avoid abseil like the plague because it's packaged into a million tiny library files.

[spacey-sooty]

Another thing is Sleipnir is already reasonably well known around the FRC and WPILib space making this tool easier to maintain down the line compared to using Ceres

[calcmogul]

I'm not too concerned about familiarity, because optimization frameworks all kinda work the same way. The optimization framework facilitating a concise expression of the problem is more important to me. Ceres looks a bit more verbose than Sleipnir or CasADi, but it's not horrible.

[mcm001]

Worth noting that Gtsam requires no external dependencies (boost is optional, iirc)

[ElliotScher]

I'll try porting it over to Sleipnir to remove the Ceres dependency.

[calcmogul]

Here's some resources on Sleipnir for reference:

Tutorial and API docs: https://sleipnirgroup.github.io/Sleipnir/docs/cpp/
Ellipse2d nearest-point example: https://github.com/wpilibsuite/allwpilib/blob/main/wpimath/src/main/native/cpp/geometry/Ellipse2d.cpp#L27-L53
Mostly trajectory optimization examples: https://github.com/SleipnirGroup/Sleipnir/tree/main/examples

I wrote Sleipnir, so feel free to reach out if you encounter any difficulties or bugs with it.

[mcm001]

For context, this is an MVP of this using gtsam + tag corners instead of solvePNP in the optimization problem.

https://github.com/mcm001/gtsam-playground/blob/tag-mapper-try-2/src/sfm_mapper/main.cpp

[calcmogul]

If we can get gtsam working, that would be ideal. Sleipnir is designed for constrained optimization on fields, while ceres and gtsam are designed for mostly unconstrained optimization on manifolds (this problem is one of the latter). gtsam has fewer dependencies but requires more background knowledge (e.g., what a factor graph is) to understand the API.

[mcm001]

I think this problem is simple enough in formulation that that shouldn't be too crazy. I still don't understand what/why tagslam does what it does with leaves of the graph and loop closures. But the problem code I posted is just a bunch of binary factors (factors attached between the camera pose and the tag pose on the field) + one pose prior fixing a tag in place, which seems simple enough to explain in code comments what's going on?

[ElliotScher]

Over the past couple of days I've been reading documentation and looking at community opinions in the PR and FIRST discord. From what I can gather, the consensus seems to be that Ceres will be hard to add to allwpilib, gtsam is a good option, but could be difficult to implement/maintain and add to allwpilib as well, and Sleipnir isn't designed for this type of optimization. Of these options, I'm thinking that using Ceres will be the best bet, purely from a maintainability standpoint. (I am familiar with it and plan on doing long term maintenance on this tool) If it is something that is at least feasible to add to wpilib (using a separate repo similar to opencv maybe) I think that would be the best course of action. That is, at least until the new solver on is finished: https://github.com/JoshuaAN/ManifoldOptimization

Is this possible?

[Gold856]

We can add Ceres as a thirdparty repo and have WPICal pull it in. If that helps maintainability, I’m all for it.

[spacey-sooty]

#6929 adds ceres as an upstream utils repository

[mcm001]

Taking over this to repost for continuity.

https://github.com/mcm001/gtsam-playground/blob/tag-mapper-try-2/src/sfm_mapper/main.cpp

Managed to map both sides of a simulated 2024 field with the gtsam holy-cows-but-with a better formulation thing! Total program runtime including my prints is 50ms for 50 images of 2-5 tags each on my i7-13850HX. The images were chose randomly without regard for pose ambiguity, since that can't happen with my formation and sufficient data. Real time spent (excluding problem setup) by the dogleg solver is 14 Ms for those 16 tags and 50 snapshots

[ElliotScher]

first draft of tool documentation has been written: wpilibsuite/frc-docs#2691

[sciencewhiz]

It's generally a good practice to do development on a branch in your fork, rather then main. This makes it easier to keep track of wpilibsuite main, and to work on multiple features at a time. https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/proposing-changes-to-your-work-with-pull-requests/about-branches

[Gold856]

The headers violate include what you use; in general, you only want to include what is actually used in a given file.

Also, when we were merging PRs, the .ico file got changed to Glass's .ico file; that should be fixed to have wpical's icon again.

[Gold856]

For the record, I'm fairly sure the reason for the sudden failure is because one of the apriltag headers pulled in Windows.h, which got included into WPICal.cpp through one of the wpical headers. Now that the apriltag header is only in the C++ files, not the headers, the main WPICal file suddenly doesn't have Windows.h anymore, which changes how WinMain is defined.

[Gold856]

ElliotScher#12 will fix the ICO file.

[mcm001]

Nit: Now that we are in allwpilib, it should be easy to use the allwpilib AprilTag wrappers. Their API is a whole lot more sane then the umich C api, LOL. Worth considering for future.

[Gold856]

Yeah, it would be good to use the AprilTag wrappers, but that would require wpimath and we'd have conflicting Eigen versions, which our build can't handle (yet.)

[mcm001]

And that's also why we can't use AprilTagFieldLayout yet right?

[Gold856]

Yep.

[ElliotScher]

/format

[Gold856]

/format doesn't work right now. You'll have to format manually.