WPIcal: Field Calibration Tool #6915
Conversation
|
Re: nlohmann json- we already have this added. It's accessible through |
|
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. |
| for (int i = 0; i < camera_matrix.rows(); i++) | ||
| { | ||
| for (int j = 0; j < camera_matrix.cols(); j++) | ||
| { | ||
| camera_matrix(i, j) = | ||
| json_data["camera_matrix"][(i * camera_matrix.cols()) + j]; | ||
| } |
There was a problem hiding this comment.
Consider replacing with a specialization of frc::from_json, like here:
| problem.AddResidualBlock(cost_function, nullptr, | ||
| pose_begin_iter->second.p.data(), | ||
| pose_begin_iter->second.q.coeffs().data(), | ||
| pose_end_iter->second.p.data(), | ||
| pose_end_iter->second.q.coeffs().data()); | ||
|
|
||
| problem.SetManifold(pose_begin_iter->second.q.coeffs().data(), | ||
| quaternion_manifold); | ||
| problem.SetManifold(pose_end_iter->second.q.coeffs().data(), | ||
| quaternion_manifold); |
There was a problem hiding this comment.
Consider reforumulating this as a cost function on tag corner reprojection error rather than pose error. This allows you to side-step pose ambiguity introduced by solvePNP. See https://github.com/mcm001/gtsam-playground/blob/a2de73d2fdb2c4feb39ea068dcfb7d333685babc/src/gtsam_utils.cpp#L43 & https://github.com/mcm001/gtsam-playground/blob/a2de73d2fdb2c4feb39ea068dcfb7d333685babc/src/localizer.cpp#L412, but basically the formulation here is:
for tag in tags:
// for each tag corner (4 per tag)
for i in range(4)
// Vec3 from camera CS to tag corner point
cameraTtag_corner = worldTtag_corner_i.relativeTo(worldTcamera_i)
// Vec2 in pixels
predicted_corner_px = camera_model.project(cameraTtag_corner )
error = predicted_corner_px - measured_corner_px
There was a problem hiding this comment.
This could also be formulated as a factor graph using gtsam - this is a specialization of a generic structure from motion problem (https://github.com/borglab/gtsam/blob/4.2.0/examples/SFMExampleExpressions.cpp) but I think that in both cases with the same noise model used for all tag corner pixel measurements, the math will reduce to a least squares optimization problem on reprojection error and I'd expect you to get the same output from both.
There was a problem hiding this comment.
|
Since mrcal-java is GPLv3, this tool would have to be GPLv3, vs the rest of WPIlib which is BSD. |
|
This is a C++ tool; what does this actually use from mrcal-java? Would the authors be willing to relicense those pieces? |
|
This tool only uses the native code, it doesn't use any of the JNI stuff. Specifically, WPIcal only uses mrcal_wrapper.cpp. |
|
Mrcal wrapper is a crime against humanity. Y’all can have that licensed under the WTFPL lmao. |
|
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 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. |
I haven't tried using Sleipnir for manifold-based optimization, so I don't know how robust it is. Instantiating |
|
Would the WPIlib community prefer to use Sleipnir to Ceres in this application? |
|
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. |
|
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 |
|
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. |
|
Worth noting that Gtsam requires no external dependencies (boost is optional, iirc) |
|
I'll try porting it over to Sleipnir to remove the Ceres dependency. |
|
Here's some resources on Sleipnir for reference: Tutorial and API docs: https://sleipnirgroup.github.io/Sleipnir/docs/cpp/ I wrote Sleipnir, so feel free to reach out if you encounter any difficulties or bugs with it. |
|
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 |
|
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. |
|
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? |
|
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? |
|
We can add Ceres as a thirdparty repo and have WPICal pull it in. If that helps maintainability, I’m all for it. |
|
#6929 adds ceres as an upstream utils repository |
|
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 |
|
first draft of tool documentation has been written: wpilibsuite/frc-docs#2691 |
|
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 |
|
Update:
Next:
Aside from that, WPIcal should be ready for release once the dependencies go in and get linked to the project! |
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:
Dependencies
WPIcal uses some dependencies that will need to be added to WPILib:
Tasks
WPIcal still has a couple of things that need to happen before it is ready:
working version of WPIcal: https://github.com/ElliotScher/wpical