[draft] switch wpical backend to gtsam #7606

mcm001 · 2024-12-29T17:16:05Z

Draft MR switching the wpical backend from ceres to GTSAM via the photonvision thirdparty repo.

Blocked on #7796

mcm001 · 2024-12-29T17:36:21Z

wpical/build.gradle

+                    it.cppCompiler.args.add("-Wno-deprecated-copy")
+                    it.cppCompiler.args.add("-Wno-redundant-move")


we should probably fix these upstream instead

mcm001 · 2024-12-29T17:37:27Z

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

-inline cameracalibration::CameraModel load_camera_model(wpi::json json_data) {
+inline CameraModel load_camera_model(wpi::json json_data) {


This function feels like it doesn't belong in fieldcalibration code?

mcm001 · 2024-12-29T17:39:38Z

wpical/src/main/native/cpp/gtsam_meme/Pose3WithVariance.h

+struct Pose3WithVariance {
+  frc::Pose3d pose;
+  // in order rx ry rz tx ty tz. Units are SI units
+  std::array<double, 6> covariance;


member is named covariance but holds variance? swap to cov

mcm001 · 2024-12-29T17:40:18Z

wpical/src/main/native/cpp/gtsam_meme/TagDetection.h

+
+struct TagDetection {
+  int32_t id;
+  std::vector<TargetCorner> corners;


always 4 corners - consider an array

mcm001 · 2024-12-29T17:41:00Z

wpical/src/main/native/cpp/gtsam_meme/gtsam_tag_map.cpp

+  return worldTtags;
+}
+
+static std::vector<Point3> MakeTagModel(double width) {


consider templating this

mcm001 · 2024-12-29T17:41:40Z

wpical/src/main/native/cpp/gtsam_meme/pose_converters.cpp

+  // cv::Mat R;
+  // cv::Rodrigues(rvec, R); // R is 3x3
+  // R = R.t();                 // rotation of inverse
+  // cv::Mat tvecI = -R * tvec; // translation of inverse


Gold856

Since we're not in a rush for this, and I know you want some of calibration things in here, considering splitting wpical into app and lib directories like Glass, so you can have a libwpical you can pull in.

mcm001 · 2024-12-29T23:50:58Z

I was trying to keep the diff small, but now that wpical is in main im less scared about merge conflicts unless @ElliotScher is planning a similar refactor already?

ElliotScher · 2024-12-30T01:27:04Z

I wasn't planning on it yet, but I can if it would help.

mcm001 · 2024-12-30T03:05:43Z

I don't think it would - I think it makes more sense here to swap out the bones and fight the build system, then we can think about other plans? The wpical docs PR going in and getting some user feedback post kickoff seem like plenty of work to me

Gold856

wpical needs _ENABLE_EXTENDED_ALIGNED_STORAGE to be defined. Confirmed this is defined in gtsam's CMake build.

mcm001 · 2025-01-15T04:34:08Z

Why does it need it?

This means Gradle won't have to generate source files containing the data for these files. Those files were never used at compile time.

…ke variable names clearer

…ntryTarget

Fixes a bug where trying to cancelling directory selection when combining calibrated field maps would spam open the directory selector

…ields

Signed-off-by: swurl <[email protected]> Co-authored-by: Tyler Veness <[email protected]>

The problem was ill-conditioned if either semiaxis had zero length.

…te#7854)

…e#7850) Throughout the code the state sqrt covariance S and innovation covariance Sy are maintained as upper triangular cholesky factors of those covariance matrices. The original paper defines P=S*S', so S should be lower triangular. The functions in the paper reflect this. In the code implementation, the sqrt covariance matrices are upper triangular, but the algorithm expects them to be lower triangular. This bug was likely missed because the incorrect version of the filter is able to converge for some systems where all the states are observed, and the test case is set up such that all states are observed. To fix the bug, a couple things needed to be changed: all instances of rankUpdate() needed to be changed to use the lower triangular cholesky factor, In the unscented transform, when S is found via QR decomposition, we need to take the transpose because R is upper triangular, P() and SetP() functions need to be modified to be P=S*S' instead of P=S'*S, and P.llt().matrixL() instead of P.llt().matrixU() respectively. Each part of the algorithm has also had the comments changed to clarify exactly which equation from the paper it implements. Co-authored-by: Tyler Veness <[email protected]>

github-actions bot added the component: wpimath Math library label Dec 29, 2024

mcm001 force-pushed the mmorley/wpical-gtsam branch 3 times, most recently from 6f2a99d to d34b60f Compare December 29, 2024 17:35

mcm001 commented Dec 29, 2024

View reviewed changes

mcm001 mentioned this pull request Dec 29, 2024

Fix deprecated copies and redundant moves borglab/gtsam#1950

Merged

Gold856 reviewed Dec 29, 2024

View reviewed changes

This comment was marked as outdated.

Sign in to view

spacey-sooty force-pushed the mmorley/wpical-gtsam branch 2 times, most recently from cb0027d to 41dc097 Compare December 30, 2024 11:49

Gold856 suggested changes Jan 15, 2025

View reviewed changes

Gold856 added 10 commits March 3, 2025 07:45

Use GPL-less Ceres and SuiteSparse

7cfc9f7

Move files used at runtime to assets folder

a14534b

This means Gradle won't have to generate source files containing the data for these files. Those files were never used at compile time.

Use camel case, combine file opening and processing functions, and ma…

692740d

…ke variable names clearer

Remove unused isCalibrating variable

eee28e4

Move calibration combination to a separate function, and inline EmitE…

f7aa86d

…ntryTarget

Simplify calibration saving

a6cfe3f

Fixes a bug where trying to cancelling directory selection when combining calibrated field maps would spam open the directory selector

Add docs for some functions

51d58d2

Move calibration visualization to a separate function

96e54f6

Deduplicate mrcal_result handling

46c13c0

Clean up camera model handling and dedupe calibration UI logic

72689e0

Gold856 added 6 commits March 3, 2025 07:45

Use AprilTagFieldLayout and wpimath and make massive refactors

ac63077

Fix field saving and fix tags being silently dropped when combining f…

bbd3644

…ields

Add additional third party notices

4550304

Fix up camera model loading

e8b98ac

Catch OpenCV exceptions

bab9b43

Fix compilation with Clang 15

8e75f13

mcm001 force-pushed the mmorley/wpical-gtsam branch from 41dc097 to 7626e6e Compare March 18, 2025 03:19

github-actions bot added the component: wpical WPIcal label Mar 18, 2025

mcm001 force-pushed the mmorley/wpical-gtsam branch from 7626e6e to 230efdf Compare March 18, 2025 03:20

crueter and others added 6 commits March 17, 2025 20:31

[wpimath] Add Debouncer type and time setters (wpilibsuite#7839)

b6285c3

Signed-off-by: swurl <[email protected]> Co-authored-by: Tyler Veness <[email protected]>

[wpimath] Fix singularities in Ellipse2d::Nearest() (wpilibsuite#7851)

9638e12

The problem was ill-conditioned if either semiaxis had zero length.

[wpiutil] Avoid including execinfo.h for Emscripten target (wpilibsui…

081e8b7

…te#7854)

Add extra eigen headers for gtsam

67a5845

Switch wpical backend to GTSAM

66216a6

mcm001 force-pushed the mmorley/wpical-gtsam branch from 230efdf to 66216a6 Compare March 18, 2025 03:32

github-actions bot added the component: wpiutil WPI utility library label Mar 18, 2025

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[draft] switch wpical backend to gtsam #7606

[draft] switch wpical backend to gtsam #7606

mcm001 commented Dec 29, 2024 •

edited

Loading

mcm001 Dec 29, 2024

mcm001 Dec 29, 2024

mcm001 Dec 29, 2024

mcm001 Dec 29, 2024

mcm001 Dec 29, 2024

mcm001 Dec 29, 2024

Gold856 left a comment

mcm001 commented Dec 29, 2024

ElliotScher commented Dec 30, 2024 •

edited

Loading

mcm001 commented Dec 30, 2024

This comment was marked as outdated.

Gold856 left a comment

mcm001 commented Jan 15, 2025

		it.cppCompiler.args.add("-Wno-deprecated-copy")
		it.cppCompiler.args.add("-Wno-redundant-move")

		inline cameracalibration::CameraModel load_camera_model(wpi::json json_data) {
		inline CameraModel load_camera_model(wpi::json json_data) {

[draft] switch wpical backend to gtsam #7606

Are you sure you want to change the base?

[draft] switch wpical backend to gtsam #7606

Conversation

mcm001 commented Dec 29, 2024 • edited Loading

mcm001 Dec 29, 2024

Choose a reason for hiding this comment

mcm001 Dec 29, 2024

Choose a reason for hiding this comment

mcm001 Dec 29, 2024

Choose a reason for hiding this comment

mcm001 Dec 29, 2024

Choose a reason for hiding this comment

mcm001 Dec 29, 2024

Choose a reason for hiding this comment

mcm001 Dec 29, 2024

Choose a reason for hiding this comment

Gold856 left a comment

Choose a reason for hiding this comment

mcm001 commented Dec 29, 2024

ElliotScher commented Dec 30, 2024 • edited Loading

mcm001 commented Dec 30, 2024

This comment was marked as outdated.

Gold856 left a comment

Choose a reason for hiding this comment

mcm001 commented Jan 15, 2025

mcm001 commented Dec 29, 2024 •

edited

Loading

ElliotScher commented Dec 30, 2024 •

edited

Loading