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]

ElliotScher#12 will fix the ICO file.

[ElliotScher]

/format

[Gold856]

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

[mcm001]

I can't comment on binary files, but the video file wpical/src/main/native/resources/testcalibration.avi isn't a fantastic example of a calibration video. It's taken nearly stationary straight on to a calibration board. https://mrcal.secretsauce.net/docs-2.2/tour-choreography.html is a good resource for how to design an optimal dance.

Similarly, the field test calibration videos have only two tags and the camera is nearly stationary - is this a representative use case?

[mcm001]

Am i missing something stupid? I can't seem to open an AVI video that i've confirmed looks good via v4l2

[ INFO:[email protected]] global videoio_registry.cpp:244 VideoBackendRegistry VIDEOIO: Enabled backends(8, sorted by priority): FFMPEG(1000); GSTREAMER(990); INTEL_MFX(980); V4L2(970); CV_IMAGES(960); CV_MJPEG(950); UEYE(940); OBSENSOR(930)
[ INFO:[email protected]] global backend_plugin.cpp:369 getPluginCandidates VideoIO plugin (FFMPEG): glob is 'libopencv_videoio_ffmpeg*.so', 1 location(s)
[ INFO:[email protected]] global backend_plugin.cpp:379 getPluginCandidates     - /home/matt/Documents/GitHub/allwpilib/wpical/build/install/wpicalTest/linuxx86-64/debug/lib: 0
[ INFO:[email protected]] global backend_plugin.cpp:383 getPluginCandidates Found 0 plugin(s) for FFMPEG
[ INFO:[email protected]] global backend_plugin.cpp:369 getPluginCandidates VideoIO plugin (GSTREAMER): glob is 'libopencv_videoio_gstreamer*.so', 1 location(s)
[ INFO:[email protected]] global backend_plugin.cpp:379 getPluginCandidates     - /home/matt/Documents/GitHub/allwpilib/wpical/build/install/wpicalTest/linuxx86-64/debug/lib: 0
[ INFO:[email protected]] global backend_plugin.cpp:383 getPluginCandidates Found 0 plugin(s) for GSTREAMER
[ INFO:[email protected]] global backend_plugin.cpp:369 getPluginCandidates VideoIO plugin (INTEL_MFX): glob is 'libopencv_videoio_intel_mfx*.so', 1 location(s)
[ INFO:[email protected]] global backend_plugin.cpp:379 getPluginCandidates     - /home/matt/Documents/GitHub/allwpilib/wpical/build/install/wpicalTest/linuxx86-64/debug/lib: 0
[ INFO:[email protected]] global backend_plugin.cpp:383 getPluginCandidates Found 0 plugin(s) for INTEL_MFX
[ INFO:[email protected]] global cap_images.cpp:289 icvExtractPattern Pattern: /home/matt/Videos/wpical-testing/%04d-12-27_08-42-50.avi @ 2024
[ INFO:[email protected]] global cap_images.cpp:338 open CAP_IMAGES: File is not an image: /home/matt/Videos/wpical-testing/2024-12-27_08-42-50.avi
Unable to open video /home/matt/Videos/wpical-testing/2024-12-27_08-42-50.avi

video.zip

Edit: even after re-encoding with ffmpeg -i ../2024-12-27_08-42-50.mkv -f avi -c:v mjpeg -b:v 4000k -c:a libmp3lame -b:a 320k 2024-12-27_08-42-50.avi, still no dice.

[ElliotScher]

Can you try removing the audio stream?

[mcm001]

Yep, removing the audio stream fixed it. FFMPEG command was ffmpeg -i ../2024-12-27_08-42-50.mkv -f avi -c:v mjpeg -b:v 4000k -an 2024-12-27_08-42-50.avi

[ElliotScher]

I'll add that to the transcoding instructions for Linux

[Gold856]

Hold on, I've got ElliotScher#13 to check the minimath generated header.