WPIcal: Field Calibration Tool #6915

ElliotScher · 2024-08-02T13:24:53Z

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 · 2024-08-02T13:52:57Z

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

mcm001 · 2024-08-02T14:43:21Z

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.

mcm001 · 2024-08-02T14:45:47Z

wpical/src/main/native/cpp/fieldcalibration.cpp

+    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];
+      }


Consider replacing with a specialization of frc::from_json, like here:

allwpilib/apriltag/src/main/native/cpp/AprilTag.cpp

Line 58 in caf424e

void frc::from_json(const wpi::json& json, AprilTag& apriltag) {

mcm001 · 2024-08-02T14:54:27Z

wpical/src/main/native/cpp/fieldcalibration.cpp

+    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);


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

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.

Other prior art: https://github.com/berndpfrommer/tagslam/blob/a8afd5d2bb85e8d783831af7cedc0838e0ed53d1/src/gtsam_optimizer.cpp#L205

sciencewhiz · 2024-08-02T15:23:19Z

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

PeterJohnson · 2024-08-03T02:09:04Z

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

ElliotScher · 2024-08-03T02:25:22Z

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

mcm001 · 2024-08-03T03:16:10Z

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

spacey-sooty · 2024-08-03T04:48:19Z

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 · 2024-08-03T18:58:36Z

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 · 2024-08-03T19:29:34Z

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 · 2024-08-03T21:14:03Z

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

Gold856 · 2024-08-03T22:11:57Z

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 · 2024-08-04T00:46:26Z

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 · 2024-08-04T07:45:11Z

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 · 2024-08-04T15:37:01Z

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

ElliotScher · 2024-08-04T16:37:40Z

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

calcmogul · 2024-08-04T16:53:14Z

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 · 2024-08-04T19:15:52Z

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 · 2024-08-04T19:18:57Z

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 · 2024-08-04T19:23:31Z

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 · 2024-08-06T19:14:55Z

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 · 2024-08-06T20:48:08Z

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 · 2024-08-07T03:07:59Z

#6929 adds ceres as an upstream utils repository

mcm001 · 2024-08-09T06:50:15Z

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 · 2024-08-11T01:54:06Z

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

sciencewhiz · 2024-08-12T18:31:27Z

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

ElliotScher · 2024-11-11T04:03:35Z

From the stacktrace, it looks like I'm not including aruco.hpp and charuco.hpp correctly. I pulled the paths from where those headers are located in the artifactory, was that correct?

Gold856 · 2024-11-11T04:40:02Z

Did you update the OpenCV version? It looks like we're still on the 4th tag (instead of the 5th)

ElliotScher · 2024-11-11T04:48:35Z

yeah, in Opencv.gradle, I updated

def opencvVersion = '4.8.0-4'
to
def opencvVersion = '4.8.0-5'

Is there somewhere else it needs to be updated as well?

shared/config.gradle

shared/opencv.gradle

Co-authored-by: Jade <[email protected]>

mcm001 · 2024-11-11T19:55:20Z

wpical/src/main/native/cpp/fieldcalibration.cpp

+  // for (const auto& constraint : constraints)
+  // {
+  // wpi::json output_constraint;
+
+  // output_constraint["from_id"] = constraint.id_begin;
+  // output_constraint["to_id"] = constraint.id_end;
+
+  // output_constraint["transform"]["translation"]["x"] =
+  // constraint.t_begin_end.p[0];
+  // output_constraint["transform"]["translation"]["y"] =
+  // constraint.t_begin_end.p[1];
+  // output_constraint["transform"]["translation"]["z"] =
+  // constraint.t_begin_end.p[2];
+
+  // output_constraint["transform"]["rotation"]["x"] =
+  // constraint.t_begin_end.q.x();
+  // output_constraint["transform"]["rotation"]["y"] =
+  // constraint.t_begin_end.q.y();
+  // output_constraint["transform"]["rotation"]["z"] =
+  // constraint.t_begin_end.q.z();
+  // output_constraint["transform"]["rotation"]["w"] =
+  // constraint.t_begin_end.q.w();
+
+  // output["constraints"].push_back(output_constraint);
+
+  // std::cout << constraint.id_begin << " -- " << constraint.id_end <<
+  // std::endl;


Dead commented-out code?

mcm001 · 2024-11-11T19:56:34Z

wpical/src/main/native/cpp/fieldcalibration.cpp

+  std::map<int, wpi::json> observed_map = ideal_map;
+
+  Eigen::Matrix<double, 4, 4> correction_a;
+  correction_a << 0, 0, -1, 0, 1, 0, 0, 0, 0, -1, 0, 0, 0, 0, 0, 1;


Black-magic rotation matrix? Where does this come from?

mcm001 · 2024-11-11T19:58:09Z

wpical/src/main/native/cpp/fieldcalibration.cpp

+  transform.block<3, 3>(0, 0) =
+      Eigen::Quaternion<double>(
+          ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["W"],
+          ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["X"],
+          ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"],
+          ideal_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"])
+          .toRotationMatrix();
+
+  transform(0, 3) = ideal_map[tag_id]["pose"]["translation"]["x"];
+  transform(1, 3) = ideal_map[tag_id]["pose"]["translation"]["y"];
+  transform(2, 3) = ideal_map[tag_id]["pose"]["translation"]["z"];


Can this be replaced with wpi::from_jsonfrc::Pose3d() and then convert from Pose3d to a 4x4 transformation matrix?

ElliotScher · 2024-11-13T19:27:35Z

The errors with linking opencv seem to be related to the fact that thirdparty-opencv doesn't publish the static aruco library. It does publish the shared library though. Would it be problematic to use the shared opencv libraries for this project instead of the static libraries?

ThadHouse · 2024-11-13T19:29:51Z

Needs to be static, but that means we forgot to merge the static aruco lib in. Make an issue in the opencv repo and we can add that.

ElliotScher · 2024-11-13T19:36:47Z

When I tried to build and publish thirdparty-opencv locally, libopencv_aruco.a was being built, but there's a line in publish.gradle that excludes it from being published:

from(resolvedDir.toFile()) {
                into project.platformPath + '/static'
                include 'libopencv*.a'
                exclude 'libopencv_*.a'
}

Is this the aruco lib that we need? Can we just include it in the publish step?

ThadHouse · 2024-11-13T19:41:05Z

There’s a step that merges all of the static libraries. So we’d want to add that library to the merge.

The reason we can’t do separate libs is we can’t control the link order between libraries in the same archive.

ElliotScher · 2024-11-15T18:36:44Z

Could the errors with the ceres dependency have to do with the link order of the libs? They're all separate, unlike the static libs with opencv.

ThadHouse · 2024-11-15T18:38:57Z

Oh. Yeah very likely.

stereo.c seemed to be causing more issues, and isn't used by mrcal-java, so we shouldn't need it for wpical

Gold856 · 2024-11-16T04:47:05Z

ElliotScher#4 should fix a bunch of MSVC errors. Pretty much the only things left should be replacing all the VLAs and fixing linking.

More MSVC fixes

wpical/src/main/native/thirdparty/mrcal_java/src/mrcal_wrapper.cpp

mcm001 reviewed Aug 2, 2024

View reviewed changes

spacey-sooty reviewed Aug 3, 2024

View reviewed changes

spacey-sooty mentioned this pull request Aug 5, 2024

Ability to send Apriltag layout to coprocessors from robot code PhotonVision/photonvision#1389

Open

ElliotScher mentioned this pull request Aug 11, 2024

WPIcal documentation initial draft wpilibsuite/frc-docs#2691

Draft

ElliotScher and others added 4 commits November 5, 2024 23:59

Fix mrcal build errors.

7111eb2

Merge branch 'wpilibsuite:main' into main

b9e57b9

Update opencv.

8ff3856

Merge branch 'wpilibsuite:main' into main

2ef720a

Merge branch 'main' into main

138e592

fix opencv version.

f260ef5

spacey-sooty suggested changes Nov 11, 2024

View reviewed changes

shared/config.gradle Outdated Show resolved Hide resolved

shared/opencv.gradle Outdated Show resolved Hide resolved

ElliotScher and others added 4 commits November 11, 2024 09:36

Update shared/config.gradle

a937a06

Co-authored-by: Jade <[email protected]>

Update shared/opencv.gradle

f158766

Co-authored-by: Jade <[email protected]>

Fix cameracalibration aruco includes.

053ce55

Add maybe-uninitialized suppression.

ef8c59e

mcm001 reviewed Nov 11, 2024

View reviewed changes

ElliotScher and others added 2 commits November 15, 2024 20:09

Merge branch 'wpilibsuite:main' into main

cd1959e

More MSVC fixes

1b55089

stereo.c seemed to be causing more issues, and isn't used by mrcal-java, so we shouldn't need it for wpical

Merge pull request #4 from Gold856/wpical

f0ff201

More MSVC fixes

mcm001 reviewed Nov 18, 2024

View reviewed changes

wpical/src/main/native/thirdparty/mrcal_java/src/mrcal_wrapper.cpp Outdated Show resolved Hide resolved

ElliotScher added 2 commits November 19, 2024 11:01

fix glog errors on linux, bump opencv version

f3caf2c

update mrcal_java upstreamutil to prevent stack allocation errors.

a2ceaff

WPIcal: Field Calibration Tool #6915

Are you sure you want to change the base?

WPIcal: Field Calibration Tool #6915

Conversation

ElliotScher commented Aug 2, 2024 • edited Loading

WPIcal

Overview

Proposal

Dependencies

Tasks

rzblue commented Aug 2, 2024

mcm001 commented Aug 2, 2024

mcm001 Aug 2, 2024

Choose a reason for hiding this comment

mcm001 Aug 2, 2024 • edited Loading

Choose a reason for hiding this comment

mcm001 Aug 2, 2024

Choose a reason for hiding this comment

mcm001 Aug 2, 2024

Choose a reason for hiding this comment

sciencewhiz commented Aug 2, 2024

PeterJohnson commented Aug 3, 2024 • edited Loading

ElliotScher commented Aug 3, 2024

mcm001 commented Aug 3, 2024 • edited Loading

spacey-sooty commented Aug 3, 2024

spacey-sooty left a comment • edited Loading

Choose a reason for hiding this comment

PeterJohnson commented Aug 3, 2024

calcmogul commented Aug 3, 2024 • edited Loading

ElliotScher commented Aug 3, 2024

Gold856 commented Aug 3, 2024 • edited Loading

spacey-sooty commented Aug 4, 2024

calcmogul commented Aug 4, 2024

mcm001 commented Aug 4, 2024

ElliotScher commented Aug 4, 2024

calcmogul commented Aug 4, 2024 • edited Loading

mcm001 commented Aug 4, 2024 • edited Loading

calcmogul commented Aug 4, 2024 • edited Loading

mcm001 commented Aug 4, 2024 • edited Loading

ElliotScher commented Aug 6, 2024

Gold856 commented Aug 6, 2024

spacey-sooty commented Aug 7, 2024

mcm001 commented Aug 9, 2024

ElliotScher commented Aug 11, 2024

sciencewhiz commented Aug 12, 2024

ElliotScher commented Nov 11, 2024

Gold856 commented Nov 11, 2024

ElliotScher commented Nov 11, 2024

mcm001 Nov 11, 2024

Choose a reason for hiding this comment

mcm001 Nov 11, 2024

Choose a reason for hiding this comment

mcm001 Nov 11, 2024

Choose a reason for hiding this comment

ElliotScher commented Nov 13, 2024 • edited Loading

ThadHouse commented Nov 13, 2024

ElliotScher commented Nov 13, 2024

ThadHouse commented Nov 13, 2024

ElliotScher commented Nov 15, 2024

ThadHouse commented Nov 15, 2024

Gold856 commented Nov 16, 2024

ElliotScher commented Aug 2, 2024 •

edited

Loading

mcm001 Aug 2, 2024 •

edited

Loading

PeterJohnson commented Aug 3, 2024 •

edited

Loading

mcm001 commented Aug 3, 2024 •

edited

Loading

spacey-sooty left a comment •

edited

Loading

calcmogul commented Aug 3, 2024 •

edited

Loading

Gold856 commented Aug 3, 2024 •

edited

Loading

calcmogul commented Aug 4, 2024 •

edited

Loading

mcm001 commented Aug 4, 2024 •

edited

Loading

calcmogul commented Aug 4, 2024 •

edited

Loading

mcm001 commented Aug 4, 2024 •

edited

Loading

ElliotScher commented Nov 13, 2024 •

edited

Loading