Switch wpical backend to GTSAM

wpilibsuite · Dec 29, 2024 · d34b60f · d34b60f
1 parent 35476da
commit d34b60f
Show file tree

Hide file tree

Showing 18 changed files with 974 additions and 353 deletions.
diff --git a/build.gradle b/build.gradle
@@ -29,8 +29,11 @@ wpilibVersioning.releaseMode = project.hasProperty('releaseMode')
 allprojects {
     repositories {
         maven {
-        url = 'https://frcmaven.wpi.edu/artifactory/ex-mvn'
-    }
+            url = 'https://frcmaven.wpi.edu/artifactory/ex-mvn'
+        }
+        maven {
+            url = "https://maven.photonvision.org/snapshots"
+        }
     }
     if (project.hasProperty('releaseMode')) {
         wpilibRepositories.addAllReleaseRepositories(it)

diff --git a/shared/gtsam.gradle b/shared/gtsam.gradle
@@ -0,0 +1,13 @@
+nativeUtils {
+    nativeDependencyContainer {
+        gtsam(getNativeDependencyTypeClass('WPIStaticMavenDependency')) {
+            groupId = "edu.wpi.first.thirdparty.frc2025.gtsam"
+            artifactId = "gtsam-cpp"
+            headerClassifier = "headers"
+            sourceClassifier = "sources"
+            ext = "zip"
+            version = '4.3-1'
+            targetPlatforms.addAll(nativeUtils.wpi.platforms.desktopPlatforms)
+        }
+    }
+}
diff --git a/wpical/CMakeLists.txt b/wpical/CMakeLists.txt
@@ -89,6 +89,7 @@ target_link_libraries(
     ${OpenCV_LIBS}
     wpigui
     wpiutil
+    wpimath
     Ceres::ceres
 )
 

diff --git a/wpical/build.gradle b/wpical/build.gradle
@@ -31,6 +31,7 @@ def wpilibVersionFileInput = file("src/main/generate/WPILibVersion.cpp.in")
 def wpilibVersionFileOutput = file("$buildDir/generated/main/cpp/WPILibVersion.cpp")
 
 apply from: "${rootDir}/shared/ceres.gradle"
+apply from: "${rootDir}/shared/gtsam.gradle"
 apply from: "${rootDir}/shared/opencv.gradle"
 
 task generateCppVersion() {
@@ -156,12 +157,14 @@ model {
                     it.buildable = false
                     return
                 }
+                lib project: ':wpimath', library: 'wpimath', linkage: 'static'
                 lib project: ':wpiutil', library: 'wpiutil', linkage: 'static'
                 lib project: ':wpigui', library: 'wpigui', linkage: 'static'
                 lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static'
                 lib project: ':apriltag', library: 'apriltag', linkage: 'static'
-                nativeUtils.useRequiredLibrary(it, 'ceres')
                 nativeUtils.useRequiredLibrary(it, 'opencv_static')
+                nativeUtils.useRequiredLibrary(it, 'gtsam')
+                nativeUtils.useRequiredLibrary(it, 'ceres')
                 it.cppCompiler.define 'GLOG_USE_GLOG_EXPORT'
                 if (it.targetPlatform.operatingSystem.isWindows()) {
                     it.cppCompiler.define('GLOG_DEPRECATED', '__declspec(deprecated)')
@@ -175,6 +178,9 @@ model {
                 } else if (it.targetPlatform.operatingSystem.isMacOsX()) {
                     it.linker.args << '-framework' << 'Metal' << '-framework' << 'MetalKit' << '-framework' << 'Cocoa' << '-framework' << 'IOKit' << '-framework' << 'CoreFoundation' << '-framework' << 'CoreVideo' << '-framework' << 'QuartzCore' << '-framework' << 'Accelerate'
                 } else {
+                    it.cppCompiler.args.add("-Wno-deprecated-copy")
+                    it.cppCompiler.args.add("-Wno-redundant-move")
+
                     it.linker.args << '-lX11' << "-lgfortran"
                     if (it.targetPlatform.name.startsWith('linuxarm')) {
                         it.linker.args << '-lGL'
@@ -211,10 +217,13 @@ model {
                 lib project: ':wpigui', library: 'wpigui', linkage: 'static'
                 lib project: ':thirdparty:imgui_suite', library: 'imguiSuite', linkage: 'static'
                 lib project: ':apriltag', library: 'apriltag', linkage: 'static'
+                lib project: ':wpimath', library: 'wpimath', linkage: 'static'
                 nativeUtils.useRequiredLibrary(it, 'ceres')
                 nativeUtils.useRequiredLibrary(it, 'opencv_static')
+                nativeUtils.useRequiredLibrary(it, 'gtsam')
                 it.cppCompiler.define('PROJECT_ROOT_PATH', testResources)
                 it.cppCompiler.define 'GLOG_USE_GLOG_EXPORT'
+
                 if (it.targetPlatform.name == nativeUtils.wpi.platforms.windowsarm64) {
                     // https://developercommunity.visualstudio.com/t/-imp-std-init-once-complete-unresolved-external-sy/1684365#T-N10041864
                     it.linker.args << '/alternatename:__imp___std_init_once_complete=__imp_InitOnceComplete'
@@ -240,6 +249,9 @@ model {
                     if (it.targetPlatform.name.startsWith('linuxarm')) {
                         it.linker.args << '-lGL'
                     }
+
+                    it.cppCompiler.args.add("-Wno-deprecated-copy")
+                    it.cppCompiler.args.add("-Wno-redundant-move")
                 }
             }
         }

diff --git a/wpical/src/main/native/cpp/cameracalibration.cpp b/wpical/src/main/native/cpp/cameracalibration.cpp
@@ -312,7 +312,11 @@ int cameracalibration::calibrate(const std::string& input_video,
   cv::Size boardSize(board_width - 1, board_height - 1);
   cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels);
 
+  int frameIdx = 0;
   while (video_capture.grab()) {
+    std::cout << "Frame index " << frameIdx << std::endl;
+    frameIdx++;
+
     cv::Mat frame;
     video_capture.retrieve(frame);
-Original file line number
+Diff line change
@@ Expand Up / @@ -89,6 +89,7 @@ target_link_libraries( @@
         ${OpenCV_LIBS}
         wpigui
         wpiutil
+        wpimath
         Ceres::ceres
     )
@@ Expand Down @@