[wpical] Add WPIcal: Field Calibration Tool (#6915)

Co-authored-by: Gold856 <117957790+Gold856@users.noreply.github.com>
Co-authored-by: Jade <spacey-sooty@proton.me>
Co-authored-by: Matthew Morley <matthew.morley.ca@gmail.com>

wpical/src/main/native/cpp/WPIcal.cpp

@@ -0,0 +1,591 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <cameracalibration.h>
+#include <fieldcalibration.h>
+#include <fieldmap.h>
+#include <fmap.h>
+
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <numbers>
+#include <string>
+#include <vector>
+
+#include <GLFW/glfw3.h>
+#include <imgui.h>
+#include <portable-file-dialogs.h>
+#include <tagpose.h>
+#include <wpi/json.h>
+#include <wpigui.h>
+
+namespace gui = wpi::gui;
+
+const char* GetWPILibVersion();
+
+#ifdef __linux__
+const bool showDebug = false;
+#else
+const bool showDebug = true;
+#endif
+
+namespace wpical {
+std::string_view GetResource_wpical_16_png();
+std::string_view GetResource_wpical_32_png();
+std::string_view GetResource_wpical_48_png();
+std::string_view GetResource_wpical_64_png();
+std::string_view GetResource_wpical_128_png();
+std::string_view GetResource_wpical_256_png();
+std::string_view GetResource_wpical_512_png();
+}  // namespace wpical
+
+void drawCheck() {
+  ImGui::SameLine();
+  ImDrawList* draw_list = ImGui::GetWindowDrawList();
+
+  ImVec2 pos = ImGui::GetCursorScreenPos();
+  float size = ImGui::GetFontSize();
+
+  ImVec2 p1 = ImVec2(pos.x + size * 0.25f, pos.y + size * 0.5f);
+  ImVec2 p2 = ImVec2(pos.x + size * 0.45f, pos.y + size * 0.7f);
+  ImVec2 p3 = ImVec2(pos.x + size * 0.75f, pos.y + size * 0.3f);
+
+  float thickness = 3.0f;
+  draw_list->AddLine(p1, p2, IM_COL32(0, 255, 0, 255), thickness);
+  draw_list->AddLine(p2, p3, IM_COL32(0, 255, 0, 255), thickness);
+  ImGui::NewLine();
+}
+
+static void DisplayGui() {
+  ImGui::GetStyle().WindowRounding = 0;
+
+  // fill entire OS window with this window
+  ImGui::SetNextWindowPos(ImVec2(0, 0));
+  int width, height;
+  glfwGetWindowSize(gui::GetSystemWindow(), &width, &height);
+  ImGui::SetNextWindowSize(
+      ImVec2(static_cast<float>(width), static_cast<float>(height)));
+
+  ImGui::Begin("Entries", nullptr,
+               ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_MenuBar |
+                   ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoMove |
+                   ImGuiWindowFlags_NoCollapse);
+
+  // main menu
+  ImGui::BeginMenuBar();
+  gui::EmitViewMenu();
+  if (ImGui::BeginMenu("View")) {
+    ImGui::EndMenu();
+  }
+  ImGui::EndMenuBar();
+
+  static std::unique_ptr<pfd::open_file> camera_intrinsics_selector;
+  static std::string selected_camera_intrinsics;
+
+  static std::unique_ptr<pfd::open_file> field_map_selector;
+  static std::string selected_field_map;
+
+  static std::unique_ptr<pfd::select_folder>
+      field_calibration_directory_selector;
+  static std::string selected_field_calibration_directory;
+
+  static std::unique_ptr<pfd::select_folder> download_directory_selector;
+  static std::string selected_download_directory;
+
+  static std::string calibration_json_path;
+
+  cameracalibration::CameraModel cameraModel = {
+      .intrinsic_matrix = Eigen::Matrix<double, 3, 3>::Identity(),
+      .distortion_coefficients = Eigen::Matrix<double, 8, 1>::Zero(),
+      .avg_reprojection_error = 0.0};
+  static bool mrcal = true;
+
+  static double squareWidth = 0.709;
+  static double markerWidth = 0.551;
+  static int boardWidth = 12;
+  static int boardHeight = 8;
+  static double imagerWidth = 1920;
+  static double imagerHeight = 1080;
+
+  static int pinnedTag = 1;
+
+  static int focusedTag = 1;
+  static int referenceTag = 1;
+
+  static Fieldmap currentCalibrationMap;
+  static Fieldmap currentReferenceMap;
+
+  // camera matrix selector button
+  if (ImGui::Button("Upload Camera Intrinsics")) {
+    camera_intrinsics_selector = std::make_unique<pfd::open_file>(
+        "Select Camera Intrinsics JSON", "",
+        std::vector<std::string>{"JSON", "*.json"}, pfd::opt::none);
+  }
+
+  ImGui::SameLine();
+  ImGui::Text("Or");
+  ImGui::SameLine();
+
+  // camera calibration button
+  if (ImGui::Button("Calibrate Camera")) {
+    selected_camera_intrinsics.clear();
+    ImGui::OpenPopup("Camera Calibration");
+  }
+
+  if (camera_intrinsics_selector) {
+    auto selectedFiles = camera_intrinsics_selector->result();
+    if (!selectedFiles.empty()) {
+      selected_camera_intrinsics = selectedFiles[0];
+    }
+    camera_intrinsics_selector.reset();
+  }
+
+  if (!selected_camera_intrinsics.empty()) {
+    drawCheck();
+  }
+
+  // field json selector button
+  if (ImGui::Button("Select Field Map JSON")) {
+    field_map_selector = std::make_unique<pfd::open_file>(
+        "Select Json File", "",
+        std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none);
+  }
+
+  if (field_map_selector) {
+    auto selectedFiles = field_map_selector->result();
+    if (!selectedFiles.empty()) {
+      selected_field_map = selectedFiles[0];
+    }
+    field_map_selector.reset();
+  }
+
+  if (!selected_field_map.empty()) {
+    drawCheck();
+  }
+
+  // field calibration directory selector button
+  if (ImGui::Button("Select Field Calibration Folder")) {
+    field_calibration_directory_selector = std::make_unique<pfd::select_folder>(
+        "Select Field Calibration Folder", "");
+  }
+
+  if (field_calibration_directory_selector) {
+    auto selectedFiles = field_calibration_directory_selector->result();
+    if (!selectedFiles.empty()) {
+      selected_field_calibration_directory = selectedFiles;
+    }
+    field_calibration_directory_selector.reset();
+  }
+
+  if (!selected_field_calibration_directory.empty()) {
+    drawCheck();
+  }
+
+  // pinned tag text field
+  ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+  ImGui::InputInt("Pinned Tag", &pinnedTag);
+
+  if (pinnedTag < 1) {
+    pinnedTag = 1;
+  } else if (pinnedTag > 16) {
+    pinnedTag = 16;
+  }
+
+  // calibrate button
+  if (ImGui::Button("Calibrate!!!")) {
+    if (!selected_field_calibration_directory.empty() &&
+        !selected_camera_intrinsics.empty() && !selected_field_map.empty() &&
+        pinnedTag > 0 && pinnedTag <= 16) {
+      download_directory_selector =
+          std::make_unique<pfd::select_folder>("Select Download Folder", "");
+      if (download_directory_selector) {
+        auto selectedFiles = download_directory_selector->result();
+        if (!selectedFiles.empty()) {
+          selected_download_directory = selectedFiles;
+        }
+        download_directory_selector.reset();
+      }
+
+      calibration_json_path = selected_download_directory + "/output.json";
+
+      int calibrationOutput = fieldcalibration::calibrate(
+          selected_field_calibration_directory.c_str(), calibration_json_path,
+          selected_camera_intrinsics, selected_field_map.c_str(), pinnedTag,
+          showDebug);
+
+      if (calibrationOutput == 1) {
+        ImGui::OpenPopup("Fmap Conversion failed");
+      } else if (calibrationOutput == 0) {
+        std::ifstream caljsonpath(calibration_json_path);
+        try {
+          wpi::json fmap = fmap::convertfmap(wpi::json::parse(caljsonpath));
+          std::ofstream out(selected_download_directory + "/output.fmap");
+          out << fmap.dump(4);
+          out.close();
+          ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
+          ImGui::OpenPopup("Visualize Calibration");
+        } catch (...) {
+          ImGui::OpenPopup("Field Calibration Error");
+        }
+      }
+    }
+  }
+  if (ImGui::Button("Visualize")) {
+    ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
+    ImGui::OpenPopup("Visualize Calibration");
+  }
+  if (selected_field_calibration_directory.empty() ||
+      selected_camera_intrinsics.empty() || selected_field_map.empty()) {
+    ImGui::TextWrapped(
+        "Some inputs are empty! please enter your camera calibration video, "
+        "field map, and field calibration directory");
+  } else if (!(pinnedTag > 0 && pinnedTag <= 16)) {
+    ImGui::TextWrapped("Make sure the pinned tag is a valid april tag (1-16)");
+  } else {
+    ImGui::TextWrapped("Calibration Ready");
+  }
+
+  // error popup window
+  if (ImGui::BeginPopupModal("Field Calibration Error", NULL,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    ImGui::TextWrapped(
+        "Field Calibration Failed - please try again, ensuring that:");
+    ImGui::TextWrapped(
+        "- You have selected the correct camera intrinsics JSON or camera "
+        "calibration video");
+    ImGui::TextWrapped("- You have selected the correct ideal field map JSON");
+    ImGui::TextWrapped(
+        "- You have selected the correct field calibration video directory");
+    ImGui::TextWrapped(
+        "- Your field calibration video directory contains only field "
+        "calibration videos and no other files");
+    ImGui::TextWrapped("- Your pinned tag is a valid FRC Apriltag");
+    ImGui::Separator();
+    if (ImGui::Button("OK", ImVec2(120, 0))) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+
+  if (ImGui::BeginPopupModal("Fmap Conversion failed", NULL,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    ImGui::TextWrapped(
+        "Fmap conversion failed - you can still use the calibration output on "
+        "Limelight platforms by converting the .json output to .fmap using the "
+        "Limelight map builder tool");
+    ImGui::TextWrapped("https://tools.limelightvision.io/map-builder");
+    ImGui::Separator();
+    if (ImGui::Button("OK", ImVec2(120, 0))) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+
+  // successful field calibration popup window
+  if (ImGui::BeginPopupModal("Success", NULL,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    ImGui::TextWrapped("Success, output JSON generated in selected directory");
+    ImGui::Separator();
+    if (ImGui::Button("OK", ImVec2(120, 0))) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+
+  // camera calibration popup window
+  if (ImGui::BeginPopupModal("Camera Calibration", NULL,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    // Camera Calibration Error calibration popup window
+    if (ImGui::BeginPopupModal("Camera Calibration Error", NULL,
+                               ImGuiWindowFlags_AlwaysAutoResize)) {
+      ImGui::TextWrapped(
+          "Camera calibration failed. Please make sure you have uploaded the "
+          "correct video file");
+      ImGui::Separator();
+      if (ImGui::Button("OK", ImVec2(120, 0))) {
+        ImGui::CloseCurrentPopup();
+      }
+      ImGui::EndPopup();
+    }
+
+    if (ImGui::Button("MRcal")) {
+      mrcal = true;
+    }
+
+    ImGui::SameLine();
+    ImGui::Text("or");
+    ImGui::SameLine();
+
+    if (ImGui::Button("OpenCV")) {
+      mrcal = false;
+    }
+
+    if (mrcal) {
+      if (ImGui::Button("Select Camera Calibration Video")) {
+        camera_intrinsics_selector = std::make_unique<pfd::open_file>(
+            "Select Camera Calibration Video", "",
+            std::vector<std::string>{"Video Files",
+                                     "*.mp4 *.mov *.m4v *.mkv *.avi"},
+            pfd::opt::none);
+      }
+
+      if (camera_intrinsics_selector) {
+        auto selectedFiles = camera_intrinsics_selector->result();
+        if (!selectedFiles.empty()) {
+          selected_camera_intrinsics = selectedFiles[0];
+        }
+        camera_intrinsics_selector.reset();
+      }
+
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputDouble("Square Width (in)", &squareWidth);
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputDouble("Marker Width (in)", &markerWidth);
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputInt("Board Width (squares)", &boardWidth);
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputInt("Board Height (squares)", &boardHeight);
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputDouble("Image Width (pixels)", &imagerWidth);
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputDouble("Image Height (pixels)", &imagerHeight);
+
+      ImGui::Separator();
+      if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) {
+        std::cout << "calibration button pressed" << std::endl;
+        int ret = cameracalibration::calibrate(
+            selected_camera_intrinsics.c_str(), cameraModel, markerWidth,
+            boardWidth, boardHeight, imagerWidth, imagerHeight, showDebug);
+        if (ret == 0) {
+          size_t lastSeparatorPos =
+              selected_camera_intrinsics.find_last_of("/\\");
+          std::string output_file_path;
+
+          if (lastSeparatorPos != std::string::npos) {
+            output_file_path =
+                selected_camera_intrinsics.substr(0, lastSeparatorPos)
+                    .append("/cameracalibration.json");
+          }
+
+          selected_camera_intrinsics = output_file_path;
+
+          cameracalibration::dumpJson(cameraModel, output_file_path);
+          ImGui::CloseCurrentPopup();
+        } else if (ret == 1) {
+          std::cout << "calibration failed and popup ready" << std::endl;
+          ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
+          ImGui::OpenPopup("Camera Calibration Error");
+        } else {
+          ImGui::CloseCurrentPopup();
+        }
+      }
+    } else {
+      if (ImGui::Button("Select Camera Calibration Video")) {
+        camera_intrinsics_selector = std::make_unique<pfd::open_file>(
+            "Select Camera Calibration Video", "",
+            std::vector<std::string>{"Video Files",
+                                     "*.mp4 *.mov *.m4v *.mkv *.avi"},
+            pfd::opt::none);
+      }
+
+      if (camera_intrinsics_selector) {
+        auto selectedFiles = camera_intrinsics_selector->result();
+        if (!selectedFiles.empty()) {
+          selected_camera_intrinsics = selectedFiles[0];
+        }
+        camera_intrinsics_selector.reset();
+      }
+
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputDouble("Square Width (in)", &squareWidth);
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputDouble("Marker Width (in)", &markerWidth);
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputInt("Board Width (squares)", &boardWidth);
+      ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+      ImGui::InputInt("Board Height (squares)", &boardHeight);
+
+      ImGui::Separator();
+      if (ImGui::Button("Calibrate") && !selected_camera_intrinsics.empty()) {
+        std::cout << "calibration button pressed" << std::endl;
+        int ret = cameracalibration::calibrate(
+            selected_camera_intrinsics.c_str(), cameraModel, squareWidth,
+            markerWidth, boardWidth, boardHeight, showDebug);
+        if (ret == 0) {
+          size_t lastSeparatorPos =
+              selected_camera_intrinsics.find_last_of("/\\");
+          std::string output_file_path;
+
+          if (lastSeparatorPos != std::string::npos) {
+            output_file_path =
+                selected_camera_intrinsics.substr(0, lastSeparatorPos)
+                    .append("/cameracalibration.json");
+          }
+
+          selected_camera_intrinsics = output_file_path;
+
+          cameracalibration::dumpJson(cameraModel, output_file_path);
+          ImGui::CloseCurrentPopup();
+        } else if (ret == 1) {
+          std::cout << "calibration failed and popup ready" << std::endl;
+          ImGui::SetNextWindowSize(ImVec2(600, 400), ImGuiCond_Always);
+          ImGui::OpenPopup("Camera Calibration Error");
+        } else {
+          ImGui::CloseCurrentPopup();
+        }
+      }
+    }
+
+    ImGui::SameLine();
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+    }
+
+    ImGui::EndPopup();
+  }
+
+  // visualize calibration popup
+  if (ImGui::BeginPopupModal("Visualize Calibration", NULL,
+                             ImGuiWindowFlags_AlwaysAutoResize)) {
+    if (ImGui::Button("Load Calibrated Field")) {
+      calibration_json_path =
+          std::make_unique<pfd::open_file>(
+              "Select Json File", "",
+              std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
+              ->result()[0];
+    }
+
+    if (!calibration_json_path.empty()) {
+      ImGui::SameLine();
+      drawCheck();
+    }
+
+    if (ImGui::Button("Load Reference Field")) {
+      selected_field_map =
+          std::make_unique<pfd::open_file>(
+              "Select Json File", "",
+              std::vector<std::string>{"JSON Files", "*.json"}, pfd::opt::none)
+              ->result()[0];
+    }
+
+    if (!selected_field_map.empty()) {
+      ImGui::SameLine();
+      drawCheck();
+    }
+
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+    ImGui::InputInt("Focused Tag", &focusedTag);
+    ImGui::SetNextItemWidth(ImGui::GetFontSize() * 12);
+    ImGui::InputInt("Reference Tag", &referenceTag);
+
+    if (focusedTag < 1) {
+      focusedTag = 1;
+    } else if (focusedTag > 16) {
+      focusedTag = 16;
+    }
+
+    if (referenceTag < 1) {
+      referenceTag = 1;
+    } else if (referenceTag > 16) {
+      referenceTag = 16;
+    }
+
+    if (!calibration_json_path.empty() && !selected_field_map.empty()) {
+      std::ifstream calJson(calibration_json_path);
+      std::ifstream refJson(selected_field_map);
+
+      currentCalibrationMap = Fieldmap(wpi::json::parse(calJson));
+      currentReferenceMap = Fieldmap(wpi::json::parse(refJson));
+
+      double xDiff = currentReferenceMap.getTag(focusedTag).xPos -
+                     currentCalibrationMap.getTag(focusedTag).xPos;
+      double yDiff = currentReferenceMap.getTag(focusedTag).yPos -
+                     currentCalibrationMap.getTag(focusedTag).yPos;
+      double zDiff = currentReferenceMap.getTag(focusedTag).zPos -
+                     currentCalibrationMap.getTag(focusedTag).zPos;
+      double yawDiff = currentReferenceMap.getTag(focusedTag).yawRot -
+                       currentCalibrationMap.getTag(focusedTag).yawRot;
+      double pitchDiff = currentReferenceMap.getTag(focusedTag).pitchRot -
+                         currentCalibrationMap.getTag(focusedTag).pitchRot;
+      double rollDiff = currentReferenceMap.getTag(focusedTag).rollRot -
+                        currentCalibrationMap.getTag(focusedTag).rollRot;
+
+      double xRef = currentCalibrationMap.getTag(referenceTag).xPos -
+                    currentCalibrationMap.getTag(focusedTag).xPos;
+      double yRef = currentCalibrationMap.getTag(referenceTag).yPos -
+                    currentCalibrationMap.getTag(focusedTag).yPos;
+      double zRef = currentCalibrationMap.getTag(referenceTag).zPos -
+                    currentCalibrationMap.getTag(focusedTag).zPos;
+
+      ImGui::TextWrapped("X Difference: %s (m)", std::to_string(xDiff).c_str());
+      ImGui::TextWrapped("Y Difference: %s (m)", std::to_string(yDiff).c_str());
+      ImGui::TextWrapped("Z Difference: %s (m)", std::to_string(zDiff).c_str());
+
+      ImGui::TextWrapped(
+          "Yaw Difference %s°",
+          std::to_string(
+              Fieldmap::minimizeAngle(yawDiff * (180.0 / std::numbers::pi)))
+              .c_str());
+      ImGui::TextWrapped(
+          "Pitch Difference %s°",
+          std::to_string(
+              Fieldmap::minimizeAngle(pitchDiff * (180.0 / std::numbers::pi)))
+              .c_str());
+      ImGui::TextWrapped(
+          "Roll Difference %s°",
+          std::to_string(
+              Fieldmap::minimizeAngle(rollDiff * (180.0 / std::numbers::pi)))
+              .c_str());
+
+      ImGui::NewLine();
+
+      ImGui::TextWrapped("X Reference: %s (m)", std::to_string(xRef).c_str());
+      ImGui::TextWrapped("Y Reference: %s (m)", std::to_string(yRef).c_str());
+      ImGui::TextWrapped("Z Reference: %s (m)", std::to_string(zRef).c_str());
+    }
+
+    if (ImGui::Button("Close")) {
+      ImGui::CloseCurrentPopup();
+    }
+    ImGui::EndPopup();
+  }
+
+  ImGui::End();
+}
+
+#ifndef RUNNING_WPICAL_TESTS
+#ifdef _WIN32
+int __stdcall WinMain(void* hInstance, void* hPrevInstance, char* pCmdLine,
+                      int nCmdShow) {
+  int argc = __argc;
+  char** argv = __argv;
+#else
+int main(int argc, char** argv) {
+#endif
+  std::string_view saveDir;
+  if (argc == 2) {
+    saveDir = argv[1];
+  }
+
+  gui::CreateContext();
+
+  gui::AddIcon(wpical::GetResource_wpical_16_png());
+  gui::AddIcon(wpical::GetResource_wpical_32_png());
+  gui::AddIcon(wpical::GetResource_wpical_48_png());
+  gui::AddIcon(wpical::GetResource_wpical_64_png());
+  gui::AddIcon(wpical::GetResource_wpical_128_png());
+  gui::AddIcon(wpical::GetResource_wpical_256_png());
+  gui::AddIcon(wpical::GetResource_wpical_512_png());
+
+  gui::AddLateExecute(DisplayGui);
+
+  gui::Initialize("wpical", 600, 400);
+  gui::Main();
+
+  gui::DestroyContext();
+
+  return 0;
+}
+#endif

wpical/src/main/native/cpp/cameracalibration.cpp

@@ -0,0 +1,404 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "cameracalibration.h"
+
+#include <fstream>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+
+#include <mrcal_wrapper.h>
+#include <opencv2/objdetect/aruco_board.hpp>
+#include <opencv2/opencv.hpp>
+#include <opencv2/videoio.hpp>
+
+static bool filter(std::vector<cv::Point2f> charuco_corners,
+                   std::vector<int> charuco_ids,
+                   std::vector<std::vector<cv::Point2f>> marker_corners,
+                   std::vector<int> marker_ids, int board_width,
+                   int board_height) {
+  if (charuco_ids.empty() || charuco_corners.empty()) {
+    return false;
+  }
+
+  if (charuco_corners.size() < 10 || charuco_ids.size() < 10) {
+    return false;
+  }
+
+  for (int i = 0; i < charuco_ids.size(); i++) {
+    if (charuco_ids.at(i) > (board_width - 1) * (board_height - 1) - 1) {
+      return false;
+    }
+  }
+
+  for (int i = 0; i < marker_ids.size(); i++) {
+    if (marker_ids.at(i) > ((board_width * board_height) / 2) - 1) {
+      return false;
+    }
+  }
+
+  return true;
+}
+
+int cameracalibration::calibrate(const std::string& input_video,
+                                 CameraModel& camera_model, float square_width,
+                                 float marker_width, int board_width,
+                                 int board_height, bool show_debug_window) {
+  // ChArUco Board
+  cv::aruco::Dictionary aruco_dict =
+      cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
+  cv::Ptr<cv::aruco::CharucoBoard> charuco_board = new cv::aruco::CharucoBoard(
+      cv::Size(board_width, board_height), square_width * 0.0254,
+      marker_width * 0.0254, aruco_dict);
+  cv::aruco::CharucoDetector charuco_detector(*charuco_board);
+
+  // Video capture
+  cv::VideoCapture video_capture(input_video);
+  cv::Size frame_shape;
+
+  std::vector<std::vector<cv::Point2f>> all_charuco_corners;
+  std::vector<std::vector<int>> all_charuco_ids;
+
+  std::vector<std::vector<cv::Point3f>> all_obj_points;
+  std::vector<std::vector<cv::Point2f>> all_img_points;
+
+  while (video_capture.grab()) {
+    cv::Mat frame;
+    video_capture.retrieve(frame);
+
+    cv::Mat debug_image = frame;
+
+    if (frame.empty()) {
+      break;
+    }
+
+    // Detect
+    cv::Mat frame_gray;
+    cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
+
+    frame_shape = frame_gray.size();
+
+    std::vector<cv::Point2f> charuco_corners;
+    std::vector<int> charuco_ids;
+    std::vector<std::vector<cv::Point2f>> marker_corners;
+    std::vector<int> marker_ids;
+
+    std::vector<cv::Point3f> obj_points;
+    std::vector<cv::Point2f> img_points;
+
+    charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids,
+                                 marker_corners, marker_ids);
+
+    if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids,
+                board_width, board_height)) {
+      continue;
+    }
+
+    charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points,
+                                    img_points);
+
+    all_charuco_corners.push_back(charuco_corners);
+    all_charuco_ids.push_back(charuco_ids);
+
+    all_obj_points.push_back(obj_points);
+    all_img_points.push_back(img_points);
+
+    if (show_debug_window) {
+      cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids);
+      cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners,
+                                            charuco_ids);
+      cv::imshow("Frame", debug_image);
+      if (cv::waitKey(1) == 'q') {
+        break;
+      }
+    }
+  }
+
+  video_capture.release();
+  if (show_debug_window) {
+    cv::destroyAllWindows();
+  }
+
+  // Calibrate
+  cv::Mat camera_matrix, dist_coeffs;
+  std::vector<cv::Mat> r_vecs, t_vecs;
+  std::vector<double> std_dev_intrinsics, std_dev_extrinsics, per_view_errors;
+  double repError;
+
+  try {
+    // see https://stackoverflow.com/a/75865177
+    int flags = cv::CALIB_RATIONAL_MODEL | cv::CALIB_USE_LU;
+    repError = cv::calibrateCamera(
+        all_obj_points, all_img_points, frame_shape, camera_matrix, dist_coeffs,
+        r_vecs, t_vecs, cv::noArray(), cv::noArray(), cv::noArray(), flags);
+  } catch (...) {
+    std::cout << "calibration failed" << std::endl;
+    return 1;
+  }
+
+  std::vector<double> matrix = {camera_matrix.begin<double>(),
+                                camera_matrix.end<double>()};
+
+  std::vector<double> distortion = {dist_coeffs.begin<double>(),
+                                    dist_coeffs.end<double>()};
+
+  camera_model.intrinsic_matrix = Eigen::Matrix<double, 3, 3>(matrix.data());
+  camera_model.distortion_coefficients =
+      Eigen::Matrix<double, 8, 1>(distortion.data());
+  camera_model.avg_reprojection_error = repError;
+  return 0;
+}
+
+int cameracalibration::calibrate(const std::string& input_video,
+                                 CameraModel& camera_model, float square_width,
+                                 float marker_width, int board_width,
+                                 int board_height, double imagerWidthPixels,
+                                 double imagerHeightPixels,
+                                 bool show_debug_window) {
+  // ChArUco Board
+  cv::aruco::Dictionary aruco_dict =
+      cv::aruco::getPredefinedDictionary(cv::aruco::DICT_5X5_1000);
+  cv::Ptr<cv::aruco::CharucoBoard> charuco_board = new cv::aruco::CharucoBoard(
+      cv::Size(board_width, board_height), square_width * 0.0254,
+      marker_width * 0.0254, aruco_dict);
+  cv::aruco::CharucoDetector charuco_detector(*charuco_board);
+
+  // Video capture
+  cv::VideoCapture video_capture(input_video);
+  cv::Size frame_shape;
+
+  // Detection output
+  std::vector<mrcal_point3_t> observation_boards;
+  std::vector<mrcal_pose_t> frames_rt_toref;
+
+  cv::Size boardSize(board_width - 1, board_height - 1);
+  cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels);
+
+  while (video_capture.grab()) {
+    cv::Mat frame;
+    video_capture.retrieve(frame);
+
+    cv::Mat debug_image = frame;
+
+    if (frame.empty()) {
+      break;
+    }
+
+    // Detect
+    cv::Mat frame_gray;
+    cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
+
+    frame_shape = frame_gray.size();
+
+    std::vector<cv::Point2f> charuco_corners;
+    std::vector<int> charuco_ids;
+    std::vector<std::vector<cv::Point2f>> marker_corners;
+    std::vector<int> marker_ids;
+
+    std::vector<cv::Point3f> obj_points;
+    std::vector<cv::Point2f> img_points;
+
+    std::vector<mrcal_point3_t> points((board_width - 1) * (board_height - 1));
+
+    charuco_detector.detectBoard(frame_gray, charuco_corners, charuco_ids,
+                                 marker_corners, marker_ids);
+
+    if (!filter(charuco_corners, charuco_ids, marker_corners, marker_ids,
+                board_width, board_height)) {
+      continue;
+    }
+
+    charuco_board->matchImagePoints(charuco_corners, charuco_ids, obj_points,
+                                    img_points);
+
+    for (int i = 0; i < charuco_ids.size(); i++) {
+      int id = charuco_ids.at(i);
+      points[id].x = img_points.at(i).x;
+      points[id].y = img_points.at(i).y;
+      points[id].z = 1.0f;
+    }
+
+    for (int i = 0; i < points.size(); i++) {
+      if (points[i].z != 1.0f) {
+        points[i].x = -1.0f;
+        points[i].y = -1.0f;
+        points[i].z = -1.0f;
+      }
+    }
+
+    frames_rt_toref.push_back(
+        getSeedPose(points.data(), boardSize, imagerSize, square_width, 1000));
+    observation_boards.insert(observation_boards.end(), points.begin(),
+                              points.end());
+
+    if (show_debug_window) {
+      cv::aruco::drawDetectedMarkers(debug_image, marker_corners, marker_ids);
+      cv::aruco::drawDetectedCornersCharuco(debug_image, charuco_corners,
+                                            charuco_ids);
+      cv::imshow("Frame", debug_image);
+      if (cv::waitKey(1) == 'q') {
+        break;
+      }
+    }
+  }
+
+  video_capture.release();
+  if (show_debug_window) {
+    cv::destroyAllWindows();
+  }
+
+  if (observation_boards.empty()) {
+    std::cout << "calibration failed" << std::endl;
+    return 1;
+  } else {
+    std::cout << "points detected" << std::endl;
+  }
+
+  std::unique_ptr<mrcal_result> cal_result =
+      mrcal_main(observation_boards, frames_rt_toref, boardSize,
+                 square_width * 0.0254, imagerSize, 1000);
+
+  auto& stats = *cal_result;
+
+  // Camera matrix and distortion coefficients
+  std::vector<double> camera_matrix = {
+      // fx 0 cx
+      stats.intrinsics[0], 0, stats.intrinsics[2],
+      // 0 fy cy
+      0, stats.intrinsics[1], stats.intrinsics[3],
+      // 0 0 1
+      0, 0, 1};
+
+  std::vector<double> distortion_coefficients = {stats.intrinsics[4],
+                                                 stats.intrinsics[5],
+                                                 stats.intrinsics[6],
+                                                 stats.intrinsics[7],
+                                                 stats.intrinsics[8],
+                                                 stats.intrinsics[9],
+                                                 stats.intrinsics[10],
+                                                 stats.intrinsics[11],
+                                                 0.0,
+                                                 0.0,
+                                                 0.0,
+                                                 0.0,
+                                                 0.0,
+                                                 0.0};
+
+  camera_model.intrinsic_matrix =
+      Eigen::Matrix<double, 3, 3>(camera_matrix.data());
+  camera_model.distortion_coefficients =
+      Eigen::Matrix<double, 8, 1>(distortion_coefficients.data());
+  camera_model.avg_reprojection_error = stats.rms_error;
+
+  return 0;
+}
+
+int cameracalibration::calibrate(const std::string& input_video,
+                                 CameraModel& camera_model, float square_width,
+                                 int board_width, int board_height,
+                                 double imagerWidthPixels,
+                                 double imagerHeightPixels,
+                                 bool show_debug_window) {
+  // Video capture
+  cv::VideoCapture video_capture(input_video);
+
+  // Detection output
+  std::vector<mrcal_point3_t> observation_boards;
+  std::vector<mrcal_pose_t> frames_rt_toref;
+
+  cv::Size boardSize(board_width - 1, board_height - 1);
+  cv::Size imagerSize(imagerWidthPixels, imagerHeightPixels);
+
+  while (video_capture.grab()) {
+    cv::Mat frame;
+    video_capture.retrieve(frame);
+
+    if (frame.empty()) {
+      break;
+    }
+
+    // Detect
+    cv::Mat frame_gray;
+    cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
+
+    std::vector<cv::Point2f> corners;
+
+    bool found = cv::findChessboardCorners(
+        frame_gray, boardSize, corners,
+        cv::CALIB_CB_ADAPTIVE_THRESH + cv::CALIB_CB_NORMALIZE_IMAGE);
+
+    if (found) {
+      std::vector<mrcal_point3_t> current_points;
+      for (int i = 0; i < corners.size(); i++) {
+        current_points.push_back(
+            mrcal_point3_t{{corners.at(i).x, corners.at(i).y, 1.0f}});
+      }
+      frames_rt_toref.push_back(getSeedPose(current_points.data(), boardSize,
+                                            imagerSize, square_width * 0.0254,
+                                            1000));
+      observation_boards.insert(observation_boards.end(),
+                                current_points.begin(), current_points.end());
+    }
+    if (show_debug_window) {
+      cv::drawChessboardCorners(frame, boardSize, corners, found);
+      cv::imshow("Checkerboard Detection", frame);
+      if (cv::waitKey(30) == 'q') {
+        break;
+      }
+    }
+  }
+
+  video_capture.release();
+  if (show_debug_window) {
+    cv::destroyAllWindows();
+  }
+
+  if (observation_boards.empty()) {
+    std::cout << "calibration failed" << std::endl;
+    return 1;
+  } else {
+    std::cout << "points detected" << std::endl;
+  }
+
+  std::unique_ptr<mrcal_result> cal_result =
+      mrcal_main(observation_boards, frames_rt_toref, boardSize,
+                 square_width * 0.0254, imagerSize, 1000);
+
+  auto& stats = *cal_result;
+
+  // Camera matrix and distortion coefficients
+  std::vector<double> camera_matrix = {
+      // fx 0 cx
+      stats.intrinsics[0], 0, stats.intrinsics[2],
+      // 0 fy cy
+      0, stats.intrinsics[1], stats.intrinsics[3],
+      // 0 0 1
+      0, 0, 1};
+
+  std::vector<double> distortion_coefficients = {stats.intrinsics[4],
+                                                 stats.intrinsics[5],
+                                                 stats.intrinsics[6],
+                                                 stats.intrinsics[7],
+                                                 stats.intrinsics[8],
+                                                 stats.intrinsics[9],
+                                                 stats.intrinsics[10],
+                                                 stats.intrinsics[11],
+                                                 0.0,
+                                                 0.0,
+                                                 0.0,
+                                                 0.0,
+                                                 0.0,
+                                                 0.0};
+
+  // Save calibration output
+  camera_model.intrinsic_matrix =
+      Eigen::Matrix<double, 3, 3>(camera_matrix.data());
+  camera_model.distortion_coefficients =
+      Eigen::Matrix<double, 8, 1>(distortion_coefficients.data());
+  camera_model.avg_reprojection_error = stats.rms_error;
+
+  return 0;
+}

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

@@ -0,0 +1,599 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "fieldcalibration.h"
+
+#include <algorithm>
+#include <filesystem>
+#include <fstream>
+#include <functional>
+#include <iostream>
+#include <map>
+#include <string>
+#include <utility>
+#include <vector>
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+#include <ceres/ceres.h>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/core/eigen.hpp>
+#include <opencv2/core/utils/logger.hpp>
+#include <opencv2/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <opencv2/videoio.hpp>
+#include <wpi/json.h>
+
+#include "apriltag.h"
+#include "tag36h11.h"
+
+struct Pose {
+  Eigen::Vector3d p;
+  Eigen::Quaterniond q;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+struct Constraint {
+  int id_begin;
+  int id_end;
+  Pose t_begin_end;
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+class PoseGraphError {
+ public:
+  explicit PoseGraphError(Pose t_ab_observed)
+      : m_t_ab_observed(std::move(t_ab_observed)) {}
+
+  template <typename T>
+  bool operator()(const T* const p_a_ptr, const T* const q_a_ptr,
+                  const T* const p_b_ptr, const T* const q_b_ptr,
+                  T* residuals_ptr) const {
+    // Tag A
+    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_a(p_a_ptr);
+    Eigen::Map<const Eigen::Quaternion<T>> q_a(q_a_ptr);
+
+    // Tag B
+    Eigen::Map<const Eigen::Matrix<T, 3, 1>> p_b(p_b_ptr);
+    Eigen::Map<const Eigen::Quaternion<T>> q_b(q_b_ptr);
+
+    // Rotation between tag A to tag B
+    Eigen::Quaternion<T> q_a_inverse = q_a.conjugate();
+    Eigen::Quaternion<T> q_ab_estimated = q_a_inverse * q_b;
+
+    // Displacement between tag A and tag B in tag A's frame
+    Eigen::Matrix<T, 3, 1> p_ab_estimated = q_a_inverse * (p_b - p_a);
+
+    // Error between the orientations
+    Eigen::Quaternion<T> delta_q =
+        m_t_ab_observed.q.template cast<T>() * q_ab_estimated.conjugate();
+
+    // Residuals
+    Eigen::Map<Eigen::Matrix<T, 6, 1>> residuals(residuals_ptr);
+    residuals.template block<3, 1>(0, 0) =
+        p_ab_estimated - m_t_ab_observed.p.template cast<T>();
+    residuals.template block<3, 1>(3, 0) = T(2.0) * delta_q.vec();
+
+    return true;
+  }
+
+  static ceres::CostFunction* Create(const Pose& t_ab_observed) {
+    return new ceres::AutoDiffCostFunction<PoseGraphError, 6, 3, 4, 3, 4>(
+        new PoseGraphError(t_ab_observed));
+  }
+
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ private:
+  const Pose m_t_ab_observed;
+};
+
+const double tagSizeMeters = 0.1651;
+
+inline cameracalibration::CameraModel load_camera_model(std::string path) {
+  Eigen::Matrix<double, 3, 3> camera_matrix;
+  Eigen::Matrix<double, 8, 1> camera_distortion;
+
+  std::ifstream file(path);
+
+  wpi::json json_data;
+
+  try {
+    json_data = wpi::json::parse(file);
+  } catch (const wpi::json::parse_error& e) {
+    std::cout << e.what() << std::endl;
+  }
+
+  bool isCalibdb = json_data.contains("camera");
+
+  if (!isCalibdb) {
+    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];
+      }
+    }
+
+    for (int i = 0; i < camera_distortion.rows(); i++) {
+      for (int j = 0; j < camera_distortion.cols(); j++) {
+        camera_distortion(i, j) = json_data["distortion_coefficients"]
+                                           [(i * camera_distortion.cols()) + j];
+      }
+    }
+  } else {
+    for (int i = 0; i < camera_matrix.rows(); i++) {
+      for (int j = 0; j < camera_matrix.cols(); j++) {
+        try {
+          camera_matrix(i, j) = json_data["camera_matrix"][i][j];
+        } catch (...) {
+          camera_matrix(i, j) = json_data["camera_matrix"]["data"]
+                                         [(i * camera_matrix.cols()) + j];
+        }
+      }
+    }
+
+    for (int i = 0; i < camera_distortion.rows(); i++) {
+      for (int j = 0; j < camera_distortion.cols(); j++) {
+        try {
+          camera_distortion(i, j) =
+              json_data["distortion_coefficients"]
+                       [(i * camera_distortion.cols()) + j];
+        } catch (...) {
+          camera_distortion(i, j) = 0.0;
+        }
+      }
+    }
+  }
+
+  cameracalibration::CameraModel camera_model{
+      camera_matrix, camera_distortion, json_data["avg_reprojection_error"]};
+  return camera_model;
+}
+
+inline cameracalibration::CameraModel load_camera_model(wpi::json json_data) {
+  // Camera matrix
+  Eigen::Matrix<double, 3, 3> camera_matrix;
+
+  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];
+    }
+  }
+
+  // Distortion coefficients
+  Eigen::Matrix<double, 8, 1> camera_distortion;
+
+  for (int i = 0; i < camera_distortion.rows(); i++) {
+    for (int j = 0; j < camera_distortion.cols(); j++) {
+      camera_distortion(i, j) = json_data["distortion_coefficients"]
+                                         [(i * camera_distortion.cols()) + j];
+    }
+  }
+
+  cameracalibration::CameraModel camera_model{
+      camera_matrix, camera_distortion, json_data["avg_reprojection_error"]};
+  return camera_model;
+}
+
+inline std::map<int, wpi::json> load_ideal_map(std::string path) {
+  std::ifstream file(path);
+  wpi::json json_data = wpi::json::parse(file);
+  std::map<int, wpi::json> ideal_map;
+
+  for (const auto& element : json_data["tags"]) {
+    ideal_map[element["ID"]] = element;
+  }
+
+  return ideal_map;
+}
+
+Eigen::Matrix<double, 4, 4> get_tag_transform(
+    std::map<int, wpi::json>& ideal_map, int tag_id) {
+  Eigen::Matrix<double, 4, 4> transform =
+      Eigen::Matrix<double, 4, 4>::Identity();
+
+  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"];
+
+  return transform;
+}
+
+inline Eigen::Matrix<double, 4, 4> estimate_tag_pose(
+    apriltag_detection_t* tag_detection,
+    const Eigen::Matrix<double, 3, 3>& camera_matrix,
+    const Eigen::Matrix<double, 8, 1>& camera_distortion, double tag_size) {
+  cv::Mat camera_matrix_cv;
+  cv::Mat camera_distortion_cv;
+
+  cv::eigen2cv(camera_matrix, camera_matrix_cv);
+  cv::eigen2cv(camera_distortion, camera_distortion_cv);
+
+  std::vector<cv::Point2f> points_2d = {
+      cv::Point2f(tag_detection->p[0][0], tag_detection->p[0][1]),
+      cv::Point2f(tag_detection->p[1][0], tag_detection->p[1][1]),
+      cv::Point2f(tag_detection->p[2][0], tag_detection->p[2][1]),
+      cv::Point2f(tag_detection->p[3][0], tag_detection->p[3][1])};
+
+  std::vector<cv::Point3f> points_3d_box_base = {
+      cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0),
+      cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0),
+      cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0),
+      cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)};
+
+  std::vector<double> r_vec;
+  std::vector<double> t_vec;
+
+  cv::solvePnP(points_3d_box_base, points_2d, camera_matrix_cv,
+               camera_distortion_cv, r_vec, t_vec, false, cv::SOLVEPNP_SQPNP);
+
+  cv::Mat r_mat;
+  cv::Rodrigues(r_vec, r_mat);
+
+  Eigen::Matrix<double, 4, 4> camera_to_tag{
+      {r_mat.at<double>(0, 0), r_mat.at<double>(0, 1), r_mat.at<double>(0, 2),
+       t_vec.at(0)},
+      {r_mat.at<double>(1, 0), r_mat.at<double>(1, 1), r_mat.at<double>(1, 2),
+       t_vec.at(1)},
+      {r_mat.at<double>(2, 0), r_mat.at<double>(2, 1), r_mat.at<double>(2, 2),
+       t_vec.at(2)},
+      {0.0, 0.0, 0.0, 1.0}};
+
+  return camera_to_tag;
+}
+
+inline void draw_tag_cube(cv::Mat& frame,
+                          Eigen::Matrix<double, 4, 4> camera_to_tag,
+                          const Eigen::Matrix<double, 3, 3>& camera_matrix,
+                          const Eigen::Matrix<double, 8, 1>& camera_distortion,
+                          double tag_size) {
+  cv::Mat camera_matrix_cv;
+  cv::Mat camera_distortion_cv;
+
+  cv::eigen2cv(camera_matrix, camera_matrix_cv);
+  cv::eigen2cv(camera_distortion, camera_distortion_cv);
+
+  std::vector<cv::Point3f> points_3d_box_base = {
+      cv::Point3f(-tag_size / 2.0, tag_size / 2.0, 0.0),
+      cv::Point3f(tag_size / 2.0, tag_size / 2.0, 0.0),
+      cv::Point3f(tag_size / 2.0, -tag_size / 2.0, 0.0),
+      cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, 0.0)};
+
+  std::vector<cv::Point3f> points_3d_box_top = {
+      cv::Point3f(-tag_size / 2.0, tag_size / 2.0, -tag_size),
+      cv::Point3f(tag_size / 2.0, tag_size / 2.0, -tag_size),
+      cv::Point3f(tag_size / 2.0, -tag_size / 2.0, -tag_size),
+      cv::Point3f(-tag_size / 2.0, -tag_size / 2.0, -tag_size)};
+
+  std::vector<cv::Point2f> points_2d_box_base = {
+      cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0),
+      cv::Point2f(0.0, 0.0)};
+
+  std::vector<cv::Point2f> points_2d_box_top = {
+      cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0), cv::Point2f(0.0, 0.0),
+      cv::Point2f(0.0, 0.0)};
+
+  Eigen::Matrix<double, 3, 3> r_vec = camera_to_tag.block<3, 3>(0, 0);
+  Eigen::Matrix<double, 3, 1> t_vec = camera_to_tag.block<3, 1>(0, 3);
+
+  cv::Mat r_vec_cv;
+  cv::Mat t_vec_cv;
+
+  cv::eigen2cv(r_vec, r_vec_cv);
+  cv::eigen2cv(t_vec, t_vec_cv);
+
+  cv::projectPoints(points_3d_box_base, r_vec_cv, t_vec_cv, camera_matrix_cv,
+                    camera_distortion_cv, points_2d_box_base);
+  cv::projectPoints(points_3d_box_top, r_vec_cv, t_vec_cv, camera_matrix_cv,
+                    camera_distortion_cv, points_2d_box_top);
+
+  for (int i = 0; i < 4; i++) {
+    cv::Point2f& point_base = points_2d_box_base.at(i);
+    cv::Point2f& point_top = points_2d_box_top.at(i);
+
+    cv::line(frame, point_base, point_top, cv::Scalar(0, 255, 255), 5);
+
+    int i_next = (i + 1) % 4;
+    cv::Point2f& point_base_next = points_2d_box_base.at(i_next);
+    cv::Point2f& point_top_next = points_2d_box_top.at(i_next);
+
+    cv::line(frame, point_base, point_base_next, cv::Scalar(0, 255, 255), 5);
+    cv::line(frame, point_top, point_top_next, cv::Scalar(0, 255, 255), 5);
+  }
+}
+
+inline bool process_video_file(
+    apriltag_detector_t* tag_detector,
+    const Eigen::Matrix<double, 3, 3>& camera_matrix,
+    const Eigen::Matrix<double, 8, 1>& camera_distortion, double tag_size,
+    const std::string& path,
+    std::map<int, Pose, std::less<int>,
+             Eigen::aligned_allocator<std::pair<const int, Pose>>>& poses,
+    std::vector<Constraint, Eigen::aligned_allocator<Constraint>>& constraints,
+    bool show_debug_window) {
+  if (show_debug_window) {
+    cv::namedWindow("Processing Frame", cv::WINDOW_NORMAL);
+  }
+  cv::VideoCapture video_input(path);
+
+  if (!video_input.isOpened()) {
+    std::cout << "Unable to open video " << path << std::endl;
+    return false;
+  }
+
+  cv::Mat frame;
+  cv::Mat frame_gray;
+  cv::Mat frame_debug;
+
+  int frame_num = 0;
+
+  while (video_input.read(frame)) {
+    std::cout << "Processing " << path << " - Frame " << frame_num++
+              << std::endl;
+
+    // Convert color frame to grayscale frame
+    cv::cvtColor(frame, frame_gray, cv::COLOR_BGR2GRAY);
+
+    // Clone color frame for debugging
+    frame_debug = frame.clone();
+
+    // Detect tags
+    image_u8_t tag_image = {frame_gray.cols, frame_gray.rows, frame_gray.cols,
+                            frame_gray.data};
+    zarray_t* tag_detections =
+        apriltag_detector_detect(tag_detector, &tag_image);
+
+    // Skip this loop if there are no tags detected
+    if (zarray_size(tag_detections) == 0) {
+      std::cout << "No tags detected" << std::endl;
+      continue;
+    }
+
+    // Find detection with the smallest tag ID
+    apriltag_detection_t* tag_detection_min = nullptr;
+    zarray_get(tag_detections, 0, &tag_detection_min);
+
+    for (int i = 0; i < zarray_size(tag_detections); i++) {
+      apriltag_detection_t* tag_detection_i;
+      zarray_get(tag_detections, i, &tag_detection_i);
+
+      if (tag_detection_i->id < tag_detection_min->id) {
+        tag_detection_min = tag_detection_i;
+      }
+    }
+
+    Eigen::Matrix<double, 4, 4> camera_to_tag_min = estimate_tag_pose(
+        tag_detection_min, camera_matrix, camera_distortion, tag_size);
+
+    // Find transformation from smallest tag ID
+    for (int i = 0; i < zarray_size(tag_detections); i++) {
+      apriltag_detection_t* tag_detection_i;
+      zarray_get(tag_detections, i, &tag_detection_i);
+
+      // Add tag ID to poses
+      if (poses.find(tag_detection_i->id) == poses.end()) {
+        poses[tag_detection_i->id] = {Eigen::Vector3d(0.0, 0.0, 0.0),
+                                      Eigen::Quaterniond(1.0, 0.0, 0.0, 0.0)};
+      }
+
+      // Estimate camera to tag pose
+      Eigen::Matrix<double, 4, 4> caamera_to_tag = estimate_tag_pose(
+          tag_detection_i, camera_matrix, camera_distortion, tag_size);
+
+      // Draw debug cube
+      if (show_debug_window) {
+        draw_tag_cube(frame_debug, caamera_to_tag, camera_matrix,
+                      camera_distortion, tag_size);
+      }
+
+      // Skip finding transformation from smallest tag ID to itself
+      if (tag_detection_i->id == tag_detection_min->id) {
+        continue;
+      }
+
+      Eigen::Matrix<double, 4, 4> tag_min_to_tag =
+          camera_to_tag_min.inverse() * caamera_to_tag;
+
+      // Constraint
+      Constraint constraint;
+      constraint.id_begin = tag_detection_min->id;
+      constraint.id_end = tag_detection_i->id;
+      constraint.t_begin_end.p = tag_min_to_tag.block<3, 1>(0, 3);
+      constraint.t_begin_end.q =
+          Eigen::Quaterniond(tag_min_to_tag.block<3, 3>(0, 0));
+
+      constraints.push_back(constraint);
+    }
+
+    apriltag_detections_destroy(tag_detections);
+
+    // Show debug
+    if (show_debug_window) {
+      cv::imshow("Processing Frame", frame_debug);
+      cv::waitKey(1);
+    }
+  }
+
+  video_input.release();
+  if (show_debug_window) {
+    cv::destroyAllWindows();
+  }
+
+  return true;
+}
+
+int fieldcalibration::calibrate(std::string input_dir_path,
+                                std::string output_file_path,
+                                std::string camera_model_path,
+                                std::string ideal_map_path, int pinned_tag_id,
+                                bool show_debug_window) {
+  // Silence OpenCV logging
+  cv::utils::logging::setLogLevel(
+      cv::utils::logging::LogLevel::LOG_LEVEL_SILENT);
+
+  // Load camera model
+  Eigen::Matrix3d camera_matrix;
+  Eigen::Matrix<double, 8, 1> camera_distortion;
+
+  try {
+    auto camera_model = load_camera_model(camera_model_path);
+    camera_matrix = camera_model.intrinsic_matrix;
+    camera_distortion = camera_model.distortion_coefficients;
+  } catch (...) {
+    return 1;
+  }
+
+  wpi::json json = wpi::json::parse(std::ifstream(ideal_map_path));
+  if (!json.contains("tags")) {
+    return 1;
+  }
+
+  // Load ideal field map
+  std::map<int, wpi::json> ideal_map;
+  try {
+    ideal_map = load_ideal_map(ideal_map_path);
+  } catch (...) {
+    return 1;
+  }
+
+  // Apriltag detector
+  apriltag_detector_t* tag_detector = apriltag_detector_create();
+  tag_detector->nthreads = 8;
+
+  apriltag_family_t* tag_family = tag36h11_create();
+  apriltag_detector_add_family(tag_detector, tag_family);
+
+  // Find tag poses
+  std::map<int, Pose, std::less<int>,
+           Eigen::aligned_allocator<std::pair<const int, Pose>>>
+      poses;
+  std::vector<Constraint, Eigen::aligned_allocator<Constraint>> constraints;
+
+  for (const auto& entry :
+       std::filesystem::directory_iterator(input_dir_path)) {
+    if (entry.path().filename().string()[0] == '.') {
+      continue;
+    }
+
+    const std::string path = entry.path().string();
+
+    bool success = process_video_file(tag_detector, camera_matrix,
+                                      camera_distortion, tagSizeMeters, path,
+                                      poses, constraints, show_debug_window);
+
+    if (!success) {
+      std::cout << "Unable to process video " << path << std::endl;
+      return 1;
+    }
+  }
+
+  // Build optimization problem
+  ceres::Problem problem;
+  ceres::Manifold* quaternion_manifold = new ceres::EigenQuaternionManifold;
+
+  for (const auto& constraint : constraints) {
+    auto pose_begin_iter = poses.find(constraint.id_begin);
+    auto pose_end_iter = poses.find(constraint.id_end);
+
+    ceres::CostFunction* cost_function =
+        PoseGraphError::Create(constraint.t_begin_end);
+
+    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);
+  }
+
+  // Pin tag
+  auto pinned_tag_iter = poses.find(pinned_tag_id);
+  if (pinned_tag_iter != poses.end()) {
+    problem.SetParameterBlockConstant(pinned_tag_iter->second.p.data());
+    problem.SetParameterBlockConstant(
+        pinned_tag_iter->second.q.coeffs().data());
+  }
+
+  // Solve
+  ceres::Solver::Options options;
+  options.max_num_iterations = 200;
+  options.linear_solver_type = ceres::SPARSE_NORMAL_CHOLESKY;
+  options.num_threads = 10;
+
+  ceres::Solver::Summary summary;
+  ceres::Solve(options, &problem, &summary);
+
+  std::cout << summary.BriefReport() << std::endl;
+
+  // Output
+  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;
+
+  Eigen::Matrix<double, 4, 4> correction_b;
+  correction_b << 0, 1, 0, 0, 0, 0, -1, 0, -1, 0, 0, 0, 0, 0, 0, 1;
+
+  Eigen::Matrix<double, 4, 4> pinned_tag_transform =
+      get_tag_transform(ideal_map, pinned_tag_id);
+
+  for (const auto& [tag_id, pose] : poses) {
+    // Transformation from pinned tag
+    Eigen::Matrix<double, 4, 4> transform =
+        Eigen::Matrix<double, 4, 4>::Identity();
+
+    transform.block<3, 3>(0, 0) = pose.q.toRotationMatrix();
+    transform.block<3, 1>(0, 3) = pose.p;
+
+    // Transformation from world
+    Eigen::Matrix<double, 4, 4> corrected_transform =
+        pinned_tag_transform * correction_a * transform * correction_b;
+    Eigen::Quaternion<double> corrected_transform_q(
+        corrected_transform.block<3, 3>(0, 0));
+
+    observed_map[tag_id]["pose"]["translation"]["x"] =
+        corrected_transform(0, 3);
+    observed_map[tag_id]["pose"]["translation"]["y"] =
+        corrected_transform(1, 3);
+    observed_map[tag_id]["pose"]["translation"]["z"] =
+        corrected_transform(2, 3);
+
+    observed_map[tag_id]["pose"]["rotation"]["quaternion"]["X"] =
+        corrected_transform_q.x();
+    observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Y"] =
+        corrected_transform_q.y();
+    observed_map[tag_id]["pose"]["rotation"]["quaternion"]["Z"] =
+        corrected_transform_q.z();
+    observed_map[tag_id]["pose"]["rotation"]["quaternion"]["W"] =
+        corrected_transform_q.w();
+  }
+
+  wpi::json observed_map_json;
+
+  for (const auto& [tag_id, tag_json] : observed_map) {
+    observed_map_json["tags"].push_back(tag_json);
+  }
+
+  observed_map_json["field"] = {
+      {"length", static_cast<double>(json.at("field").at("length"))},
+      {"width", static_cast<double>(json.at("field").at("width"))}};
+
+  std::ofstream output_file(output_file_path);
+  output_file << observed_map_json.dump(4) << std::endl;
+
+  return 0;
+}

wpical/src/main/native/cpp/fmap.cpp

@@ -0,0 +1,40 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include "fmap.h"
+
+#include <string>
+#include <vector>
+
+wpi::json fmap::singleTag(int tag, const tag::Pose& tagpose) {
+  std::vector<double> transform = {};
+  for (int i = 0; i < 4; i++) {
+    for (int j = 0; j < 4; j++) {
+      transform.push_back(tagpose.transformMatrixFmap(i, j));
+    }
+  }
+
+  return {{"family", "apriltag3_36h11_classic"},
+          {"id", tag},
+          {"size", 165.1},
+          {"transform", transform},
+          {"unique", true}};
+}
+
+wpi::json fmap::convertfmap(const wpi::json& json) {
+  std::string fmapstart = "{\"fiducials\":[";
+
+  std::string fmapend = "],\"type\":\"frc\"}";
+
+  Fieldmap fieldmap(json);
+
+  for (int i = 0; i < fieldmap.getNumTags(); i++) {
+    fmapstart += singleTag(i + 1, fieldmap.getTag(i + 1)).dump();
+    if (i != fieldmap.getNumTags() - 1) {
+      fmapstart += ",";
+    }
+  }
+
+  return wpi::json::parse(fmapstart.append(fmapend));
+}

wpical/src/main/native/cpp/tagpose.cpp

@@ -0,0 +1,29 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#include <tagpose.h>
+
+namespace tag {
+Pose::Pose(double xpos, double ypos, double zpos, double w, double x, double y,
+           double z, double field_length_meters, double field_width_meters) {
+  xPos = xpos;
+  yPos = ypos;
+  zPos = zpos;
+  quaternion = Eigen::Quaterniond(w, x, y, z);
+  rotationMatrix = quaternion.toRotationMatrix();
+  transformMatrixFmap.setZero();
+  transformMatrixFmap.block<3, 3>(0, 0) = rotationMatrix;
+  transformMatrixFmap(0, 3) = xpos - (field_length_meters / 2.0);
+  transformMatrixFmap(1, 3) = ypos - (field_width_meters / 2.0);
+  transformMatrixFmap(2, 3) = zpos;
+  transformMatrixFmap(3, 3) = 1;
+  transformMatrixFmap(3, 0) = 0;
+  transformMatrixFmap(3, 1) = 0;
+  transformMatrixFmap(3, 2) = 0;
+  Eigen::Vector3d eulerAngles = rotationMatrix.eulerAngles(0, 1, 2);
+  rollRot = eulerAngles[0];
+  pitchRot = eulerAngles[1];
+  yawRot = eulerAngles[2];
+}
+}  // namespace tag

wpical/src/main/native/include/cameracalibration.h

@@ -0,0 +1,51 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <fstream>
+#include <string>
+#include <vector>
+
+#include <Eigen/Core>
+#include <wpi/json.h>
+
+namespace cameracalibration {
+struct CameraModel {
+  Eigen::Matrix<double, 3, 3> intrinsic_matrix;
+  Eigen::Matrix<double, 8, 1> distortion_coefficients;
+  double avg_reprojection_error;
+};
+
+int calibrate(const std::string& input_video, CameraModel& camera_model,
+              float square_width, float marker_width, int board_width,
+              int board_height, bool show_debug_window);
+int calibrate(const std::string& input_video, CameraModel& camera_model,
+              float square_width, float marker_width, int board_width,
+              int board_height, double imagerWidthPixels,
+              double imagerHeightPixels, bool show_debug_window);
+int calibrate(const std::string& input_video, CameraModel& camera_model,
+              float square_width, int board_width, int board_height,
+              double imagerWidthPixels, double imagerHeightPixels,
+              bool show_debug_window);
+static void dumpJson(CameraModel& camera_model,
+                     const std::string& output_file_path) {
+  std::vector<double> camera_matrix(camera_model.intrinsic_matrix.data(),
+                                    camera_model.intrinsic_matrix.data() +
+                                        camera_model.intrinsic_matrix.size());
+  std::vector<double> distortion_coefficients(
+      camera_model.distortion_coefficients.data(),
+      camera_model.distortion_coefficients.data() +
+          camera_model.distortion_coefficients.size());
+
+  wpi::json result = {
+      {"camera_matrix", camera_matrix},
+      {"distortion_coefficients", distortion_coefficients},
+      {"avg_reprojection_error", camera_model.avg_reprojection_error}};
+
+  std::ofstream output_file(output_file_path);
+  output_file << result.dump(4) << std::endl;
+  output_file.close();
+}
+}  // namespace cameracalibration

wpical/src/main/native/include/fieldcalibration.h

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <string>
+
+#include "cameracalibration.h"
+
+namespace fieldcalibration {
+int calibrate(std::string input_dir_path, std::string output_file_path,
+              std::string camera_model_path, std::string ideal_map_path,
+              int pinned_tag_id, bool show_debug_window);
+}  // namespace fieldcalibration

wpical/src/main/native/include/fieldmap.h

@@ -0,0 +1,59 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <cmath>
+#include <vector>
+
+#include <tagpose.h>
+#include <wpi/json.h>
+
+class Fieldmap {
+ public:
+  Fieldmap() = default;
+  explicit Fieldmap(const wpi::json& json) {
+    double field_length_meters =
+        static_cast<double>(json.at("field").at("length"));
+    double field_width_meters =
+        static_cast<double>(json.at("field").at("width"));
+    for (const auto& tag : json.at("tags").items()) {
+      double tagXPos =
+          static_cast<double>(tag.value().at("pose").at("translation").at("x"));
+      double tagYPos =
+          static_cast<double>(tag.value().at("pose").at("translation").at("y"));
+      double tagZPos =
+          static_cast<double>(tag.value().at("pose").at("translation").at("z"));
+      double tagWQuat = static_cast<double>(
+          tag.value().at("pose").at("rotation").at("quaternion").at("W"));
+      double tagXQuat = static_cast<double>(
+          tag.value().at("pose").at("rotation").at("quaternion").at("X"));
+      double tagYQuat = static_cast<double>(
+          tag.value().at("pose").at("rotation").at("quaternion").at("Y"));
+      double tagZQuat = static_cast<double>(
+          tag.value().at("pose").at("rotation").at("quaternion").at("Z"));
+
+      tagVec.emplace_back(tagXPos, tagYPos, tagZPos, tagWQuat, tagXQuat,
+                          tagYQuat, tagZQuat, field_length_meters,
+                          field_width_meters);
+    }
+  }
+
+  const tag::Pose& getTag(size_t tag) const { return tagVec[tag - 1]; }
+
+  int getNumTags() const { return tagVec.size(); }
+
+  static double minimizeAngle(double angle) {
+    angle = std::fmod(angle, 360);
+    if (angle > 180) {
+      return angle - 360;
+    } else if (angle < -180) {
+      return angle + 360;
+    }
+    return angle;
+  }
+
+ private:
+  std::vector<tag::Pose> tagVec;
+};

wpical/src/main/native/include/fmap.h

@@ -0,0 +1,15 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <fieldmap.h>
+
+#include <tagpose.h>
+#include <wpi/json.h>
+
+namespace fmap {
+wpi::json singleTag(int tag, const tag::Pose& tagpose);
+wpi::json convertfmap(const wpi::json& json);
+}  // namespace fmap

wpical/src/main/native/include/tagpose.h

@@ -0,0 +1,20 @@
+// Copyright (c) FIRST and other WPILib contributors.
+// Open Source Software; you can modify and/or share it under the terms of
+// the WPILib BSD license file in the root directory of this project.
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Geometry>
+
+namespace tag {
+class Pose {
+ public:
+  Pose(double xpos, double ypos, double zpos, double w, double x, double y,
+       double z, double field_length_meters, double field_width_meters);
+  double xPos, yPos, zPos, yawRot, rollRot, pitchRot;
+  Eigen::Quaterniond quaternion;
+  Eigen::Matrix3d rotationMatrix;
+  Eigen::Matrix4d transformMatrixFmap;
+};
+}  // namespace tag

wpical/src/main/native/resources/2024-crescendo.json

@@ -0,0 +1,296 @@
+{
+  "tags": [
+    {
+      "ID": 1,
+      "pose": {
+        "translation": {
+          "x": 15.079471999999997,
+          "y": 0.24587199999999998,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 2,
+      "pose": {
+        "translation": {
+          "x": 16.185134,
+          "y": 0.883666,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 3,
+      "pose": {
+        "translation": {
+          "x": 16.579342,
+          "y": 4.982717999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 4,
+      "pose": {
+        "translation": {
+          "x": 16.579342,
+          "y": 5.547867999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 5,
+      "pose": {
+        "translation": {
+          "x": 14.700757999999999,
+          "y": 8.2042,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 6,
+      "pose": {
+        "translation": {
+          "x": 1.8415,
+          "y": 8.2042,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.7071067811865475,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.7071067811865476
+          }
+        }
+      }
+    },
+    {
+      "ID": 7,
+      "pose": {
+        "translation": {
+          "x": -0.038099999999999995,
+          "y": 5.547867999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 8,
+      "pose": {
+        "translation": {
+          "x": -0.038099999999999995,
+          "y": 4.982717999999999,
+          "z": 1.4511020000000001
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 9,
+      "pose": {
+        "translation": {
+          "x": 0.356108,
+          "y": 0.883666,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 10,
+      "pose": {
+        "translation": {
+          "x": 1.4615159999999998,
+          "y": 0.24587199999999998,
+          "z": 1.355852
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 11,
+      "pose": {
+        "translation": {
+          "x": 11.904726,
+          "y": 3.7132259999999997,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.8660254037844387,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 12,
+      "pose": {
+        "translation": {
+          "x": 11.904726,
+          "y": 4.49834,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.8660254037844387,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.49999999999999994
+          }
+        }
+      }
+    },
+    {
+      "ID": 13,
+      "pose": {
+        "translation": {
+          "x": 11.220196,
+          "y": 4.105148,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 6.123233995736766e-17,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 1.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 14,
+      "pose": {
+        "translation": {
+          "x": 5.320792,
+          "y": 4.105148,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 1.0,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.0
+          }
+        }
+      }
+    },
+    {
+      "ID": 15,
+      "pose": {
+        "translation": {
+          "x": 4.641342,
+          "y": 4.49834,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": 0.5000000000000001,
+            "X": 0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844386
+          }
+        }
+      }
+    },
+    {
+      "ID": 16,
+      "pose": {
+        "translation": {
+          "x": 4.641342,
+          "y": 3.7132259999999997,
+          "z": 1.3208
+        },
+        "rotation": {
+          "quaternion": {
+            "W": -0.4999999999999998,
+            "X": -0.0,
+            "Y": 0.0,
+            "Z": 0.8660254037844387
+          }
+        }
+      }
+    }
+  ],
+  "field": {
+    "length": 16.541,
+    "width": 8.211
+  }
+}

wpical/src/main/native/thirdparty/libdogleg/include/dogleg.h

@@ -0,0 +1,301 @@
+// -*- mode: C; c-basic-offset: 2 -*-
+// Copyright 2011 Oblong Industries
+//           2017 Dima Kogan <dima@secretsauce.net>
+// License: GNU LGPL <http://www.gnu.org/licenses>.
+
+#pragma once
+
+#include <suitesparse/cholmod.h>
+#include <stdbool.h>
+
+typedef void (dogleg_callback_t)(const double*   p,
+                                 double*         x,
+                                 cholmod_sparse* Jt,
+                                 void*           cookie);
+typedef void (dogleg_callback_dense_t)(const double*   p,
+                                       double*         x,
+                                       double*         J,
+                                       void*           cookie);
+
+// an operating point of the solver
+typedef struct
+{
+  // The pointers in this structure are all indices into a single "pool" array
+  // (see allocOperatingPoint()). I thus don't need to store the pointers at
+  // all, and can just index that one array directly, but that would break my
+  // ABI
+  double*         p;
+  double*         x;
+  double          norm2_x;
+  union
+  {
+    cholmod_sparse* Jt;
+    double*         J_dense; // row-first: grad0, grad1, grad2, ...
+  };
+  double*         Jt_x;
+
+  // the cached update vectors. It's useful to cache these so that when a step is rejected, we can
+  // reuse these when we retry
+  double*        updateCauchy;
+  union
+  {
+    cholmod_dense* updateGN_cholmoddense;
+    double*        updateGN_dense;
+  };
+  double         updateCauchy_lensq, updateGN_lensq; // update vector lengths
+
+  // whether the current update vectors are correct or not
+  int updateCauchy_valid, updateGN_valid;
+
+  int didStepToEdgeOfTrustRegion;
+
+  double* step_to_here;
+  double  step_to_here_len_sq;
+
+} dogleg_operatingPoint_t;
+
+// The newer APIs ( dogleg_...2() ) take a dogleg_parameters2_t argument for
+// their settings, while the older ones use a global set of parameters specified
+// with dogleg_set_...(). This global-parameters approach doesn't work if we
+// have multiple users of libdogleg in the same application
+typedef struct
+{
+  int max_iterations;
+  int dogleg_debug;
+
+  // it is cheap to reject a too-large trust region, so I start with something
+  // "large". The solver will quickly move down to something reasonable. Only the
+  // user really knows if this is "large" or not, so they should change this via
+  // the API
+  double trustregion0;
+
+  // These are probably OK to leave alone. Tweaking them can maybe result in
+  // slightly faster convergence
+  double trustregion_decrease_factor;
+  double trustregion_decrease_threshold;
+  double trustregion_increase_factor;
+  double trustregion_increase_threshold;
+
+  // The termination thresholds. Documented in the header
+  double Jt_x_threshold;
+  double update_threshold;
+  double trustregion_threshold;
+} dogleg_parameters2_t;
+
+// solver context. This has all the internal state of the solver
+typedef struct
+{
+  cholmod_common  common;
+
+  union
+  {
+    dogleg_callback_t*       f;
+    dogleg_callback_dense_t* f_dense;
+  };
+  void*              cookie;
+
+  // between steps, beforeStep contains the operating point of the last step.
+  // afterStep is ONLY used while making the step. Externally, use beforeStep
+  // unless you really know what you're doing
+  dogleg_operatingPoint_t* beforeStep;
+  dogleg_operatingPoint_t* afterStep;
+
+  // The result of the last JtJ factorization performed. Note that JtJ is not
+  // necessarily factorized at every step, so this is NOT guaranteed to contain
+  // the factorization of the most recent JtJ
+  union
+  {
+    cholmod_factor* factorization;
+
+    // This is a factorization of JtJ, stored as a packed symmetric matrix
+    // returned by dpptrf('L', ...)
+    double*         factorization_dense;
+  };
+
+  // Have I ever seen a singular JtJ? If so, I add this constant to the diagonal
+  // from that point on. This is a simple and fast way to deal with
+  // singularities. This constant starts at 0, and is increased every time a
+  // singular JtJ is encountered. This is suboptimal but works for me for now.
+  double                   lambda;
+
+  // Are we using sparse math (cholmod)?
+  int                      is_sparse;
+  int                      Nstate, Nmeasurements;
+
+  const dogleg_parameters2_t* parameters;
+
+} dogleg_solverContext_t;
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+// Fills in the given structure with the default parameter set
+void dogleg_getDefaultParameters(dogleg_parameters2_t* parameters);
+
+void dogleg_setMaxIterations(int n);
+void dogleg_setTrustregionUpdateParameters(double downFactor, double downThreshold,
+                                           double upFactor,   double upThreshold);
+
+// The argument is a bit-mapped integer. Should be a bit-field structure or
+// enum, but for API backwards-compatibility, I keep this an integer.
+//
+// if(debug == 0                 ): no diagnostic output
+// if(debug &  DOGLEG_DEBUG_VNLOG): output vnlog diagnostics to stdout
+// if(debug & ~DOGLEG_DEBUG_VNLOG): output human-oriented diagnostics to stderr
+#define DOGLEG_DEBUG_VNLOG   (1 << 30)
+void dogleg_setDebug(int debug);
+
+
+// The following parameters reflect the scaling of the problem being solved, so
+// the user is strongly encouraged to tweak these. The defaults are set
+// semi-arbitrarily
+
+// The size of the trust region at start. It is cheap to reject a too-large
+// trust region, so this should be something "large". Say 10x the length of an
+// "expected" step size
+void dogleg_setInitialTrustregion(double t);
+
+// termination thresholds. These really depend on the scaling of the input
+// problem, so the user should set these appropriately
+//
+// Jt_x threshold:
+// The function being minimized is E = norm2(x) where x is a function of the state p.
+// dE/dp = 2*Jt*x where Jt is transpose(dx/dp).
+//   if( for every i  fabs(Jt_x[i]) < JT_X_THRESHOLD )
+//   { we are done; }
+//
+// update threshold:
+//   if(for every i  fabs(state_update[i]) < UPDATE_THRESHOLD)
+//   { we are done; }
+//
+// trust region threshold:
+//   if(trustregion < TRUSTREGION_THRESHOLD)
+//   { we are done; }
+//
+// to leave a particular threshold alone, use a value <= 0 here
+void dogleg_setThresholds(double Jt_x, double update, double trustregion);
+
+
+// The main optimization function. Initial estimate of the solution passed in p,
+// final optimized solution returned in p. p has Nstate variables. There are
+// Nmeas measurements, the jacobian matrix has NJnnz non-zero entries.
+//
+// The evaluation function is given in the callback f; this function is passed
+// the given cookie
+//
+// If we want to get the full solver state when we're done, a non-NULL
+// returnContext pointer can be given. If this is done then THE USER IS
+// RESPONSIBLE FOR FREEING ITS MEMORY WITH dogleg_freeContext()
+double dogleg_optimize(double* p, unsigned int Nstate,
+                       unsigned int Nmeas, unsigned int NJnnz,
+                       dogleg_callback_t* f, void* cookie,
+                       dogleg_solverContext_t** returnContext);
+double dogleg_optimize2(double* p, unsigned int Nstate,
+                        unsigned int Nmeas, unsigned int NJnnz,
+                        dogleg_callback_t* f, void* cookie,
+                        const dogleg_parameters2_t* parameters,
+                        dogleg_solverContext_t** returnContext);
+
+// Main optimization function if we're using dense linear algebra. The arguments
+// are very similar to the sparse version: dogleg_optimize()
+double dogleg_optimize_dense(double* p, unsigned int Nstate,
+                             unsigned int Nmeas,
+                             dogleg_callback_dense_t* f, void* cookie,
+                             dogleg_solverContext_t** returnContext);
+double dogleg_optimize_dense2(double* p, unsigned int Nstate,
+                              unsigned int Nmeas,
+                              dogleg_callback_dense_t* f, void* cookie,
+                              const dogleg_parameters2_t* parameters,
+                              dogleg_solverContext_t** returnContext);
+
+// Compute the cholesky decomposition of JtJ. This function is only exposed if
+// you need to touch libdogleg internals via returnContext. Sometimes after
+// computing an optimization you want to do stuff with the factorization of JtJ,
+// and this call ensures that the factorization is available. Most people don't
+// need this function. If the comment wasn't clear, you don't need this
+// function.
+void dogleg_computeJtJfactorization(dogleg_operatingPoint_t* point, dogleg_solverContext_t* ctx);
+
+// Generates a plain text table that contains gradient checks. This table can be
+// easily parsed with "vnlog" tools
+void dogleg_testGradient(unsigned int var, const double* p0,
+                         unsigned int Nstate, unsigned int Nmeas, unsigned int NJnnz,
+                         dogleg_callback_t* f, void* cookie);
+void dogleg_testGradient_dense(unsigned int var, const double* p0,
+                               unsigned int Nstate, unsigned int Nmeas,
+                               dogleg_callback_dense_t* f, void* cookie);
+
+
+// If we want to get the full solver state when we're done optimizing, we can
+// pass a non-NULL returnContext pointer to dogleg_optimize(). If we do this,
+// then the user MUST call dogleg_freeContext() to deallocate the pointer when
+// the USER is done
+void dogleg_freeContext(dogleg_solverContext_t** ctx);
+
+
+// Computes outlierness factors. This function is experimental, and subject to
+// change.
+bool dogleg_getOutliernessFactors( // output
+                                  double* factors, // Nfeatures factors
+
+                                  // output, input
+                                  double* scale, // if <0 then I recompute
+
+                                  // inputs
+                                  // if outliers are grouped into features, the feature size is
+                                  // stated here
+                                  int featureSize,
+                                  int Nfeatures,
+                                  int NoutlierFeatures, // how many outliers we already have
+                                  dogleg_operatingPoint_t* point,
+                                  dogleg_solverContext_t* ctx );
+
+
+
+
+// This stuff is experimental, and subject to change.
+struct dogleg_outliers_t
+{
+    unsigned char marked : 1;
+};
+bool dogleg_markOutliers(// output, input
+                         struct dogleg_outliers_t* markedOutliers,
+                         double* scale, // if <0 then I recompute
+
+                         // output, input
+                         int* Noutliers, // number of outliers before and after this call
+
+                         // input
+                         double (getConfidence)(int i_feature_exclude),
+
+                         // if outliers are grouped into features, the feature size is
+                         // stated here
+                         int featureSize,
+                         int Nfeatures,
+
+                         dogleg_operatingPoint_t* point,
+                         dogleg_solverContext_t* ctx);
+
+void dogleg_reportOutliers( double (getConfidence)(int i_feature_exclude),
+                            double* scale, // if <0 then I recompute
+
+                            // if outliers are grouped into features, the feature size
+                            // is stated here
+                            int featureSize,
+                            int Nfeatures,
+                            int Noutliers, // how many outliers we already have
+
+                            dogleg_operatingPoint_t* point,
+                            dogleg_solverContext_t* ctx);
+
+double dogleg_getOutliernessTrace_newFeature_sparse(const double*            JqueryFeature,
+                                                    int                      istateActive,
+                                                    int                      NstateActive,
+                                                    int                      featureSize,
+                                                    int                      NoutlierFeatures,
+                                                    dogleg_operatingPoint_t* point,
+                                                    dogleg_solverContext_t*  ctx);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif

wpical/src/main/native/thirdparty/mrcal/include/autodiff.hh

@@ -0,0 +1,582 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+/*
+  Automatic differentiation routines. Used in poseutils-uses-autodiff.cc. See
+  that file for usage examples
+ */
+
+// Apparently I need this in MSVC to get constants
+#define _USE_MATH_DEFINES
+#include <math.h>
+#include <string.h>
+#include "strides.h"
+
+template<int NGRAD, int NVEC> struct vec_withgrad_t;
+
+
+template<int NGRAD>
+struct val_withgrad_t
+{
+    double x;
+    double j[NGRAD == 0 ? 1 : NGRAD];
+
+    
+    val_withgrad_t(double _x = 0.0) : x(_x)
+    {
+        for(int i=0; i<NGRAD; i++) j[i] = 0.0;
+    }
+
+    
+    val_withgrad_t<NGRAD> operator+( const val_withgrad_t<NGRAD>& b ) const
+    {
+        val_withgrad_t<NGRAD> y = *this;
+        y.x += b.x;
+        for(int i=0; i<NGRAD; i++)
+            y.j[i] += b.j[i];
+        return y;
+    }
+    
+    val_withgrad_t<NGRAD> operator+( double b ) const
+    {
+        val_withgrad_t<NGRAD> y = *this;
+        y.x += b;
+        return y;
+    }
+    
+    void operator+=( const val_withgrad_t<NGRAD>& b )
+    {
+        *this = (*this) + b;
+    }
+    
+    val_withgrad_t<NGRAD> operator-( const val_withgrad_t<NGRAD>& b ) const
+    {
+        val_withgrad_t<NGRAD> y = *this;
+        y.x -= b.x;
+        for(int i=0; i<NGRAD; i++)
+            y.j[i] -= b.j[i];
+        return y;
+    }
+    
+    val_withgrad_t<NGRAD> operator-( double b ) const
+    {
+        val_withgrad_t<NGRAD> y = *this;
+        y.x -= b;
+        return y;
+    }
+    
+    void operator-=( const val_withgrad_t<NGRAD>& b )
+    {
+        *this = (*this) - b;
+    }
+    
+    val_withgrad_t<NGRAD> operator-() const
+    {
+        return (*this) * (-1);
+    }
+    
+    val_withgrad_t<NGRAD> operator*( const val_withgrad_t<NGRAD>& b ) const
+    {
+        val_withgrad_t<NGRAD> y;
+        y.x = x * b.x;
+        for(int i=0; i<NGRAD; i++)
+            y.j[i] = j[i]*b.x + x*b.j[i];
+        return y;
+    }
+    
+    val_withgrad_t<NGRAD> operator*( double b ) const
+    {
+        val_withgrad_t<NGRAD> y;
+        y.x = x * b;
+        for(int i=0; i<NGRAD; i++)
+            y.j[i] = j[i]*b;
+        return y;
+    }
+    
+    void operator*=( const val_withgrad_t<NGRAD>& b )
+    {
+        *this = (*this) * b;
+    }
+    
+    void operator*=( const double b )
+    {
+        *this = (*this) * b;
+    }
+    
+    val_withgrad_t<NGRAD> operator/( const val_withgrad_t<NGRAD>& b ) const
+    {
+        val_withgrad_t<NGRAD> y;
+        y.x = x / b.x;
+        for(int i=0; i<NGRAD; i++)
+            y.j[i] = (j[i]*b.x - x*b.j[i]) / (b.x*b.x);
+        return y;
+    }
+    
+    val_withgrad_t<NGRAD> operator/( double b ) const
+    {
+        return (*this) * (1./b);
+    }
+    
+    void operator/=( const val_withgrad_t<NGRAD>& b )
+    {
+        *this = (*this) / b;
+    }
+    
+    void operator/=( const double b )
+    {
+        *this = (*this) / b;
+    }
+    
+    val_withgrad_t<NGRAD> sqrt(void) const
+    {
+        val_withgrad_t<NGRAD> y;
+        y.x = ::sqrt(x);
+        for(int i=0; i<NGRAD; i++)
+            y.j[i] = j[i] / (2. * y.x);
+        return y;
+    }
+
+    
+    val_withgrad_t<NGRAD> square(void) const
+    {
+        val_withgrad_t<NGRAD> s;
+        s.x = x*x;
+        for(int i=0; i<NGRAD; i++)
+            s.j[i] = 2. * x * j[i];
+        return s;
+    }
+
+    
+    val_withgrad_t<NGRAD> sin(void) const
+    {
+        const double s = ::sin(x);
+        const double c = ::cos(x);
+        val_withgrad_t<NGRAD> y;
+        y.x = s;
+        for(int i=0; i<NGRAD; i++)
+            y.j[i] = c*j[i];
+        return y;
+    }
+
+    
+    val_withgrad_t<NGRAD> cos(void) const
+    {
+        const double s = ::sin(x);
+        const double c = ::cos(x);
+        val_withgrad_t<NGRAD> y;
+        y.x = c;
+        for(int i=0; i<NGRAD; i++)
+            y.j[i] = -s*j[i];
+        return y;
+    }
+
+    
+    vec_withgrad_t<NGRAD, 2> sincos(void) const
+    {
+        const double s = ::sin(x);
+        const double c = ::cos(x);
+        vec_withgrad_t<NGRAD, 2> sc;
+        sc.v[0].x = s;
+        sc.v[1].x = c;
+        for(int i=0; i<NGRAD; i++)
+        {
+            sc.v[0].j[i] =  c*j[i];
+            sc.v[1].j[i] = -s*j[i];
+        }
+        return sc;
+    }
+
+    
+    val_withgrad_t<NGRAD> tan(void) const
+    {
+        const double s = ::sin(x);
+        const double c = ::cos(x);
+        val_withgrad_t<NGRAD> y;
+        y.x = s/c;
+        for(int i=0; i<NGRAD; i++)
+            y.j[i] = j[i] / (c*c);
+        return y;
+    }
+
+    
+    val_withgrad_t<NGRAD> atan2(val_withgrad_t<NGRAD>& x) const
+    {
+        val_withgrad_t<NGRAD> th;
+        const val_withgrad_t<NGRAD>& y = *this;
+
+        th.x = ::atan2(y.x, x.x);
+        // dth/dv = d/dv atan2(y,x)
+        //        = d/dv atan(y/x)
+        //        = 1 / (1 + y^2/x^2) d/dv (y/x)
+        //        = x^2 / (x^2 + y^2) / x^2 * (dy/dv x - y dx/dv)
+        //        = 1 / (x^2 + y^2) * (dy/dv x - y dx/dv)
+        double norm2 = y.x*y.x + x.x*x.x;
+        for(int i=0; i<NGRAD; i++)
+            th.j[i] = (y.j[i]*x.x - y.x*x.j[i]) / norm2;
+        return th;
+    }
+
+    // This function does NOT check for overflow. The gradient is infinite at
+    // the valid bounds of the input. So the caller has to do the right thing in
+    // those cases
+    
+    val_withgrad_t<NGRAD> asin(void) const
+    {
+        val_withgrad_t<NGRAD> th;
+        th.x = ::asin(x);
+        double dasin_dx = 1. / ::sqrt( 1. - x*x );
+        for(int i=0; i<NGRAD; i++)
+            th.j[i] = dasin_dx * j[i];
+        return th;
+    }
+
+    // This function does NOT check for overflow. The gradient is infinite at
+    // the valid bounds of the input. So the caller has to do the right thing in
+    // those cases
+    
+    val_withgrad_t<NGRAD> acos(void) const
+    {
+        val_withgrad_t<NGRAD> th;
+        th.x = ::acos(x);
+        double dacos_dx = -1. / ::sqrt( 1. - x*x );
+        for(int i=0; i<NGRAD; i++)
+            th.j[i] = dacos_dx * j[i];
+        return th;
+    }
+
+    
+    val_withgrad_t<NGRAD> sinx_over_x(// To avoid recomputing it
+                                      const val_withgrad_t<NGRAD>& sinx) const
+    {
+        // For small x I need special-case logic. In the limit as x->0 I have
+        // sin(x)/x -> 1. But I'm propagating gradients, so I need to capture
+        // that. I have
+        //
+        //   d(sin(x)/x)/dx =
+        //     (x cos(x) - sin(x))/x^2
+        //
+        // As x -> 0 this is
+        //
+        //     (cos(x) - x sin(x) - cos(x)) / (2x) =
+        //     (- x sin(x)) / (2x) =
+        //     -sin(x) / 2 =
+        //     0
+        //
+        // So for small x the gradient is 0
+        if(fabs(x) < 1e-5)
+            return val_withgrad_t<NGRAD>(1.0);
+
+        return sinx / (*this);
+    }
+};
+
+
+template<int NGRAD, int NVEC>
+struct vec_withgrad_t
+{
+    val_withgrad_t<NGRAD> v[NVEC == 0 ? 1 : NVEC];
+
+    vec_withgrad_t() {}
+
+    
+    void init_vars(const double* x_in, int ivar0, int Nvars, int i_gradvec0 = -1,
+                   int stride = sizeof(double))
+    {
+        // Initializes vector entries ivar0..ivar0+Nvars-1 inclusive using the
+        // data in x_in[]. x_in[0] corresponds to vector entry ivar0. If
+        // i_gradvec0 >= 0 then vector ivar0 corresponds to gradient index
+        // i_gradvec0, with all subsequent entries being filled-in
+        // consecutively. It's very possible that NGRAD > Nvars. Initially the
+        // subset of the gradient array corresponding to variables
+        // i_gradvec0..i_gradvec0+Nvars-1 is an identity, with the rest being 0
+        memset((char*)&v[ivar0], 0, Nvars*sizeof(v[0]));
+        for(int i=ivar0; i<ivar0+Nvars; i++)
+        {
+            v[i].x = _P1(x_in,stride,  i-ivar0);
+            if(i_gradvec0 >= 0)
+                v[i].j[i_gradvec0+i-ivar0] = 1.0;
+        }
+    }
+
+    
+    vec_withgrad_t(const double* x_in, int i_gradvec0 = -1,
+                   int stride = sizeof(double))
+    {
+        init_vars(x_in, 0, NVEC, i_gradvec0, stride);
+    }
+
+    
+    val_withgrad_t<NGRAD>& operator[](int i)
+    {
+        return v[i];
+    }
+
+    
+    const val_withgrad_t<NGRAD>& operator[](int i) const
+    {
+        return v[i];
+    }
+
+    
+    void operator+=( const vec_withgrad_t<NGRAD,NVEC>& x )
+    {
+        (*this) = (*this) + x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator+( const vec_withgrad_t<NGRAD,NVEC>& x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] + x.v[i];
+        return p;
+    }
+
+    
+    void operator+=( const val_withgrad_t<NGRAD>& x )
+    {
+        (*this) = (*this) + x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator+( const val_withgrad_t<NGRAD>& x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] + x;
+        return p;
+    }
+
+    
+    void operator+=( double x )
+    {
+        (*this) = (*this) + x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator+( double x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] + x;
+        return p;
+    }
+
+    
+    void operator-=( const vec_withgrad_t<NGRAD,NVEC>& x )
+    {
+        (*this) = (*this) - x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator-( const vec_withgrad_t<NGRAD,NVEC>& x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] - x.v[i];
+        return p;
+    }
+
+    
+    void operator-=( const val_withgrad_t<NGRAD>& x )
+    {
+        (*this) = (*this) - x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator-( const val_withgrad_t<NGRAD>& x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] - x;
+        return p;
+    }
+
+    
+    void operator-=( double x )
+    {
+        (*this) = (*this) - x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator-( double x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] - x;
+        return p;
+    }
+
+    
+    void operator*=( const vec_withgrad_t<NGRAD,NVEC>& x )
+    {
+        (*this) = (*this) * x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator*( const vec_withgrad_t<NGRAD,NVEC>& x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] * x.v[i];
+        return p;
+    }
+
+    
+    void operator*=( const val_withgrad_t<NGRAD>& x )
+    {
+        (*this) = (*this) * x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator*( const val_withgrad_t<NGRAD>& x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] * x;
+        return p;
+    }
+
+    
+    void operator*=( double x )
+    {
+        (*this) = (*this) * x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator*( double x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] * x;
+        return p;
+    }
+
+    
+    void operator/=( const vec_withgrad_t<NGRAD,NVEC>& x )
+    {
+        (*this) = (*this) / x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator/( const vec_withgrad_t<NGRAD,NVEC>& x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] / x.v[i];
+        return p;
+    }
+
+    
+    void operator/=( const val_withgrad_t<NGRAD>& x )
+    {
+        (*this) = (*this) / x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator/( const val_withgrad_t<NGRAD>& x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] / x;
+        return p;
+    }
+
+    
+    void operator/=( double x )
+    {
+        (*this) = (*this) / x;
+    }
+    
+    vec_withgrad_t<NGRAD,NVEC> operator/( double x ) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> p;
+        for(int i=0; i<NVEC; i++)
+            p[i] = v[i] / x;
+        return p;
+    }
+
+    
+    val_withgrad_t<NGRAD> dot( const vec_withgrad_t<NGRAD,NVEC>& x) const
+    {
+        val_withgrad_t<NGRAD> d; // initializes to 0
+        for(int i=0; i<NVEC; i++)
+        {
+            val_withgrad_t<NGRAD> e = x.v[i]*v[i];
+            d += e;
+        }
+        return d;
+    }
+
+    
+    vec_withgrad_t<NGRAD,NVEC> cross( const vec_withgrad_t<NGRAD,NVEC>& x) const
+    {
+        vec_withgrad_t<NGRAD,NVEC> c;
+        c[0] = v[1]*x.v[2] - v[2]*x.v[1];
+        c[1] = v[2]*x.v[0] - v[0]*x.v[2];
+        c[2] = v[0]*x.v[1] - v[1]*x.v[0];
+        return c;
+    }
+
+    
+    val_withgrad_t<NGRAD> norm2(void) const
+    {
+        return dot(*this);
+    }
+
+    
+    val_withgrad_t<NGRAD> mag(void) const
+    {
+        val_withgrad_t<NGRAD> l2 = norm2();
+        return l2.sqrt();
+    }
+
+    
+    void extract_value(double* out,
+                       int stride = sizeof(double),
+                       int ivar0 = 0, int Nvars = NVEC) const
+    {
+        for(int i=ivar0; i<ivar0+Nvars; i++)
+            _P1(out,stride, i-ivar0) = v[i].x;
+    }
+    
+    void extract_grad(double* J,
+                      int i_gradvec0, int N_gradout,
+                      int ivar0,
+                      int J_stride0, int J_stride1,
+                      int Nvars = NVEC) const
+    {
+        for(int i=ivar0; i<ivar0+Nvars; i++)
+            for(int j=0; j<N_gradout; j++)
+                _P2(J,J_stride0,J_stride1, i-ivar0,j) = v[i].j[i_gradvec0+j];
+    }
+};
+
+template<int NGRAD>
+static
+vec_withgrad_t<NGRAD, 3>
+cross( const vec_withgrad_t<NGRAD, 3>& a,
+       const vec_withgrad_t<NGRAD, 3>& b )
+{
+    vec_withgrad_t<NGRAD, 3> c;
+    c.v[0] = a.v[1]*b.v[2] - a.v[2]*b.v[1];
+    c.v[1] = a.v[2]*b.v[0] - a.v[0]*b.v[2];
+    c.v[2] = a.v[0]*b.v[1] - a.v[1]*b.v[0];
+    return c;
+}
+
+template<int NGRAD>
+static
+val_withgrad_t<NGRAD>
+cross_norm2( const vec_withgrad_t<NGRAD, 3>& a,
+             const vec_withgrad_t<NGRAD, 3>& b )
+{
+    vec_withgrad_t<NGRAD, 3> c = cross(a,b);
+    return c.norm2();
+}
+
+template<int NGRAD>
+static
+val_withgrad_t<NGRAD>
+cross_mag( const vec_withgrad_t<NGRAD, 3>& a,
+           const vec_withgrad_t<NGRAD, 3>& b )
+{
+    vec_withgrad_t<NGRAD, 3> c = cross(a,b);
+    return c.mag();
+}

wpical/src/main/native/thirdparty/mrcal/include/basic-geometry.h

@@ -0,0 +1,130 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+// A 2D point or vector
+//
+// The individual elements can be accessed via .x and .y OR the vector can be
+// accessed as an .xy[] array:
+//
+//   mrcal_point2_t p = f();
+//
+// Now p.x and p.xy[0] refer to the same value.
+typedef union
+{
+    struct
+    {
+        double x,y;
+    };
+
+    double xy[2];
+} mrcal_point2_t;
+
+// A 3D point or vector
+//
+// The individual elements can be accessed via .x and .y and .z OR the vector
+// can be accessed as an .xyz[] array:
+//
+//   mrcal_point3_t p = f();
+//
+// Now p.x and p.xy[0] refer to the same value.
+typedef union
+{
+    struct
+    {
+        double x,y,z;
+    };
+    double xyz[3];
+} mrcal_point3_t;
+
+// Unconstrained 6DOF pose containing a Rodrigues rotation and a translation
+typedef struct
+{
+    mrcal_point3_t r,t;
+} mrcal_pose_t;
+
+
+//////////// Easy convenience stuff
+////// point2
+
+static double mrcal_point2_inner(const mrcal_point2_t a, const mrcal_point2_t b)
+{
+    return
+        a.x*b.x +
+        a.y*b.y;
+}
+
+static double mrcal_point2_norm2(const mrcal_point2_t a)
+{
+    return mrcal_point2_inner(a,a);
+}
+#define mrcal_point2_mag(a) sqrt(norm2(a)) // macro to not require #include <math.h>
+
+
+static mrcal_point2_t mrcal_point2_add(const mrcal_point2_t a, const mrcal_point2_t b)
+{
+    return (mrcal_point2_t){ .x = a.x + b.x,
+                             .y = a.y + b.y };
+}
+
+static mrcal_point2_t mrcal_point2_sub(const mrcal_point2_t a, const mrcal_point2_t b)
+{
+    return (mrcal_point2_t){ .x = a.x - b.x,
+                             .y = a.y - b.y };
+}
+
+static mrcal_point2_t mrcal_point2_scale(const mrcal_point2_t a, const double s)
+{
+    return (mrcal_point2_t){ .x = a.x * s,
+                             .y = a.y * s };
+}
+////// point3
+
+static double mrcal_point3_inner(const mrcal_point3_t a, const mrcal_point3_t b)
+{
+    return
+        a.x*b.x +
+        a.y*b.y +
+        a.z*b.z;
+}
+
+static double mrcal_point3_norm2(const mrcal_point3_t a)
+{
+    return mrcal_point3_inner(a,a);
+}
+#define mrcal_point3_mag(a) sqrt(mrcal_point3_norm2(a)) // macro to not require #include <math.h>
+
+
+static mrcal_point3_t mrcal_point3_add(const mrcal_point3_t a, const mrcal_point3_t b)
+{
+    return (mrcal_point3_t){ .x = a.x + b.x,
+                             .y = a.y + b.y,
+                             .z = a.z + b.z };
+}
+
+static mrcal_point3_t mrcal_point3_sub(const mrcal_point3_t a, const mrcal_point3_t b)
+{
+    return (mrcal_point3_t){ .x = a.x - b.x,
+                             .y = a.y - b.y,
+                             .z = a.z - b.z };
+}
+
+static mrcal_point3_t mrcal_point3_scale(const mrcal_point3_t a, const double s)
+{
+    return (mrcal_point3_t){ .x = a.x * s,
+                             .y = a.y * s,
+                             .z = a.z * s };
+}
+
+static mrcal_point3_t mrcal_point3_cross(const mrcal_point3_t a, const mrcal_point3_t b)
+{
+    return (mrcal_point3_t){ .x = a.y*b.z - a.z*b.y,
+                             .y = a.z*b.x - a.x*b.z,
+                             .z = a.x*b.y - a.y*b.x };
+}

wpical/src/main/native/thirdparty/mrcal/include/cahvore.h

@@ -0,0 +1,23 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+#include <stdbool.h>
+#include "basic-geometry.h"
+
+bool project_cahvore_internals( // outputs
+                                mrcal_point3_t* __restrict pdistorted,
+                                double*         __restrict dpdistorted_dintrinsics_nocore,
+                                double*         __restrict dpdistorted_dp,
+
+                                // inputs
+                                const mrcal_point3_t* __restrict p,
+                                const double* __restrict intrinsics_nocore,
+                                const double cahvore_linearity);
+

wpical/src/main/native/thirdparty/mrcal/include/minimath/minimath.h

@@ -0,0 +1,820 @@
+#pragma once
+
+#ifdef __cplusplus
+#define restrict
+#endif
+
+
+#include "minimath_generated.h"
+
+// In all the following computations I use the expression obtained before a
+// maxima expand(). factorsum()-ing the result doesn't appear to do much.  Doing
+// an expand() greatly increases the number of multiplications, but reduces the
+// number of additions. In the specific case of a 4x4 symmetric determinant,
+// expand() changes (23,40) to (16,58) +,*. 18 more *, but 7 fewer +.
+// Session:
+/*
+(%i13) display2d : false;
+(%o13) false
+
+(%i31) sym2 : matrix([m0,m1],
+                     [m1,m2]);
+
+(%o31) matrix([m0,m1],[m1,m2])
+(%i32) sym3 : matrix([m0,m1,m2],
+                     [m1,m3,m4],
+                     [m2,m4,m5]);
+
+(%o32) matrix([m0,m1,m2],[m1,m3,m4],[m2,m4,m5])
+(%i33) sym4 : matrix([m0,m1,m2,m3],
+                     [m1,m4,m5,m6],
+                     [m2,m5,m7,m8],
+                     [m3,m6,m8,m9]);
+(%i99) sym5 : matrix([m0,m1,m2, m3, m4 ],
+                     [m1,m5,m6, m7, m8 ],
+                     [m2,m6,m9, m10,m11],
+                     [m3,m7,m10,m12,m13],
+                     [m4,m8,m11,m13,m14]);
+
+(%o99) matrix([m0,m1,m2,m3,m4],[m1,m5,m6,m7,m8],[m2,m6,m9,m10,m11],
+              [m3,m7,m10,m12,m13],[m4,m8,m11,m13,m14])
+
+(%i34) determinant(sym4);
+
+(%o34) m0*(m4*(m7*m9-m8^2)-m5*(m5*m9-m6*m8)+m6*(m5*m8-m6*m7))
+        -m1*(m1*(m7*m9-m8^2)-m5*(m2*m9-m3*m8)+m6*(m2*m8-m3*m7))
+        +m2*(m1*(m5*m9-m6*m8)-m4*(m2*m9-m3*m8)+m6*(m2*m6-m3*m5))
+        -m3*(m1*(m5*m8-m6*m7)-m4*(m2*m8-m3*m7)+m5*(m2*m6-m3*m5))
+(%i35) expand(determinant(sym4));
+
+(%o35) m0*m4*m7*m9-m1^2*m7*m9-m0*m5^2*m9+2*m1*m2*m5*m9-m2^2*m4*m9-m0*m4*m8^2
+                  +m1^2*m8^2+2*m0*m5*m6*m8-2*m1*m2*m6*m8-2*m1*m3*m5*m8
+                  +2*m2*m3*m4*m8-m0*m6^2*m7+2*m1*m3*m6*m7-m3^2*m4*m7+m2^2*m6^2
+                  -2*m2*m3*m5*m6+m3^2*m5^2
+*/
+
+/* The session to compute the determinants appears above. The only postprocessing was to
+   replace m.^2 -> m.*m, m. -> m[.]
+
+(%i36) determinant(sym2);
+
+(%o36) m0*m2-m1^2
+(%i37) determinant(sym3);
+
+(%o37) m0*(m3*m5-m4^2)-m1*(m1*m5-m2*m4)+m2*(m1*m4-m2*m3)
+(%i38) determinant(sym4);
+
+(%o38) m0*(m4*(m7*m9-m8^2)-m5*(m5*m9-m6*m8)+m6*(m5*m8-m6*m7))
+        -m1*(m1*(m7*m9-m8^2)-m5*(m2*m9-m3*m8)+m6*(m2*m8-m3*m7))
+        +m2*(m1*(m5*m9-m6*m8)-m4*(m2*m9-m3*m8)+m6*(m2*m6-m3*m5))
+        -m3*(m1*(m5*m8-m6*m7)-m4*(m2*m8-m3*m7)+m5*(m2*m6-m3*m5))
+
+(%i100) determinant(sym5);
+
+(%o100) -m3*(-m8*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6)
+            +m1*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8)+(m10*m14-m11*m13)*m6)
+            -m5*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4)+(m10*m14-m11*m13)*m2)
+            +m6*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6))
+         +m4*(-m7*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6)
+             +m1*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8)
+                                     +(m10*m13-m11*m12)*m6)
+             -m5*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4)
+                                     +(m10*m13-m11*m12)*m2)
+             +m6*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6))
+         +m0*(m7*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8)
+                                     +(m10*m14-m11*m13)*m6)
+             -m8*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8)
+                                     +(m10*m13-m11*m12)*m6)
+             +m5*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13)
+                                    +m11*(m10*m13-m11*m12))
+             -m6*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8)
+                                      +(m12*m14-m13^2)*m6))
+         -m1*(m7*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4)
+                                     +(m10*m14-m11*m13)*m2)
+             -m8*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4)
+                                     +(m10*m13-m11*m12)*m2)
+             +m1*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13)
+                                    +m11*(m10*m13-m11*m12))
+             -(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)+(m12*m14-m13^2)*m2)
+              *m6)
+         +m2*(m7*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6)
+             -m8*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6)
+             +m1*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8)+(m12*m14-m13^2)*m6)
+             -(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)+(m12*m14-m13^2)*m2)
+              *m5)
+
+*/
+
+#if 0
+#error You really should be using the cofactors functions below to do this. So far Ive only needed the determinant as a part of computing the inverse, and the below functions do this much more efficiently
+static inline double det_sym2(const double* m)
+{
+  return m[0]*m[2]-m[1]*m[1];
+}
+static inline double det_sym3(const double* m)
+{
+  return m[0]*(m[3]*m[5]-m[4]*m[4])-m[1]*(m[1]*m[5]-m[2]*m[4])+m[2]*(m[1]*m[4]-m[2]*m[3]);
+}
+static inline double det_sym4(const double* m)
+{
+  return
+    +m[0]*(m[4]*(m[7]*m[9]-m[8]*m[8])-m[5]*(m[5]*m[9]-m[6]*m[8])+m[6]*(m[5]*m[8]-m[6]*m[7]))
+    -m[1]*(m[1]*(m[7]*m[9]-m[8]*m[8])-m[5]*(m[2]*m[9]-m[3]*m[8])+m[6]*(m[2]*m[8]-m[3]*m[7]))
+    +m[2]*(m[1]*(m[5]*m[9]-m[6]*m[8])-m[4]*(m[2]*m[9]-m[3]*m[8])+m[6]*(m[2]*m[6]-m[3]*m[5]))
+    -m[3]*(m[1]*(m[5]*m[8]-m[6]*m[7])-m[4]*(m[2]*m[8]-m[3]*m[7])+m[5]*(m[2]*m[6]-m[3]*m[5]));
+}
+static inline double det_sym5(const double* m)
+{
+  return
+    -m[3]*(-m[8]*((m[3]*m[8]-m[4]*m[7])*m[9]+m[2]*(m[11]*m[7]-m[10]*m[8])-(m[11]*m[3]-m[10]*m[4])*m[6])
+           +m[1]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[6])
+           -m[5]*(-(m[14]*m[3]-m[13]*m[4])*m[9]+m[11]*(m[11]*m[3]-m[10]*m[4])+(m[10]*m[14]-m[11]*m[13])*m[2])
+           +m[6]*(m[11]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[14]*m[7]-m[13]*m[8])-(m[14]*m[3]-m[13]*m[4])*m[6]))
+    +m[4]*(-m[7]*((m[3]*m[8]-m[4]*m[7])*m[9]+m[2]*(m[11]*m[7]-m[10]*m[8])-(m[11]*m[3]-m[10]*m[4])*m[6])
+           +m[1]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8])
+                  +(m[10]*m[13]-m[11]*m[12])*m[6])
+           -m[5]*(-(m[13]*m[3]-m[12]*m[4])*m[9]+m[10]*(m[11]*m[3]-m[10]*m[4])
+                  +(m[10]*m[13]-m[11]*m[12])*m[2])
+           +m[6]*(m[10]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[13]*m[7]-m[12]*m[8])-(m[13]*m[3]-m[12]*m[4])*m[6]))
+    +m[0]*(m[7]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])
+                 +(m[10]*m[14]-m[11]*m[13])*m[6])
+           -m[8]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8])
+                  +(m[10]*m[13]-m[11]*m[12])*m[6])
+           +m[5]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])
+                  +m[11]*(m[10]*m[13]-m[11]*m[12]))
+           -m[6]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])
+                  +(m[12]*m[14]-m[13]*m[13])*m[6]))
+    -m[1]*(m[7]*(-(m[14]*m[3]-m[13]*m[4])*m[9]+m[11]*(m[11]*m[3]-m[10]*m[4])
+                 +(m[10]*m[14]-m[11]*m[13])*m[2])
+           -m[8]*(-(m[13]*m[3]-m[12]*m[4])*m[9]+m[10]*(m[11]*m[3]-m[10]*m[4])
+                  +(m[10]*m[13]-m[11]*m[12])*m[2])
+           +m[1]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])
+                  +m[11]*(m[10]*m[13]-m[11]*m[12]))
+           -(-m[10]*(m[14]*m[3]-m[13]*m[4])+m[11]*(m[13]*m[3]-m[12]*m[4])+(m[12]*m[14]-m[13]*m[13])*m[2])
+           *m[6])
+    +m[2]*(m[7]*(m[11]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[14]*m[7]-m[13]*m[8])-(m[14]*m[3]-m[13]*m[4])*m[6])
+           -m[8]*(m[10]*(m[3]*m[8]-m[4]*m[7])+m[2]*(m[13]*m[7]-m[12]*m[8])-(m[13]*m[3]-m[12]*m[4])*m[6])
+           +m[1]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[6])
+           -(-m[10]*(m[14]*m[3]-m[13]*m[4])+m[11]*(m[13]*m[3]-m[12]*m[4])+(m[12]*m[14]-m[13]*m[13])*m[2])
+           *m[5]);
+}
+#endif
+
+/* I now do inverses. I return transposed cofactors. Original matrix * cofactors / det = identity
+As before, I replaced  m.^2 -> m.*m, m. -> m[.]
+I also removed the lower triangle, since I'm dealing with symmetric matrices here
+Session:
+
+
+(%i64) num( ev(invert(sym2),detout) );
+
+(%o64) matrix([m2,-m1],[-m1,m0])
+(%i65) num( ev(invert(sym3),detout) );
+
+(%o65) matrix([m3*m5-m4^2,m2*m4-m1*m5,m1*m4-m2*m3],
+              [m2*m4-m1*m5,m0*m5-m2^2,m1*m2-m0*m4],
+              [m1*m4-m2*m3,m1*m2-m0*m4,m0*m3-m1^2])
+(%i66) num( ev(invert(sym4),detout) );
+
+(%o66) matrix([m4*(m7*m9-m8^2)-m5*(m5*m9-m6*m8)+m6*(m5*m8-m6*m7),
+               -m1*(m7*m9-m8^2)+m2*(m5*m9-m6*m8)-m3*(m5*m8-m6*m7),
+               m1*(m5*m9-m6*m8)-m2*(m4*m9-m6^2)+m3*(m4*m8-m5*m6),
+               -m1*(m5*m8-m6*m7)+m2*(m4*m8-m5*m6)-m3*(m4*m7-m5^2)],
+              [-m1*(m7*m9-m8^2)+m5*(m2*m9-m3*m8)-m6*(m2*m8-m3*m7),
+               m0*(m7*m9-m8^2)-m2*(m2*m9-m3*m8)+m3*(m2*m8-m3*m7),
+               -m0*(m5*m9-m6*m8)+m2*(m1*m9-m3*m6)-m3*(m1*m8-m3*m5),
+               m0*(m5*m8-m6*m7)-m2*(m1*m8-m2*m6)+m3*(m1*m7-m2*m5)],
+              [m1*(m5*m9-m6*m8)-m4*(m2*m9-m3*m8)+m6*(m2*m6-m3*m5),
+               -m0*(m5*m9-m6*m8)+m1*(m2*m9-m3*m8)-m3*(m2*m6-m3*m5),
+               m0*(m4*m9-m6^2)-m1*(m1*m9-m3*m6)+m3*(m1*m6-m3*m4),
+               -m0*(m4*m8-m5*m6)+m1*(m1*m8-m2*m6)-m3*(m1*m5-m2*m4)],
+              [-m1*(m5*m8-m6*m7)+m4*(m2*m8-m3*m7)-m5*(m2*m6-m3*m5),
+               m0*(m5*m8-m6*m7)-m1*(m2*m8-m3*m7)+m2*(m2*m6-m3*m5),
+               -m0*(m4*m8-m5*m6)+m1*(m1*m8-m3*m5)-m2*(m1*m6-m3*m4),
+               m0*(m4*m7-m5^2)-m1*(m1*m7-m2*m5)+m2*(m1*m5-m2*m4)])
+(%i103) num( ev(invert(sym5),detout) );
+
+(%o103) matrix([m7*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8)
+                                       +(m10*m14-m11*m13)*m6)
+                 -m8*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8)
+                                         +(m10*m13-m11*m12)*m6)
+                 +m5*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13)
+                                        +m11*(m10*m13-m11*m12))
+                 -m6*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8)
+                                          +(m12*m14-m13^2)*m6),
+                -m3*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8)
+                                        +(m10*m14-m11*m13)*m6)
+                 +m4*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8)
+                                         +(m10*m13-m11*m12)*m6)
+                 -m1*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13)
+                                        +m11*(m10*m13-m11*m12))
+                 +m2*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8)
+                                          +(m12*m14-m13^2)*m6),
+                -m2*(-m7*(m14*m7-m13*m8)+m8*(m13*m7-m12*m8)
+                                        +(m12*m14-m13^2)*m5)
+                 +m3*(-m6*(m14*m7-m13*m8)+m8*(m11*m7-m10*m8)
+                                         +(m10*m14-m11*m13)*m5)
+                 -m4*(-m6*(m13*m7-m12*m8)+m7*(m11*m7-m10*m8)
+                                         +(m10*m13-m11*m12)*m5)
+                 +m1*((m10*m13-m11*m12)*m8-(m10*m14-m11*m13)*m7
+                                          +(m12*m14-m13^2)*m6),
+                -m3*(m8*(m11*m6-m8*m9)+m5*(m14*m9-m11^2)-m6*(m14*m6-m11*m8))
+                 +m4*(m7*(m11*m6-m8*m9)+m5*(m13*m9-m10*m11)
+                                       -m6*(m13*m6-m10*m8))
+                 -m1*(-m7*(m14*m9-m11^2)+m8*(m13*m9-m10*m11)
+                                        +(m10*m14-m11*m13)*m6)
+                 +m2*(-m7*(m14*m6-m11*m8)+m8*(m13*m6-m10*m8)
+                                         +(m10*m14-m11*m13)*m5),
+                m3*(m8*(m10*m6-m7*m9)+m5*(m13*m9-m10*m11)-m6*(m13*m6-m11*m7))
+                 -m4*(m7*(m10*m6-m7*m9)+m5*(m12*m9-m10^2)-m6*(m12*m6-m10*m7))
+                 +m1*(-m7*(m13*m9-m10*m11)+m8*(m12*m9-m10^2)
+                                          +(m10*m13-m11*m12)*m6)
+                 -m2*((m12*m6-m10*m7)*m8-m7*(m13*m6-m11*m7)
+                                        +(m10*m13-m11*m12)*m5)],
+               [-m7*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4)
+                                        +(m10*m14-m11*m13)*m2)
+                 +m8*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4)
+                                         +(m10*m13-m11*m12)*m2)
+                 -m1*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13)
+                                        +m11*(m10*m13-m11*m12))
+                 +(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)
+                                       +(m12*m14-m13^2)*m2)
+                  *m6,
+                m3*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4)
+                                       +(m10*m14-m11*m13)*m2)
+                 -m4*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4)
+                                         +(m10*m13-m11*m12)*m2)
+                 +m0*((m12*m14-m13^2)*m9-m10*(m10*m14-m11*m13)
+                                        +m11*(m10*m13-m11*m12))
+                 -m2*(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)
+                                          +(m12*m14-m13^2)*m2),
+                m2*((m13*m3-m12*m4)*m8-(m14*m3-m13*m4)*m7+m1*(m12*m14-m13^2))
+                 -m3*((m11*m3-m10*m4)*m8-(m14*m3-m13*m4)*m6
+                                        +m1*(m10*m14-m11*m13))
+                 -m0*((m10*m13-m11*m12)*m8-(m10*m14-m11*m13)*m7
+                                          +(m12*m14-m13^2)*m6)
+                 +m4*((m11*m3-m10*m4)*m7-(m13*m3-m12*m4)*m6
+                                        +m1*(m10*m13-m11*m12)),
+                m3*(m8*(m11*m2-m4*m9)+m1*(m14*m9-m11^2)-(m14*m2-m11*m4)*m6)
+                 -m4*(m7*(m11*m2-m4*m9)+m1*(m13*m9-m10*m11)
+                                       -(m13*m2-m10*m4)*m6)
+                 +m0*(-m7*(m14*m9-m11^2)+m8*(m13*m9-m10*m11)
+                                        +(m10*m14-m11*m13)*m6)
+                 -m2*((m13*m2-m10*m4)*m8-(m14*m2-m11*m4)*m7
+                                        +m1*(m10*m14-m11*m13)),
+                -m3*(m8*(m10*m2-m3*m9)+m1*(m13*m9-m10*m11)-(m13*m2-m11*m3)*m6)
+                 +m4*(m7*(m10*m2-m3*m9)+m1*(m12*m9-m10^2)-(m12*m2-m10*m3)*m6)
+                 -m0*(-m7*(m13*m9-m10*m11)+m8*(m12*m9-m10^2)
+                                          +(m10*m13-m11*m12)*m6)
+                 +m2*((m12*m2-m10*m3)*m8-(m13*m2-m11*m3)*m7
+                                        +m1*(m10*m13-m11*m12))],
+               [m7*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6)
+                 -m8*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6)
+                 +m1*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8)
+                                          +(m12*m14-m13^2)*m6)
+                 -(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)
+                                       +(m12*m14-m13^2)*m2)
+                  *m5,
+                -m3*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m6)
+                 +m4*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m6)
+                 -m0*(-m10*(m14*m7-m13*m8)+m11*(m13*m7-m12*m8)
+                                          +(m12*m14-m13^2)*m6)
+                 +m1*(-m10*(m14*m3-m13*m4)+m11*(m13*m3-m12*m4)
+                                          +(m12*m14-m13^2)*m2),
+                m3*(m8*(m3*m8-m4*m7)+m1*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m5)
+                 -m4*(m7*(m3*m8-m4*m7)+m1*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m5)
+                 +m0*(-m7*(m14*m7-m13*m8)+m8*(m13*m7-m12*m8)
+                                         +(m12*m14-m13^2)*m5)
+                 -m1*((m13*m3-m12*m4)*m8-(m14*m3-m13*m4)*m7
+                                        +m1*(m12*m14-m13^2)),
+                -m3*(m8*(m2*m8-m4*m6)+m1*(m14*m6-m11*m8)-(m14*m2-m11*m4)*m5)
+                 +m4*(m7*(m2*m8-m4*m6)+m1*(m13*m6-m10*m8)-(m13*m2-m10*m4)*m5)
+                 -m0*(-m7*(m14*m6-m11*m8)+m8*(m13*m6-m10*m8)
+                                         +(m10*m14-m11*m13)*m5)
+                 +m1*((m13*m2-m10*m4)*m8-(m14*m2-m11*m4)*m7
+                                        +m1*(m10*m14-m11*m13)),
+                m3*((m2*m7-m3*m6)*m8+m1*(m13*m6-m11*m7)-(m13*m2-m11*m3)*m5)
+                 +m0*((m12*m6-m10*m7)*m8-m7*(m13*m6-m11*m7)
+                                        +(m10*m13-m11*m12)*m5)
+                 -m1*((m12*m2-m10*m3)*m8-(m13*m2-m11*m3)*m7
+                                        +m1*(m10*m13-m11*m12))
+                 -m4*(m7*(m2*m7-m3*m6)+m1*(m12*m6-m10*m7)
+                                      -(m12*m2-m10*m3)*m5)],
+               [m8*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6)
+                 -m1*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8)
+                                         +(m10*m14-m11*m13)*m6)
+                 +m5*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4)
+                                         +(m10*m14-m11*m13)*m2)
+                 -m6*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)
+                                       -(m14*m3-m13*m4)*m6),
+                -m4*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6)
+                 +m0*(-(m14*m7-m13*m8)*m9+m11*(m11*m7-m10*m8)
+                                         +(m10*m14-m11*m13)*m6)
+                 -m1*(-(m14*m3-m13*m4)*m9+m11*(m11*m3-m10*m4)
+                                         +(m10*m14-m11*m13)*m2)
+                 +m2*(m11*(m3*m8-m4*m7)+m2*(m14*m7-m13*m8)
+                                       -(m14*m3-m13*m4)*m6),
+                -m2*(m8*(m3*m8-m4*m7)+m1*(m14*m7-m13*m8)-(m14*m3-m13*m4)*m5)
+                 +m4*(m6*(m3*m8-m4*m7)+m1*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m5)
+                 -m0*(-m6*(m14*m7-m13*m8)+m8*(m11*m7-m10*m8)
+                                         +(m10*m14-m11*m13)*m5)
+                 +m1*((m11*m3-m10*m4)*m8-(m14*m3-m13*m4)*m6
+                                        +m1*(m10*m14-m11*m13)),
+                m0*(m8*(m11*m6-m8*m9)+m5*(m14*m9-m11^2)-m6*(m14*m6-m11*m8))
+                 -m4*(m1*(m11*m6-m8*m9)-m5*(m11*m2-m4*m9)+m6*(m2*m8-m4*m6))
+                 -m1*(m8*(m11*m2-m4*m9)+m1*(m14*m9-m11^2)-(m14*m2-m11*m4)*m6)
+                 +m2*(m8*(m2*m8-m4*m6)+m1*(m14*m6-m11*m8)-(m14*m2-m11*m4)*m5),
+                -m0*(m8*(m10*m6-m7*m9)+m5*(m13*m9-m10*m11)-m6*(m13*m6-m11*m7))
+                 +m4*(m1*(m10*m6-m7*m9)-m5*(m10*m2-m3*m9)+m6*(m2*m7-m3*m6))
+                 +m1*(m8*(m10*m2-m3*m9)+m1*(m13*m9-m10*m11)
+                                       -(m13*m2-m11*m3)*m6)
+                 -m2*((m2*m7-m3*m6)*m8+m1*(m13*m6-m11*m7)
+                                      -(m13*m2-m11*m3)*m5)],
+               [-m7*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6)
+                 +m1*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8)
+                                         +(m10*m13-m11*m12)*m6)
+                 -m5*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4)
+                                         +(m10*m13-m11*m12)*m2)
+                 +m6*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)
+                                       -(m13*m3-m12*m4)*m6),
+                m3*((m3*m8-m4*m7)*m9+m2*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m6)
+                 -m0*(-(m13*m7-m12*m8)*m9+m10*(m11*m7-m10*m8)
+                                         +(m10*m13-m11*m12)*m6)
+                 +m1*(-(m13*m3-m12*m4)*m9+m10*(m11*m3-m10*m4)
+                                         +(m10*m13-m11*m12)*m2)
+                 -m2*(m10*(m3*m8-m4*m7)+m2*(m13*m7-m12*m8)
+                                       -(m13*m3-m12*m4)*m6),
+                m2*(m7*(m3*m8-m4*m7)+m1*(m13*m7-m12*m8)-(m13*m3-m12*m4)*m5)
+                 -m3*(m6*(m3*m8-m4*m7)+m1*(m11*m7-m10*m8)-(m11*m3-m10*m4)*m5)
+                 +m0*(-m6*(m13*m7-m12*m8)+m7*(m11*m7-m10*m8)
+                                         +(m10*m13-m11*m12)*m5)
+                 -m1*((m11*m3-m10*m4)*m7-(m13*m3-m12*m4)*m6
+                                        +m1*(m10*m13-m11*m12)),
+                -m0*(m7*(m11*m6-m8*m9)+m5*(m13*m9-m10*m11)-m6*(m13*m6-m10*m8))
+                 +m3*(m1*(m11*m6-m8*m9)-m5*(m11*m2-m4*m9)+m6*(m2*m8-m4*m6))
+                 +m1*(m7*(m11*m2-m4*m9)+m1*(m13*m9-m10*m11)
+                                       -(m13*m2-m10*m4)*m6)
+                 -m2*(m7*(m2*m8-m4*m6)+m1*(m13*m6-m10*m8)-(m13*m2-m10*m4)*m5),
+                m0*(m7*(m10*m6-m7*m9)+m5*(m12*m9-m10^2)-m6*(m12*m6-m10*m7))
+                 -m3*(m1*(m10*m6-m7*m9)-m5*(m10*m2-m3*m9)+m6*(m2*m7-m3*m6))
+                 -m1*(m7*(m10*m2-m3*m9)+m1*(m12*m9-m10^2)-(m12*m2-m10*m3)*m6)
+                 +m2*(m7*(m2*m7-m3*m6)+m1*(m12*m6-m10*m7)
+                                      -(m12*m2-m10*m3)*m5)])
+*/
+
+static inline double cofactors_sym2(const double* restrict m, double* restrict c)
+{
+  c[0] = m[2];
+  c[1] = -m[1];
+  c[2] = -m[1];
+
+  return m[0]*c[0] + m[1]*c[1];
+}
+
+static inline double cofactors_sym3(const double* restrict m, double* restrict c)
+{
+  c[0] = m[3]*m[5]-m[4]*m[4];
+  c[1] = m[2]*m[4]-m[1]*m[5];
+  c[2] = m[1]*m[4]-m[2]*m[3];
+  c[3] = m[0]*m[5]-m[2]*m[2];
+  c[4] = m[1]*m[2]-m[0]*m[4];
+  c[5] = m[0]*m[3]-m[1]*m[1];
+
+  return m[0]*c[0] + m[1]*c[1] + m[2]*c[2];
+}
+
+static inline double cofactors_sym4(const double* restrict m, double* restrict c)
+{
+  c[0] = m[4]*(m[7]*m[9]-m[8]*m[8])-m[5]*(m[5]*m[9]-m[6]*m[8])+m[6]*(m[5]*m[8]-m[6]*m[7]);
+  c[1] = -m[1]*(m[7]*m[9]-m[8]*m[8])+m[2]*(m[5]*m[9]-m[6]*m[8])-m[3]*(m[5]*m[8]-m[6]*m[7]);
+  c[2] = m[1]*(m[5]*m[9]-m[6]*m[8])-m[2]*(m[4]*m[9]-m[6]*m[6])+m[3]*(m[4]*m[8]-m[5]*m[6]);
+  c[3] = -m[1]*(m[5]*m[8]-m[6]*m[7])+m[2]*(m[4]*m[8]-m[5]*m[6])-m[3]*(m[4]*m[7]-m[5]*m[5]);
+  c[4] = m[0]*(m[7]*m[9]-m[8]*m[8])-m[2]*(m[2]*m[9]-m[3]*m[8])+m[3]*(m[2]*m[8]-m[3]*m[7]);
+  c[5] = -m[0]*(m[5]*m[9]-m[6]*m[8])+m[2]*(m[1]*m[9]-m[3]*m[6])-m[3]*(m[1]*m[8]-m[3]*m[5]);
+  c[6] = m[0]*(m[5]*m[8]-m[6]*m[7])-m[2]*(m[1]*m[8]-m[2]*m[6])+m[3]*(m[1]*m[7]-m[2]*m[5]);
+  c[7] = m[0]*(m[4]*m[9]-m[6]*m[6])-m[1]*(m[1]*m[9]-m[3]*m[6])+m[3]*(m[1]*m[6]-m[3]*m[4]);
+  c[8] = -m[0]*(m[4]*m[8]-m[5]*m[6])+m[1]*(m[1]*m[8]-m[2]*m[6])-m[3]*(m[1]*m[5]-m[2]*m[4]);
+  c[9] = m[0]*(m[4]*m[7]-m[5]*m[5])-m[1]*(m[1]*m[7]-m[2]*m[5])+m[2]*(m[1]*m[5]-m[2]*m[4]);
+
+  return m[0]*c[0] + m[1]*c[1] + m[2]*c[2] + m[3]*c[3];
+}
+
+static inline double cofactors_sym5(const double* restrict m, double* restrict c)
+{
+  c[0] =   m[7]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[6])-m[8]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[13]-m[11]*m[12])*m[6])+m[5]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])+m[11]*(m[10]*m[13]-m[11]*m[12]))-m[6]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[6]);
+  c[1] =   -m[3]*(-(m[14]*m[7]-m[13]*m[8])*m[9]+m[11]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[6])+m[4]*(-(m[13]*m[7]-m[12]*m[8])*m[9]+m[10]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[13]-m[11]*m[12])*m[6])-m[1]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])+m[11]*(m[10]*m[13]-m[11]*m[12]))+m[2]*(-m[10]*(m[14]*m[7]-m[13]*m[8])+m[11]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[6]);
+  c[2] =   -m[2]*(-m[7]*(m[14]*m[7]-m[13]*m[8])+m[8]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[5])+m[3]*(-m[6]*(m[14]*m[7]-m[13]*m[8])+m[8]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[5])-m[4]*(-m[6]*(m[13]*m[7]-m[12]*m[8])+m[7]*(m[11]*m[7]-m[10]*m[8])+(m[10]*m[13]-m[11]*m[12])*m[5])+m[1]*((m[10]*m[13]-m[11]*m[12])*m[8]-(m[10]*m[14]-m[11]*m[13])*m[7]+(m[12]*m[14]-m[13]*m[13])*m[6]);
+  c[3] =   -m[3]*(m[8]*(m[11]*m[6]-m[8]*m[9])+m[5]*(m[14]*m[9]-m[11]*m[11])-m[6]*(m[14]*m[6]-m[11]*m[8]))+m[4]*(m[7]*(m[11]*m[6]-m[8]*m[9])+m[5]*(m[13]*m[9]-m[10]*m[11])-m[6]*(m[13]*m[6]-m[10]*m[8]))-m[1]*(-m[7]*(m[14]*m[9]-m[11]*m[11])+m[8]*(m[13]*m[9]-m[10]*m[11])+(m[10]*m[14]-m[11]*m[13])*m[6])+m[2]*(-m[7]*(m[14]*m[6]-m[11]*m[8])+m[8]*(m[13]*m[6]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[5]);
+  c[4] =   m[3]*(m[8]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[13]*m[9]-m[10]*m[11])-m[6]*(m[13]*m[6]-m[11]*m[7]))-m[4]*(m[7]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[12]*m[9]-m[10]*m[10])-m[6]*(m[12]*m[6]-m[10]*m[7]))+m[1]*(-m[7]*(m[13]*m[9]-m[10]*m[11])+m[8]*(m[12]*m[9]-m[10]*m[10])+(m[10]*m[13]-m[11]*m[12])*m[6])-m[2]*((m[12]*m[6]-m[10]*m[7])*m[8]-m[7]*(m[13]*m[6]-m[11]*m[7])+(m[10]*m[13]-m[11]*m[12])*m[5]);
+  c[5] =   m[3]*(-(m[14]*m[3]-m[13]*m[4])*m[9]+m[11]*(m[11]*m[3]-m[10]*m[4])+(m[10]*m[14]-m[11]*m[13])*m[2])-m[4]*(-(m[13]*m[3]-m[12]*m[4])*m[9]+m[10]*(m[11]*m[3]-m[10]*m[4])+(m[10]*m[13]-m[11]*m[12])*m[2])+m[0]*((m[12]*m[14]-m[13]*m[13])*m[9]-m[10]*(m[10]*m[14]-m[11]*m[13])+m[11]*(m[10]*m[13]-m[11]*m[12]))-m[2]*(-m[10]*(m[14]*m[3]-m[13]*m[4])+m[11]*(m[13]*m[3]-m[12]*m[4])+(m[12]*m[14]-m[13]*m[13])*m[2]);
+  c[6] =   m[2]*((m[13]*m[3]-m[12]*m[4])*m[8]-(m[14]*m[3]-m[13]*m[4])*m[7]+m[1]*(m[12]*m[14]-m[13]*m[13]))-m[3]*((m[11]*m[3]-m[10]*m[4])*m[8]-(m[14]*m[3]-m[13]*m[4])*m[6]+m[1]*(m[10]*m[14]-m[11]*m[13]))-m[0]*((m[10]*m[13]-m[11]*m[12])*m[8]-(m[10]*m[14]-m[11]*m[13])*m[7]+(m[12]*m[14]-m[13]*m[13])*m[6])+m[4]*((m[11]*m[3]-m[10]*m[4])*m[7]-(m[13]*m[3]-m[12]*m[4])*m[6]+m[1]*(m[10]*m[13]-m[11]*m[12]));
+  c[7] =   m[3]*(m[8]*(m[11]*m[2]-m[4]*m[9])+m[1]*(m[14]*m[9]-m[11]*m[11])-(m[14]*m[2]-m[11]*m[4])*m[6])-m[4]*(m[7]*(m[11]*m[2]-m[4]*m[9])+m[1]*(m[13]*m[9]-m[10]*m[11])-(m[13]*m[2]-m[10]*m[4])*m[6])+m[0]*(-m[7]*(m[14]*m[9]-m[11]*m[11])+m[8]*(m[13]*m[9]-m[10]*m[11])+(m[10]*m[14]-m[11]*m[13])*m[6])-m[2]*((m[13]*m[2]-m[10]*m[4])*m[8]-(m[14]*m[2]-m[11]*m[4])*m[7]+m[1]*(m[10]*m[14]-m[11]*m[13]));
+  c[8] =   -m[3]*(m[8]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[13]*m[9]-m[10]*m[11])-(m[13]*m[2]-m[11]*m[3])*m[6])+m[4]*(m[7]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[12]*m[9]-m[10]*m[10])-(m[12]*m[2]-m[10]*m[3])*m[6])-m[0]*(-m[7]*(m[13]*m[9]-m[10]*m[11])+m[8]*(m[12]*m[9]-m[10]*m[10])+(m[10]*m[13]-m[11]*m[12])*m[6])+m[2]*((m[12]*m[2]-m[10]*m[3])*m[8]-(m[13]*m[2]-m[11]*m[3])*m[7]+m[1]*(m[10]*m[13]-m[11]*m[12]));
+  c[9] =   m[3]*(m[8]*(m[3]*m[8]-m[4]*m[7])+m[1]*(m[14]*m[7]-m[13]*m[8])-(m[14]*m[3]-m[13]*m[4])*m[5])-m[4]*(m[7]*(m[3]*m[8]-m[4]*m[7])+m[1]*(m[13]*m[7]-m[12]*m[8])-(m[13]*m[3]-m[12]*m[4])*m[5])+m[0]*(-m[7]*(m[14]*m[7]-m[13]*m[8])+m[8]*(m[13]*m[7]-m[12]*m[8])+(m[12]*m[14]-m[13]*m[13])*m[5])-m[1]*((m[13]*m[3]-m[12]*m[4])*m[8]-(m[14]*m[3]-m[13]*m[4])*m[7]+m[1]*(m[12]*m[14]-m[13]*m[13]));
+  c[10] =   -m[3]*(m[8]*(m[2]*m[8]-m[4]*m[6])+m[1]*(m[14]*m[6]-m[11]*m[8])-(m[14]*m[2]-m[11]*m[4])*m[5])+m[4]*(m[7]*(m[2]*m[8]-m[4]*m[6])+m[1]*(m[13]*m[6]-m[10]*m[8])-(m[13]*m[2]-m[10]*m[4])*m[5])-m[0]*(-m[7]*(m[14]*m[6]-m[11]*m[8])+m[8]*(m[13]*m[6]-m[10]*m[8])+(m[10]*m[14]-m[11]*m[13])*m[5])+m[1]*((m[13]*m[2]-m[10]*m[4])*m[8]-(m[14]*m[2]-m[11]*m[4])*m[7]+m[1]*(m[10]*m[14]-m[11]*m[13]));
+  c[11] =   m[3]*((m[2]*m[7]-m[3]*m[6])*m[8]+m[1]*(m[13]*m[6]-m[11]*m[7])-(m[13]*m[2]-m[11]*m[3])*m[5])+m[0]*((m[12]*m[6]-m[10]*m[7])*m[8]-m[7]*(m[13]*m[6]-m[11]*m[7])+(m[10]*m[13]-m[11]*m[12])*m[5])-m[1]*((m[12]*m[2]-m[10]*m[3])*m[8]-(m[13]*m[2]-m[11]*m[3])*m[7]+m[1]*(m[10]*m[13]-m[11]*m[12]))-m[4]*(m[7]*(m[2]*m[7]-m[3]*m[6])+m[1]*(m[12]*m[6]-m[10]*m[7])-(m[12]*m[2]-m[10]*m[3])*m[5]);
+  c[12] =   m[0]*(m[8]*(m[11]*m[6]-m[8]*m[9])+m[5]*(m[14]*m[9]-m[11]*m[11])-m[6]*(m[14]*m[6]-m[11]*m[8]))-m[4]*(m[1]*(m[11]*m[6]-m[8]*m[9])-m[5]*(m[11]*m[2]-m[4]*m[9])+m[6]*(m[2]*m[8]-m[4]*m[6]))-m[1]*(m[8]*(m[11]*m[2]-m[4]*m[9])+m[1]*(m[14]*m[9]-m[11]*m[11])-(m[14]*m[2]-m[11]*m[4])*m[6])+m[2]*(m[8]*(m[2]*m[8]-m[4]*m[6])+m[1]*(m[14]*m[6]-m[11]*m[8])-(m[14]*m[2]-m[11]*m[4])*m[5]);
+  c[13] =   -m[0]*(m[8]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[13]*m[9]-m[10]*m[11])-m[6]*(m[13]*m[6]-m[11]*m[7]))+m[4]*(m[1]*(m[10]*m[6]-m[7]*m[9])-m[5]*(m[10]*m[2]-m[3]*m[9])+m[6]*(m[2]*m[7]-m[3]*m[6]))+m[1]*(m[8]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[13]*m[9]-m[10]*m[11])-(m[13]*m[2]-m[11]*m[3])*m[6])-m[2]*((m[2]*m[7]-m[3]*m[6])*m[8]+m[1]*(m[13]*m[6]-m[11]*m[7])-(m[13]*m[2]-m[11]*m[3])*m[5]);
+  c[14] =   m[0]*(m[7]*(m[10]*m[6]-m[7]*m[9])+m[5]*(m[12]*m[9]-m[10]*m[10])-m[6]*(m[12]*m[6]-m[10]*m[7]))-m[3]*(m[1]*(m[10]*m[6]-m[7]*m[9])-m[5]*(m[10]*m[2]-m[3]*m[9])+m[6]*(m[2]*m[7]-m[3]*m[6]))-m[1]*(m[7]*(m[10]*m[2]-m[3]*m[9])+m[1]*(m[12]*m[9]-m[10]*m[10])-(m[12]*m[2]-m[10]*m[3])*m[6])+m[2]*(m[7]*(m[2]*m[7]-m[3]*m[6])+m[1]*(m[12]*m[6]-m[10]*m[7])-(m[12]*m[2]-m[10]*m[3])*m[5]);
+
+  return m[0]*c[0] + m[1]*c[1] + m[2]*c[2] + m[3]*c[3] + m[4]*c[4];
+}
+
+/*
+The upper-triangular and lower-triangular routines have a similar API to the
+symmetric ones. Note that as with symmetric matrices, we don't store redundant
+data, and we store data row-first. So the upper-triangular matrices have N
+elements in the first row in memory, but lower-triangular matrices have only 1.
+
+Inverses of triangular matrices are similarly triangular, and that's how I store
+them
+
+
+Session:
+
+// upper triangular
+(%i2) ut2 : matrix([m0,m1],[0,m2]);
+
+(%o2) matrix([m0,m1],[0,m2])
+(%i3) ut3 : matrix([m0,m1,m2],[0,m3,m4],[0,0,m5]);
+
+(%o3) matrix([m0,m1,m2],[0,m3,m4],[0,0,m5])
+(%i4) ut4 : matrix([m0,m1,m2,m3],[0,m4,m5,m6],[0,0,m7,m8],[0,0,0,m9]);
+
+(%o4) matrix([m0,m1,m2,m3],[0,m4,m5,m6],[0,0,m7,m8],[0,0,0,m9])
+(%i5) ut5 : matrix([m0,m1,m2,m3,m4],[0,m5,m6,m7,m8],[0,0,m9,m10,m11],[0,0,0,m12,m13],[0,0,0,0,m14]);
+
+(%o5) matrix([m0,m1,m2,m3,m4],[0,m5,m6,m7,m8],[0,0,m9,m10,m11],
+             [0,0,0,m12,m13],[0,0,0,0,m14])
+
+(%i11) num( ev(invert(ut2),detout) );
+
+(%o11) matrix([m2,-m1],[0,m0])
+(%i12) num( ev(invert(ut3),detout) );
+
+(%o12) matrix([m3*m5,-m1*m5,m1*m4-m2*m3],[0,m0*m5,-m0*m4],[0,0,m0*m3])
+(%i13) num( ev(invert(ut4),detout) );
+
+(%o13) matrix([m4*m7*m9,-m1*m7*m9,m1*m5*m9-m2*m4*m9,
+               (-m1*(m5*m8-m6*m7))+m2*m4*m8-m3*m4*m7],
+              [0,m0*m7*m9,-m0*m5*m9,m0*(m5*m8-m6*m7)],
+              [0,0,m0*m4*m9,-m0*m4*m8],[0,0,0,m0*m4*m7])
+(%i14) num( ev(invert(ut5),detout) );
+
+(%o14) matrix([m12*m14*m5*m9,-m1*m12*m14*m9,m1*m12*m14*m6-m12*m14*m2*m5,
+               (-m1*(m10*m14*m6-m14*m7*m9))-m14*m3*m5*m9+m10*m14*m2*m5,
+               m1*(m12*m8*m9-m13*m7*m9+(m10*m13-m11*m12)*m6)
+                -m12*m4*m5*m9+m13*m3*m5*m9-(m10*m13-m11*m12)*m2*m5],
+              [0,m0*m12*m14*m9,-m0*m12*m14*m6,m0*(m10*m14*m6-m14*m7*m9),
+               -m0*(m12*m8*m9-m13*m7*m9+(m10*m13-m11*m12)*m6)],
+              [0,0,m0*m12*m14*m5,-m0*m10*m14*m5,m0*(m10*m13-m11*m12)*m5],
+              [0,0,0,m0*m14*m5*m9,-m0*m13*m5*m9],[0,0,0,0,m0*m12*m5*m9])
+
+
+// lower-triangular
+(%i19) lt2 : matrix([m0,0],[m1,m2]);
+
+(%o19) matrix([m0,0],[m1,m2])
+(%i20) lt3 : matrix([m0,0,0],[m1,m2,0],[m3,m4,m5]);
+
+(%o20) matrix([m0,0,0],[m1,m2,0],[m3,m4,m5])
+(%i21) lt4 : matrix([m0,0,0,0],[m1,m2,0,0],[m3,m4,m5,0],[m6,m7,m8,m9]);
+
+(%o21) matrix([m0,0,0,0],[m1,m2,0,0],[m3,m4,m5,0],[m6,m7,m8,m9])
+(%i22) lt5 : matrix([m0,0,0,0,0],[m1,m2,0,0,0],[m3,m4,m5,0,0],[m6,m7,m8,m9,0],[m10,m11,m12,m13,m14]);
+
+(%o22) matrix([m0,0,0,0,0],[m1,m2,0,0,0],[m3,m4,m5,0,0],[m6,m7,m8,m9,0],
+              [m10,m11,m12,m13,m14])
+(%i23) num( ev(invert(lt2),detout) );
+
+(%o23) matrix([m2,0],[-m1,m0])
+(%i24) num( ev(invert(lt3),detout) );
+
+(%o24) matrix([m2*m5,0,0],[-m1*m5,m0*m5,0],[m1*m4-m2*m3,-m0*m4,m0*m2])
+(%i25) num( ev(invert(lt4),detout) );
+
+(%o25) matrix([m2*m5*m9,0,0,0],[-m1*m5*m9,m0*m5*m9,0,0],
+              [m1*m4*m9-m2*m3*m9,-m0*m4*m9,m0*m2*m9,0],
+              [m2*(m3*m8-m5*m6)-m1*(m4*m8-m5*m7),m0*(m4*m8-m5*m7),-m0*m2*m8,
+               m0*m2*m5])
+(%i26) num( ev(invert(lt5),detout) );
+
+(%o26) matrix([m14*m2*m5*m9,0,0,0,0],[-m1*m14*m5*m9,m0*m14*m5*m9,0,0,0],
+              [m1*m14*m4*m9-m14*m2*m3*m9,-m0*m14*m4*m9,m0*m14*m2*m9,0,0],
+              [m2*(m14*m3*m8-m14*m5*m6)-m1*(m14*m4*m8-m14*m5*m7),
+               m0*(m14*m4*m8-m14*m5*m7),-m0*m14*m2*m8,m0*m14*m2*m5,0],
+              [m1*(m4*(m13*m8-m12*m9)-m5*(m13*m7-m11*m9))
+                -m2*(m3*(m13*m8-m12*m9)-m5*(m13*m6-m10*m9)),
+               -m0*(m4*(m13*m8-m12*m9)-m5*(m13*m7-m11*m9)),
+               m0*m2*(m13*m8-m12*m9),-m0*m13*m2*m5,m0*m2*m5*m9])
+
+ */
+static inline double cofactors_ut2(const double* restrict m, double* restrict c)
+{
+    int i=0;
+    c[i++] = m[2];
+    c[i++] = -m[1];
+    c[i++] = m[0];
+    return m[0]*m[2];
+}
+static inline double cofactors_ut3(const double* restrict m, double* restrict c)
+{
+    int i=0;
+    c[i++] = m[3]*m[5];
+    c[i++] = -m[1]*m[5];
+    c[i++] = m[1]*m[4]-m[2]*m[3];
+    c[i++] = m[0]*m[5];
+    c[i++] = -m[0]*m[4];
+    c[i++] = m[0]*m[3];
+    return m[0]*m[3]*m[5];
+}
+static inline double cofactors_ut4(const double* restrict m, double* restrict c)
+{
+    int i=0;
+    c[i++] = m[4]*m[7]*m[9];
+    c[i++] = -m[1]*m[7]*m[9];
+    c[i++] = m[1]*m[5]*m[9]-m[2]*m[4]*m[9];
+    c[i++] = (-m[1]*(m[5]*m[8]-m[6]*m[7]))+m[2]*m[4]*m[8]-m[3]*m[4]*m[7];
+    c[i++] = m[0]*m[7]*m[9];
+    c[i++] = -m[0]*m[5]*m[9];
+    c[i++] = m[0]*(m[5]*m[8]-m[6]*m[7]);
+    c[i++] = m[0]*m[4]*m[9];
+    c[i++] = -m[0]*m[4]*m[8];
+    c[i++] = m[0]*m[4]*m[7];
+    return m[0]*m[4]*m[7]*m[9];
+}
+static inline double cofactors_ut5(const double* restrict m, double* restrict c)
+{
+    int i=0;
+    c[i++] = m[12]*m[14]*m[5]*m[9];
+    c[i++] = -m[1]*m[12]*m[14]*m[9];
+    c[i++] = m[1]*m[12]*m[14]*m[6]-m[12]*m[14]*m[2]*m[5];
+    c[i++] = (-m[1]*(m[10]*m[14]*m[6]-m[14]*m[7]*m[9]))-m[14]*m[3]*m[5]*m[9]+m[10]*m[14]*m[2]*m[5];
+    c[i++] = m[1]*(m[12]*m[8]*m[9]-m[13]*m[7]*m[9]+(m[10]*m[13]-m[11]*m[12])*m[6])-m[12]*m[4]*m[5]*m[9]+m[13]*m[3]*m[5]*m[9]-(m[10]*m[13]-m[11]*m[12])*m[2]*m[5];
+    c[i++] = m[0]*m[12]*m[14]*m[9];
+    c[i++] = -m[0]*m[12]*m[14]*m[6];
+    c[i++] = m[0]*(m[10]*m[14]*m[6]-m[14]*m[7]*m[9]);
+    c[i++] = -m[0]*(m[12]*m[8]*m[9]-m[13]*m[7]*m[9]+(m[10]*m[13]-m[11]*m[12])*m[6]);
+    c[i++] = m[0]*m[12]*m[14]*m[5];
+    c[i++] = -m[0]*m[10]*m[14]*m[5];
+    c[i++] = m[0]*(m[10]*m[13]-m[11]*m[12])*m[5];
+    c[i++] = m[0]*m[14]*m[5]*m[9];
+    c[i++] = -m[0]*m[13]*m[5]*m[9];
+    c[i++] = m[0]*m[12]*m[5]*m[9];
+    return m[0]*m[5]*m[9]*m[12]*m[14];
+}
+static inline double cofactors_lt2(const double* restrict m, double* restrict c)
+{
+    int i=0;
+    c[i++] = m[2];
+    c[i++] = -m[1];
+    c[i++] = m[0];
+    return m[0]*m[2];
+}
+static inline double cofactors_lt3(const double* restrict m, double* restrict c)
+{
+    int i=0;
+    c[i++] = m[2]*m[5];
+    c[i++] = -m[1]*m[5];
+    c[i++] = m[0]*m[5];
+    c[i++] = m[1]*m[4]-m[2]*m[3];
+    c[i++] = -m[0]*m[4];
+    c[i++] = m[0]*m[2];
+    return m[0]*m[2]*m[5];
+}
+static inline double cofactors_lt4(const double* restrict m, double* restrict c)
+{
+    int i=0;
+    c[i++] = m[2]*m[5]*m[9];
+    c[i++] = -m[1]*m[5]*m[9];
+    c[i++] = m[0]*m[5]*m[9];
+    c[i++] = m[1]*m[4]*m[9]-m[2]*m[3]*m[9];
+    c[i++] = -m[0]*m[4]*m[9];
+    c[i++] = m[0]*m[2]*m[9];
+    c[i++] = m[2]*(m[3]*m[8]-m[5]*m[6])-m[1]*(m[4]*m[8]-m[5]*m[7]);
+    c[i++] = m[0]*(m[4]*m[8]-m[5]*m[7]);
+    c[i++] = -m[0]*m[2]*m[8];
+    c[i++] = m[0]*m[2]*m[5];
+    return m[0]*m[2]*m[5]*m[9];
+}
+static inline double cofactors_lt5(const double* restrict m, double* restrict c)
+{
+    int i=0;
+    c[i++] = m[14]*m[2]*m[5]*m[9];
+    c[i++] = -m[1]*m[14]*m[5]*m[9];
+    c[i++] = m[0]*m[14]*m[5]*m[9];
+    c[i++] = m[1]*m[14]*m[4]*m[9]-m[14]*m[2]*m[3]*m[9];
+    c[i++] = -m[0]*m[14]*m[4]*m[9];
+    c[i++] = m[0]*m[14]*m[2]*m[9];
+    c[i++] = m[2]*(m[14]*m[3]*m[8]-m[14]*m[5]*m[6])-m[1]*(m[14]*m[4]*m[8]-m[14]*m[5]*m[7]);
+    c[i++] = m[0]*(m[14]*m[4]*m[8]-m[14]*m[5]*m[7]);
+    c[i++] = -m[0]*m[14]*m[2]*m[8];
+    c[i++] = m[0]*m[14]*m[2]*m[5];
+    c[i++] = m[1]*(m[4]*(m[13]*m[8]-m[12]*m[9])-m[5]*(m[13]*m[7]-m[11]*m[9]))-m[2]*(m[3]*(m[13]*m[8]-m[12]*m[9])-m[5]*(m[13]*m[6]-m[10]*m[9]));
+    c[i++] = -m[0]*(m[4]*(m[13]*m[8]-m[12]*m[9])-m[5]*(m[13]*m[7]-m[11]*m[9]));
+    c[i++] = m[0]*m[2]*(m[13]*m[8]-m[12]*m[9]);
+    c[i++] = -m[0]*m[13]*m[2]*m[5];
+    c[i++] = m[0]*m[2]*m[5]*m[9];
+    return m[0]*m[2]*m[5]*m[9]*m[14];
+}
+
+/*
+(%i27) a : matrix([a0,a1,a2],[0,a3,a4],[0,0,a5]);
+
+(%o27) matrix([a0,a1,a2],[0,a3,a4],[0,0,a5])
+(%i28) b : matrix([b0,b1,b2],[0,b3,b4],[0,0,b5]);
+
+(%o28) matrix([b0,b1,b2],[0,b3,b4],[0,0,b5])
+(%i29) a . b;
+
+(%o29) matrix([a0*b0,a1*b3+a0*b1,a2*b5+a1*b4+a0*b2],[0,a3*b3,a4*b5+a3*b4],[0,0,a5*b5])
+*/
+static inline void mul_ut3_ut3(const double* restrict a, const double* restrict b,
+                               double* restrict ab)
+{
+    ab[0] = a[0] * b[0];
+    ab[1] = a[1] * b[3]+a[0] * b[1];
+    ab[2] = a[2] * b[5]+a[1] * b[4]+a[0] * b[2];
+    ab[3] = a[3] * b[3];
+    ab[4] = a[4] * b[5]+a[3] * b[4];
+    ab[5] = a[5] * b[5];
+}
+
+// symmetrix 3x3 by symmetrix 3x3, written into a new non-symmetric matrix,
+// scaled. This is a special-case function that I needed for something...
+static inline void mul_sym33_sym33_scaled_out(const double* restrict s0, const double* restrict s1, double* restrict mout, double scale)
+{
+// (%i106) matrix([m0_0,m0_1,m0_2],
+//                [m0_1,m0_3,m0_4],
+//                [m0_2,m0_4,m0_5]) .
+//         matrix([m1_0,m1_1,m1_2],
+//                [m1_1,m1_3,m1_4],
+//                [m1_2,m1_4,m1_5]);
+
+// (%o106) matrix([m0_2*m1_2+m0_1*m1_1+m0_0*m1_0,m0_2*m1_4+m0_1*m1_3+m0_0*m1_1,
+//                 m0_2*m1_5+m0_1*m1_4+m0_0*m1_2],
+//                [m0_4*m1_2+m0_3*m1_1+m0_1*m1_0,m0_4*m1_4+m0_3*m1_3+m0_1*m1_1,
+//                 m0_4*m1_5+m0_3*m1_4+m0_1*m1_2],
+//                [m0_5*m1_2+m0_4*m1_1+m0_2*m1_0,m0_5*m1_4+m0_4*m1_3+m0_2*m1_1,
+//                 m0_5*m1_5+m0_4*m1_4+m0_2*m1_2])
+
+  mout[0] = scale * (s0[2]*s1[2]+s0[1]*s1[1]+s0[0]*s1[0]);
+  mout[1] = scale * (s0[2]*s1[4]+s0[1]*s1[3]+s0[0]*s1[1]);
+  mout[2] = scale * (s0[2]*s1[5]+s0[1]*s1[4]+s0[0]*s1[2]);
+  mout[3] = scale * (s0[4]*s1[2]+s0[3]*s1[1]+s0[1]*s1[0]);
+  mout[4] = scale * (s0[4]*s1[4]+s0[3]*s1[3]+s0[1]*s1[1]);
+  mout[5] = scale * (s0[4]*s1[5]+s0[3]*s1[4]+s0[1]*s1[2]);
+  mout[6] = scale * (s0[5]*s1[2]+s0[4]*s1[1]+s0[2]*s1[0]);
+  mout[7] = scale * (s0[5]*s1[4]+s0[4]*s1[3]+s0[2]*s1[1]);
+  mout[8] = scale * (s0[5]*s1[5]+s0[4]*s1[4]+s0[2]*s1[2]);
+}
+
+static inline void outerproduct3(const double* restrict v, double* restrict P)
+{
+  P[0] = v[0]*v[0];
+  P[1] = v[0]*v[1];
+  P[2] = v[0]*v[2];
+  P[3] = v[1]*v[1];
+  P[4] = v[1]*v[2];
+  P[5] = v[2]*v[2];
+}
+
+static inline void outerproduct3_scaled(const double* restrict v, double* restrict P, double scale)
+{
+  P[0] = scale * v[0]*v[0];
+  P[1] = scale * v[0]*v[1];
+  P[2] = scale * v[0]*v[2];
+  P[3] = scale * v[1]*v[1];
+  P[4] = scale * v[1]*v[2];
+  P[5] = scale * v[2]*v[2];
+}
+
+// conjugate 2 vectors (a, b) through a symmetric matrix S: a->transpose x S x b
+// (%i2) sym3 : matrix([s0,s1,s2],
+//                     [s1,s3,s4],
+//                     [s2,s4,s5]);
+
+// (%o2) matrix([s0,s1,s2],[s1,s3,s4],[s2,s4,s5])
+// (%i6) a : matrix([a0],[a1],[a2]);
+
+// (%o6) matrix([a0],[a1],[a2])
+// (%i7) b : matrix([b0],[b1],[b2]);
+
+// (%o7) matrix([b0],[b1],[b2])
+// (%i10) transpose(a) . sym3 . b;
+
+// (%o10) a2*(b2*s5+b1*s4+b0*s2)+a1*(b2*s4+b1*s3+b0*s1)+a0*(b2*s2+b1*s1+b0*s0)
+static inline double conj_3(const double* restrict a, const double* restrict s, const double* restrict b)
+{
+  return a[2]*(b[2]*s[5]+b[1]*s[4]+b[0]*s[2])+a[1]*(b[2]*s[4]+b[1]*s[3]+b[0]*s[1])+a[0]*(b[2]*s[2]+b[1]*s[1]+b[0]*s[0]);
+}
+
+// Given an orthonormal matrix, returns the det. This is always +1 or -1
+static inline double det_orthonormal33(const double* m)
+{
+    // cross(row0,row1) = det * row3
+
+    // I find a nice non-zero element of row3, and see if the signs match
+    if( m[6] < -0.1 )
+    {
+        // looking at col0 of the last row. It is <0
+        double cross = m[1]*m[5] - m[2]*m[4];
+        return cross > 0.0 ? -1.0 : 1.0;
+    }
+    if( m[6] > 0.1)
+    {
+        // looking at col0 of the last row. It is > 0
+        double cross = m[1]*m[5] - m[2]*m[4];
+        return cross > 0.0 ? 1.0 : -1.0;
+    }
+
+    if( m[7] < -0.1 )
+    {
+        // looking at col1 of the last row. It is <0
+        double cross = m[2]*m[3] - m[0]*m[5];
+        return cross > 0.0 ? -1.0 : 1.0;
+    }
+    if( m[7] > 0.1)
+    {
+        // looking at col1 of the last row. It is > 0
+        double cross = m[2]*m[3] - m[0]*m[5];
+        return cross > 0.0 ? 1.0 : -1.0;
+    }
+
+    if( m[8] < -0.1 )
+    {
+        // looking at col2 of the last row. It is <0
+        double cross = m[0]*m[4] - m[1]*m[3];
+        return cross > 0.0 ? -1.0 : 1.0;
+    }
+
+    // last option. This MUST be true, so I don't even bother to check
+    {
+        // looking at col2 of the last row. It is > 0
+        double cross = m[0]*m[4] - m[1]*m[3];
+        return cross > 0.0 ? 1.0 : -1.0;
+    }
+}
+
+static void minimath_xchg(double* m, int i, int j)
+{
+    double t = m[i];
+    m[i] = m[j];
+    m[j] = t;
+}
+static inline void gen33_transpose(double* m)
+{
+    minimath_xchg(m, 1, 3);
+    minimath_xchg(m, 2, 6);
+    minimath_xchg(m, 5, 7);
+}
+
+static inline void gen33_transpose_vout(const double* m, double* mout)
+{
+    for(int i=0; i<3; i++)
+        for(int j=0; j<3; j++)
+            mout[i*3+j] = m[j*3+i];
+}
+
+static inline double cofactors_gen33(// output
+                                     double* restrict c,
+
+                                     // input
+                                     const double* restrict m)
+{
+    /*
+(%i1) display2d : false;
+
+(%o1) false
+(%i5) linel : 100000;
+
+(%o5) 100000
+(%i6) mat33 : matrix( [m0,m1,m2], [m3,m4,m5], [m6,m7,m8] );
+
+(%o6) matrix([m0,m1,m2],[m3,m4,m5],[m6,m7,m8])
+(%i7) num( ev(invert(mat33)), detout );
+
+(%o7) matrix([(m4*m8-m5*m7)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m2*m7-m1*m8)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m1*m5-m2*m4)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6))],[(m5*m6-m3*m8)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m0*m8-m2*m6)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m2*m3-m0*m5)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6))],[(m3*m7-m4*m6)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m1*m6-m0*m7)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6)),(m0*m4-m1*m3)/(m0*(m4*m8-m5*m7)+m1*(m5*m6-m3*m8)+m2*(m3*m7-m4*m6))])
+(%i8) determinant(mat33);
+
+(%o8) m0*(m4*m8-m5*m7)-m1*(m3*m8-m5*m6)+m2*(m3*m7-m4*m6)
+    */
+
+    double det = m[0]*(m[4]*m[8]-m[5]*m[7])-m[1]*(m[3]*m[8]-m[5]*m[6])+m[2]*(m[3]*m[7]-m[4]*m[6]);
+
+    c[0] = m[4]*m[8]-m[5]*m[7];
+    c[1] = m[2]*m[7]-m[1]*m[8];
+    c[2] = m[1]*m[5]-m[2]*m[4];
+    c[3] = m[5]*m[6]-m[3]*m[8];
+    c[4] = m[0]*m[8]-m[2]*m[6];
+    c[5] = m[2]*m[3]-m[0]*m[5];
+    c[6] = m[3]*m[7]-m[4]*m[6];
+    c[7] = m[1]*m[6]-m[0]*m[7];
+    c[8] = m[0]*m[4]-m[1]*m[3];
+
+    return det;
+}
+
+#ifdef __cplusplus
+#undef restrict
+#endif

wpical/src/main/native/thirdparty/mrcal/include/mrcal-image.h

@@ -0,0 +1,124 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+// mrcal images. These are completely uninteresting, and don't do anything
+// better that other image read/write APIS. If you have image libraries running,
+// use those. If not, the ones defined here should be light and painless
+
+// I support several image types:
+// - "uint8":  8-bit grayscale
+// - "uint16": 16-bit grayscale (using the system endian-ness)
+// - "bgr":    24-bit BGR color
+//
+// Each type defines several functions in the MRCAL_IMAGE_DECLARE() macro:
+//
+// - mrcal_image_TYPE_t container image
+// - mrcal_image_TYPE_at(mrcal_image_TYPE_t* image, int x, int y)
+// - mrcal_image_TYPE_at_const(const mrcal_image_TYPE_t* image, int x, int y)
+// - mrcal_image_TYPE_t mrcal_image_TYPE_crop(mrcal_image_TYPE_t* image, in x0, int y0, int w, int h)
+// - mrcal_image_TYPE_save (const char* filename, const mrcal_image_TYPE_t*  image);
+// - mrcal_image_TYPE_load( mrcal_image_TYPE_t*  image, const char* filename);
+//
+// The image-loading functions require a few notes:
+//
+// An image structure to fill in is given. image->data will be allocated to the
+// proper size. It is the caller's responsibility to free(image->data) when
+// they're done. Usage sample:
+//
+//   mrcal_image_uint8_t image;
+//   mrcal_image_uint8_load(&image, image_filename);
+//   .... do stuff ...
+//   free(image.data);
+//
+// mrcal_image_uint8_load() converts images to 8-bpp grayscale. Color and
+// palettized images are accepted
+//
+// mrcal_image_uint16_load() does NOT convert images. The images being read must
+// already be stored as 16bpp grayscale images
+//
+// mrcal_image_bgr_load() converts images to 24-bpp color
+
+#include <stdint.h>
+#include <stdbool.h>
+
+
+typedef struct { uint8_t bgr[3]; } mrcal_bgr_t;
+
+#define MRCAL_IMAGE_DECLARE(T, Tname)                                   \
+typedef struct                                                          \
+{                                                                       \
+    union                                                               \
+    {                                                                   \
+        /* in pixels */                                                 \
+        struct {int w, h;};                                             \
+        struct {int width, height;};                                    \
+        struct {int cols, rows;};                                       \
+    };                                                                  \
+    int stride; /* in bytes  */                                         \
+    T* data;                                                            \
+} mrcal_image_ ## Tname ## _t;                                          \
+                                                                        \
+static inline                                                           \
+T* mrcal_image_ ## Tname ## _at(mrcal_image_ ## Tname ## _t* image, int x, int y) \
+{                                                                       \
+    return &image->data[x + y*image->stride / sizeof(T)];               \
+}                                                                       \
+                                                                        \
+static inline                                                           \
+const T* mrcal_image_ ## Tname ## _at_const(const mrcal_image_ ## Tname ## _t* image, int x, int y) \
+{                                                                       \
+    return &image->data[x + y*image->stride / sizeof(T)];               \
+}                                                                       \
+                                                                        \
+static inline                                                           \
+mrcal_image_ ## Tname ## _t                                             \
+mrcal_image_ ## Tname ## _crop(mrcal_image_ ## Tname ## _t* image,      \
+                              int x0, int y0,                           \
+                              int w,  int h)                            \
+{                                                                       \
+    return (mrcal_image_ ## Tname ## _t){ .w      = w,                  \
+                                          .h      = h,                  \
+                                          .stride = image->stride,      \
+                                          .data   = mrcal_image_ ## Tname ## _at(image,x0,y0) }; \
+}
+
+#define MRCAL_IMAGE_SAVE_LOAD_DECLARE(T, Tname)                         \
+bool mrcal_image_ ## Tname ## _save (const char* filename, const mrcal_image_ ## Tname ## _t*  image); \
+bool mrcal_image_ ## Tname ## _load( mrcal_image_ ## Tname ## _t*  image, const char* filename);
+
+
+// Common images types
+MRCAL_IMAGE_DECLARE(uint8_t,     uint8);
+MRCAL_IMAGE_DECLARE(uint16_t,    uint16);
+MRCAL_IMAGE_DECLARE(mrcal_bgr_t, bgr);
+MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint8_t,     uint8);
+MRCAL_IMAGE_SAVE_LOAD_DECLARE(uint16_t,    uint16);
+MRCAL_IMAGE_SAVE_LOAD_DECLARE(mrcal_bgr_t, bgr);
+
+// Uncommon types. Not everything supports these
+MRCAL_IMAGE_DECLARE(int8_t,  int8);
+MRCAL_IMAGE_DECLARE(int16_t, int16);
+
+MRCAL_IMAGE_DECLARE(int32_t,  int32);
+MRCAL_IMAGE_DECLARE(uint32_t, uint32);
+MRCAL_IMAGE_DECLARE(int64_t,  int64);
+MRCAL_IMAGE_DECLARE(uint64_t, uint64);
+
+MRCAL_IMAGE_DECLARE(float,    float);
+MRCAL_IMAGE_DECLARE(double,   double);
+
+// Load the image into whatever type is stored on disk
+bool mrcal_image_anytype_load(// output
+                              // This is ONE of the known types
+                              mrcal_image_uint8_t* image,
+                              int* bits_per_pixel,
+                              int* channels,
+                              // input
+                              const char* filename);

wpical/src/main/native/thirdparty/mrcal/include/mrcal-internal.h

@@ -0,0 +1,109 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+// THESE ARE NOT A PART OF THE EXTERNAL API. Exported for the mrcal python
+// wrapper only
+
+// These models have no precomputed data
+typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__precomputed_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_CAHVORE__precomputed_t;
+
+// The splined stereographic models configuration parameters can be used to
+// compute the segment size. I cache this computation
+typedef struct
+{
+    // The distance between adjacent knots (1 segment) is u_per_segment =
+    // 1/segments_per_u
+    double segments_per_u;
+} mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__precomputed_t;
+
+typedef struct
+{
+    bool ready;
+    union
+    {
+#define PRECOMPUTED_STRUCT(s,n) mrcal_ ##s##__precomputed_t s##__precomputed;
+        MRCAL_LENSMODEL_LIST(PRECOMPUTED_STRUCT)
+#undef PRECOMPUTED_STRUCT
+    };
+} mrcal_projection_precomputed_t;
+
+
+void _mrcal_project_internal_opencv( // outputs
+                                    mrcal_point2_t* q,
+                                    mrcal_point3_t* dq_dp,               // may be NULL
+                                    double* dq_dintrinsics_nocore, // may be NULL
+
+                                    // inputs
+                                    const mrcal_point3_t* p,
+                                    int N,
+                                    const double* intrinsics,
+                                    int Nintrinsics);
+bool _mrcal_project_internal( // out
+                             mrcal_point2_t* q,
+
+                             // Stored as a row-first array of shape (N,2,3). Each
+                             // trailing ,3 dimension element is a mrcal_point3_t
+                             mrcal_point3_t* dq_dp,
+                             // core, distortions concatenated. Stored as a row-first
+                             // array of shape (N,2,Nintrinsics). This is a DENSE array.
+                             // High-parameter-count lens models have very sparse
+                             // gradients here, and the internal project() function
+                             // returns those sparsely. For now THIS function densifies
+                             // all of these
+                             double*   dq_dintrinsics,
+
+                             // in
+                             const mrcal_point3_t* p,
+                             int N,
+                             const mrcal_lensmodel_t* lensmodel,
+                             // core, distortions concatenated
+                             const double* intrinsics,
+
+                             int Nintrinsics,
+                             const mrcal_projection_precomputed_t* precomputed);
+void _mrcal_precompute_lensmodel_data(mrcal_projection_precomputed_t* precomputed,
+                                      const mrcal_lensmodel_t* lensmodel);
+bool _mrcal_unproject_internal( // out
+                               mrcal_point3_t* out,
+
+                               // in
+                               const mrcal_point2_t* q,
+                               int N,
+                               const mrcal_lensmodel_t* lensmodel,
+                               // core, distortions concatenated
+                               const double* intrinsics,
+                               const mrcal_projection_precomputed_t* precomputed);
+
+// Report the number of non-zero entries in the optimization jacobian
+int _mrcal_num_j_nonzero(int Nobservations_board,
+                         int Nobservations_point,
+
+                         // May be NULL if we don't have any of these
+                         const mrcal_observation_point_triangulated_t* observations_point_triangulated,
+                         int Nobservations_point_triangulated,
+
+                         int calibration_object_width_n,
+                         int calibration_object_height_n,
+                         int Ncameras_intrinsics, int Ncameras_extrinsics,
+                         int Nframes,
+                         int Npoints, int Npoints_fixed,
+                         const mrcal_observation_board_t* observations_board,
+                         const mrcal_observation_point_t* observations_point,
+                         mrcal_problem_selections_t problem_selections,
+                         const mrcal_lensmodel_t* lensmodel);

wpical/src/main/native/thirdparty/mrcal/include/mrcal-types.h

@@ -0,0 +1,396 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "basic-geometry.h"
+
+////////////////////////////////////////////////////////////////////////////////
+//////////////////// Lens models
+////////////////////////////////////////////////////////////////////////////////
+
+// These are an "X macro": https://en.wikipedia.org/wiki/X_Macro
+//
+// The supported lens models and their parameter counts. Models with a
+// configuration may have a dynamic parameter count; this is indicated here with
+// a <0 value. These models report their parameter counts in the
+// LENSMODEL_XXX__lensmodel_num_params() function, called by
+// mrcal_lensmodel_num_params().
+#define MRCAL_LENSMODEL_NOCONFIG_LIST(_)                                         \
+    _(LENSMODEL_PINHOLE,               4)                                        \
+    _(LENSMODEL_STEREOGRAPHIC,         4)  /* Simple stereographic-only model */ \
+    _(LENSMODEL_LONLAT,                4)                                        \
+    _(LENSMODEL_LATLON,                4)                                        \
+    _(LENSMODEL_OPENCV4,               8)                                        \
+    _(LENSMODEL_OPENCV5,               9)                                        \
+    _(LENSMODEL_OPENCV8,               12)                                       \
+    _(LENSMODEL_OPENCV12,              16) /* available in OpenCV >= 3.0.0) */   \
+    _(LENSMODEL_CAHVOR,                9)
+#define MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(_)                        \
+    _(LENSMODEL_CAHVORE,               12)
+#define MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(_)                       \
+    _(LENSMODEL_SPLINED_STEREOGRAPHIC, -1)
+#define MRCAL_LENSMODEL_LIST(_)                         \
+    MRCAL_LENSMODEL_NOCONFIG_LIST(_)                    \
+    MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(_)   \
+    MRCAL_LENSMODEL_WITHCONFIG_DYNAMIC_NPARAMS_LIST(_)
+
+
+// parametric models have no extra configuration
+typedef struct { int dummy; } mrcal_LENSMODEL_PINHOLE__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_STEREOGRAPHIC__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_LONLAT__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_LATLON__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV4__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV5__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV8__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_OPENCV12__config_t;
+typedef struct { int dummy; } mrcal_LENSMODEL_CAHVOR__config_t;
+
+#define _MRCAL_ITEM_DEFINE_ELEMENT(name, type, pybuildvaluecode, PRIcode,SCNcode, bitfield, cookie) type name bitfield;
+
+// Configuration for CAHVORE. These are given as an an
+// "X macro": https://en.wikipedia.org/wiki/X_Macro
+#define MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_, cookie) \
+    _(linearity,    double, "d", ".2f", "lf", , cookie)
+typedef struct
+{
+    MRCAL_LENSMODEL_CAHVORE_CONFIG_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, )
+} mrcal_LENSMODEL_CAHVORE__config_t;
+
+// Configuration for the splined stereographic models. These are given as an an
+// "X macro": https://en.wikipedia.org/wiki/X_Macro
+#define MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(_, cookie)                \
+    /* Maximum degree of each 1D polynomial. This is almost certainly 2 */          \
+    /* (quadratic splines, C1 continuous) or 3 (cubic splines, C2 continuous) */    \
+    _(order,        uint16_t, "H", PRIu16,SCNu16, , cookie)                         \
+    /* We have a Nx by Ny grid of control points */                                 \
+    _(Nx,           uint16_t, "H", PRIu16,SCNu16, , cookie)                         \
+    _(Ny,           uint16_t, "H", PRIu16,SCNu16, , cookie)                         \
+    /* The horizontal field of view. Not including fov_y. It's proportional with */ \
+    /* Ny and Nx */                                                                 \
+    _(fov_x_deg,    uint16_t, "H", PRIu16,SCNu16, , cookie)
+typedef struct
+{
+    MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC_CONFIG_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, )
+} mrcal_LENSMODEL_SPLINED_STEREOGRAPHIC__config_t;
+
+
+// An X-macro-generated enum mrcal_lensmodel_type_t. This has an element for
+// each entry in MRCAL_LENSMODEL_LIST (with "MRCAL_" prepended). This lensmodel
+// type selects the lens model, but does NOT provide the configuration.
+// mrcal_lensmodel_t does that.
+#define _LIST_WITH_COMMA(s,n) ,MRCAL_ ## s
+typedef enum
+    { // Generic error. Rarely used in current mrcal
+      MRCAL_LENSMODEL_INVALID               = -2,
+
+      // Configuration parsing failed
+      MRCAL_LENSMODEL_INVALID_BADCONFIG     = -1,
+
+      // A configuration was required, but was missing entirely
+      MRCAL_LENSMODEL_INVALID_MISSINGCONFIG = -3,
+
+      // The model type wasn't known
+      MRCAL_LENSMODEL_INVALID_TYPE          = -4,
+
+      // Dummy entry. Must be -1 so that successive values start at 0
+      _MRCAL_LENSMODEL_DUMMY                = -1
+
+      // The rest, starting with 0
+      MRCAL_LENSMODEL_LIST( _LIST_WITH_COMMA ) } mrcal_lensmodel_type_t;
+#undef _LIST_WITH_COMMA
+
+
+// Defines a lens model: the type AND the configuration values
+typedef struct
+{
+    // The type of lensmodel. This is an enum, selecting elements of
+    // MRCAL_LENSMODEL_LIST (with "MRCAL_" prepended)
+    mrcal_lensmodel_type_t type;
+
+    // A union of all the possible configuration structures. We pick the
+    // structure type based on the value of "type
+    union
+    {
+#define CONFIG_STRUCT(s,n) mrcal_ ##s##__config_t s##__config;
+        MRCAL_LENSMODEL_LIST(CONFIG_STRUCT)
+#undef CONFIG_STRUCT
+    };
+} mrcal_lensmodel_t;
+
+
+typedef union
+{
+    struct
+    {
+        double x2, y2;
+    };
+    double values[2];
+} mrcal_calobject_warp_t;
+
+#define MRCAL_NSTATE_CALOBJECT_WARP ((int)((sizeof(mrcal_calobject_warp_t)/sizeof(double))))
+
+//// ADD CHANGES TO THE DOCS IN lensmodels.org
+////
+// An X-macro-generated mrcal_lensmodel_metadata_t. Each lens model type has
+// some metadata that describes its inherent properties. These properties can be
+// queried by calling mrcal_lensmodel_metadata() in C and
+// mrcal.lensmodel_metadata_and_config() in Python. The available properties and
+// their meaning are listed in MRCAL_LENSMODEL_META_LIST()
+#define MRCAL_LENSMODEL_META_LIST(_, cookie)                            \
+    /* If true, this model contains an "intrinsics core". This is described */ \
+    /* in mrcal_intrinsics_core_t. If present, the 4 core parameters ALWAYS */ \
+    /* appear at the start of a model's parameter vector                    */ \
+    _(has_core,                  bool, "i",,, :1, cookie)               \
+                                                                        \
+    /* Whether a model is able to project points behind the camera          */ \
+    /* (z<0 in the camera coordinate system). Models based on a pinhole     */ \
+    /* projection (pinhole, OpenCV, CAHVOR(E)) cannot do this. models based */ \
+    /* on a stereographic projection (stereographic, splined stereographic) */ \
+    /* can                                                                  */ \
+    _(can_project_behind_camera, bool, "i",,, :1, cookie)               \
+                                                                        \
+    /* Whether gradients are available for this model. Currently only */ \
+    /* CAHVORE does not have gradients                                */ \
+    _(has_gradients,             bool, "i",,, :1, cookie)               \
+                                                                        \
+    /* Whether this is a noncentral model.Currently the only noncentral  */ \
+    /* model we have is CAHVORE. There will be more later.               */ \
+    _(noncentral,                bool, "i",,, :1, cookie)
+
+typedef struct
+{
+    MRCAL_LENSMODEL_META_LIST(_MRCAL_ITEM_DEFINE_ELEMENT, )
+} mrcal_lensmodel_metadata_t;
+
+
+////////////////////////////////////////////////////////////////////////////////
+//////////////////// Optimization
+////////////////////////////////////////////////////////////////////////////////
+
+// Used to specify which camera is making an observation. The "intrinsics" index
+// is used to identify a specific camera, while the "extrinsics" index is used
+// to locate a camera in space. If I have a camera that is moving over time, the
+// intrinsics index will remain the same, while the extrinsics index will change
+#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
+// #warning "triangulated-solve: there should be a pool of these, and I should be indexing that pool"
+#endif
+typedef struct
+{
+    // indexes the intrinsics array
+    int  intrinsics;
+    // indexes the extrinsics array. -1 means "at coordinate system reference"
+    int  extrinsics;
+} mrcal_camera_index_t;
+
+
+// An observation of a calibration board. Each "observation" is ONE camera
+// observing a board
+typedef struct
+{
+    // which camera is making this observation
+    mrcal_camera_index_t icam;
+
+    // indexes the "frames" array to select the pose of the calibration object
+    // being observed
+    int                  iframe;
+} mrcal_observation_board_t;
+
+// An observation of a discrete point. Each "observation" is ONE camera
+// observing a single point in space
+typedef struct
+{
+    // which camera is making this observation
+    mrcal_camera_index_t icam;
+
+    // indexes the "points" array to select the position of the point being
+    // observed
+    int                  i_point;
+} mrcal_observation_point_t;
+
+#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
+// #warning "triangulated-solve: triangulated points into a pool. maybe allowing the intrinsics to move in the process"
+#endif
+
+// An observation of a discrete point where the point itself is NOT a part of
+// the optimization, but computed implicitly via triangulation. This structure
+// is very similar to mrcal_observation_point_t, except instead of i_point
+// identifying the point being observed we have
+#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
+// #warning "triangulated-solve: FINISH DOC"
+#endif
+typedef struct
+{
+    // which camera is making this observation
+    mrcal_camera_index_t icam;
+
+#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
+// #warning "triangulated-solve: DOCUMENT. CAN THIS BIT FIELD BE PACKED NICELY?"
+#endif
+    // Set if this is the last camera observing this point. Any set of N>=2
+    // cameras can observe any point. All observations of a given point are
+    // stored consecutively, the last one being noted by this bit
+#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
+// #warning "triangulated-solve: do I really need this? I cannot look at the next observation to determine when this one is done?"
+#endif
+    bool                 last_in_set : 1;
+
+#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
+// #warning "triangulated-solve: this is temporary. Should be a weight in observations_point_pool like all the other observations"
+#endif
+    bool                 outlier     : 1;
+
+    // Observed pixel coordinates. This works just like elements of
+    // observations_board_pool and observations_point_pool
+    mrcal_point3_t px;
+} mrcal_observation_point_triangulated_t;
+
+
+#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
+// #warning "triangulated-solve: need a function to identify a vanilla calibration problem. It needs to not include any triangulated points. The noise propagation is different"
+#endif
+
+
+// Bits indicating which parts of the optimization problem being solved. We can
+// ask mrcal to solve for ALL the lens parameters and ALL the geometry and
+// everything else. OR we can ask mrcal to lock down some part of the
+// optimization problem, and to solve for the rest. If any variables are locked
+// down, we use their initial values passed-in to mrcal_optimize()
+typedef struct
+{
+    // If true, we solve for the intrinsics core. Applies only to those models
+    // that HAVE a core (fx,fy,cx,cy)
+    bool do_optimize_intrinsics_core        : 1;
+
+    // If true, solve for the non-core lens parameters
+    bool do_optimize_intrinsics_distortions : 1;
+
+    // If true, solve for the geometry of the cameras
+    bool do_optimize_extrinsics             : 1;
+
+    // If true, solve for the poses of the calibration object
+    bool do_optimize_frames                 : 1;
+
+    // If true, optimize the shape of the calibration object
+    bool do_optimize_calobject_warp         : 1;
+
+#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
+// #warning "triangulated-solve: Need finer-grained regularization flags"
+// #warning "triangulated-solve: Regularization flags should reflect do_optimize stuff and Ncameras stuff"
+#endif
+    // If true, apply the regularization terms in the solver
+    bool do_apply_regularization            : 1;
+
+    // Whether to try to find NEW outliers. The outliers given on
+    // input are respected regardless
+    bool do_apply_outlier_rejection         : 1;
+
+    // Pull the distance between the first two cameras to 1.0
+    bool do_apply_regularization_unity_cam01: 1;
+
+} mrcal_problem_selections_t;
+
+// Constants used in a mrcal optimization. This is similar to
+// mrcal_problem_selections_t, but contains numerical values rather than just
+// bits
+typedef struct
+{
+    // The minimum distance of an observed discrete point from its observing
+    // camera. Any observation of a point below this range will be penalized to
+    // encourage the optimizer to move the point further away from the camera
+    double  point_min_range;
+
+
+    // The maximum distance of an observed discrete point from its observing
+    // camera. Any observation of a point abive this range will be penalized to
+    // encourage the optimizer to move the point closer to the camera
+    double  point_max_range;
+} mrcal_problem_constants_t;
+
+
+// An X-macro-generated mrcal_stats_t. This structure is returned by the
+// optimizer, and contains some statistics about the optimization
+#define MRCAL_STATS_ITEM(_)                                             \
+    /* The RMS error of the optimized fit at the optimum. Generally the residual */ \
+    /* vector x contains error values for each element of q, so N observed pixels */ \
+    /* produce 2N measurements: len(x) = 2*N. And the RMS error is */   \
+    /*   sqrt( norm2(x) / N ) */                                        \
+    _(double,         rms_reproj_error__pixels,   PyFloat_FromDouble)   \
+                                                                        \
+    /* How many pixel observations were thrown out as outliers. Each pixel */ \
+    /* observation produces two measurements. Note that this INCLUDES any */ \
+    /* outliers that were passed-in at the start */                     \
+    _(int,            Noutliers_board,                  PyLong_FromLong) \
+                                                                        \
+    /* How many pixel observations were thrown out as outliers. Each pixel */ \
+    /* observation produces two measurements. Note that this INCLUDES any */ \
+    /* outliers that were passed-in at the start */                     \
+    _(int,            Noutliers_triangulated_point,     PyLong_FromLong)
+#if defined ENABLE_TRIANGULATED_WARNINGS && ENABLE_TRIANGULATED_WARNINGS
+// #warning "triangulated-solve: implement stats.Noutliers_triangulated_point; add to c-api.org"
+#endif
+#define MRCAL_STATS_ITEM_DEFINE(type, name, pyconverter) type name;
+typedef struct
+{
+    MRCAL_STATS_ITEM(MRCAL_STATS_ITEM_DEFINE)
+} mrcal_stats_t;
+
+////////////////////////////////////////////////////////////////////////////////
+//////////////////// Layout of the measurement and state vectors
+////////////////////////////////////////////////////////////////////////////////
+
+// The "intrinsics core" of a camera. This defines the final step of a
+// projection operation. For instance with a pinhole model we have
+//
+//   q[0] = focal_xy[0] * x/z + center_xy[0]
+//   q[1] = focal_xy[1] * y/z + center_xy[1]
+typedef struct
+{
+    double focal_xy [2];
+    double center_xy[2];
+} mrcal_intrinsics_core_t;
+
+
+// structure containing a camera pose + lens model. Used for .cameramodel
+// input/output
+#define MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS        \
+    double            rt_cam_ref[6];                    \
+    unsigned int      imagersize[2];                    \
+    mrcal_lensmodel_t lensmodel                         \
+
+typedef struct
+{
+    MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS;
+    // mrcal_cameramodel_t works for all lensmodels, so its intrinsics count is
+    // not known at compile time. mrcal_cameramodel_t is thus usable only as
+    // part of a larger structure or as a mrcal_cameramodel_t* argument to
+    // functions. To allocate new mrcal_cameramodel_t objects, use
+    // mrcal_cameramodel_LENSMODEL_XXX_t or malloc() with the proper intrinsics
+    // size taken into account
+    double            intrinsics[0];
+} mrcal_cameramodel_t;
+
+
+#define DEFINE_mrcal_cameramodel_MODEL_t(s,n)           \
+typedef union                                           \
+{                                                       \
+    mrcal_cameramodel_t m;                              \
+    struct                                              \
+    {                                                   \
+        MRCAL_CAMERAMODEL_ELEMENTS_NO_INTRINSICS;       \
+        double intrinsics[n];                           \
+    };                                                  \
+} mrcal_cameramodel_ ## s ## _t;
+
+
+MRCAL_LENSMODEL_NOCONFIG_LIST(                 DEFINE_mrcal_cameramodel_MODEL_t)
+MRCAL_LENSMODEL_WITHCONFIG_STATIC_NPARAMS_LIST(DEFINE_mrcal_cameramodel_MODEL_t)

wpical/src/main/native/thirdparty/mrcal/include/mrcal.h

@@ -0,0 +1,924 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "mrcal-types.h"
+#include "poseutils.h"
+#include "stereo.h"
+#include "triangulation.h"
+#include "mrcal-image.h"
+
+////////////////////////////////////////////////////////////////////////////////
+//////////////////// Lens models
+////////////////////////////////////////////////////////////////////////////////
+#ifdef __cplusplus
+extern "C" {
+#endif
+// Return an array of strings listing all the available lens models
+//
+// These are all "unconfigured" strings that use "..." placeholders for any
+// configuration values. Each returned string is a \0-terminated const char*. The
+// end of the list is signified by a NULL pointer
+const char* const* mrcal_supported_lensmodel_names( void ); // NULL-terminated array of char* strings
+
+
+// Return true if the given mrcal_lensmodel_type_t specifies a valid lens model
+
+static bool mrcal_lensmodel_type_is_valid(mrcal_lensmodel_type_t t)
+{
+    return t >= 0;
+}
+
+// Evaluates to true if the given lens model is one of the supported OpenCV
+// types
+#define MRCAL_LENSMODEL_IS_OPENCV(d) (MRCAL_LENSMODEL_OPENCV4 <= (d) && (d) <= MRCAL_LENSMODEL_OPENCV12)
+
+
+// Return a string describing a lens model.
+//
+// This function returns a static string. For models with no configuration, this
+// is the FULL string for that model. For models with a configuration, the
+// configuration values have "..." placeholders. These placeholders mean that
+// the resulting strings do not define a lens model fully, and cannot be
+// converted to a mrcal_lensmodel_t with mrcal_lensmodel_from_name()
+//
+// This is the inverse of mrcal_lensmodel_type_from_name()
+const char* mrcal_lensmodel_name_unconfigured( const mrcal_lensmodel_t* lensmodel );
+
+
+// Return a CONFIGURED string describing a lens model.
+//
+// This function generates a fully-configured string describing the given lens
+// model. For models with no configuration, this is just the static string
+// returned by mrcal_lensmodel_name_unconfigured(). For models that have a
+// configuration, however, the configuration values are filled-in. The resulting
+// string may be converted back into a mrcal_lensmodel_t by calling
+// mrcal_lensmodel_from_name().
+//
+// This function writes the string into the given buffer "out". The size of the
+// buffer is passed in the "size" argument. The meaning of "size" is as with
+// snprintf(), which is used internally. Returns true on success
+//
+// This is the inverse of mrcal_lensmodel_from_name()
+bool mrcal_lensmodel_name( char* out, int size,
+                           const mrcal_lensmodel_t* lensmodel );
+
+
+// Parse the lens model type from a lens model name string
+//
+// The configuration is ignored. Thus this function works even if the
+// configuration is missing or unparseable. Unknown model names return
+// MRCAL_LENSMODEL_INVALID_TYPE
+//
+// This is the inverse of mrcal_lensmodel_name_unconfigured()
+mrcal_lensmodel_type_t mrcal_lensmodel_type_from_name( const char* name );
+
+
+// Parse the full configured lens model from a lens model name string
+//
+// The lens mode type AND the configuration are read into a mrcal_lensmodel_t
+// structure, which this function returns.
+//
+// On error returns false with lensmodel->type set to MRCAL_LENSMODEL_INVALID_...
+//
+// This is the inverse of mrcal_lensmodel_name()
+bool mrcal_lensmodel_from_name( // output
+                                mrcal_lensmodel_t* lensmodel,
+
+                                // input
+                                const char* name );
+
+// Return a structure containing a model's metadata
+//
+// The available metadata is described in the definition of the
+// MRCAL_LENSMODEL_META_LIST() macro
+mrcal_lensmodel_metadata_t mrcal_lensmodel_metadata( const mrcal_lensmodel_t* lensmodel );
+
+
+// Return the number of parameters required to specify a given lens model
+//
+// For models that have a configuration, the parameter count value generally
+// depends on the configuration. For instance, splined models use the model
+// parameters as the spline control points, so the spline density (specified in
+// the configuration) directly affects how many parameters such a model requires
+int mrcal_lensmodel_num_params( const mrcal_lensmodel_t* lensmodel );
+
+
+// Return the locations of x and y spline knots
+
+// Splined models are defined by the locations of their control points. These
+// are arranged in a grid, the size and density of which is set by the model
+// configuration. We fill-in the x knot locations into ux[] and the y locations
+// into uy[]. ux[] and uy[] must be large-enough to hold configuration->Nx and
+// configuration->Ny values respectively.
+//
+// This function applies to splined models only. Returns true on success
+bool mrcal_knots_for_splined_models( double* ux, double* uy,
+                                     const mrcal_lensmodel_t* lensmodel);
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//////////////////// Projections
+////////////////////////////////////////////////////////////////////////////////
+
+// Project the given camera-coordinate-system points
+//
+// Compute a "projection", a mapping of points defined in the camera coordinate
+// system to their observed pixel coordinates. If requested, gradients are
+// computed as well.
+//
+// We project N 3D points p to N 2D pixel coordinates q using the given
+// lensmodel and intrinsics parameter values.
+//
+// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
+// ((N,2) mrcal_point3_t objects).
+//
+// if (dq_dintrinsics != NULL) we report the gradient dq/dintrinsics in a dense
+// (N,2,Nintrinsics) array. Note that splined models have very high Nintrinsics
+// and very sparse gradients. THIS function reports the gradients densely,
+// however, so it is inefficient for splined models.
+//
+// This function supports CAHVORE distortions only if we don't ask for any
+// gradients
+//
+// Projecting out-of-bounds points (beyond the field of view) returns undefined
+// values. Generally things remain continuous even as we move off the imager
+// domain. Pinhole-like projections will work normally if projecting a point
+// behind the camera. Splined projections clamp to the nearest spline segment:
+// the projection will fly off to infinity quickly since we're extrapolating a
+// polynomial, but the function will remain continuous.
+bool mrcal_project( // out
+                   mrcal_point2_t* q,
+                   mrcal_point3_t* dq_dp,
+                   double*         dq_dintrinsics,
+
+                   // in
+                   const mrcal_point3_t* p,
+                   int N,
+                   const mrcal_lensmodel_t* lensmodel,
+                   // core, distortions concatenated
+                   const double* intrinsics);
+
+
+// Unproject the given pixel coordinates
+//
+// Compute an "unprojection", a mapping of pixel coordinates to the camera
+// coordinate system.
+//
+// We unproject N 2D pixel coordinates q to N 3D direction vectors v using the
+// given lensmodel and intrinsics parameter values. The returned vectors v are
+// not normalized, and may have any length.
+
+// This is the "reverse" direction, so an iterative nonlinear optimization is
+// performed internally to compute this result. This is much slower than
+// mrcal_project(). For OpenCV models specifically, OpenCV has
+// cvUndistortPoints() (and cv2.undistortPoints()), but these are unreliable:
+// https://github.com/opencv/opencv/issues/8811
+//
+// This function does NOT support CAHVORE
+bool mrcal_unproject( // out
+                     mrcal_point3_t* v,
+
+                     // in
+                     const mrcal_point2_t* q,
+                     int N,
+                     const mrcal_lensmodel_t* lensmodel,
+                     // core, distortions concatenated
+                     const double* intrinsics);
+
+
+// Project the given camera-coordinate-system points using a pinhole
+// model. See the docs for projection details:
+// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole
+//
+// This is a simplified special case of mrcal_project(). We project N
+// camera-coordinate-system points p to N pixel coordinates q
+//
+// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
+// ((N,2) mrcal_point3_t objects).
+void mrcal_project_pinhole( // output
+                            mrcal_point2_t* q,
+                            mrcal_point3_t* dq_dp,
+
+                             // input
+                            const mrcal_point3_t* p,
+                            int N,
+                            const double* fxycxy);
+
+
+// Unproject the given pixel coordinates using a pinhole model.
+// See the docs for projection details:
+// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-pinhole
+//
+// This is a simplified special case of mrcal_unproject(). We unproject N 2D
+// pixel coordinates q to N camera-coordinate-system vectors v. The returned
+// vectors v are not normalized, and may have any length.
+//
+// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array
+// ((N,3) mrcal_point2_t objects).
+void mrcal_unproject_pinhole( // output
+                              mrcal_point3_t* v,
+                              mrcal_point2_t* dv_dq,
+
+                              // input
+                              const mrcal_point2_t* q,
+                              int N,
+                              const double* fxycxy);
+
+// Project the given camera-coordinate-system points using a stereographic
+// model. See the docs for projection details:
+// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic
+//
+// This is a simplified special case of mrcal_project(). We project N
+// camera-coordinate-system points p to N pixel coordinates q
+//
+// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
+// ((N,2) mrcal_point3_t objects).
+void mrcal_project_stereographic( // output
+                                 mrcal_point2_t* q,
+                                 mrcal_point3_t* dq_dp,
+
+                                  // input
+                                 const mrcal_point3_t* p,
+                                 int N,
+                                 const double* fxycxy);
+
+
+// Unproject the given pixel coordinates using a stereographic model.
+// See the docs for projection details:
+// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-stereographic
+//
+// This is a simplified special case of mrcal_unproject(). We unproject N 2D
+// pixel coordinates q to N camera-coordinate-system vectors v. The returned
+// vectors v are not normalized, and may have any length.
+//
+// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array
+// ((N,3) mrcal_point2_t objects).
+void mrcal_unproject_stereographic( // output
+                                   mrcal_point3_t* v,
+                                   mrcal_point2_t* dv_dq,
+
+                                   // input
+                                   const mrcal_point2_t* q,
+                                   int N,
+                                   const double* fxycxy);
+
+
+// Project the given camera-coordinate-system points using an equirectangular
+// projection. See the docs for projection details:
+// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat
+//
+// This is a simplified special case of mrcal_project(). We project N
+// camera-coordinate-system points p to N pixel coordinates q
+//
+// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
+// ((N,2) mrcal_point3_t objects).
+void mrcal_project_lonlat( // output
+                           mrcal_point2_t* q,
+                           mrcal_point3_t* dq_dv, // May be NULL. Each point
+                                                  // gets a block of 2 mrcal_point3_t
+                                                  // objects
+
+                           // input
+                           const mrcal_point3_t* v,
+                           int N,
+                           const double* fxycxy);
+
+// Unproject the given pixel coordinates using an equirectangular projection.
+// See the docs for projection details:
+// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-lonlat
+//
+// This is a simplified special case of mrcal_unproject(). We unproject N 2D
+// pixel coordinates q to N camera-coordinate-system vectors v. The returned
+// vectors v are normalized.
+//
+// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array
+// ((N,3) mrcal_point2_t objects).
+void mrcal_unproject_lonlat( // output
+                            mrcal_point3_t* v,
+                            mrcal_point2_t* dv_dq, // May be NULL. Each point
+                                                   // gets a block of 3 mrcal_point2_t
+                                                   // objects
+
+                            // input
+                            const mrcal_point2_t* q,
+                            int N,
+                            const double* fxycxy);
+
+
+// Project the given camera-coordinate-system points using a transverse
+// equirectangular projection. See the docs for projection details:
+// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon
+//
+// This is a simplified special case of mrcal_project(). We project N
+// camera-coordinate-system points p to N pixel coordinates q
+//
+// if (dq_dp != NULL) we report the gradient dq/dp in a dense (N,2,3) array
+// ((N,2) mrcal_point3_t objects).
+void mrcal_project_latlon( // output
+                           mrcal_point2_t* q,
+                           mrcal_point3_t* dq_dv, // May be NULL. Each point
+                                                  // gets a block of 2 mrcal_point3_t
+                                                  // objects
+
+                           // input
+                           const mrcal_point3_t* v,
+                           int N,
+                           const double* fxycxy);
+
+// Unproject the given pixel coordinates using a transverse equirectangular
+// projection. See the docs for projection details:
+// https://mrcal.secretsauce.net/lensmodels.html#lensmodel-latlon
+//
+// This is a simplified special case of mrcal_unproject(). We unproject N 2D
+// pixel coordinates q to N camera-coordinate-system vectors v. The returned
+// vectors v are normalized.
+//
+// if (dv_dq != NULL) we report the gradient dv/dq in a dense (N,3,2) array
+// ((N,3) mrcal_point2_t objects).
+void mrcal_unproject_latlon( // output
+                            mrcal_point3_t* v,
+                            mrcal_point2_t* dv_dq, // May be NULL. Each point
+                                                   // gets a block of 3 mrcal_point2_t
+                                                   // objects
+
+                            // input
+                            const mrcal_point2_t* q,
+                            int N,
+                            const double* fxycxy);
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//////////////////// Optimization
+////////////////////////////////////////////////////////////////////////////////
+
+// Return the number of parameters needed in optimizing the given lens model
+//
+// This is identical to mrcal_lensmodel_num_params(), but takes into account the
+// problem selections. Any intrinsics parameters locked down in the
+// mrcal_problem_selections_t do NOT count towards the optimization parameters
+int mrcal_num_intrinsics_optimization_params( mrcal_problem_selections_t problem_selections,
+                                              const mrcal_lensmodel_t* lensmodel );
+
+
+// Scales a state vector to the packed, unitless form used by the optimizer
+//
+// In order to make the optimization well-behaved, we scale all the variables in
+// the state and the gradients before passing them to the optimizer. The internal
+// optimization library thus works only with unitless (or "packed") data.
+//
+// This function takes an (Nstate,) array of full-units values b[], and scales
+// it to produce packed data. This function applies the scaling directly to the
+// input array; the input is modified, and nothing is returned.
+//
+// This is the inverse of mrcal_unpack_solver_state_vector()
+void mrcal_pack_solver_state_vector( // out, in
+                                     double* b,
+
+                                     // in
+                                     int Ncameras_intrinsics, int Ncameras_extrinsics,
+                                     int Nframes,
+                                     int Npoints, int Npoints_fixed, int Nobservations_board,
+                                     mrcal_problem_selections_t problem_selections,
+                                     const mrcal_lensmodel_t* lensmodel);
+
+
+// Scales a state vector from the packed, unitless form used by the optimizer
+//
+// In order to make the optimization well-behaved, we scale all the variables in
+// the state and the gradients before passing them to the optimizer. The internal
+// optimization library thus works only with unitless (or "packed") data.
+//
+// This function takes an (Nstate,) array of unitless values b[], and scales it
+// to produce full-units data. This function applies the scaling directly to the
+// input array; the input is modified, and nothing is returned.
+//
+// This is the inverse of mrcal_pack_solver_state_vector()
+void mrcal_unpack_solver_state_vector( // out, in
+                                       double* b, // unitless state on input,
+                                                  // scaled, meaningful state on
+                                                  // output
+
+                                       // in
+                                       int Ncameras_intrinsics, int Ncameras_extrinsics,
+                                       int Nframes,
+                                       int Npoints, int Npoints_fixed, int Nobservations_board,
+                                       mrcal_problem_selections_t problem_selections,
+                                       const mrcal_lensmodel_t* lensmodel);
+
+
+// Reports the icam_extrinsics corresponding to a given icam_intrinsics.
+//
+// If we're solving a vanilla calibration problem (stationary cameras observing
+// a moving calibration object), each camera has a unique intrinsics index and a
+// unique extrinsics index. This function reports the latter, given the former.
+//
+// On success, the result is written to *icam_extrinsics, and we return true. If
+// the given camera is at the reference coordinate system, it has no extrinsics,
+// and we report -1.
+//
+// If we have moving cameras (NOT a vanilla calibration problem), there isn't a
+// single icam_extrinsics for a given icam_intrinsics, and we report an error by
+// returning false
+bool mrcal_corresponding_icam_extrinsics(// out
+                                         int* icam_extrinsics,
+
+                                         // in
+                                         int icam_intrinsics,
+                                         int Ncameras_intrinsics,
+                                         int Ncameras_extrinsics,
+                                         int Nobservations_board,
+                                         const mrcal_observation_board_t* observations_board,
+                                         int Nobservations_point,
+                                         const mrcal_observation_point_t* observations_point);
+
+// Solve the given optimization problem
+//
+// This is the entry point to the mrcal optimization routine. The argument list
+// is commented.
+mrcal_stats_t
+mrcal_optimize( // out
+                // Each one of these output pointers may be NULL
+                // Shape (Nstate,)
+                double* b_packed,
+                // used only to confirm that the user passed-in the buffer they
+                // should have passed-in. The size must match exactly
+                int buffer_size_b_packed,
+
+                // Shape (Nmeasurements,)
+                double* x,
+                // used only to confirm that the user passed-in the buffer they
+                // should have passed-in. The size must match exactly
+                int buffer_size_x,
+
+                // out, in
+
+                // These are a seed on input, solution on output
+
+                // intrinsics is a concatenation of the intrinsics core and the
+                // distortion params. The specific distortion parameters may
+                // vary, depending on lensmodel, so this is a variable-length
+                // structure
+                double*             intrinsics,         // Ncameras_intrinsics * NlensParams
+                mrcal_pose_t*       extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame
+                mrcal_pose_t*       frames_toref,       // Nframes of these.    Transform TO the reference frame
+                mrcal_point3_t*     points,             // Npoints of these.    In the reference frame
+                mrcal_calobject_warp_t* calobject_warp,     // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp
+
+                // in
+                int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
+                int Npoints, int Npoints_fixed, // at the end of points[]
+
+                const mrcal_observation_board_t* observations_board,
+                const mrcal_observation_point_t* observations_point,
+                int Nobservations_board,
+                int Nobservations_point,
+
+                const mrcal_observation_point_triangulated_t* observations_point_triangulated,
+                int Nobservations_point_triangulated,
+
+                // All the board pixel observations, in an array of shape
+                //
+                // ( Nobservations_board,
+                //   calibration_object_height_n,
+                //   calibration_object_width_n )
+                //
+                // .x, .y are the pixel observations .z is the weight of the
+                // observation. Most of the weights are expected to be 1.0. Less
+                // precise observations have lower weights.
+                //
+                // .z<0 indicates that this is an outlier. This is respected on
+                // input (even if !do_apply_outlier_rejection). New outliers are
+                // marked with .z<0 on output, so this isn't const
+                mrcal_point3_t* observations_board_pool,
+
+                // Same this, but for discrete points. Array of shape
+                //
+                // ( Nobservations_point,)
+                mrcal_point3_t* observations_point_pool,
+
+                const mrcal_lensmodel_t* lensmodel,
+                const int* imagersizes, // Ncameras_intrinsics*2 of these
+                mrcal_problem_selections_t       problem_selections,
+                const mrcal_problem_constants_t* problem_constants,
+                double calibration_object_spacing,
+                int calibration_object_width_n,
+                int calibration_object_height_n,
+                bool verbose,
+
+                bool check_gradient);
+
+
+// These are cholmod_sparse, cholmod_factor, cholmod_common. I don't want to
+// include the full header that defines these in mrcal.h, and I don't need to:
+// mrcal.h just needs to know that these are a structure
+struct cholmod_sparse_struct;
+struct cholmod_factor_struct;
+struct cholmod_common_struct;
+
+// Evaluate the value of the callback function at the given operating point
+//
+// The main optimization routine in mrcal_optimize() searches for optimal
+// parameters by repeatedly calling a function to evaluate each hypothethical
+// parameter set. This evaluation function is available by itself here,
+// separated from the optimization loop. The arguments are largely the same as
+// those to mrcal_optimize(), but the inputs are all read-only It is expected
+// that this will be called from Python only.
+bool mrcal_optimizer_callback(// out
+
+                             // These output pointers may NOT be NULL, unlike
+                             // their analogues in mrcal_optimize()
+
+                             // Shape (Nstate,)
+                             double* b_packed,
+                             // used only to confirm that the user passed-in the buffer they
+                             // should have passed-in. The size must match exactly
+                             int buffer_size_b_packed,
+
+                             // Shape (Nmeasurements,)
+                             double* x,
+                             // used only to confirm that the user passed-in the buffer they
+                             // should have passed-in. The size must match exactly
+                             int buffer_size_x,
+
+                             // output Jacobian. May be NULL if we don't need
+                             // it. This is the unitless Jacobian, used by the
+                             // internal optimization routines
+                             struct cholmod_sparse_struct* Jt,
+
+
+                             // in
+
+                             // intrinsics is a concatenation of the intrinsics core
+                             // and the distortion params. The specific distortion
+                             // parameters may vary, depending on lensmodel, so
+                             // this is a variable-length structure
+                             const double*             intrinsics,         // Ncameras_intrinsics * NlensParams
+                             const mrcal_pose_t*       extrinsics_fromref, // Ncameras_extrinsics of these. Transform FROM the reference frame
+                             const mrcal_pose_t*       frames_toref,       // Nframes of these.    Transform TO the reference frame
+                             const mrcal_point3_t*     points,             // Npoints of these.    In the reference frame
+                             const mrcal_calobject_warp_t* calobject_warp, // 1 of these. May be NULL if !problem_selections.do_optimize_calobject_warp
+
+                             int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
+                             int Npoints, int Npoints_fixed, // at the end of points[]
+
+                             const mrcal_observation_board_t* observations_board,
+                             const mrcal_observation_point_t* observations_point,
+                             int Nobservations_board,
+                             int Nobservations_point,
+
+                             const mrcal_observation_point_triangulated_t* observations_point_triangulated,
+                             int Nobservations_point_triangulated,
+
+                             // All the board pixel observations, in an array of shape
+                             //
+                             // ( Nobservations_board,
+                             //   calibration_object_height_n,
+                             //   calibration_object_width_n )
+                             //
+                             // .x, .y are the pixel observations .z is the
+                             // weight of the observation. Most of the weights
+                             // are expected to be 1.0. Less precise
+                             // observations have lower weights.
+                             //
+                             // .z<0 indicates that this is an outlier
+                             const mrcal_point3_t* observations_board_pool,
+
+                             // Same this, but for discrete points. Array of shape
+                             //
+                             // ( Nobservations_point,)
+                             const mrcal_point3_t* observations_point_pool,
+
+                             const mrcal_lensmodel_t* lensmodel,
+                             const int* imagersizes, // Ncameras_intrinsics*2 of these
+                             mrcal_problem_selections_t       problem_selections,
+                             const mrcal_problem_constants_t* problem_constants,
+                             double calibration_object_spacing,
+                             int calibration_object_width_n,
+                             int calibration_object_height_n,
+                             bool verbose);
+
+bool mrcal_drt_ref_refperturbed__dbpacked(// output
+                                          // Shape (6,Nstate_frames)
+                                          double* Kpackedf,
+                                          int Kpackedf_stride0, // in bytes. <= 0 means "contiguous"
+                                          int Kpackedf_stride1, // in bytes. <= 0 means "contiguous"
+
+                                          // Shape (6,Nstate_points)
+                                          double* Kpackedp,
+                                          int Kpackedp_stride0, // in bytes. <= 0 means "contiguous"
+                                          int Kpackedp_stride1, // in bytes. <= 0 means "contiguous"
+
+                                          // Shape (6,Nstate_calobject_warp)
+                                          double* Kpackedcw,
+                                          int Kpackedcw_stride0, // in bytes. <= 0 means "contiguous"
+                                          int Kpackedcw_stride1, // in bytes. <= 0 means "contiguous"
+
+                                          // inputs
+                                          // stuff that describes this solve
+                                          const double* b_packed,
+                                          // used only to confirm that the user passed-in the buffer they
+                                          // should have passed-in. The size must match exactly
+                                          int buffer_size_b_packed,
+
+                                          // The unitless (packed) Jacobian,
+                                          // used by the internal optimization
+                                          // routines cholmod_analyze() and
+                                          // cholmod_factorize() require
+                                          // non-const
+                                          /* const */
+                                          struct cholmod_sparse_struct* Jt,
+
+                                          // meta-parameters
+                                          int Ncameras_intrinsics, int Ncameras_extrinsics, int Nframes,
+                                          int Npoints, int Npoints_fixed, // at the end of points[]
+                                          int Nobservations_board,
+                                          int Nobservations_point,
+
+                                          const mrcal_lensmodel_t* lensmodel,
+                                          mrcal_problem_selections_t problem_selections,
+
+                                          int calibration_object_width_n,
+                                          int calibration_object_height_n);
+
+
+////////////////////////////////////////////////////////////////////////////////
+//////////////////// Layout of the measurement and state vectors
+////////////////////////////////////////////////////////////////////////////////
+
+// The optimization routine tries to minimize the length of the measurement
+// vector x by moving around the state vector b.
+//
+// Depending on the specific optimization problem being solved and the
+// mrcal_problem_selections_t, the state vector may contain any of
+// - The lens parameters
+// - The geometry of the cameras
+// - The geometry of the observed chessboards and discrete points
+// - The chessboard shape
+//
+// The measurement vector may contain
+// - The errors in observations of the chessboards
+// - The errors in observations of discrete points
+// - The penalties in the solved point positions
+// - The regularization terms
+//
+// Given the problem selections and a vector b or x it is often useful to know
+// where specific quantities lie in those vectors. We have 4 sets of functions
+// to answer such questions:
+//
+// int mrcal_measurement_index_THING()
+//   Returns the index in the measurement vector x where the contiguous block of
+//   values describing the THING begins. THING is any of
+//   - boards
+//   - points
+//   - regularization
+//
+// int mrcal_num_measurements_THING()
+//   Returns the number of values in the contiguous block in the measurement
+//   vector x that describe the given THING. THING is any of
+//   - boards
+//   - points
+//   - regularization
+//
+// int mrcal_state_index_THING()
+//   Returns the index in the state vector b where the contiguous block of
+//   values describing the THING begins. THING is any of
+//   - intrinsics
+//   - extrinsics
+//   - frames
+//   - points
+//   - calobject_warp
+//   If we're not optimizing the THING, return <0
+//
+// int mrcal_num_states_THING()
+//   Returns the number of values in the contiguous block in the state
+//   vector b that describe the given THING. THING is any of
+//   - intrinsics
+//   - extrinsics
+//   - frames
+//   - points
+//   - calobject_warp
+//   If we're not optimizing the THING, return 0
+int mrcal_measurement_index_boards(int i_observation_board,
+                                   int Nobservations_board,
+                                   int Nobservations_point,
+                                   int calibration_object_width_n,
+                                   int calibration_object_height_n);
+int mrcal_num_measurements_boards(int Nobservations_board,
+                                  int calibration_object_width_n,
+                                  int calibration_object_height_n);
+int mrcal_measurement_index_points(int i_observation_point,
+                                   int Nobservations_board,
+                                   int Nobservations_point,
+                                   int calibration_object_width_n,
+                                   int calibration_object_height_n);
+int mrcal_num_measurements_points(int Nobservations_point);
+int mrcal_measurement_index_points_triangulated(int i_point_triangulated,
+                                                int Nobservations_board,
+                                                int Nobservations_point,
+
+                                                // May be NULL if we don't have any of these
+                                                const mrcal_observation_point_triangulated_t* observations_point_triangulated,
+                                                int Nobservations_point_triangulated,
+
+                                                int calibration_object_width_n,
+                                                int calibration_object_height_n);
+int mrcal_num_measurements_points_triangulated(// May be NULL if we don't have any of these
+                                               const mrcal_observation_point_triangulated_t* observations_point_triangulated,
+                                               int Nobservations_point_triangulated);
+int mrcal_num_measurements_points_triangulated_initial_Npoints(// May be NULL if we don't have any of these
+                                                               const mrcal_observation_point_triangulated_t* observations_point_triangulated,
+                                                               int Nobservations_point_triangulated,
+
+                                                               // Only consider the leading Npoints. If Npoints < 0: take ALL the points
+                                                               int Npoints);
+bool mrcal_decode_observation_indices_points_triangulated(
+    // output
+    int* iobservation0,
+    int* iobservation1,
+    int* iobservation_point0,
+    int* Nobservations_this_point,
+    int* Nmeasurements_this_point,
+    int* ipoint,
+
+    // input
+    const int imeasurement,
+
+    const mrcal_observation_point_triangulated_t* observations_point_triangulated,
+    int Nobservations_point_triangulated);
+
+int mrcal_measurement_index_regularization(// May be NULL if we don't have any of these
+                                           const mrcal_observation_point_triangulated_t* observations_point_triangulated,
+                                           int Nobservations_point_triangulated,
+
+                                           int calibration_object_width_n,
+                                           int calibration_object_height_n,
+                                           int Ncameras_intrinsics, int Ncameras_extrinsics,
+                                           int Nframes,
+                                           int Npoints, int Npoints_fixed, int Nobservations_board, int Nobservations_point,
+                                           mrcal_problem_selections_t problem_selections,
+                                           const mrcal_lensmodel_t* lensmodel);
+int mrcal_num_measurements_regularization(int Ncameras_intrinsics, int Ncameras_extrinsics,
+                                          int Nframes,
+                                          int Npoints, int Npoints_fixed, int Nobservations_board,
+                                          mrcal_problem_selections_t problem_selections,
+                                          const mrcal_lensmodel_t* lensmodel);
+
+int mrcal_num_measurements(int Nobservations_board,
+                           int Nobservations_point,
+
+                           // May be NULL if we don't have any of these
+                           const mrcal_observation_point_triangulated_t* observations_point_triangulated,
+                           int Nobservations_point_triangulated,
+
+                           int calibration_object_width_n,
+                           int calibration_object_height_n,
+                           int Ncameras_intrinsics, int Ncameras_extrinsics,
+                           int Nframes,
+                           int Npoints, int Npoints_fixed,
+                           mrcal_problem_selections_t problem_selections,
+                           const mrcal_lensmodel_t* lensmodel);
+
+int mrcal_num_states(int Ncameras_intrinsics, int Ncameras_extrinsics,
+                     int Nframes,
+                     int Npoints, int Npoints_fixed, int Nobservations_board,
+                     mrcal_problem_selections_t problem_selections,
+                     const mrcal_lensmodel_t* lensmodel);
+int mrcal_state_index_intrinsics(int icam_intrinsics,
+                                 int Ncameras_intrinsics, int Ncameras_extrinsics,
+                                 int Nframes,
+                                 int Npoints, int Npoints_fixed, int Nobservations_board,
+                                 mrcal_problem_selections_t problem_selections,
+                                 const mrcal_lensmodel_t* lensmodel);
+int mrcal_num_states_intrinsics(int Ncameras_intrinsics,
+                                mrcal_problem_selections_t problem_selections,
+                                const mrcal_lensmodel_t* lensmodel);
+int mrcal_state_index_extrinsics(int icam_extrinsics,
+                                 int Ncameras_intrinsics, int Ncameras_extrinsics,
+                                 int Nframes,
+                                 int Npoints, int Npoints_fixed, int Nobservations_board,
+                                 mrcal_problem_selections_t problem_selections,
+                                 const mrcal_lensmodel_t* lensmodel);
+int mrcal_num_states_extrinsics(int Ncameras_extrinsics,
+                                mrcal_problem_selections_t problem_selections);
+int mrcal_state_index_frames(int iframe,
+                             int Ncameras_intrinsics, int Ncameras_extrinsics,
+                             int Nframes,
+                             int Npoints, int Npoints_fixed, int Nobservations_board,
+                             mrcal_problem_selections_t problem_selections,
+                             const mrcal_lensmodel_t* lensmodel);
+int mrcal_num_states_frames(int Nframes,
+                            mrcal_problem_selections_t problem_selections);
+int mrcal_state_index_points(int i_point,
+                             int Ncameras_intrinsics, int Ncameras_extrinsics,
+                             int Nframes,
+                             int Npoints, int Npoints_fixed, int Nobservations_board,
+                             mrcal_problem_selections_t problem_selections,
+                             const mrcal_lensmodel_t* lensmodel);
+int mrcal_num_states_points(int Npoints, int Npoints_fixed,
+                            mrcal_problem_selections_t problem_selections);
+int mrcal_state_index_calobject_warp(int Ncameras_intrinsics, int Ncameras_extrinsics,
+                                     int Nframes,
+                                     int Npoints, int Npoints_fixed, int Nobservations_board,
+                                     mrcal_problem_selections_t problem_selections,
+                                     const mrcal_lensmodel_t* lensmodel);
+int mrcal_num_states_calobject_warp(mrcal_problem_selections_t problem_selections,
+                                    int Nobservations_board);
+
+
+// if len>0, the string doesn't need to be 0-terminated. If len<=0, the end of
+// the buffer IS indicated by a '\0' byte
+//
+// return NULL on error
+mrcal_cameramodel_t* mrcal_read_cameramodel_string(const char* string, int len);
+mrcal_cameramodel_t* mrcal_read_cameramodel_file  (const char* filename);
+void                 mrcal_free_cameramodel(mrcal_cameramodel_t** cameramodel);
+
+bool mrcal_write_cameramodel_file(const char* filename,
+                                  const mrcal_cameramodel_t* cameramodel);
+
+#ifdef __cplusplus
+} // extern "C"
+#endif
+#define DECLARE_mrcal_apply_color_map(T,Tname)                          \
+    bool mrcal_apply_color_map_##Tname(                                 \
+        mrcal_image_bgr_t*    out,                                      \
+        const mrcal_image_##Tname##_t* in,                              \
+                                                                        \
+        /* If true, I set in_min/in_max from the */                     \
+        /* min/max of the input data */                                 \
+        const bool auto_min,                                            \
+        const bool auto_max,                                            \
+                                                                        \
+        /* If true, I implement gnuplot's default 7,5,15 mapping. */    \
+        /* This is a reasonable default choice. */                      \
+        /* function_red/green/blue are ignored if true */               \
+        const bool auto_function,                                       \
+                                                                        \
+        /* min/max input values to use if not */                        \
+        /* auto_min/auto_max */                                         \
+        T in_min, /* will map to 0 */                                   \
+        T in_max, /* will map to 255 */                                 \
+                                                                        \
+        /* The color mappings to use. If !auto_function */              \
+        int function_red,                                               \
+        int function_green,                                             \
+        int function_blue)
+
+DECLARE_mrcal_apply_color_map(uint8_t,  uint8);
+DECLARE_mrcal_apply_color_map(uint16_t, uint16);
+DECLARE_mrcal_apply_color_map(uint32_t, uint32);
+DECLARE_mrcal_apply_color_map(uint64_t, uint64);
+
+DECLARE_mrcal_apply_color_map(int8_t,   int8);
+DECLARE_mrcal_apply_color_map(int16_t,  int16);
+DECLARE_mrcal_apply_color_map(int32_t,  int32);
+DECLARE_mrcal_apply_color_map(int64_t,  int64);
+
+DECLARE_mrcal_apply_color_map(float,    float);
+DECLARE_mrcal_apply_color_map(double,   double);
+
+#undef DECLARE_mrcal_apply_color_map
+
+
+
+// returns false on error
+typedef bool (*mrcal_callback_sensor_link_t)(const uint16_t idx_to,
+                                             const uint16_t idx_from,
+                                             void* cookie);
+
+// Traverses a connectivity graph of sensors to find the best connection from
+// the root sensor (idx==0) to every other sensor. This is useful to seed a
+// problem with sparse connections, where every sensor doesn't have overlapping
+// observations with every other sensor. See the docstring for
+// mrcal.traverse_sensor_links() for details (that Python function wraps
+// this one). Note: this C function takes a packed connectivity matrix (just the
+// upper triangle stored), while the Python function takes a full (N,N) array,
+// while assuming it is symmetric and has a 0 diagonal
+//
+// returns false on error
+bool mrcal_traverse_sensor_links( const uint16_t Nsensors,
+
+                                        // (N,N) symmetric matrix with a 0 diagonal.
+                                        // I store the upper triangle only,
+                                        // row-first: a 1D array of (N*(N-1)/2)
+                                        // values. use pairwise_index() to index
+                                        const uint16_t* connectivity_matrix,
+                                        const mrcal_callback_sensor_link_t cb,
+                                        void* cookie);
+
+
+// Public ABI stuff, that's not for end-user consumption
+#include "mrcal-internal.h"
+

wpical/src/main/native/thirdparty/mrcal/include/poseutils.h

@@ -0,0 +1,586 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+#include <stdbool.h>
+
+// Unless specified all arrays stored in contiguous matrices in row-major order.
+//
+// All functions are defined using the mrcal_..._full() form, which supports
+// non-contiguous input and output arrays, and some optional arguments. Strides
+// are used to specify the array layout.
+//
+// All functions have a convenience wrapper macro that is a simpler way to call
+// the function, usable with contiguous arrays and defaults.
+//
+// All the functions use double-precision floating point to store the data, and
+// C ints to store strides. The strides are given in bytes. In the
+// mrcal_..._full() functions, each array is followed by the strides, one per
+// dimension.
+//
+// I have two different representations of pose transformations:
+//
+// - Rt is a concatenated (4,3) array: Rt = nps.glue(R,t, axis=-2). The
+//   transformation is R*x+t
+//
+// - rt is a concatenated (6,) array: rt = nps.glue(r,t, axis=-1). The
+//   transformation is R*x+t where R = R_from_r(r)
+//
+// I treat all vectors as column vectors, so matrix multiplication works from
+// the left: to rotate a vector x by a rotation matrix R I have
+//
+//   x_rotated = R * x
+
+
+// Store an identity rotation matrix into the given (3,3) array
+//
+// This is simply an identity matrix
+#define mrcal_identity_R(R) mrcal_identity_R_full(R,0,0)
+void mrcal_identity_R_full(double* R,      // (3,3) array
+                           int R_stride0,  // in bytes. <= 0 means "contiguous"
+                           int R_stride1   // in bytes. <= 0 means "contiguous"
+                           );
+
+// Store an identity rodrigues rotation into the given (3,) array
+//
+// This is simply an array of zeros
+#define mrcal_identity_r(r) mrcal_identity_r_full(r,0)
+void mrcal_identity_r_full(double* r,      // (3,) array
+                           int r_stride0   // in bytes. <= 0 means "contiguous"
+                           );
+
+// Store an identity Rt transformation into the given (4,3) array
+#define mrcal_identity_Rt(Rt) mrcal_identity_Rt_full(Rt,0,0)
+void mrcal_identity_Rt_full(double* Rt,      // (4,3) array
+                            int Rt_stride0,  // in bytes. <= 0 means "contiguous"
+                            int Rt_stride1   // in bytes. <= 0 means "contiguous"
+                            );
+
+// Store an identity rt transformation into the given (6,) array
+#define mrcal_identity_rt(rt)  mrcal_identity_rt_full(rt,0)
+void mrcal_identity_rt_full(double* rt,      // (6,) array
+                            int rt_stride0   // in bytes. <= 0 means "contiguous"
+                            );
+
+// Rotate the point x_in in a (3,) array by the rotation matrix R in a (3,3)
+// array. This is simply the matrix-vector multiplication R x_in
+//
+// The result is returned in a (3,) array x_out.
+//
+// The gradient dx_out/dR is returned in a (3, 3,3) array J_R. Set to NULL if
+// this is not wanted
+//
+// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply
+// the matrix R. Set to NULL if this is not wanted
+//
+// In-place operation is supported; the output array may be the same as the
+// input arrays to overwrite the input.
+#define mrcal_rotate_point_R(         x_out,J_R,J_x,R,x_in) mrcal_rotate_point_R_full(x_out,0,J_R,0,0,0,J_x,0,0,R,0,0,x_in,0, false)
+#define mrcal_rotate_point_R_inverted(x_out,J_R,J_x,R,x_in) mrcal_rotate_point_R_full(x_out,0,J_R,0,0,0,J_x,0,0,R,0,0,x_in,0, true)
+void mrcal_rotate_point_R_full( // output
+                               double* x_out,      // (3,) array
+                               int x_out_stride0,  // in bytes. <= 0 means "contiguous"
+                               double* J_R,        // (3,3,3) array. May be NULL
+                               int J_R_stride0,    // in bytes. <= 0 means "contiguous"
+                               int J_R_stride1,    // in bytes. <= 0 means "contiguous"
+                               int J_R_stride2,    // in bytes. <= 0 means "contiguous"
+                               double* J_x,        // (3,3) array. May be NULL
+                               int J_x_stride0,    // in bytes. <= 0 means "contiguous"
+                               int J_x_stride1,    // in bytes. <= 0 means "contiguous"
+
+                               // input
+                               const double* R,    // (3,3) array. May be NULL
+                               int R_stride0,      // in bytes. <= 0 means "contiguous"
+                               int R_stride1,      // in bytes. <= 0 means "contiguous"
+                               const double* x_in, // (3,) array. May be NULL
+                               int x_in_stride0,   // in bytes. <= 0 means "contiguous"
+
+                               bool inverted       // if true, I apply a
+                                                   // rotation in the opposite
+                                                   // direction. J_R corresponds
+                                                   // to the input R
+                               );
+
+// Rotate the point x_in in a (3,) array by the rodrigues rotation in a (3,)
+// array.
+//
+// The result is returned in a (3,) array x_out.
+//
+// The gradient dx_out/dr is returned in a (3,3) array J_r. Set to NULL if this
+// is not wanted
+//
+// The gradient dx_out/dx_in is returned in a (3,3) array J_x. Set to NULL if
+// this is not wanted
+//
+// In-place operation is supported; the output array may be the same as the
+// input arrays to overwrite the input.
+#define mrcal_rotate_point_r(         x_out,J_r,J_x,r,x_in) mrcal_rotate_point_r_full(x_out,0,J_r,0,0,J_x,0,0,r,0,x_in,0, false)
+#define mrcal_rotate_point_r_inverted(x_out,J_r,J_x,r,x_in) mrcal_rotate_point_r_full(x_out,0,J_r,0,0,J_x,0,0,r,0,x_in,0, true)
+void mrcal_rotate_point_r_full( // output
+                               double* x_out,      // (3,) array
+                               int x_out_stride0,  // in bytes. <= 0 means "contiguous"
+                               double* J_r,        // (3,3) array. May be NULL
+                               int J_r_stride0,    // in bytes. <= 0 means "contiguous"
+                               int J_r_stride1,    // in bytes. <= 0 means "contiguous"
+                               double* J_x,        // (3,3) array. May be NULL
+                               int J_x_stride0,    // in bytes. <= 0 means "contiguous"
+                               int J_x_stride1,    // in bytes. <= 0 means "contiguous"
+
+                               // input
+                               const double* r,    // (3,) array. May be NULL
+                               int r_stride0,      // in bytes. <= 0 means "contiguous"
+                               const double* x_in, // (3,) array. May be NULL
+                               int x_in_stride0,   // in bytes. <= 0 means "contiguous"
+
+                               bool inverted       // if true, I apply a
+                                                   // rotation in the opposite
+                                                   // direction. J_r corresponds
+                                                   // to the input r
+                               );
+
+
+// Transform the point x_in in a (3,) array by the Rt transformation in a (4,3)
+// array.
+//
+// The result is returned in a (3,) array x_out.
+//
+// The gradient dx_out/dRt is returned in a (3, 4,3) array J_Rt. Set to NULL if
+// this is not wanted
+//
+// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply
+// the matrix R. Set to NULL if this is not wanted
+//
+// In-place operation is supported; the output array may be the same as the
+// input arrays to overwrite the input.
+#define mrcal_transform_point_Rt(         x_out,J_Rt,J_x,Rt,x_in) mrcal_transform_point_Rt_full(x_out,0,J_Rt,0,0,0,J_x,0,0,Rt,0,0,x_in,0, false)
+#define mrcal_transform_point_Rt_inverted(x_out,J_Rt,J_x,Rt,x_in) mrcal_transform_point_Rt_full(x_out,0,J_Rt,0,0,0,J_x,0,0,Rt,0,0,x_in,0, true)
+void mrcal_transform_point_Rt_full( // output
+                                   double* x_out,      // (3,) array
+                                   int x_out_stride0,  // in bytes. <= 0 means "contiguous"
+                                   double* J_Rt,       // (3,4,3) array. May be NULL
+                                   int J_Rt_stride0,   // in bytes. <= 0 means "contiguous"
+                                   int J_Rt_stride1,   // in bytes. <= 0 means "contiguous"
+                                   int J_Rt_stride2,   // in bytes. <= 0 means "contiguous"
+                                   double* J_x,        // (3,3) array. May be NULL
+                                   int J_x_stride0,    // in bytes. <= 0 means "contiguous"
+                                   int J_x_stride1,    // in bytes. <= 0 means "contiguous"
+
+                                   // input
+                                   const double* Rt,   // (4,3) array. May be NULL
+                                   int Rt_stride0,     // in bytes. <= 0 means "contiguous"
+                                   int Rt_stride1,     // in bytes. <= 0 means "contiguous"
+                                   const double* x_in, // (3,) array. May be NULL
+                                   int x_in_stride0,   // in bytes. <= 0 means "contiguous"
+
+                                   bool inverted       // if true, I apply a
+                                                       // transformation in the opposite
+                                                       // direction. J_Rt corresponds
+                                                       // to the input Rt
+                                   );
+
+// Transform the point x_in in a (3,) array by the rt transformation in a (6,)
+// array.
+//
+// The result is returned in a (3,) array x_out.
+//
+// The gradient dx_out/drt is returned in a (3,6) array J_rt. Set to NULL if
+// this is not wanted
+//
+// The gradient dx_out/dx_in is returned in a (3,3) array J_x. This is simply
+// the matrix R. Set to NULL if this is not wanted
+//
+// In-place operation is supported; the output array may be the same as the
+// input arrays to overwrite the input.
+#define mrcal_transform_point_rt(         x_out,J_rt,J_x,rt,x_in) mrcal_transform_point_rt_full(x_out,0,J_rt,0,0,J_x,0,0,rt,0,x_in,0, false)
+#define mrcal_transform_point_rt_inverted(x_out,J_rt,J_x,rt,x_in) mrcal_transform_point_rt_full(x_out,0,J_rt,0,0,J_x,0,0,rt,0,x_in,0, true)
+void mrcal_transform_point_rt_full( // output
+                                   double* x_out,      // (3,) array
+                                   int x_out_stride0,  // in bytes. <= 0 means "contiguous"
+                                   double* J_rt,       // (3,6) array. May be NULL
+                                   int J_rt_stride0,   // in bytes. <= 0 means "contiguous"
+                                   int J_rt_stride1,   // in bytes. <= 0 means "contiguous"
+                                   double* J_x,        // (3,3) array. May be NULL
+                                   int J_x_stride0,    // in bytes. <= 0 means "contiguous"
+                                   int J_x_stride1,    // in bytes. <= 0 means "contiguous"
+
+                                   // input
+                                   const double* rt,   // (6,) array. May be NULL
+                                   int rt_stride0,     // in bytes. <= 0 means "contiguous"
+                                   const double* x_in, // (3,) array. May be NULL
+                                   int x_in_stride0,   // in bytes. <= 0 means "contiguous"
+
+                                   bool inverted       // if true, I apply the
+                                                       // transformation in the
+                                                       // opposite direction.
+                                                       // J_rt corresponds to
+                                                       // the input rt
+                                   );
+
+// Convert a rotation matrix in a (3,3) array to a rodrigues vector in a (3,)
+// array
+//
+// The result is returned in a (3,) array r
+//
+// The gradient dr/dR is returned in a (3, 3,3) array J. Set to NULL if this is
+// not wanted
+#define mrcal_r_from_R(r,J,R) mrcal_r_from_R_full(r,0,J,0,0,0,R,0,0)
+void mrcal_r_from_R_full( // output
+                         double* r,       // (3,) vector
+                         int r_stride0,   // in bytes. <= 0 means "contiguous"
+                         double* J,       // (3,3,3) array. Gradient. May be NULL
+                         int J_stride0,   // in bytes. <= 0 means "contiguous"
+                         int J_stride1,   // in bytes. <= 0 means "contiguous"
+                         int J_stride2,   // in bytes. <= 0 means "contiguous"
+
+                         // input
+                         const double* R, // (3,3) array
+                         int R_stride0,   // in bytes. <= 0 means "contiguous"
+                         int R_stride1    // in bytes. <= 0 means "contiguous"
+                         );
+
+// Convert a rodrigues vector in a (3,) array to a rotation matrix in a (3,3)
+// array
+//
+// The result is returned in a (3,3) array R
+//
+// The gradient dR/dr is returned in a (3,3 ,3) array J. Set to NULL if this is
+// not wanted
+#define mrcal_R_from_r(R,J,r) mrcal_R_from_r_full(R,0,0,J,0,0,0,r,0)
+void mrcal_R_from_r_full( // outputs
+                         double* R,       // (3,3) array
+                         int R_stride0,   // in bytes. <= 0 means "contiguous"
+                         int R_stride1,   // in bytes. <= 0 means "contiguous"
+                         double* J,       // (3,3,3) array. Gradient. May be NULL
+                         int J_stride0,   // in bytes. <= 0 means "contiguous"
+                         int J_stride1,   // in bytes. <= 0 means "contiguous"
+                         int J_stride2,   // in bytes. <= 0 means "contiguous"
+
+                         // input
+                         const double* r, // (3,) vector
+                         int r_stride0    // in bytes. <= 0 means "contiguous"
+                          );
+
+// Invert a rotation matrix. This is a transpose
+//
+// The input is given in R_in in a (3,3) array
+//
+// The result is returned in a (3,3) array R_out
+//
+// In-place operation is supported; the output array may be the same as the
+// input arrays to overwrite the input.
+#define mrcal_invert_R(R_out,R_in) mrcal_invert_R_full(R_out,0,0,R_in,0,0)
+void mrcal_invert_R_full( // output
+                         double* R_out,      // (3,3) array
+                         int R_out_stride0,  // in bytes. <= 0 means "contiguous"
+                         int R_out_stride1,  // in bytes. <= 0 means "contiguous"
+
+                         // input
+                         const double* R_in, // (3,3) array
+                         int R_in_stride0,   // in bytes. <= 0 means "contiguous"
+                         int R_in_stride1    // in bytes. <= 0 means "contiguous"
+                         );
+
+// Convert an Rt transformation in a (4,3) array to an rt transformation in a
+// (6,) array
+//
+// The result is returned in a (6,) array rt
+//
+// The gradient dr/dR is returned in a (3, 3,3) array J_R. Set to NULL if this
+// is not wanted
+//
+// The t terms are identical, so dt/dt = identity and I do not return it
+//
+// The r and R terms are independent of the t terms, so dr/dt and dt/dR are both
+// 0, and I do not return them
+#define mrcal_rt_from_Rt(rt,J_R,Rt) mrcal_rt_from_Rt_full(rt,0,J_R,0,0,0,Rt,0,0)
+void mrcal_rt_from_Rt_full(   // output
+                           double* rt,      // (6,) vector
+                           int rt_stride0,  // in bytes. <= 0 means "contiguous"
+                           double* J_R,     // (3,3,3) array. Gradient. May be NULL
+                           // No J_t. It's always the identity
+                           int J_R_stride0, // in bytes. <= 0 means "contiguous"
+                           int J_R_stride1, // in bytes. <= 0 means "contiguous"
+                           int J_R_stride2, // in bytes. <= 0 means "contiguous"
+
+                           // input
+                           const double* Rt,  // (4,3) array
+                           int Rt_stride0,    // in bytes. <= 0 means "contiguous"
+                           int Rt_stride1     // in bytes. <= 0 means "contiguous"
+                           );
+
+// Convert an rt transformation in a (6,) array to an Rt transformation in a
+// (4,3) array
+//
+// The result is returned in a (4,3) array Rt
+//
+// The gradient dR/dr is returned in a (3,3 ,3) array J_r. Set to NULL if this
+// is not wanted
+//
+// The t terms are identical, so dt/dt = identity and I do not return it
+//
+// The r and R terms are independent of the t terms, so dR/dt and dt/dr are both
+// 0, and I do not return them
+#define mrcal_Rt_from_rt(Rt,J_r,rt) mrcal_Rt_from_rt_full(Rt,0,0,J_r,0,0,0,rt,0)
+void mrcal_Rt_from_rt_full(   // output
+                           double* Rt,      // (4,3) array
+                           int Rt_stride0,  // in bytes. <= 0 means "contiguous"
+                           int Rt_stride1,  // in bytes. <= 0 means "contiguous"
+                           double* J_r,     // (3,3,3) array. Gradient. May be NULL
+                           // No J_t. It's just the identity
+                           int J_r_stride0, // in bytes. <= 0 means "contiguous"
+                           int J_r_stride1, // in bytes. <= 0 means "contiguous"
+                           int J_r_stride2, // in bytes. <= 0 means "contiguous"
+
+                           // input
+                           const double* rt, // (6,) vector
+                           int rt_stride0    // in bytes. <= 0 means "contiguous"
+                           );
+
+// Invert an Rt transformation
+//
+// The input is given in Rt_in in a (4,3) array
+//
+// The result is returned in a (4,3) array Rt_out
+//
+// In-place operation is supported; the output array may be the same as the
+// input arrays to overwrite the input.
+#define mrcal_invert_Rt(Rt_out,Rt_in) mrcal_invert_Rt_full(Rt_out,0,0,Rt_in,0,0)
+void mrcal_invert_Rt_full( // output
+                          double* Rt_out,      // (4,3) array
+                          int Rt_out_stride0,  // in bytes. <= 0 means "contiguous"
+                          int Rt_out_stride1,  // in bytes. <= 0 means "contiguous"
+
+                          // input
+                          const double* Rt_in, // (4,3) array
+                          int Rt_in_stride0,   // in bytes. <= 0 means "contiguous"
+                          int Rt_in_stride1    // in bytes. <= 0 means "contiguous"
+                          );
+
+// Invert an rt transformation
+//
+// The input is given in rt_in in a (6,) array
+//
+// The result is returned in a (6,) array rt_out
+//
+// The gradient dtout/drin is returned in a (3,3) array dtout_drin. Set to NULL
+// if this is not wanted
+//
+// The gradient dtout/dtin is returned in a (3,3) array dtout_dtin. Set to NULL
+// if this is not wanted
+//
+// The gradient drout/drin is always -identity. So it is not returned
+//
+// The gradient drout/dtin is always 0. So it is not returned
+//
+// In-place operation is supported; the output array may be the same as the
+// input arrays to overwrite the input.
+#define mrcal_invert_rt(rt_out,dtout_drin,dtout_dtin,rt_in) mrcal_invert_rt_full(rt_out,0,dtout_drin,0,0,dtout_dtin,0,0,rt_in,0)
+void mrcal_invert_rt_full( // output
+                          double* rt_out,          // (6,) array
+                          int rt_out_stride0,      // in bytes. <= 0 means "contiguous"
+                          double* dtout_drin,      // (3,3) array
+                          int dtout_drin_stride0,  // in bytes. <= 0 means "contiguous"
+                          int dtout_drin_stride1,  // in bytes. <= 0 means "contiguous"
+                          double* dtout_dtin,      // (3,3) array
+                          int dtout_dtin_stride0,  // in bytes. <= 0 means "contiguous"
+                          int dtout_dtin_stride1,  // in bytes. <= 0 means "contiguous"
+
+                          // input
+                          const double* rt_in,     // (6,) array
+                          int rt_in_stride0        // in bytes. <= 0 means "contiguous"
+                          );
+
+// Compose two Rt transformations
+//
+// Rt = Rt0 * Rt1  --->  Rt(x) = Rt0( Rt1(x) )
+//
+// The input transformations are given in (4,3) arrays Rt_0 and Rt_1
+//
+// The result is returned in a (4,3) array Rt_out
+//
+// In-place operation is supported; the output array may be the same as either
+// of the input arrays to overwrite the input.
+#define mrcal_compose_Rt(Rt_out,Rt_0,Rt_1) mrcal_compose_Rt_full(Rt_out,0,0,Rt_0,0,0,Rt_1,0,0)
+void mrcal_compose_Rt_full( // output
+                           double* Rt_out,      // (4,3) array
+                           int Rt_out_stride0,  // in bytes. <= 0 means "contiguous"
+                           int Rt_out_stride1,  // in bytes. <= 0 means "contiguous"
+
+                           // input
+                           const double* Rt_0,  // (4,3) array
+                           int Rt_0_stride0,    // in bytes. <= 0 means "contiguous"
+                           int Rt_0_stride1,    // in bytes. <= 0 means "contiguous"
+                           const double* Rt_1,  // (4,3) array
+                           int Rt_1_stride0,    // in bytes. <= 0 means "contiguous"
+                           int Rt_1_stride1     // in bytes. <= 0 means "contiguous"
+                           );
+
+// Compose two rt transformations
+//
+// rt = rt0 * rt1  --->  rt(x) = rt0( rt1(x) )
+//
+// The input transformations are given in (6,) arrays rt_0 and rt_1
+//
+// The result is returned in a (6,) array rt_out
+//
+// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this
+// is not wanted
+//
+// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this
+// is not wanted
+//
+// The gradient dt/dr0 is returned in a (3,3) array dt_dr0. Set to NULL if this
+// is not wanted
+//
+// The gradient dt/dt1 is returned in a (3,3) array dt_dt1. Set to NULL if this
+// is not wanted
+//
+// The gradients dr/dt0, dr/dt1, dt/dr1 are always 0, so they are never returned
+//
+// The gradient dt/dt0 is always identity, so it is never returned
+//
+// In-place operation is supported; the output array may be the same as either
+// of the input arrays to overwrite the input.
+#define mrcal_compose_rt(rt_out,dr_dr0,dr_dr1,dt_dr0,dt_dt1,rt_0,rt_1) mrcal_compose_rt_full(rt_out,0,dr_dr0,0,0,dr_dr1,0,0,dt_dr0,0,0,dt_dt1,0,0,rt_0,0,rt_1,0)
+void mrcal_compose_rt_full( // output
+                           double* rt_out,       // (6,) array
+                           int rt_out_stride0,   // in bytes. <= 0 means "contiguous"
+                           double* dr_dr0,       // (3,3) array; may be NULL
+                           int dr_dr0_stride0,   // in bytes. <= 0 means "contiguous"
+                           int dr_dr0_stride1,   // in bytes. <= 0 means "contiguous"
+                           double* dr_dr1,       // (3,3) array; may be NULL
+                           int dr_dr1_stride0,   // in bytes. <= 0 means "contiguous"
+                           int dr_dr1_stride1,   // in bytes. <= 0 means "contiguous"
+                           double* dt_dr0,       // (3,3) array; may be NULL
+                           int dt_dr0_stride0,   // in bytes. <= 0 means "contiguous"
+                           int dt_dr0_stride1,   // in bytes. <= 0 means "contiguous"
+                           double* dt_dt1,       // (3,3) array; may be NULL
+                           int dt_dt1_stride0,   // in bytes. <= 0 means "contiguous"
+                           int dt_dt1_stride1,   // in bytes. <= 0 means "contiguous"
+
+                           // input
+                           const double* rt_0,   // (6,) array
+                           int rt_0_stride0,     // in bytes. <= 0 means "contiguous"
+                           const double* rt_1,   // (6,) array
+                           int rt_1_stride0      // in bytes. <= 0 means "contiguous"
+                           );
+
+// Compose two angle-axis rotations
+//
+// r = r0 * r1  --->  r(x) = r0( r1(x) )
+//
+// The input rotations are given in (3,) arrays r_0 and r_1
+//
+// The result is returned in a (3,) array r_out
+//
+// The gradient dr/dr0 is returned in a (3,3) array dr_dr0. Set to NULL if this
+// is not wanted
+//
+// The gradient dr/dr1 is returned in a (3,3) array dr_dr1. Set to NULL if this
+// is not wanted
+//
+// In-place operation is supported; the output array may be the same as either
+// of the input arrays to overwrite the input.
+#define mrcal_compose_r(r_out,dr_dr0,dr_dr1,r_0,r_1) mrcal_compose_r_full(r_out,0,dr_dr0,0,0,dr_dr1,0,0,r_0,0,r_1,0)
+void mrcal_compose_r_full( // output
+                           double* r_out,       // (3,) array
+                           int r_out_stride0,   // in bytes. <= 0 means "contiguous"
+                           double* dr_dr0,      // (3,3) array; may be NULL
+                           int dr_dr0_stride0,  // in bytes. <= 0 means "contiguous"
+                           int dr_dr0_stride1,  // in bytes. <= 0 means "contiguous"
+                           double* dr_dr1,      // (3,3) array; may be NULL
+                           int dr_dr1_stride0,  // in bytes. <= 0 means "contiguous"
+                           int dr_dr1_stride1,  // in bytes. <= 0 means "contiguous"
+
+                           // input
+                           const double* r_0,   // (3,) array
+                           int r_0_stride0,     // in bytes. <= 0 means "contiguous"
+                           const double* r_1,   // (3,) array
+                           int r_1_stride0      // in bytes. <= 0 means "contiguous"
+                           );
+
+// Special-case rotation compositions for the uncertainty computation
+//
+// Same as mrcal_compose_r() except
+//
+// - r0 is assumed to be 0, so we don't ingest it, and we don't report the
+//   composition result
+// - we ONLY report the dr01/dr0 gradient
+//
+// In python this function is equivalent to
+//
+//   _,dr01_dr0,_ = compose_r(np.zeros((3,),),
+//                            r1,
+//                            get_gradients=True)
+#define mrcal_compose_r_tinyr0_gradientr0(dr_dr0,r_1) \
+    mrcal_compose_r_tinyr0_gradientr0_full(dr_dr0,0,0,r_1,0)
+void mrcal_compose_r_tinyr0_gradientr0_full( // output
+                           double* dr_dr0,      // (3,3) array; may be NULL
+                           int dr_dr0_stride0,  // in bytes. <= 0 means "contiguous"
+                           int dr_dr0_stride1,  // in bytes. <= 0 means "contiguous"
+
+                           // input
+                           const double* r_1,   // (3,) array
+                           int r_1_stride0      // in bytes. <= 0 means "contiguous"
+                           );
+// Same as mrcal_compose_r() except
+//
+// - r1 is assumed to be 0, so we don't ingest it, and we don't report the
+//   composition result
+// - we ONLY report the dr01/dr1 gradient
+//
+// In python this function is equivalent to
+//
+//   _,_,dr01_dr1 = compose_r(r0,
+//                            np.zeros((3,),),
+//                            get_gradients=True)
+#define mrcal_compose_r_tinyr1_gradientr1(dr_dr1,r_0) \
+    mrcal_compose_r_tinyr1_gradientr1_full(dr_dr1,0,0,r_0,0)
+void mrcal_compose_r_tinyr1_gradientr1_full( // output
+                           double* dr_dr1,      // (3,3) array; may be NULL
+                           int dr_dr1_stride0,  // in bytes. <= 0 means "contiguous"
+                           int dr_dr1_stride1,  // in bytes. <= 0 means "contiguous"
+
+                           // input
+                           const double* r_0,   // (3,) array
+                           int r_0_stride0      // in bytes. <= 0 means "contiguous"
+                           );
+
+// Procrustes fit functions. Align two corresponding sets of normalized
+// direction vectors or points. Return true on success
+bool mrcal_align_procrustes_vectors_R01(// out
+                                        double* R01,
+                                        // in
+                                        const int N,
+                                        // (N,3) arrays
+                                        // normalized direction vectors
+                                        const double* v0,
+                                        const double* v1,
+
+                                        // (N,) array; may be NULL to use an even
+                                        // weighting
+                                        const double* weights);
+
+bool mrcal_align_procrustes_points_Rt01(// out
+                                        double* Rt01,
+                                        // in
+                                        const int N,
+                                        // (N,3) arrays
+                                        const double* p0,
+                                        const double* p1,
+
+                                        // (N,) array; may be NULL to use an even
+                                        // weighting
+                                        const double* weights);
+
+// Compute a non-unique rotation to map a given vector to [0,0,1]
+void mrcal_R_aligned_to_vector(// out
+                               double* R,
+                               // in
+                               const double* v);

wpical/src/main/native/thirdparty/mrcal/include/scales.h

@@ -0,0 +1,43 @@
+#pragma once
+
+// These are parameter variable scales. They have the units of the parameters
+// themselves, so the optimizer sees x/SCALE_X for each parameter. I.e. as far
+// as the optimizer is concerned, the scale of each variable is 1. This doesn't
+// need to be precise; just need to get all the variables to be within the same
+// order of magnitute. This is important because the dogleg solve treats the
+// trust region as a ball in state space, and this ball is isotropic, and has a
+// radius that applies in every direction
+//
+// Can be visualized like this:
+//
+//   b0,x0,J0 = mrcal.optimizer_callback(**optimization_inputs)[:3]
+//   J0 = J0.toarray()
+//   ss = np.sum(np.abs(J0), axis=-2)
+//   gp.plot(ss, _set=mrcal.plotoptions_state_boundaries(**optimization_inputs))
+//
+// This visualizes the overall effect of each variable. If the scales aren't
+// tuned properly, some variables will have orders of magnitude stronger
+// response than others, and the optimization problem won't converge well.
+//
+// The scipy.optimize.least_squares() function claims to be able to estimate
+// these automatically, without requiring these hard-coded values from the user.
+// See the description of the "x_scale" argument:
+//
+//   https://docs.scipy.org/doc/scipy/reference/generated/scipy.optimize.least_squares.html
+//
+// Supposedly this paper describes the method:
+//
+//   J. J. More, "The Levenberg-Marquardt Algorithm: Implementation and Theory,"
+//   Numerical Analysis, ed. G. A. Watson, Lecture Notes in Mathematics 630,
+//   Springer Verlag, pp. 105-116, 1977.
+//
+// Please somebody look at this
+#define SCALE_INTRINSICS_FOCAL_LENGTH 500.0
+#define SCALE_INTRINSICS_CENTER_PIXEL 20.0
+#define SCALE_ROTATION_CAMERA         (0.1 * M_PI/180.0)
+#define SCALE_TRANSLATION_CAMERA      1.0
+#define SCALE_ROTATION_FRAME          (15.0 * M_PI/180.0)
+#define SCALE_TRANSLATION_FRAME       1.0
+#define SCALE_POSITION_POINT          SCALE_TRANSLATION_FRAME
+#define SCALE_CALOBJECT_WARP          0.01
+#define SCALE_DISTORTION              1.0

wpical/src/main/native/thirdparty/mrcal/include/stereo.h

@@ -0,0 +1,129 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+#include "mrcal-types.h"
+#include "mrcal-image.h"
+
+// The reference implementation in Python is _rectified_resolution_python() in
+// stereo.py
+//
+// The Python wrapper is mrcal.rectified_resolution(), and the documentation is
+// in the docstring of that function
+bool mrcal_rectified_resolution( // output and input
+                                 // > 0: use given value
+                                 // < 0: autodetect and scale
+                                 double* pixels_per_deg_az,
+                                 double* pixels_per_deg_el,
+
+                                 // input
+                                 const mrcal_lensmodel_t*     lensmodel,
+                                 const double*                intrinsics,
+                                 const mrcal_point2_t*        azel_fov_deg,
+                                 const mrcal_point2_t*        azel0_deg,
+                                 const double*                R_cam0_rect0,
+                                 const mrcal_lensmodel_type_t rectification_model_type);
+
+// The reference implementation in Python is _rectified_system_python() in
+// stereo.py
+//
+// The Python wrapper is mrcal.rectified_system(), and the documentation is in
+// the docstring of that function
+bool mrcal_rectified_system(// output
+                            unsigned int*     imagersize_rectified,
+                            double*           fxycxy_rectified,
+                            double*           rt_rect0_ref,
+                            double*           baseline,
+
+                            // input, output
+                            // > 0: use given value
+                            // < 0: autodetect and scale
+                            double* pixels_per_deg_az,
+                            double* pixels_per_deg_el,
+
+                            // input, output
+                            // if(..._autodetect) { the results are returned here }
+                            mrcal_point2_t* azel_fov_deg,
+                            mrcal_point2_t* azel0_deg,
+
+                            // input
+                            const mrcal_lensmodel_t* lensmodel0,
+                            const double*            intrinsics0,
+
+                            const double*            rt_cam0_ref,
+                            const double*            rt_cam1_ref,
+
+                            const mrcal_lensmodel_type_t rectification_model_type,
+
+                            bool   az0_deg_autodetect,
+                            bool   el0_deg_autodetect,
+                            bool   az_fov_deg_autodetect,
+                            bool   el_fov_deg_autodetect);
+
+// The reference implementation in Python is _rectification_maps_python() in
+// stereo.py
+//
+// The Python wrapper is mrcal.rectification_maps(), and the documentation is in
+// the docstring of that function
+bool mrcal_rectification_maps(// output
+                              // Dense array of shape (Ncameras=2, Nel, Naz, Nxy=2)
+                              float* rectification_maps,
+
+                              // input
+                              const mrcal_lensmodel_t* lensmodel0,
+                              const double*            intrinsics0,
+                              const double*            r_cam0_ref,
+
+                              const mrcal_lensmodel_t* lensmodel1,
+                              const double*            intrinsics1,
+                              const double*            r_cam1_ref,
+
+                              const mrcal_lensmodel_type_t rectification_model_type,
+                              const double*                fxycxy_rectified,
+                              const unsigned int*          imagersize_rectified,
+                              const double*                r_rect0_ref);
+
+
+// The reference implementation in Python is _stereo_range_python() in
+// stereo.py
+//
+// The Python wrapper is mrcal.stereo_range(), and the documentation is in
+// the docstring of that function
+bool mrcal_stereo_range_sparse(// output
+                               double* range, // N of these
+
+                               // input
+                               const double*                disparity, // N of these
+                               const mrcal_point2_t*        qrect0,    // N of these
+                               const int                    N,         // how many points
+
+                               const double                 disparity_min,
+                               const double                 disparity_max,
+
+                               // models_rectified
+                               const mrcal_lensmodel_type_t rectification_model_type,
+                               const double*                fxycxy_rectified,
+                               const double                 baseline);
+
+bool mrcal_stereo_range_dense(// output
+                              mrcal_image_double_t* range,
+
+                              // input
+                              const mrcal_image_uint16_t*  disparity_scaled,
+                              const uint16_t               disparity_scale,
+
+                              // Used to detect invalid values. Set to
+                              // 0,UINT16_MAX to ignore
+                              const uint16_t               disparity_scaled_min,
+                              const uint16_t               disparity_scaled_max,
+
+                              // models_rectified
+                              const mrcal_lensmodel_type_t rectification_model_type,
+                              const double*                fxycxy_rectified,
+                              const double                 baseline);

wpical/src/main/native/thirdparty/mrcal/include/strides.h

@@ -0,0 +1,42 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+// This is an internal header to make the stride logic work. Not to be seen by
+// the end-users or installed
+
+// stride-aware matrix access
+#define _P1(x, stride0, i0)                     \
+    (*(double*)( (char*)(x) +                   \
+    (i0) * (stride0)))
+#define _P2(x, stride0, stride1, i0, i1)        \
+    (*(double*)( (char*)(x) +                   \
+    (i0) * (stride0) +                          \
+    (i1) * (stride1)))
+#define _P3(x, stride0, stride1, stride2,i0, i1, i2)    \
+    (*(double*)( (char*)(x) +                           \
+    (i0) * (stride0) +                                  \
+    (i1) * (stride1) +                                  \
+    (i2) * (stride2)))
+
+#define P1(x, i0)       _P1(x, x##_stride0,                           i0)
+#define P2(x, i0,i1)    _P2(x, x##_stride0, x##_stride1,              i0,i1)
+#define P3(x, i0,i1,i2) _P3(x, x##_stride0, x##_stride1, x##_stride2, i0,i1,i2)
+
+// Init strides. If a given stride is <= 0, set the default, as we would have if
+// the data was contiguous
+#define init_stride_1D(x, d0) \
+    if( x ## _stride0 <= 0) x ## _stride0 = sizeof(*x)
+#define init_stride_2D(x, d0, d1)                       \
+    if( x ## _stride1 <= 0) x ## _stride1 = sizeof(*x); \
+    if( x ## _stride0 <= 0) x ## _stride0 = d1 * x ## _stride1
+#define init_stride_3D(x, d0, d1, d2)                   \
+    if( x ## _stride2 <= 0) x ## _stride2 = sizeof(*x); \
+    if( x ## _stride1 <= 0) x ## _stride1 = d2 * x ## _stride2; \
+    if( x ## _stride0 <= 0) x ## _stride0 = d1 * x ## _stride1

wpical/src/main/native/thirdparty/mrcal/include/triangulation.h

@@ -0,0 +1,153 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+#include "basic-geometry.h"
+
+// All of these return (0,0,0) if the rays are parallel or divergent, or if the
+// intersection is behind either of the two cameras. No gradients are reported
+// in that case
+
+// Basic closest-approach-in-3D routine. This is the "Mid" method in
+// "Triangulation: Why Optimize?", Seong Hun Lee and Javier Civera
+// https://arxiv.org/abs/1907.11917
+mrcal_point3_t
+mrcal_triangulate_geometric(// outputs
+                            // These all may be NULL
+                            mrcal_point3_t* dm_dv0,
+                            mrcal_point3_t* dm_dv1,
+                            mrcal_point3_t* dm_dt01,
+
+                            // inputs
+
+                            // not-necessarily normalized vectors in the camera-0
+                            // coord system
+                            const mrcal_point3_t* v0,
+                            const mrcal_point3_t* v1,
+                            const mrcal_point3_t* t01);
+
+// Minimize L2 pinhole reprojection error. Described in "Triangulation Made
+// Easy", Peter Lindstrom, IEEE Conference on Computer Vision and Pattern
+// Recognition, 2010. This is the "L2 img 5-iteration" method (but with only 2
+// iterations) in "Triangulation: Why Optimize?", Seong Hun Lee and Javier
+// Civera. https://arxiv.org/abs/1907.11917
+// Lindstrom's paper recommends 2 iterations
+mrcal_point3_t
+mrcal_triangulate_lindstrom(// outputs
+                            // These all may be NULL
+                            mrcal_point3_t* dm_dv0,
+                            mrcal_point3_t* dm_dv1,
+                            mrcal_point3_t* dm_dRt01,
+
+                            // inputs
+
+                            // not-necessarily normalized vectors in the LOCAL
+                            // coordinate system. This is different from the other
+                            // triangulation routines
+                            const mrcal_point3_t* v0_local,
+                            const mrcal_point3_t* v1_local,
+                            const mrcal_point3_t* Rt01);
+
+// Minimize L1 angle error. Described in "Closed-Form Optimal Two-View
+// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV
+// 2019. This is the "L1 ang" method in "Triangulation: Why Optimize?", Seong
+// Hun Lee and Javier Civera. https://arxiv.org/abs/1907.11917
+mrcal_point3_t
+mrcal_triangulate_leecivera_l1(// outputs
+                               // These all may be NULL
+                               mrcal_point3_t* dm_dv0,
+                               mrcal_point3_t* dm_dv1,
+                               mrcal_point3_t* dm_dt01,
+
+                               // inputs
+
+                               // not-necessarily normalized vectors in the camera-0
+                               // coord system
+                               const mrcal_point3_t* v0,
+                               const mrcal_point3_t* v1,
+                               const mrcal_point3_t* t01);
+
+// Minimize L-infinity angle error. Described in "Closed-Form Optimal Two-View
+// Triangulation Based on Angular Errors", Seong Hun Lee and Javier Civera. ICCV
+// 2019. This is the "L-infinity ang" method in "Triangulation: Why Optimize?",
+// Seong Hun Lee and Javier Civera. https://arxiv.org/abs/1907.11917
+mrcal_point3_t
+mrcal_triangulate_leecivera_linf(// outputs
+                                 // These all may be NULL
+                                 mrcal_point3_t* dm_dv0,
+                                 mrcal_point3_t* dm_dv1,
+                                 mrcal_point3_t* dm_dt01,
+
+                                 // inputs
+
+                                 // not-necessarily normalized vectors in the camera-0
+                                 // coord system
+                                 const mrcal_point3_t* v0,
+                                 const mrcal_point3_t* v1,
+                                 const mrcal_point3_t* t01);
+
+// The "Mid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and Javier
+// Civera. https://arxiv.org/abs/1907.11917
+mrcal_point3_t
+mrcal_triangulate_leecivera_mid2(// outputs
+                                 // These all may be NULL
+                                 mrcal_point3_t* dm_dv0,
+                                 mrcal_point3_t* dm_dv1,
+                                 mrcal_point3_t* dm_dt01,
+
+                                 // inputs
+
+                                 // not-necessarily normalized vectors in the camera-0
+                                 // coord system
+                                 const mrcal_point3_t* v0,
+                                 const mrcal_point3_t* v1,
+                                 const mrcal_point3_t* t01);
+
+// The "wMid2" method in "Triangulation: Why Optimize?", Seong Hun Lee and Javier
+// Civera. https://arxiv.org/abs/1907.11917
+mrcal_point3_t
+mrcal_triangulate_leecivera_wmid2(// outputs
+                                  // These all may be NULL
+                                  mrcal_point3_t* dm_dv0,
+                                  mrcal_point3_t* dm_dv1,
+                                  mrcal_point3_t* dm_dt01,
+
+                                  // inputs
+
+                                  // not-necessarily normalized vectors in the camera-0
+                                  // coord system
+                                  const mrcal_point3_t* v0,
+                                  const mrcal_point3_t* v1,
+                                  const mrcal_point3_t* t01);
+
+// I don't implement triangulate_leecivera_l2() yet because it requires
+// computing an SVD, which is far slower than what the rest of these functions
+// do
+
+double
+_mrcal_triangulated_error(// outputs
+                          mrcal_point3_t* _derr_dv1,
+                          mrcal_point3_t* _derr_dt01,
+
+                          // inputs
+
+                          // not-necessarily normalized vectors in the camera-0
+                          // coord system
+                          const mrcal_point3_t* _v0,
+                          const mrcal_point3_t* _v1,
+                          const mrcal_point3_t* _t01);
+
+bool
+_mrcal_triangulate_leecivera_mid2_is_convergent(// inputs
+
+                                                // not-necessarily normalized vectors in the camera-0
+                                                // coord system
+                                                const mrcal_point3_t* _v0,
+                                                const mrcal_point3_t* _v1,
+                                                const mrcal_point3_t* _t01);

wpical/src/main/native/thirdparty/mrcal/include/util.h

@@ -0,0 +1,13 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#pragma once
+
+#include <stdio.h>
+
+#define MSG(fmt, ...) fprintf(stderr, "%s(%d): " fmt "\n", __FILE__, __LINE__, ##__VA_ARGS__)

wpical/src/main/native/thirdparty/mrcal/src/cahvore.cpp

@@ -0,0 +1,332 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#include <stdio.h>
+#include <assert.h>
+
+#include "autodiff.hh"
+
+extern "C" {
+#include "cahvore.h"
+}
+
+template <int N>
+static
+bool _project_cahvore_internals( // outputs
+                                 vec_withgrad_t<N,3>* pdistorted,
+
+                                 // inputs
+                                 const vec_withgrad_t<N,3>& p,
+                                 const val_withgrad_t<N>&   alpha,
+                                 const val_withgrad_t<N>&   beta,
+                                 const val_withgrad_t<N>&   r0,
+                                 const val_withgrad_t<N>&   r1,
+                                 const val_withgrad_t<N>&   r2,
+                                 const val_withgrad_t<N>&   e0,
+                                 const val_withgrad_t<N>&   e1,
+                                 const val_withgrad_t<N>&   e2,
+                                 const double cahvore_linearity)
+{
+    // Apply a CAHVORE warp to an un-distorted point
+
+    //  Given intrinsic parameters of a CAHVORE model and a set of
+    //  camera-coordinate points, return the projected point(s)
+
+    // This comes from cmod_cahvore_3d_to_2d_general() in
+    // m-jplv/libcmod/cmod_cahvore.c
+    //
+    // The lack of documentation here comes directly from the lack of
+    // documentation in that function.
+
+    // I parametrize the optical axis such that
+    // - o(alpha=0, beta=0) = (0,0,1) i.e. the optical axis is at the center
+    //   if both parameters are 0
+    // - The gradients are cartesian. I.e. do/dalpha and do/dbeta are both
+    //   NOT 0 at (alpha=0,beta=0). This would happen at the poles (gimbal
+    //   lock), and that would make my solver unhappy
+    // So o = { s_al*c_be, s_be,  c_al*c_be }
+    vec_withgrad_t<N,2> sca = alpha.sincos();
+    vec_withgrad_t<N,2> scb = beta .sincos();
+
+    vec_withgrad_t<N,3> o;
+    o[0] = scb[1]*sca[0];
+    o[1] = scb[0];
+    o[2] = scb[1]*sca[1];
+
+    // Note: CAHVORE is noncentral: project(p) and project(k*p) do NOT
+    // project to the same point
+
+    // What is called "omega" in the canonical CAHVOR implementation is
+    // called "zeta" in the canonical CAHVORE implementation. They're the
+    // same thing
+
+    // cos( angle between p and o ) = inner(p,o) / (norm(o) * norm(p)) =
+    // zeta/norm(p)
+    val_withgrad_t<N> zeta = p.dot(o);
+
+    // Basic Computations
+    // Calculate initial terms
+    vec_withgrad_t<N,3> u  = o*zeta;
+    vec_withgrad_t<N,3> ll = p - u;
+    val_withgrad_t<N>   l  = ll.mag();
+
+    // Calculate theta using Newton's Method
+    val_withgrad_t<N> theta = l.atan2(zeta);
+
+    int inewton;
+    for( inewton = 100; inewton; inewton--)
+    {
+        // Compute terms from the current value of theta
+        vec_withgrad_t<N,2> scth = theta.sincos();
+
+        val_withgrad_t<N> theta2  = theta * theta;
+        val_withgrad_t<N> theta3  = theta * theta2;
+        val_withgrad_t<N> theta4  = theta * theta3;
+        val_withgrad_t<N> upsilon =
+            zeta*scth[1] + l*scth[0]
+            + (scth[1] -  1.0 ) * (e0 +      e1*theta2 +     e2*theta4)
+            - (theta - scth[0]) * (      e1*theta*2.0  + e2*theta3*4.0);
+
+        // Update theta
+        val_withgrad_t<N> dtheta =
+            (zeta*scth[0] - l*scth[1]
+             - (theta - scth[0]) * (e0 + e1*theta2 + e2*theta4)) / upsilon;
+
+        theta -= dtheta;
+
+        // Check exit criterion from last update
+        if(fabs(dtheta.x) < 1e-8)
+            break;
+    }
+    if(inewton == 0)
+    {
+        fprintf(stderr, "%s(): too many iterations\n", __func__);
+        return false;
+    }
+
+    // got a theta
+
+    // Check the value of theta
+    if(theta.x * fabs(cahvore_linearity) > M_PI/2.)
+    {
+        fprintf(stderr, "%s(): theta out of bounds\n", __func__);
+        return false;
+    }
+
+    // If we aren't close enough to use the small-angle approximation ...
+    if (theta.x > 1e-8)
+    {
+        // ... do more math!
+        val_withgrad_t<N> linth = theta * cahvore_linearity;
+        val_withgrad_t<N> chi;
+        if (cahvore_linearity < -1e-15)
+            chi = linth.sin() / cahvore_linearity;
+        else if (cahvore_linearity > 1e-15)
+            chi = linth.tan() / cahvore_linearity;
+        else
+            chi = theta;
+        val_withgrad_t<N> chi2 = chi * chi;
+        val_withgrad_t<N> chi3 = chi * chi2;
+        val_withgrad_t<N> chi4 = chi * chi3;
+
+        val_withgrad_t<N> zetap = l / chi;
+
+        val_withgrad_t<N> mu = r0 + r1*chi2 + r2*chi4;
+
+        vec_withgrad_t<N,3> uu = o*zetap;
+        vec_withgrad_t<N,3> vv = ll * (mu + 1.);
+        *pdistorted = uu + vv;
+    }
+    else
+        *pdistorted = p;
+    return true;
+}
+
+
+// Not meant to be touched by the end user. Implemented separate from mrcal.c so
+// that I can get automated gradient propagation with c++
+extern "C"
+
+bool project_cahvore_internals( // outputs
+                                mrcal_point3_t* __restrict pdistorted,
+                                double*         __restrict dpdistorted_dintrinsics_nocore,
+                                double*         __restrict dpdistorted_dp,
+
+                                // inputs
+                                const mrcal_point3_t* __restrict p,
+                                const double* __restrict intrinsics_nocore,
+                                const double cahvore_linearity)
+{
+    if( dpdistorted_dintrinsics_nocore == NULL &&
+        dpdistorted_dp                 == NULL )
+    {
+        // No gradients. NGRAD in all the templates is 0
+
+        vec_withgrad_t<0,3> pdistortedg;
+
+        vec_withgrad_t<0,8> ig;
+        ig.init_vars(intrinsics_nocore,
+                     0,8, // ivar0,Nvars
+                     -1   // i_gradvec0
+                     );
+
+        vec_withgrad_t<0,3> pg;
+        pg.init_vars(p->xyz,
+                     0,3, // ivar0,Nvars
+                     -1   // i_gradvec0
+                     );
+
+        if(!_project_cahvore_internals(&pdistortedg,
+                                       pg,
+                                       ig[0],
+                                       ig[1],
+                                       ig[2],
+                                       ig[3],
+                                       ig[4],
+                                       ig[5],
+                                       ig[6],
+                                       ig[7],
+                                       cahvore_linearity))
+            return false;
+
+        pdistortedg.extract_value(pdistorted->xyz);
+        return true;
+    }
+
+    if( dpdistorted_dintrinsics_nocore == NULL &&
+        dpdistorted_dp                 != NULL )
+    {
+        // 3 variables:  p (3 vars)
+        vec_withgrad_t<3,3> pdistortedg;
+
+        vec_withgrad_t<3,8> ig;
+        ig.init_vars(intrinsics_nocore,
+                     0,8, // ivar0,Nvars
+                     -1   // i_gradvec0
+                     );
+
+        vec_withgrad_t<3,3> pg;
+        pg.init_vars(p->xyz,
+                     0,3, // ivar0,Nvars
+                     0    // i_gradvec0
+                     );
+
+        if(!_project_cahvore_internals(&pdistortedg,
+                                       pg,
+                                       ig[0],
+                                       ig[1],
+                                       ig[2],
+                                       ig[3],
+                                       ig[4],
+                                       ig[5],
+                                       ig[6],
+                                       ig[7],
+                                       cahvore_linearity))
+            return false;
+
+        pdistortedg.extract_value(pdistorted->xyz);
+        pdistortedg.extract_grad (dpdistorted_dp,
+                                  0,3, // ivar0,Nvars
+                                  0,   // i_gradvec0
+                                  sizeof(double)*3, sizeof(double),
+                                  3    // Nvars
+                                  );
+        return true;
+    }
+
+    if( dpdistorted_dintrinsics_nocore != NULL &&
+        dpdistorted_dp                 == NULL )
+    {
+        // 8 variables:  alpha,beta,R,E (8 vars)
+        vec_withgrad_t<8,3> pdistortedg;
+
+        vec_withgrad_t<8,8> ig;
+        ig.init_vars(intrinsics_nocore,
+                     0,8, // ivar0,Nvars
+                     0    // i_gradvec0
+                     );
+
+        vec_withgrad_t<8,3> pg;
+        pg.init_vars(p->xyz,
+                     0,3, // ivar0,Nvars
+                     -1   // i_gradvec0
+                     );
+
+        if(!_project_cahvore_internals(&pdistortedg,
+                                       pg,
+                                       ig[0],
+                                       ig[1],
+                                       ig[2],
+                                       ig[3],
+                                       ig[4],
+                                       ig[5],
+                                       ig[6],
+                                       ig[7],
+                                       cahvore_linearity))
+            return false;
+
+        pdistortedg.extract_value(pdistorted->xyz);
+        pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore,
+                                  0,8, // i_gradvec0,N_gradout
+                                  0,   // ivar0
+                                  sizeof(double)*8, sizeof(double),
+                                  3    // Nvars
+                                  );
+        return true;
+    }
+
+    if( dpdistorted_dintrinsics_nocore != NULL &&
+        dpdistorted_dp                 != NULL )
+    {
+        // 11 variables:  alpha,beta,R,E (8 vars)  +  p (3 vars)
+        vec_withgrad_t<11,3> pdistortedg;
+
+        vec_withgrad_t<11,8> ig;
+        ig.init_vars(intrinsics_nocore,
+                     0,8, // ivar0,Nvars
+                     0    // i_gradvec0
+                     );
+
+        vec_withgrad_t<11,3> pg;
+        pg.init_vars(p->xyz,
+                     0,3, // ivar0,Nvars
+                     8    // i_gradvec0
+                     );
+
+        if(!_project_cahvore_internals(&pdistortedg,
+                                       pg,
+                                       ig[0],
+                                       ig[1],
+                                       ig[2],
+                                       ig[3],
+                                       ig[4],
+                                       ig[5],
+                                       ig[6],
+                                       ig[7],
+                                       cahvore_linearity))
+            return false;
+
+        pdistortedg.extract_value(pdistorted->xyz);
+        pdistortedg.extract_grad (dpdistorted_dintrinsics_nocore,
+                                  0,8, // i_gradvec0,N_gradout
+                                  0,   // ivar0
+                                  sizeof(double)*8, sizeof(double),
+                                  3    // Nvars
+                                  );
+        pdistortedg.extract_grad (dpdistorted_dp,
+                                  8,3, // ivar0,Nvars
+                                  0,   // i_gradvec0
+                                  sizeof(double)*3, sizeof(double),
+                                  3    // Nvars
+                                  );
+        return true;
+    }
+
+    fprintf(stderr, "Getting here is a bug. Please report\n");
+    assert(0);
+}

wpical/src/main/native/thirdparty/mrcal/src/minimath/minimath_generate.pl

@@ -0,0 +1,463 @@
+#!/usr/bin/env perl
+use strict;
+use warnings;
+use feature qw(say);
+use List::Util qw(min);
+use List::MoreUtils qw(pairwise);
+
+say "// THIS IS AUTO-GENERATED BY $0. DO NOT EDIT BY HAND\n";
+say "// This contains dot products, norms, basic vector arithmetic and multiplication\n";
+
+my @sizes = 2..6;
+
+# the dot products, norms and basic arithmetic functions take the size as an
+# argument. I'm assuming that the compiler will expand these out for each
+# particular invocation
+dotProducts();
+norms();
+vectorArithmetic();
+
+foreach my $n(@sizes)
+{
+  matrixVectorSym($n);
+
+  foreach my $m (@sizes)
+  {
+    matrixVectorGen($n, $m)
+  }
+
+  matrixMatrixSym($n);
+  matrixMatrixGen($n);
+}
+
+# this is only defined for N=3. I haven't made the others yet and I don't yet need them
+matrixMatrixMatrixSym(3);
+
+
+
+
+
+
+sub dotProducts
+{
+  print <<EOC;
+static inline double dot_vec(int n, const double* restrict a, const double* restrict b)
+{
+  double dot = 0.0;
+  for(int i=0; i<n; i++)
+    dot += a[i]*b[i];
+  return dot;
+}
+EOC
+}
+
+sub norms
+{
+  print <<EOC;
+static inline double norm2_vec(int n, const double* restrict a)
+{
+  double dot = 0.0;
+  for(int i=0; i<n; i++)
+    dot += a[i]*a[i];
+  return dot;
+}
+EOC
+}
+
+sub vectorArithmetic
+{
+  my $vout = <<EOC;
+// a + b -> vout
+static inline void add_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout)
+{
+  for(int i=0; i<n; i++)
+    vout[i] = a[i] + b[i];
+}
+// a - b -> vout
+static inline void sub_vec_vout(int n, const double* restrict a, const double* restrict b, double* restrict vout)
+{
+  for(int i=0; i<n; i++)
+    vout[i] = a[i] - b[i];
+}
+EOC
+
+  print $vout;
+  print _makeScaled_arithmetic($vout);
+
+  my $arg0     = _getFirstDataArg($vout);
+  my $vinplace = _makeInplace_mulVector($vout, $arg0);
+
+  print $vinplace;
+  print _makeScaled_arithmetic($vinplace);
+  print _makeVaccum($vout);
+}
+
+sub matrixVectorSym
+{
+  my $n = shift;
+
+  my $vout = <<EOC;
+// $n-vector by symmetric ${n}x$n
+static inline void mul_vec${n}_sym$n${n}_vout(const double* restrict v, const double* restrict s, double* restrict vout)
+{
+EOC
+
+  # I now have the header, opening brace. Writing each row element output
+  my %isymHash = (next => 0);
+
+  for my $i(0..$n-1)
+  {
+    my $isym_row = _getSymmetricIndices_row(\%isymHash, $i, $n);
+    my @cols = 0..$n-1;
+
+    our ($a,$b);
+    my @sum_components = pairwise {"s[$a]*v[$b]"} @$isym_row, @cols;
+    $vout .= "  vout[$i] = " . join(' + ', @sum_components) . ";\n";
+  }
+
+  $vout .= "}";
+
+
+  print _multiplicationVersions($vout, $n, $n);
+}
+
+
+sub matrixVectorGen
+{
+  my $n = shift;
+  my $m = shift;
+
+  # I now make NxM matrix-vector multiplication. I describe matrices math-style
+  # with the number of rows first (NxM has N rows, M columns). I store the
+  # matrices row-first and treat vectors as row-vectors. Thus these functons
+  # compute v*A where v is the row vector and A is the NxM matrix
+
+  my $vout = <<EOC;
+// $n-vector by ${n}x$m matrix multiplication
+static inline void mul_vec${n}_gen$n${m}_vout(const double* restrict v, const double* restrict m, double* restrict vout)
+{
+EOC
+
+  # I now have the header, opening brace. Writing each row element output
+  for my $i(0..$m-1)
+  {
+    my @js = 0..$n-1;
+    my @im = map {$i + $_*$m} @js;
+
+    our ($a,$b);
+    my @sum_components = pairwise {"m[$a]*v[$b]"} @im, @js;
+    $vout .= "  vout[$i] = " . join(' + ', @sum_components) . ";\n";
+  }
+
+  $vout .= "}";
+  print _multiplicationVersions($vout, $m, $n);
+
+
+# now the transposed version
+  $vout = <<EOC;
+// $n-vector by ${m}x$n-transposed matrix multiplication
+static inline void mul_vec${n}_gen$m${n}t_vout(const double* restrict v, const double* restrict mt, double* restrict vout)
+{
+EOC
+
+  # I now have the header, opening brace. Writing each row element output
+  for my $i(0..$m-1)
+  {
+    my @js = 0..$n-1;
+    my @im = map {$i*$n + $_} @js;
+
+    our ($a,$b);
+    my @sum_components = pairwise {"mt[$a]*v[$b]"} @im, @js;
+    $vout .= "  vout[$i] = " . join(' + ', @sum_components) . ";\n";
+  }
+
+  $vout .= "}";
+  print _multiplicationVersions($vout, $m,$n);
+}
+
+sub matrixMatrixSym
+{
+  my $n = shift;
+
+  # I now make NxM matrix-vector multiplication. I describe matrices math-style
+  # with the number of rows first (NxM has N rows, M columns). I store the
+  # matrices row-first and treat vectors as row-vectors. Thus these functons
+  # compute v*A where v is the row vector and A is the NxM matrix
+
+  my $vout = <<EOC;
+// general Nx$n matrix by symmetric ${n}x$n
+static inline void mul_genN${n}_sym${n}${n}_vout(int n, const double* restrict v, const double* restrict s, double* restrict vout)
+{
+  for(int i=0; i<n; i++)
+    mul_vec${n}_sym${n}${n}_vout(v + $n*i, s, vout + $n*i);
+}
+EOC
+
+  print _multiplicationVersions($vout);
+}
+
+sub matrixMatrixMatrixSym
+{
+  my $n = shift;
+  die 'matrixMatrixMatrixSym ONLY defined for $n==3 right now' if $n != 3;
+
+
+  print <<'EOC';
+// (%i2) sym3 : matrix([m0,m1,m2],
+//                      [m1,m3,m4],
+//                      [m2,m4,m5]);
+
+// (%o2) matrix([m0,m1,m2],[m1,m3,m4],[m2,m4,m5])
+// (%i3) sym3_a : matrix([a0,a1,a2],
+//                       [a1,a3,a4],
+//                       [a2,a4,a5]);
+
+// (%o3) matrix([a0,a1,a2],[a1,a3,a4],[a2,a4,a5])
+// (%i4) sym3_b : matrix([b0,b1,b2],
+//                       [b1,b3,b4],
+//                       [b2,b4,b5]);
+
+// (%o4) matrix([b0,b1,b2],[b1,b3,b4],[b2,b4,b5])
+// (%i5) sym3_a . sym3_b . sym3_a;
+
+// (%o5) matrix([a2*(a2*b5+a1*b4+a0*b2)+a1*(a2*b4+a1*b3+a0*b1) + a0*(a2*b2+a1*b1+a0*b0), a2*(a4*b5+a3*b4+a1*b2)+a1*(a4*b4+a3*b3+a1*b1) + a0*(a4*b2+a3*b1+a1*b0), a2*(a5*b5+a4*b4+a2*b2)+a1*(a5*b4+a4*b3+a2*b1) + a0*(a5*b2+a4*b1+a2*b0)],
+//              [a4*(a2*b5+a1*b4+a0*b2)+a3*(a2*b4+a1*b3+a0*b1) + a1*(a2*b2+a1*b1+a0*b0), a4*(a4*b5+a3*b4+a1*b2)+a3*(a4*b4+a3*b3+a1*b1) + a1*(a4*b2+a3*b1+a1*b0), a4*(a5*b5+a4*b4+a2*b2)+a3*(a5*b4+a4*b3+a2*b1) + a1*(a5*b2+a4*b1+a2*b0)],
+//              [a5*(a2*b5+a1*b4+a0*b2)+a4*(a2*b4+a1*b3+a0*b1) + a2*(a2*b2+a1*b1+a0*b0), a5*(a4*b5+a3*b4+a1*b2)+a4*(a4*b4+a3*b3+a1*b1) + a2*(a4*b2+a3*b1+a1*b0), a5*(a5*b5+a4*b4+a2*b2)+a4*(a5*b4+a4*b3+a2*b1) + a2*(a5*b2+a4*b1+a2*b0)])
+EOC
+
+  my $vout = <<'EOC';
+// symmetric A * B * A
+static inline void mul_sym33_sym33_sym33_vout(const double* restrict a, const double* restrict b, double* restrict vout)
+{
+  double t0 = a[2]*b[5]+a[1]*b[4]+a[0]*b[2];
+  double t1 = a[2]*b[4]+a[1]*b[3]+a[0]*b[1];
+  double t2 = a[2]*b[2]+a[1]*b[1]+a[0]*b[0];
+  double t3 = a[4]*b[2]+a[3]*b[1]+a[1]*b[0];
+  double t4 = a[4]*b[5]+a[3]*b[4]+a[1]*b[2];
+  double t5 = a[4]*b[4]+a[3]*b[3]+a[1]*b[1];
+
+  vout[0] = a[2]*t0+a[1]*t1+a[0]*t2;
+  vout[1] = a[4]*t0+a[3]*t1+a[1]*t2;
+  vout[2] = a[5]*t0+a[4]*t1+a[2]*t2;
+  vout[3] = a[4]*t4+a[3]*t5+a[1]*t3;
+  vout[4] = a[5]*t4+a[4]*t5+a[2]*t3;
+  vout[5] = a[5]*(a[5]*b[5]+a[4]*b[4]+a[2]*b[2])+a[4]*(a[5]*b[4]+a[4]*b[3]+a[2]*b[1]) + a[2]*(a[5]*b[2]+a[4]*b[1]+a[2]*b[0]);
+}
+EOC
+
+  print $vout;
+}
+
+sub matrixMatrixGen
+{
+  my $n = shift;
+
+  # I now make NxM matrix-vector multiplication. I describe matrices math-style
+  # with the number of rows first (NxM has N rows, M columns). I store the
+  # matrices row-first and treat vectors as row-vectors. Thus these functons
+  # compute v*A where v is the row vector and A is the NxM matrix
+
+  my $vout = <<EOC;
+// general Nx${n} matrix by general ${n}x${n}
+static inline void mul_genN${n}_gen${n}${n}_vout(int n, const double* restrict v, const double* restrict m, double* restrict vout)
+{
+  for(int i=0; i<n; i++)
+    mul_vec${n}_gen${n}${n}_vout(v + $n*i, m, vout + $n*i);
+}
+
+// general Nx${n} matrix by general ${n}x${n}
+static inline void mul_genN${n}_gen${n}${n}t_vout(int n, const double* restrict v, const double* restrict mt, double* restrict vout)
+{
+  for(int i=0; i<n; i++)
+    mul_vec${n}_gen${n}${n}t_vout(v + $n*i, mt, vout + $n*i);
+}
+
+EOC
+
+  print _multiplicationVersions($vout);
+}
+
+
+
+
+
+
+
+sub _multiplicationVersions
+{
+  my $vout = shift;
+  my $n    = shift;
+  my $m    = shift;
+
+  my $arg0 = _getFirstDataArg($vout);
+
+  my $funcs = $vout . "\n";
+  $funcs .= (defined $n ?
+             _makeInplace_mulVector($vout, $arg0, $n, $m) : _makeInplace_mulMatrix($vout) ) . "\n";
+  $funcs .= _makeVaccum ($vout) . "\n";
+  $funcs .= (defined $n ?
+             _makeScaled_mulVector ($funcs) : _makeScaled_mulMatrix ($funcs) ) . "\n";
+
+  return $funcs;
+}
+
+sub _getSymmetricIndices_row
+{
+  my $hash     = shift;
+  my $i        = shift;
+  my $n        = shift;
+
+  my @isym;
+  for my $j (0..$n-1)
+  {
+    my $key = join(',', sort($i,$j));
+    if( !exists $hash->{$key} )
+    {
+      $hash->{$key} = $hash->{next};
+      $hash->{next}++;
+    }
+
+    push @isym, $hash->{$key};
+  }
+
+  return \@isym;
+}
+
+sub _getFirstDataArg
+{
+  my $v = shift;
+
+  # I have a string with a bunch of functions. Get the first argument. I ASSUME
+  # THE FIRST ARGUMENT HAS THE SAME NAME IN ALL OF THESE
+  my ($arg0) = $v =~ m/^static inline.*\(.*?double\* restrict ([a-z0-9_]+),/m or die "Couldn't recognize function in '$v'";
+  return $arg0;
+}
+
+sub _makeInplace_mulVector
+{
+  my $v       = shift;
+  my $arg0    = shift;
+  my $n       = shift;
+  my $m       = shift;
+
+  # rename functions
+  $v =~ s/_vout//gm;
+
+  # get rid of the 'vout argument'
+  $v =~ s/, double\* restrict vout//gm;
+
+  # un-const first argument
+  $v =~ s/^(static inline.*\(.*?)const (double.*)$/$1$2/gm;
+
+  # use the first argument instead of vout
+  $v =~ s/vout/$arg0/gm;
+
+  # if we're asked to make some temporary variables, do it
+  if(defined $n)
+  {
+    # if no $m is given, use $m;
+    $m //= $n;
+
+    my $nt = min($n-1,$m);
+
+    # use the temporaries instead of the main variable when possible
+    foreach my $t(0..$nt-1)
+    {
+      $v =~ s/(=.*)${arg0}\[$t\]/$1t[$t]/mg;
+    }
+
+    # define the temporaries. I need one fewer than n
+    my $tempDef = "  double t[$nt] = {" . join(', ', map {"${arg0}[$_]"} 0..$nt-1) . "};";
+    $v =~ s/^\{$/{\n$tempDef/mg;
+  }
+
+  return $v;
+}
+sub _makeInplace_mulMatrix
+{
+  my $v       = shift;
+
+  # rename functions
+  $v =~ s/_vout//gm;
+
+  # get rid of the 'vout argument'
+  $v =~ s/, double\* restrict vout//gm;
+
+  # un-const first argument
+  $v =~ s/^(static inline.*\(.*?)const (double.*)$/$1$2/gm;
+
+  # use the first argument instead of vout
+  $v =~ s/,[^\),]*vout[^\),]*([\),])/$1/gm;
+
+  return $v;
+}
+
+sub _makeVaccum
+{
+  my $v    = shift;
+
+  # rename functions
+  $v =~ s/_vout/_vaccum/gm;
+
+  # vout -> vaccum
+  $v =~ s/vout/vaccum/gm;
+
+  # make sure we accumulate
+  $v =~ s/(vaccum\[.*?\]\s*)=/$1+=/gm;
+
+  # better comment
+  $v =~ s/-> vaccum/-> + vaccum/gm;
+
+  return $v;
+}
+
+sub _makeScaled_arithmetic
+{
+  my $f = shift;
+
+  # rename functions
+  $f =~ s/^(static inline .*)(\s*\()/${1}_scaled$2/gm;
+
+  # add the scale argument
+  $f =~ s/^(static inline .*)\)$/$1, double scale)/gm;
+
+  # apply the scaling
+  $f =~ s/([+-]) b/$1 scale*b/gm;
+
+  return $f;
+}
+
+sub _makeScaled_mulVector
+{
+  my $f = shift;
+
+  # rename functions
+  $f =~ s/^(static inline .*)(\s*\()/${1}_scaled$2/gm;
+
+  # add the scale argument
+  $f =~ s/^(static inline .*)\)$/$1, double scale)/gm;
+
+  # apply the scaling
+  $f =~ s/(.*=\s*)([^{}]*?);$/${1}scale * ($2);/gm;
+
+  return $f;
+}
+
+sub _makeScaled_mulMatrix
+{
+  my $f = shift;
+
+  # rename functions
+  $f =~ s/^(static inline .*)(\s*\()/${1}_scaled$2/gm;
+
+  # add the scale argument
+  $f =~ s/^(static inline .*)\)$/$1, double scale)/gm;
+
+  # apply the scaling. This is simply an argument to the vector function I call
+  $f =~ s/^(\s*mul_.*)(\).*)/$1, scale$2/gm;
+
+  # apply the scaling. Call the _scaled vector function
+  $f =~ s/^(\s*mul_.*?)(\s*\()/${1}_scaled$2/gm;
+
+  return $f;
+}

wpical/src/main/native/thirdparty/mrcal/src/opencv.c

@@ -0,0 +1,152 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#include "mrcal.h"
+
+
+// The implementation of _mrcal_project_internal_opencv is based on opencv. The
+// sources have been heavily modified, but the opencv logic remains. This
+// function is a cut-down cvProjectPoints2Internal() to keep only the
+// functionality I want and to use my interfaces. Putting this here allows me to
+// drop the C dependency on opencv. Which is a good thing, since opencv dropped
+// their C API
+//
+// from opencv-4.2.0+dfsg/modules/calib3d/src/calibration.cpp
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+
+// NOT A PART OF THE EXTERNAL API. This is exported for the mrcal python wrapper
+// only
+void _mrcal_project_internal_opencv( // outputs
+                                    mrcal_point2_t* q,
+                                    mrcal_point3_t* dq_dp,         // may be NULL
+                                    double* dq_dintrinsics_nocore, // may be NULL
+
+                                    // inputs
+                                    const mrcal_point3_t* p,
+                                    int N,
+                                    const double* intrinsics,
+                                    int Nintrinsics)
+{
+    const double fx = intrinsics[0];
+    const double fy = intrinsics[1];
+    const double cx = intrinsics[2];
+    const double cy = intrinsics[3];
+
+    double k[12] = {};
+    for(int i=0; i<Nintrinsics-4; i++)
+        k[i] = intrinsics[i+4];
+
+    for( int i = 0; i < N; i++ )
+    {
+        double z_recip = 1./p[i].z;
+        double x = p[i].x * z_recip;
+        double y = p[i].y * z_recip;
+
+        double r2      = x*x + y*y;
+        double r4      = r2*r2;
+        double r6      = r4*r2;
+        double a1      = 2*x*y;
+        double a2      = r2 + 2*x*x;
+        double a3      = r2 + 2*y*y;
+        double cdist   = 1 + k[0]*r2 + k[1]*r4 + k[4]*r6;
+        double icdist2 = 1./(1 + k[5]*r2 + k[6]*r4 + k[7]*r6);
+        double xd      = x*cdist*icdist2 + k[2]*a1 + k[3]*a2 + k[8]*r2+k[9]*r4;
+        double yd      = y*cdist*icdist2 + k[2]*a3 + k[3]*a1 + k[10]*r2+k[11]*r4;
+
+        q[i].x = xd*fx + cx;
+        q[i].y = yd*fy + cy;
+
+
+        if( dq_dp )
+        {
+            double dx_dp[] = { z_recip, 0,       -x*z_recip };
+            double dy_dp[] = { 0,       z_recip, -y*z_recip };
+            for( int j = 0; j < 3; j++ )
+            {
+                double dr2_dp = 2*x*dx_dp[j] + 2*y*dy_dp[j];
+                double dcdist_dp = k[0]*dr2_dp + 2*k[1]*r2*dr2_dp + 3*k[4]*r4*dr2_dp;
+                double dicdist2_dp = -icdist2*icdist2*(k[5]*dr2_dp + 2*k[6]*r2*dr2_dp + 3*k[7]*r4*dr2_dp);
+                double da1_dp = 2*(x*dy_dp[j] + y*dx_dp[j]);
+                double dmx_dp = (dx_dp[j]*cdist*icdist2 + x*dcdist_dp*icdist2 + x*cdist*dicdist2_dp +
+                                k[2]*da1_dp + k[3]*(dr2_dp + 4*x*dx_dp[j]) + k[8]*dr2_dp + 2*r2*k[9]*dr2_dp);
+                double dmy_dp = (dy_dp[j]*cdist*icdist2 + y*dcdist_dp*icdist2 + y*cdist*dicdist2_dp +
+                                k[2]*(dr2_dp + 4*y*dy_dp[j]) + k[3]*da1_dp + k[10]*dr2_dp + 2*r2*k[11]*dr2_dp);
+                dq_dp[i*2 + 0].xyz[j] = fx*dmx_dp;
+                dq_dp[i*2 + 1].xyz[j] = fy*dmy_dp;
+            }
+        }
+        if( dq_dintrinsics_nocore )
+        {
+            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 0] = fx*x*icdist2*r2;
+            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 0] = fy*(y*icdist2*r2);
+
+            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 1] = fx*x*icdist2*r4;
+            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 1] = fy*y*icdist2*r4;
+
+            if( Nintrinsics-4 > 2 )
+            {
+                dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 2] = fx*a1;
+                dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 2] = fy*a3;
+                dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 3] = fx*a2;
+                dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 3] = fy*a1;
+                if( Nintrinsics-4 > 4 )
+                {
+                    dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 4] = fx*x*icdist2*r6;
+                    dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 4] = fy*y*icdist2*r6;
+
+                    if( Nintrinsics-4 > 5 )
+                    {
+                        dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 5] = fx*x*cdist*(-icdist2)*icdist2*r2;
+                        dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 5] = fy*y*cdist*(-icdist2)*icdist2*r2;
+                        dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 6] = fx*x*cdist*(-icdist2)*icdist2*r4;
+                        dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 6] = fy*y*cdist*(-icdist2)*icdist2*r4;
+                        dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 7] = fx*x*cdist*(-icdist2)*icdist2*r6;
+                        dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 7] = fy*y*cdist*(-icdist2)*icdist2*r6;
+                        if( Nintrinsics-4 > 8 )
+                        {
+                            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 8]  = fx*r2; //s1
+                            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 8]  = fy*0;  //s1
+                            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 9]  = fx*r4; //s2
+                            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 9]  = fy*0;  //s2
+                            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 10] = fx*0;  //s3
+                            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 10] = fy*r2; //s3
+                            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 0) + 11] = fx*0;  //s4
+                            dq_dintrinsics_nocore[(Nintrinsics-4)*(2*i + 1) + 11] = fy*r4; //s4
+                        }
+                    }
+                }
+            }
+        }
+    }
+}

wpical/src/main/native/thirdparty/mrcal/src/poseutils-opencv.c

@@ -0,0 +1,155 @@
+// The implementation of mrcal_R_from_r is based on opencv.
+// The sources have been heavily modified, but the opencv logic remains.
+//
+// from opencv-4.1.2+dfsg/modules/calib3d/src/calibration.cpp
+//
+// Copyright (C) 2000-2008, Intel Corporation, all rights reserved.
+// Copyright (C) 2009, Willow Garage Inc., all rights reserved.
+// Third party copyrights are property of their respective owners.
+//
+// Redistribution and use in source and binary forms, with or without modification,
+// are permitted provided that the following conditions are met:
+//
+//   * Redistribution's of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//
+//   * Redistribution's in binary form must reproduce the above copyright notice,
+//     this list of conditions and the following disclaimer in the documentation
+//     and/or other materials provided with the distribution.
+//
+//   * The name of the copyright holders may not be used to endorse or promote products
+//     derived from this software without specific prior written permission.
+//
+// This software is provided by the copyright holders and contributors "as is" and
+// any express or implied warranties, including, but not limited to, the implied
+// warranties of merchantability and fitness for a particular purpose are disclaimed.
+// In no event shall the Intel Corporation or contributors be liable for any direct,
+// indirect, incidental, special, exemplary, or consequential damages
+// (including, but not limited to, procurement of substitute goods or services;
+// loss of use, data, or profits; or business interruption) however caused
+// and on any theory of liability, whether in contract, strict liability,
+// or tort (including negligence or otherwise) arising in any way out of
+
+// Apparently I need this in MSVC to get constants
+#define _USE_MATH_DEFINES
+
+#include <math.h>
+#include <float.h>
+
+#include "poseutils.h"
+#include "strides.h"
+
+void mrcal_R_from_r_full( // outputs
+                         double* R,       // (3,3) array
+                         int R_stride0,   // in bytes. <= 0 means "contiguous"
+                         int R_stride1,   // in bytes. <= 0 means "contiguous"
+                         double* J,       // (3,3,3) array. Gradient. May be NULL
+                         int J_stride0,   // in bytes. <= 0 means "contiguous"
+                         int J_stride1,   // in bytes. <= 0 means "contiguous"
+                         int J_stride2,   // in bytes. <= 0 means "contiguous"
+
+                         // input
+                         const double* r, // (3,) vector
+                         int r_stride0    // in bytes. <= 0 means "contiguous"
+                          )
+{
+    init_stride_2D(R, 3,3);
+    init_stride_3D(J, 3,3,3 );
+    init_stride_1D(r, 3 );
+
+    double norm2r = 0.0;
+    for(int i=0; i<3; i++)
+        norm2r += P1(r,i)*P1(r,i);
+
+    if( norm2r < DBL_EPSILON*DBL_EPSILON )
+    {
+        mrcal_identity_R_full(R, R_stride0, R_stride1);
+
+        if( J )
+        {
+            for(int i=0; i<3; i++)
+                for(int j=0; j<3; j++)
+                    for(int k=0; k<3; k++)
+                        P3(J,i,j,k) = 0.;
+
+            P3(J,1,2,0) = -1.;
+            P3(J,2,0,1) = -1.;
+            P3(J,0,1,2) = -1.;
+
+            P3(J,2,1,0) =  1.;
+            P3(J,0,2,1) =  1.;
+            P3(J,1,0,2) =  1.;
+        }
+        return;
+    }
+
+    double theta = sqrt(norm2r);
+
+    double s = sin(theta);
+    double c = cos(theta);
+    double c1 = 1. - c;
+    double itheta = 1./theta;
+
+    double r_unit[3];
+    for(int i=0; i<3; i++)
+        r_unit[i] = P1(r,i) * itheta;
+
+    // R = cos(theta)*I + (1 - cos(theta))*r*rT + sin(theta)*[r_x]
+    P2(R, 0,0) = c + c1*r_unit[0]*r_unit[0];
+    P2(R, 0,1) =     c1*r_unit[0]*r_unit[1] - s*r_unit[2];
+    P2(R, 0,2) =     c1*r_unit[0]*r_unit[2] + s*r_unit[1];
+    P2(R, 1,0) =     c1*r_unit[0]*r_unit[1] + s*r_unit[2];
+    P2(R, 1,1) = c + c1*r_unit[1]*r_unit[1];
+    P2(R, 1,2) =     c1*r_unit[1]*r_unit[2] - s*r_unit[0];
+    P2(R, 2,0) =     c1*r_unit[0]*r_unit[2] - s*r_unit[1];
+    P2(R, 2,1) =     c1*r_unit[1]*r_unit[2] + s*r_unit[0];
+    P2(R, 2,2) = c + c1*r_unit[2]*r_unit[2];
+
+    if( J )
+    {
+        // opencv had some logic with lots of 0s. I unrolled all of the
+        // loops, and removed all the resulting 0 terms
+        double a0, a1, a3;
+        double a2 = itheta * c1;
+        double a4 = itheta * s;
+
+        a0 = -s        *r_unit[0];
+        a1 = (s - 2*a2)*r_unit[0];
+        a3 = (c -   a4)*r_unit[0];
+        P3(J,0,0,0) = a0 + a1*r_unit[0]*r_unit[0] + a2*(r_unit[0]+r_unit[0]);
+        P3(J,0,1,0) =      a1*r_unit[0]*r_unit[1] + a2*r_unit[1]   - a3*r_unit[2];
+        P3(J,0,2,0) =      a1*r_unit[0]*r_unit[2] + a2*r_unit[2]   + a3*r_unit[1];
+        P3(J,1,0,0) =      a1*r_unit[0]*r_unit[1] + a2*r_unit[1]   + a3*r_unit[2];
+        P3(J,1,1,0) = a0 + a1*r_unit[1]*r_unit[1];
+        P3(J,1,2,0) =      a1*r_unit[1]*r_unit[2]                  - a3*r_unit[0] - a4;
+        P3(J,2,0,0) =      a1*r_unit[0]*r_unit[2] + a2*r_unit[2]   - a3*r_unit[1];
+        P3(J,2,1,0) =      a1*r_unit[1]*r_unit[2]                  + a3*r_unit[0] + a4;
+        P3(J,2,2,0) = a0 + a1*r_unit[2]*r_unit[2];
+
+        a0 = -s        *r_unit[1];
+        a1 = (s - 2*a2)*r_unit[1];
+        a3 = (c -   a4)*r_unit[1];
+        P3(J,0,0,1) = a0 + a1*r_unit[0]*r_unit[0];
+        P3(J,0,1,1) =      a1*r_unit[0]*r_unit[1] + a2*r_unit[0]   - a3*r_unit[2];
+        P3(J,0,2,1) =      a1*r_unit[0]*r_unit[2]                  + a3*r_unit[1] + a4;
+        P3(J,1,0,1) =      a1*r_unit[0]*r_unit[1] + a2*r_unit[0]   + a3*r_unit[2];
+        P3(J,1,1,1) = a0 + a1*r_unit[1]*r_unit[1] + a2*(r_unit[1]+r_unit[1]);
+        P3(J,1,2,1) =      a1*r_unit[1]*r_unit[2] + a2*r_unit[2]   - a3*r_unit[0];
+        P3(J,2,0,1) =      a1*r_unit[0]*r_unit[2]                  - a3*r_unit[1] - a4;
+        P3(J,2,1,1) =      a1*r_unit[1]*r_unit[2] + a2*r_unit[2]   + a3*r_unit[0];
+        P3(J,2,2,1) = a0 + a1*r_unit[2]*r_unit[2];
+
+        a0 = -s        *r_unit[2];
+        a1 = (s - 2*a2)*r_unit[2];
+        a3 = (c -   a4)*r_unit[2];
+        P3(J,0,0,2) = a0 + a1*r_unit[0]*r_unit[0];
+        P3(J,0,1,2) =      a1*r_unit[0]*r_unit[1]                  - a3*r_unit[2] - a4;
+        P3(J,0,2,2) =      a1*r_unit[0]*r_unit[2] + a2*r_unit[0]   + a3*r_unit[1];
+        P3(J,1,0,2) =      a1*r_unit[0]*r_unit[1]                  + a3*r_unit[2] + a4;
+        P3(J,1,1,2) = a0 + a1*r_unit[1]*r_unit[1];
+        P3(J,1,2,2) =      a1*r_unit[1]*r_unit[2] + a2*r_unit[1]   - a3*r_unit[0];
+        P3(J,2,0,2) =      a1*r_unit[0]*r_unit[2] + a2*r_unit[0]   - a3*r_unit[1];
+        P3(J,2,1,2) =      a1*r_unit[1]*r_unit[2] + a2*r_unit[1]   + a3*r_unit[0];
+        P3(J,2,2,2) = a0 + a1*r_unit[2]*r_unit[2] + a2*(r_unit[2]+r_unit[2]);
+    }
+}

wpical/src/main/native/thirdparty/mrcal/src/poseutils-uses-autodiff.cpp

@@ -0,0 +1,891 @@
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+
+#include "autodiff.hh"
+#include "strides.h"
+
+extern "C" {
+#include "poseutils.h"
+}
+
+template<int N>
+static void
+rotate_point_r_core(// output
+                    val_withgrad_t<N>* x_outg,
+
+                    // inputs
+                    const val_withgrad_t<N>* rg,
+                    const val_withgrad_t<N>* x_ing,
+                    bool inverted)
+{
+    // Rodrigues rotation formula:
+    //   xrot = x cos(th) + cross(axis, x) sin(th) + axis axist x (1 - cos(th))
+    //
+    // I have r = axis*th -> th = mag(r) ->
+    //   xrot = x cos(th) + cross(r, x) sin(th)/th + r rt x (1 - cos(th)) / (th*th)
+
+    // if inverted: we have r <- -r, axis <- axis, th <- -th
+    //
+    // According to the expression above, this only flips the sign on the
+    // cross() term
+    double sign = inverted ? -1.0 : 1.0;
+
+    const val_withgrad_t<N> th2 =
+        rg[0]*rg[0] +
+        rg[1]*rg[1] +
+        rg[2]*rg[2];
+    const val_withgrad_t<N> cross[3] =
+        {
+            (rg[1]*x_ing[2] - rg[2]*x_ing[1])*sign,
+            (rg[2]*x_ing[0] - rg[0]*x_ing[2])*sign,
+            (rg[0]*x_ing[1] - rg[1]*x_ing[0])*sign
+        };
+    const val_withgrad_t<N> inner =
+        rg[0]*x_ing[0] +
+        rg[1]*x_ing[1] +
+        rg[2]*x_ing[2];
+
+    if(th2.x < 1e-10)
+    {
+        // Small rotation. I don't want to divide by 0, so I take the limit
+        //   lim(th->0, xrot) =
+        //     = x + cross(r, x) + r rt x lim(th->0, (1 - cos(th)) / (th*th))
+        //     = x + cross(r, x) + r rt x lim(th->0, sin(th) / (2*th))
+        //     = x + cross(r, x) + r rt x/2
+        for(int i=0; i<3; i++)
+            x_outg[i] =
+                x_ing[i] +
+                cross[i] +
+                rg[i]*inner / 2.;
+    }
+    else
+    {
+        // Non-small rotation. This is the normal path. Note that this path is
+        // still valid even if th ~ 180deg
+
+        const val_withgrad_t<N> th = th2.sqrt();
+        const vec_withgrad_t<N, 2> sc = th.sincos();
+
+        for(int i=0; i<3; i++)
+            x_outg[i] =
+                x_ing[i]*sc.v[1] +
+                cross[i] * sc.v[0]/th +
+                rg[i] * inner * (val_withgrad_t<N>(1.) - sc.v[1]) / th2;
+    }
+}
+
+extern "C"
+void mrcal_rotate_point_r_full( // output
+                               double* x_out,      // (3,) array
+                               int x_out_stride0,  // in bytes. <= 0 means "contiguous"
+                               double* J_r,        // (3,3) array. May be NULL
+                               int J_r_stride0,    // in bytes. <= 0 means "contiguous"
+                               int J_r_stride1,    // in bytes. <= 0 means "contiguous"
+                               double* J_x,        // (3,3) array. May be NULL
+                               int J_x_stride0,    // in bytes. <= 0 means "contiguous"
+                               int J_x_stride1,    // in bytes. <= 0 means "contiguous"
+
+                               // input
+                               const double* r,    // (3,) array. May be NULL
+                               int r_stride0,      // in bytes. <= 0 means "contiguous"
+                               const double* x_in, // (3,) array. May be NULL
+                               int x_in_stride0,   // in bytes. <= 0 means "contiguous"
+
+                               bool inverted       // if true, I apply a
+                                                   // rotation in the opposite
+                                                   // direction. J_r corresponds
+                                                   // to the input r
+                                )
+{
+    init_stride_1D(x_out, 3);
+    init_stride_2D(J_r,   3,3);
+    init_stride_2D(J_x,   3,3);
+    init_stride_1D(r,     3);
+    init_stride_1D(x_in,  3);
+
+    if(J_r == NULL && J_x == NULL)
+    {
+        vec_withgrad_t<0, 3> rg   (r,    -1, r_stride0);
+        vec_withgrad_t<0, 3> x_ing(x_in, -1, x_in_stride0);
+        vec_withgrad_t<0, 3> x_outg;
+        rotate_point_r_core<0>(x_outg.v,
+                               rg.v, x_ing.v,
+                               inverted);
+        x_outg.extract_value(x_out, x_out_stride0);
+    }
+    else if(J_r != NULL && J_x == NULL)
+    {
+        vec_withgrad_t<3, 3> rg   (r,     0, r_stride0);
+        vec_withgrad_t<3, 3> x_ing(x_in, -1, x_in_stride0);
+        vec_withgrad_t<3, 3> x_outg;
+        rotate_point_r_core<3>(x_outg.v,
+                               rg.v, x_ing.v,
+                               inverted);
+        x_outg.extract_value(x_out, x_out_stride0);
+        x_outg.extract_grad (J_r, 0, 3, 0, J_r_stride0, J_r_stride1);
+    }
+    else if(J_r == NULL && J_x != NULL)
+    {
+        vec_withgrad_t<3, 3> rg   (r,   -1, r_stride0);
+        vec_withgrad_t<3, 3> x_ing(x_in, 0, x_in_stride0);
+        vec_withgrad_t<3, 3> x_outg;
+        rotate_point_r_core<3>(x_outg.v,
+                               rg.v, x_ing.v,
+                               inverted);
+        x_outg.extract_value(x_out, x_out_stride0);
+        x_outg.extract_grad (J_x, 0, 3, 0, J_x_stride0,J_x_stride1);
+    }
+    else
+    {
+        vec_withgrad_t<6, 3> rg   (r,    0, r_stride0);
+        vec_withgrad_t<6, 3> x_ing(x_in, 3, x_in_stride0);
+        vec_withgrad_t<6, 3> x_outg;
+        rotate_point_r_core<6>(x_outg.v,
+                               rg.v, x_ing.v,
+                               inverted);
+        x_outg.extract_value(x_out, x_out_stride0);
+        x_outg.extract_grad (J_r, 0, 3, 0, J_r_stride0, J_r_stride1);
+        x_outg.extract_grad (J_x, 3, 3, 0, J_x_stride0, J_x_stride1);
+    }
+}
+
+extern "C"
+void mrcal_transform_point_rt_full( // output
+                                   double* x_out,      // (3,) array
+                                   int x_out_stride0,  // in bytes. <= 0 means "contiguous"
+                                   double* J_rt,       // (3,6) array. May be NULL
+                                   int J_rt_stride0,   // in bytes. <= 0 means "contiguous"
+                                   int J_rt_stride1,   // in bytes. <= 0 means "contiguous"
+                                   double* J_x,        // (3,3) array. May be NULL
+                                   int J_x_stride0,    // in bytes. <= 0 means "contiguous"
+                                   int J_x_stride1,    // in bytes. <= 0 means "contiguous"
+
+                                   // input
+                                   const double* rt,   // (6,) array. May be NULL
+                                   int rt_stride0,     // in bytes. <= 0 means "contiguous"
+                                   const double* x_in, // (3,) array. May be NULL
+                                   int x_in_stride0,   // in bytes. <= 0 means "contiguous"
+
+                                   bool inverted       // if true, I apply the
+                                                       // transformation in the
+                                                       // opposite direction.
+                                                       // J_rt corresponds to
+                                                       // the input rt
+                                    )
+{
+    if(!inverted)
+    {
+        init_stride_1D(x_out, 3);
+        init_stride_2D(J_rt,  3,6);
+        // init_stride_2D(J_x,   3,3 );
+        init_stride_1D(rt,    6 );
+        // init_stride_1D(x_in,  3 );
+
+        // to make in-place operations work
+        double t[3] = { P1(rt, 3),
+                        P1(rt, 4),
+                        P1(rt, 5) };
+        // I want rotate(x) + t
+        // First rotate(x)
+        mrcal_rotate_point_r_full(x_out, x_out_stride0,
+                                  J_rt,  J_rt_stride0,  J_rt_stride1,
+                                  J_x,   J_x_stride0,   J_x_stride1,
+                                  rt,    rt_stride0,
+                                  x_in,  x_in_stride0, false);
+
+        // And now +t. The J_r, J_x gradients are unaffected. J_t is identity
+        for(int i=0; i<3; i++)
+            P1(x_out,i) += t[i];
+        if(J_rt)
+            mrcal_identity_R_full(&P2(J_rt,0,3), J_rt_stride0, J_rt_stride1);
+    }
+    else
+    {
+        // I use the special-case rx_minus_rt() to efficiently rotate both x and
+        // t by the same r
+        init_stride_1D(x_out, 3);
+        init_stride_2D(J_rt,  3,6);
+        init_stride_2D(J_x,   3,3 );
+        init_stride_1D(rt,    6 );
+        init_stride_1D(x_in,  3 );
+
+        if(J_rt == NULL && J_x == NULL)
+        {
+            vec_withgrad_t<0, 3> x_minus_t(x_in,      -1, x_in_stride0);
+            x_minus_t -= vec_withgrad_t<0, 3>(&P1(rt,3), -1, rt_stride0);
+
+            vec_withgrad_t<0, 3> rg   (&rt[0],    -1, rt_stride0);
+            vec_withgrad_t<0, 3> x_outg;
+            rotate_point_r_core<0>(x_outg.v,
+                                   rg.v, x_minus_t.v,
+                                   true);
+            x_outg.extract_value(x_out, x_out_stride0);
+        }
+        else if(J_rt != NULL && J_x == NULL)
+        {
+            vec_withgrad_t<6, 3> x_minus_t(x_in,     -1, x_in_stride0);
+            x_minus_t -= vec_withgrad_t<6, 3>(&P1(rt,3), 3, rt_stride0);
+
+            vec_withgrad_t<6, 3> rg   (&rt[0],    0, rt_stride0);
+            vec_withgrad_t<6, 3> x_outg;
+            rotate_point_r_core<6>(x_outg.v,
+                                   rg.v, x_minus_t.v,
+                                   true);
+            x_outg.extract_value(x_out, x_out_stride0);
+            x_outg.extract_grad (J_rt,          0, 3, 0, J_rt_stride0, J_rt_stride1);
+            x_outg.extract_grad (&P2(J_rt,0,3), 3, 3, 0, J_rt_stride0, J_rt_stride1);
+        }
+        else if(J_rt == NULL && J_x != NULL)
+        {
+            vec_withgrad_t<3, 3> x_minus_t(x_in,      0, x_in_stride0);
+            x_minus_t -= vec_withgrad_t<3, 3>(&P1(rt,3),-1, rt_stride0);
+
+            vec_withgrad_t<3, 3> rg   (&rt[0],   -1, rt_stride0);
+            vec_withgrad_t<3, 3> x_outg;
+            rotate_point_r_core<3>(x_outg.v,
+                                   rg.v, x_minus_t.v,
+                                   true);
+            x_outg.extract_value(x_out, x_out_stride0);
+            x_outg.extract_grad (J_x, 0, 3, 0, J_x_stride0,  J_x_stride1);
+        }
+        else
+        {
+            vec_withgrad_t<9, 3> x_minus_t(x_in,      3, x_in_stride0);
+            x_minus_t -= vec_withgrad_t<9, 3>(&P1(rt,3), 6, rt_stride0);
+
+            vec_withgrad_t<9, 3> rg   (&rt[0],    0, rt_stride0);
+            vec_withgrad_t<9, 3> x_outg;
+
+
+            rotate_point_r_core<9>(x_outg.v,
+                                   rg.v, x_minus_t.v,
+                                   true);
+
+            x_outg.extract_value(x_out, x_out_stride0);
+            x_outg.extract_grad (J_rt,          0, 3, 0, J_rt_stride0, J_rt_stride1);
+            x_outg.extract_grad (&P2(J_rt,0,3), 6, 3, 0, J_rt_stride0, J_rt_stride1);
+            x_outg.extract_grad (J_x,           3, 3, 0, J_x_stride0,  J_x_stride1);
+        }
+    }
+}
+
+
+template<int N>
+static void
+compose_r_core(// output
+               vec_withgrad_t<N, 3>* r,
+
+               // inputs
+               const vec_withgrad_t<N, 3>* r0,
+               const vec_withgrad_t<N, 3>* r1)
+{
+    /*
+
+    Described here:
+
+        Altmann, Simon L. "Hamilton, Rodrigues, and the Quaternion Scandal."
+        Mathematics Magazine, vol. 62, no. 5, 1989, pp. 291–308
+
+    Available here:
+
+        https://www.jstor.org/stable/2689481
+
+    I use Equations (19) and (20) on page 302 of this paper. These equations say
+    that
+
+      R(angle=gamma, axis=n) =
+      compose( R(angle=alpha, axis=l), R(angle=beta, axis=m) )
+
+    I need to compute r01 = gamma*n from r0=alpha*l, r1=beta*m; and these are
+    given as solutions to:
+
+      cos(gamma/2) =
+        cos(alpha/2)*cos(beta/2) -
+        sin(alpha/2)*sin(beta/2) * inner(l,m)
+      sin(gamma/2) n =
+        sin(alpha/2)*cos(beta/2)*l +
+        cos(alpha/2)*sin(beta/2)*m +
+        sin(alpha/2)*sin(beta/2) * cross(l,m)
+
+    For nicer notation, I define
+
+      A = alpha/2
+      B = beta /2
+      C = gamma/2
+
+      l = r0 /(2A)
+      m = r1 /(2B)
+      n = r01/(2C)
+
+    I rewrite:
+
+      cos(C) =
+        cos(A)*cos(B) -
+        sin(A)*sin(B) * inner(r0,r1) / 4AB
+      sin(C) r01 / 2C =
+        sin(A)*cos(B)*r0 / 2A +
+        cos(A)*sin(B)*r1 / 2B +
+        sin(A)*sin(B) * cross(r0,r1) / 4AB
+    ->
+      r01 =
+        C/sin(C) ( sin(A)/A cos(B)*r0 +
+                   sin(B)/B cos(A)*r1 +
+                   sin(A)/A sin(B)/B * cross(r0,r1) / 2 )
+
+    I compute cos(C) and then get C and sin(C) and r01 from that
+
+    Note that each r = th*axis has equivalent axis*(k*2*pi + th)*axis and
+    -axis*(k*2*pi - th) for any integer k.
+
+    Let's confirm that rotating A or B by any number full rotations has no
+    effect on r01.
+
+    We'll have
+
+      alpha += k*2*pi -> A += k*pi
+      r0    += r0/mag(r0)*k*2*pi
+
+      if k is even:
+        sin(A), cos(A) stays the same;
+        r0 / A -> r0 * (1 + k 2pi/mag(r0)) / (A + k pi)
+                = r0 * (1 + k 2pi/2A) / (A + k pi)
+                = r0 * (1 + k pi/A) / (A + k pi)
+                = r0 / A
+
+    So adding an even number of full rotations produces the same exact r01. If
+    k is odd (adding an odd number of full rotations; should still produce the
+    same result) we get
+
+      sin(A), cos(A) -> -sin(A),-cos(A)
+      r0 / A stays the same, as before
+
+      -> cos(C) and sin(C) r01 / 2C = sin(C) n change sign.
+
+      This means that C -> C +- pi. So an odd k switches to a different mode,
+      but the meaning of the rotation vector r01 does not change
+
+
+    What about shifts of r01?
+
+    Let u = sin(A)/A cos(B)*r0 +
+            sin(B)/B cos(A)*r1 +
+            sin(A)/A sin(B)/B * cross(r0,r1) / 2
+
+    and r01 = C/sinC u
+
+      norm2(u) =
+        = norm2( sA/A cB 2A l +
+                 sB/B cA 2B m +
+                 sA/A sB/B 4AB cross(l,m) / 2 )
+        = norm2( sA cB 2 l +
+                 sB cA 2 m +
+                 sA sB 2 cross(l,m) )
+        = 4 (sA cB sA cB +
+             sB cA sB cA +
+             2 sA cB sB cA inner(l,m) +
+             sA sB sA sB norm2(cross(l,m)))
+        = 4 (sA cB sA cB +
+             sB cA sB cA +
+             2 sA cB sB cA inner(l,m) +
+             sA sB sA sB (1 - inner^2(l,m)) )
+
+    I have
+
+      cC = cA cB - sA sB inner(r0,r1) / 4AB
+         = cA cB - sA sB inner(l,m)
+
+    So inner(l,m) = (cA cB - cC)/(sA sB)
+
+      norm2(u) =
+        = 4 (sA cB sA cB +
+             sB cA sB cA +
+             2 sA cB sB cA (cA cB - cC)/(sA sB) +
+             sA sB sA sB (1 - ((cA cB - cC)/(sA sB))^2) )
+        = 4 (sA cB sA cB +
+             sB cA sB cA +
+             2 sA cB sB cA (cA cB - cC)/(sA sB) +
+             sA sB sA sB (1 - (cA cB - cC)^2/(sA sB)^2) )
+        = 4 (sA cB sA cB +
+             sB cA sB cA +
+             2 cB cA cA cB
+            -2 cB cA cC
+             sA sB sA sB
+            -cA cB cA cB
+            -cC cC +
+             2 cA cB cC)
+        = 4 (sA cB sA cB +
+             sB cA sB cA +
+             cB cA cA cB
+             sA sB sA sB
+            -cC cC)
+        = 4 (sA sA +
+             cA cA +
+            -cC cC)
+        = 4 (1 - cC cC)
+        = 4 sC^2
+
+        r01 = C/sinC u ->
+        norm2(r01) = C^2/sin^2(C) norm2(u)
+                   = C^2/sin^2(C) 4 sin^2(C)
+                   = 4 C^2
+      So mag(r01) = 2C
+
+    This is what we expect. And any C that satisfies the above expressions will
+    have mag(r01) = 2C
+
+    */
+
+    const val_withgrad_t<N> A = r0->mag() / 2.;
+    const val_withgrad_t<N> B = r1->mag() / 2.;
+
+    const val_withgrad_t<N>    inner = r0->dot(*r1);
+    const vec_withgrad_t<N, 3> cross = r0->cross(*r1);
+
+    // In radians. If my angle is this close to 0, I use the special-case paths
+    const double eps = 1e-8;
+
+    // Here I special-case A and B near 0. I do this so avoid dividing by 0 in
+    // the /A and /B expressions. There are no /sin(A) and /sin(B) expressions,
+    // so I don't need to think about A ~ k pi
+    if(A.x < eps/2.)
+    {
+        // A ~ 0. I simplify
+        //
+        //   cosC ~
+        //     + cosB
+        //     - A*sinB * inner(r0,r1) / 4AB
+        //   sinC r01 / 2C ~
+        //     + A*cosB* r0 / 2A
+        //     + sinB  * r1 / 2B
+        //     + A*sinB * cross(r0,r1) / 4AB
+        //
+        // I have C = B + dB where dB ~ 0, so
+        //
+        //   cosC ~ cos(B + dB) ~ cosB - dB sinB
+        //   -> dB = A * inner(r0,r1) / 4AB =
+        //           inner(r0,r1) / 4B
+        //   -> C = B + inner(r0,r1) / 4B
+        //
+        // Now let's look at the axis expression. Assuming A ~ 0
+        //
+        //   sinC r01 / 2C ~
+        //     + A*cosB r0 / 2A
+        //     + sinB   r1 / 2B
+        //     + A*sinB * cross(r0,r1) / 4AB
+        // ->
+        //   sinC/C * r01 ~
+        //     + cosB r0
+        //     + sinB r1 / B
+        //     + sinB * cross(r0,r1) / 2B
+        //
+        // I linearize the left-hand side:
+        //
+        //   sinC/C = sin(B+dB)/(B+dB) ~
+        //     sinB/B + d( sinB/B )/dB dB =
+        //     sinB/B + dB  (B cosB - sinB) / B^2
+        //
+        // So
+        //
+        //   (sinB/B + dB  (B cosB - sinB) / B^2) r01 ~
+        //     + cosB r0
+        //     + sinB r1 / B
+        //     + sinB * cross(r0,r1) / 2B
+        // ->
+        //   (sinB + dB  (B cosB - sinB) / B) r01 ~
+        //     + cosB*B r0
+        //     + sinB r1
+        //     + sinB * cross(r0,r1) / 2
+        // ->
+        //   sinB (r01 - r1) + dB (B cosB - sinB) / B r01 ~
+        //     + cosB*B r0
+        //     + sinB * cross(r0,r1) / 2
+        //
+        // I want to find the perturbation to give me r01 ~ r1 + deltar ->
+        //
+        //   sinB deltar + dB (B cosB - sinB) / B (r1 + deltar) ~
+        //     + cosB*B r0
+        //     + sinB * cross(r0,r1) / 2
+        //
+        // All terms here are linear or quadratic in r0. For tiny r0, I can
+        // ignore the quadratic terms:
+        //
+        //   sinB deltar + dB (B cosB - sinB) / B r1 ~
+        //     + cosB*B r0
+        //     + sinB * cross(r0,r1) / 2
+        // ->
+        //   deltar ~
+        //     - dB (B/tanB - 1) / B r1
+        //     + B/tanB r0
+        //     + cross(r0,r1) / 2
+        //
+        // I substitute in the dB from above, and I simplify:
+        //
+        //   deltar ~
+        //     - inner(r0,r1) / 4B (B/tanB - 1) / B r1
+        //     + B/tanB r0
+        //     + cross(r0,r1) / 2
+        //   =
+        //     - inner(r0,r1) (B/tanB - 1) / 4B^2 r1
+        //     + B/tanB r0
+        //     + cross(r0,r1) / 2
+        //
+        // So
+        //
+        // r01 = r1
+        //     - inner(r0,r1) (B/tanB - 1) / 4B^2 r1
+        //     + B/tanB r0
+        //     + cross(r0,r1) / 2
+        if(B.x < eps)
+        {
+            // what if B is ALSO near 0? I simplify further
+            //
+            // lim(B->0) (B/tanB) = lim( 1 / sec^2 B) = 1.
+            // lim(B->0) d(B/tanB)/dB = 0
+            //
+            // (B/tanB - 1) / 4B^2 =
+            // (B - tanB) / (4 B^2 tanB)
+            // lim(B->0) = 0
+            // lim(B->0) d/dB = 0
+            //
+            // So
+            // r01 = r1
+            //     + r0
+            //     + cross(r0,r1) / 2
+            //
+            // Here I have linear and quadratic terms. With my tiny numbers, the
+            // quadratic terms can be ignored, so simply
+            //
+            //   r01 = r0 + r1
+            *r = *r0 + *r1;
+            return;
+        }
+
+        const val_withgrad_t<N>& B_over_tanB = B / B.tan();
+        for(int i=0; i<3; i++)
+            (*r)[i] =
+                (*r1)[i] * (val_withgrad_t<N>(1.0)
+                            - inner * (B_over_tanB - 1.) / (B*B*4.))
+                + (*r0)[i] * B_over_tanB
+                + cross[i] / 2.;
+        return;
+    }
+    else if(B.x < eps)
+    {
+        // B ~ 0. I simplify
+        //
+        //   cosC =
+        //     cosA -
+        //     sinA*B * inner(r0,r1) / 4AB
+        //   sinC r01 / 2C =
+        //     sinA*r0 / 2A +
+        //     cosA*B*r1 / 2B +
+        //     sinA*B * cross(r0,r1) / 4AB
+        //
+        // I have C = A + dA where dA ~ 0, so
+        //
+        //   cosC ~ cos(A + dA) ~ cosA - dA sinA
+        //   -> dA = B * inner(r0,r1) / 4AB =
+        //         = inner(r0,r1) / 4A
+        //   -> C = A + inner(r0,r1) / 4A
+        //
+        // Now let's look at the axis expression. Assuming B ~ 0
+        //
+        //   sinC r01 / 2C =
+        //     + sinA*r0 / 2A
+        //     + cosA*B*r1 / 2B
+        //     + sinA*B * cross(r0,r1) / 4AB
+        // ->
+        //   sinC/C r01 =
+        //     + sinA*r0 / A
+        //     + cosA*r1
+        //     + sinA * cross(r0,r1) / 2A
+        //
+        // I linearize the left-hand side:
+        //
+        //   sinC/C = sin(A+dA)/(A+dA) ~
+        //     sinA/A + d( sinA/A )/dA dA =
+        //     sinA/A + dA  (A cosA - sinA) / A^2
+        //
+        // So
+        //
+        //   (sinA/A + dA  (A cosA - sinA) / A^2) r01 ~
+        //     + sinA*r0 / A
+        //     + cosA*r1
+        //     + sinA * cross(r0,r1) / 2A
+        // ->
+        //   (sinA + dA  (A cosA - sinA) / A) r01 ~
+        //     + sinA*r0
+        //     + cosA*r1*A
+        //     + sinA * cross(r0,r1) / 2
+        // ->
+        //   sinA (r01 - r0) + dA (A cosA - sinA) / A r01 ~
+        //     + cosA*A r1
+        //     + sinA * cross(r0,r1) / 2
+        //
+        //
+        // I want to find the perturbation to give me r01 ~ r0 + deltar ->
+        //
+        //   sinA deltar + dA (A cosA - sinA) / A (r0 + deltar) ~
+        //     + cosA*A r1
+        //     + sinA * cross(r0,r1) / 2
+        //
+        // All terms here are linear or quadratic in r1. For tiny r1, I can
+        // ignore the quadratic terms:
+        //
+        //   sinA deltar + dA (A cosA - sinA) / A r0 ~
+        //     + cosA*A r1
+        //     + sinA * cross(r0,r1) / 2
+        // ->
+        //   deltar ~
+        //     - dA (A/tanA - 1) / A r0
+        //     + A/tanA r1
+        //     + cross(r0,r1) / 2
+        //
+        // I substitute in the dA from above, and I simplify:
+        //
+        //   deltar ~
+        //     - inner(r0,r1) / 4A (A/tanA - 1) / A r0
+        //     + A/tanA r1
+        //     + cross(r0,r1) / 2
+        //   =
+        //     - inner(r0,r1) (A/tanA - 1) / 4A^2 r0
+        //     + A/tanA r1
+        //     + cross(r0,r1) / 2
+        //
+        // So
+        //
+        // r01 = r0
+        //     - inner(r0,r1) (A/tanA - 1) / 4A^2 r0
+        //     + A/tanA r1
+        //     + cross(r0,r1) / 2
+
+        // I don't have an if(A.x < eps){} case here; this is handled in
+        // the above if() branch
+
+        const val_withgrad_t<N>& A_over_tanA = A / A.tan();
+        for(int i=0; i<3; i++)
+            (*r)[i] =
+                (*r0)[i] * (val_withgrad_t<N>(1.0)
+                            - inner * (A_over_tanA - 1.) / (A*A*4.))
+                + (*r1)[i] * A_over_tanA
+                + cross[i] / 2.;
+        return;
+    }
+
+    const vec_withgrad_t<N, 2> sincosA = A.sincos();
+    const vec_withgrad_t<N, 2> sincosB = B.sincos();
+
+    const val_withgrad_t<N>& sinA = sincosA.v[0];
+    const val_withgrad_t<N>& cosA = sincosA.v[1];
+    const val_withgrad_t<N>& sinB = sincosB.v[0];
+    const val_withgrad_t<N>& cosB = sincosB.v[1];
+
+    const val_withgrad_t<N>& sinA_over_A = A.sinx_over_x(sinA);
+    const val_withgrad_t<N>& sinB_over_B = B.sinx_over_x(sinB);
+
+    val_withgrad_t<N> cosC =
+        cosA*cosB -
+        sinA_over_A*sinB_over_B*inner/4.;
+
+    for(int i=0; i<3; i++)
+        (*r)[i] =
+            sinA_over_A*cosB*(*r0)[i] +
+            sinB_over_B*cosA*(*r1)[i] +
+            sinA_over_A*sinB_over_B*cross[i]/2.;
+
+    // To handle numerical fuzz
+    // cos(x ~ 0) ~ 1 - x^2/2
+    if     (cosC.x - 1.0 >  -eps*eps/2.)
+    {
+        // special-case. C ~ 0 -> sin(C)/C ~ 1 -> r01 is already computed. We're
+        // done
+    }
+    // cos(x ~ pi) ~ cos(pi) + (x-pi)^2/2 (-cos(pi))
+    //             ~ -1 + (x-pi)^2/2
+    else if(cosC.x + 1.0 < eps*eps/2. )
+    {
+        // special-case. cos(C) ~ -1 -> C ~ pi. This corresponds to a full
+        // rotation: gamma = 2C ~ 2pi. I wrap it around to avoid dividing by
+        // 0.
+        //
+        // For any r = gamma n where mag(n)=1 I can use an equivalent r' =
+        // (gamma-2pi)n
+        //
+        // Here gamma = 2C so gamma-2pi = 2(C-pi)
+
+        /*
+          I have cosC and u = r01 sin(C)/C   (stored in the "r" variable)
+
+          gamma = mag(r01) = 2C
+          mag(u) * C/sin(C) = 2C
+          mag(u) = 2 sin(C)
+
+          r' = (mag(r01) - 2pi) * r01/mag(r01)
+             = (1 - 2pi/mag(r01)) * r01
+             = (1 - 2pi/2C) * u C/sin(C)
+             = ((C - pi)/C) * u C/sin(C)
+             = (C - pi)/sin(C) * u
+             = -(pi - C)/sin(pi - C) * u
+             ~ -u
+        */
+        for(int i=0; i<3; i++)
+            (*r)[i] *= -1.;
+    }
+    // Not-strictly-necessary special case. I would like to have C in
+    // [-pi/2,pi/2] instead of [0,pi]. This will produce a nicer-looking
+    // (smaller numbers) r01
+    else if(cosC.x < 0.)
+    {
+        // Need to subtract a rotation
+
+        // I have cos(C) and u (stored in "r")
+        // r01 = C/sin(C) u
+        //
+        // I want r01 - r01/mag(r01)*2pi
+        //        = C/sin(C) u ( 1 - 2pi/mag(r01))
+        //        = C/sin(C) u ( 1 - pi/C )
+        //        = 1/sin(C) u ( C - pi )
+        //        = (C - pi)/sin(C) u
+        const val_withgrad_t<N> C    = cosC.acos();
+        const val_withgrad_t<N> sinC = (val_withgrad_t<N>(1.) - cosC*cosC).sqrt();
+
+        for(int i=0; i<3; i++)
+            (*r)[i] *= (C - M_PI) / sinC;
+    }
+    else
+    {
+        // nominal case
+        const val_withgrad_t<N> C    = cosC.acos();
+        const val_withgrad_t<N> sinC = (val_withgrad_t<N>(1.) - cosC*cosC).sqrt();
+
+        for(int i=0; i<3; i++)
+            (*r)[i] *= C / sinC;
+    }
+}
+
+extern "C"
+void mrcal_compose_r_full( // output
+                           double* r_out,       // (3,) array
+                           int r_out_stride0,   // in bytes. <= 0 means "contiguous"
+                           double* dr_dr0,      // (3,3) array; may be NULL
+                           int dr_dr0_stride0,  // in bytes. <= 0 means "contiguous"
+                           int dr_dr0_stride1,  // in bytes. <= 0 means "contiguous"
+                           double* dr_dr1,      // (3,3) array; may be NULL
+                           int dr_dr1_stride0,  // in bytes. <= 0 means "contiguous"
+                           int dr_dr1_stride1,  // in bytes. <= 0 means "contiguous"
+
+                           // input
+                           const double* r_0,   // (3,) array
+                           int r_0_stride0,     // in bytes. <= 0 means "contiguous"
+                           const double* r_1,   // (3,) array
+                           int r_1_stride0      // in bytes. <= 0 means "contiguous"
+                           )
+{
+    init_stride_1D(r_out,  3);
+    init_stride_2D(dr_dr0, 3,3);
+    init_stride_2D(dr_dr1, 3,3);
+    init_stride_1D(r_0,    3);
+    init_stride_1D(r_1,    3);
+
+    if(dr_dr0 == NULL && dr_dr1 == NULL)
+    {
+        // no gradients
+        vec_withgrad_t<0, 3> r0g, r1g;
+
+        r0g.init_vars(r_0,
+                      0, 3, -1,
+                      r_0_stride0);
+        r1g.init_vars(r_1,
+                      0, 3, -1,
+                      r_1_stride0);
+
+        vec_withgrad_t<0, 3> r01g;
+        compose_r_core<0>( &r01g,
+                           &r0g, &r1g );
+
+        r01g.extract_value(r_out, r_out_stride0,
+                           0, 3);
+    }
+    else if(dr_dr0 != NULL && dr_dr1 == NULL)
+    {
+        // r0 gradient only
+        vec_withgrad_t<3, 3> r0g, r1g;
+
+        r0g.init_vars(r_0,
+                      0, 3, 0,
+                      r_0_stride0);
+        r1g.init_vars(r_1,
+                      0, 3, -1,
+                      r_1_stride0);
+
+        vec_withgrad_t<3, 3> r01g;
+        compose_r_core<3>( &r01g,
+                           &r0g, &r1g );
+
+        r01g.extract_value(r_out, r_out_stride0,
+                           0, 3);
+        r01g.extract_grad(dr_dr0,
+                          0,3,
+                          0,
+                          dr_dr0_stride0, dr_dr0_stride1,
+                          3);
+    }
+    else if(dr_dr0 == NULL && dr_dr1 != NULL)
+    {
+        // r1 gradient only
+        vec_withgrad_t<3, 3> r0g, r1g;
+
+        r0g.init_vars(r_0,
+                      0, 3, -1,
+                      r_0_stride0);
+        r1g.init_vars(r_1,
+                      0, 3, 0,
+                      r_1_stride0);
+
+        vec_withgrad_t<3, 3> r01g;
+        compose_r_core<3>( &r01g,
+                           &r0g, &r1g );
+
+        r01g.extract_value(r_out, r_out_stride0,
+                           0, 3);
+        r01g.extract_grad(dr_dr1,
+                          0,3,
+                          0,
+                          dr_dr1_stride0, dr_dr1_stride1,
+                          3);
+    }
+    else
+    {
+        // r0 AND r1 gradients
+        vec_withgrad_t<6, 3> r0g, r1g;
+
+        r0g.init_vars(r_0,
+                      0, 3, 0,
+                      r_0_stride0);
+        r1g.init_vars(r_1,
+                      0, 3, 3,
+                      r_1_stride0);
+
+        vec_withgrad_t<6, 3> r01g;
+        compose_r_core<6>( &r01g,
+                           &r0g, &r1g );
+
+        r01g.extract_value(r_out, r_out_stride0,
+                           0, 3);
+        r01g.extract_grad(dr_dr0,
+                          0,3,
+                          0,
+                          dr_dr0_stride0, dr_dr0_stride1,
+                          3);
+        r01g.extract_grad(dr_dr1,
+                          3,3,
+                          0,
+                          dr_dr1_stride0, dr_dr1_stride1,
+                          3);
+    }
+}

wpical/src/main/native/thirdparty/mrcal_java/include/mrcal_wrapper.h

@@ -0,0 +1,84 @@
+/*
+ * Copyright (C) Photon Vision.
+ * 
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#pragma once
+
+extern "C" {
+// Seems to be missing C++ guards
+#include <mrcal.h>
+
+} // extern "C"
+
+// Seems like these people don't properly extern-c their headers either
+extern "C" {
+#include <suitesparse/SuiteSparse_config.h>
+#include <suitesparse/cholmod_core.h>
+} // extern "C"
+
+#include <memory>
+#include <opencv2/opencv.hpp>
+#include <span>
+#include <utility>
+#include <vector>
+
+struct mrcal_result {
+  bool success;
+  std::vector<double> intrinsics;
+  double rms_error;
+  std::vector<double> residuals;
+  mrcal_calobject_warp_t calobject_warp;
+  int Noutliers_board;
+  // TODO standard devs
+
+  mrcal_result() = default;
+  mrcal_result(bool success_, std::vector<double> intrinsics_,
+               double rms_error_, std::vector<double> residuals_,
+               mrcal_calobject_warp_t calobject_warp_, int Noutliers_board_)
+      : success{success_}, intrinsics{std::move(intrinsics_)},
+        rms_error{rms_error_}, residuals{std::move(residuals_)},
+        calobject_warp{calobject_warp_}, Noutliers_board{Noutliers_board_} {}
+  mrcal_result(mrcal_result &&) = delete;
+  ~mrcal_result();
+};
+
+mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool,
+                         cv::Size boardSize, cv::Size imagerSize,
+                         double squareSize, double focal_len_guess);
+
+std::unique_ptr<mrcal_result> mrcal_main(
+    // List, depth is ordered array observation[N frames, object_height,
+    // object_width] = [x,y, weight] weight<0 means ignored)
+    std::span<mrcal_point3_t> observations_board,
+    // RT transform from camera to object
+    std::span<mrcal_pose_t> frames_rt_toref,
+    // Chessboard size, in corners (not squares)
+    cv::Size calobjectSize, double boardSpacing,
+    // res, pixels
+    cv::Size cameraRes,
+    // focal length, in pixels
+    double focal_len_guess);
+
+enum class CameraLensModel {
+  LENSMODEL_OPENCV5 = 0,
+  LENSMODEL_OPENCV8,
+  LENSMODEL_STEREOGRAPHIC,
+  LENSMODEL_SPLINED_STEREOGRAPHIC
+};
+
+bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat,
+                     const cv::Mat *distCoeffs, CameraLensModel lensModel,
+                     // Extra stuff for splined stereographic models
+                     uint16_t order, uint16_t Nx, uint16_t Ny,
+                     uint16_t fov_x_deg);

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

@@ -0,0 +1,614 @@
+/*
+ * Copyright (C) Photon Vision.
+ *
+ * Permission to use, copy, modify, and/or distribute this software for any
+ * purpose with or without fee is hereby granted.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
+ * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR ANY
+ * SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
+ * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN ACTION
+ * OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF OR IN
+ * CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
+ */
+
+#include "mrcal_wrapper.h"
+
+
+#include <stdint.h>
+
+#include <algorithm>
+#include <chrono>
+#include <cstdio>
+#include <random>
+#include <span>
+#include <stdexcept>
+#include <vector>
+
+using namespace cv;
+
+class CholmodCtx {
+public:
+  cholmod_common Common, *cc;
+  CholmodCtx() {
+    cc = &Common;
+    cholmod_l_start(cc);
+  }
+
+  ~CholmodCtx() { cholmod_l_finish(cc); }
+};
+static CholmodCtx cctx;
+
+#define BARF(...) std::fprintf(stderr, __VA_ARGS__)
+
+// forward declarations for functions borrowed from mrcal-pywrap
+static mrcal_problem_selections_t construct_problem_selections(
+    int do_optimize_intrinsics_core, int do_optimize_intrinsics_distortions,
+    int do_optimize_extrinsics, int do_optimize_frames,
+    int do_optimize_calobject_warp, int do_apply_regularization,
+    int do_apply_outlier_rejection, int Ncameras_intrinsics,
+    int Ncameras_extrinsics, int Nframes, int Nobservations_board);
+
+bool lensmodel_one_validate_args(mrcal_lensmodel_t *mrcal_lensmodel,
+                                 std::vector<double> intrinsics,
+                                 bool do_check_layout);
+
+mrcal_point3_t* observations_point = nullptr;
+mrcal_pose_t*
+    extrinsics_rt_fromref = nullptr; // Always zero for single camera, it seems?
+mrcal_point3_t* points = nullptr;     // Seems to always to be None for single camera?
+
+static std::unique_ptr<mrcal_result> mrcal_calibrate(
+    // List, depth is ordered array observation[N frames, object_height,
+    // object_width] = [x,y, weight] weight<0 means ignored)
+    std::span<mrcal_point3_t> observations_board,
+    // RT transform from camera to object
+    std::span<mrcal_pose_t> frames_rt_toref,
+    // Chessboard size, in corners (not squares)
+    Size calobjectSize, double calibration_object_spacing,
+    // res, pixels
+    Size cameraRes,
+    // solver options
+    mrcal_problem_selections_t problem_selections,
+    // seed intrinsics/cameramodel to optimize for
+    mrcal_lensmodel_t mrcal_lensmodel, std::vector<double> intrinsics) {
+  // Number of board observations we've got. List of boards. in python, it's
+  // (number of chessboard pictures) x (rows) x (cos) x (3)
+  // hard-coded to 8, since that's what I've got below
+  int Nobservations_board = frames_rt_toref.size();
+
+  // Looks like this is hard-coded to 0 in Python for initial single-camera
+  // solve?
+  int Nobservations_point = 0;
+
+  int calibration_object_width_n =
+      calobjectSize.width; // Object width, in # of corners
+  int calibration_object_height_n =
+      calobjectSize.height; // Object height, in # of corners
+
+  // TODO set sizes and populate
+  int imagersize[] = {cameraRes.width, cameraRes.height};
+
+  mrcal_calobject_warp_t calobject_warp = {0, 0};
+
+  // int Nobservations_point_triangulated = 0; // no clue what this is
+
+  int Npoints = 0;       // seems like this is also unused? whack
+  int Npoints_fixed = 0; // seems like this is also unused? whack
+
+  // Number of cameras to solve for intrinsics
+  int Ncameras_intrinsics = 1;
+
+  // Hard-coded to match out 8 frames from above (borrowed from python)
+  std::vector<mrcal_point3_t> indices_frame_camintrinsics_camextrinsics;
+  // Frame index, camera number, (camera number)-1???
+  for (int i = 0; i < Nobservations_board; i++) {
+    indices_frame_camintrinsics_camextrinsics.push_back(
+        {static_cast<double>(i), 0, -1});
+  }
+
+  // Pool is the raw observation backing array
+  mrcal_point3_t *c_observations_board_pool = (observations_board.data());
+  mrcal_point3_t *c_observations_point_pool = observations_point;
+
+  // Copy from board/point pool above, using some code borrowed from
+  // mrcal-pywrap
+  std::vector<mrcal_observation_board_t> observations_board_data(Nobservations_board);
+  auto c_observations_board = observations_board_data.data();
+  // Try to make sure we don't accidentally make a zero-length array or
+  // something stupid
+  std::vector<mrcal_observation_point_t>
+      observations_point_data(std::max(Nobservations_point, 1));
+  mrcal_observation_point_t*
+      c_observations_point = observations_point_data.data();
+
+  for (int i_observation = 0; i_observation < Nobservations_board;
+       i_observation++) {
+    int32_t iframe =
+        indices_frame_camintrinsics_camextrinsics.at(i_observation).x;
+    int32_t icam_intrinsics =
+        indices_frame_camintrinsics_camextrinsics.at(i_observation).y;
+    int32_t icam_extrinsics =
+        indices_frame_camintrinsics_camextrinsics.at(i_observation).z;
+
+    c_observations_board[i_observation].icam.intrinsics = icam_intrinsics;
+    c_observations_board[i_observation].icam.extrinsics = icam_extrinsics;
+    c_observations_board[i_observation].iframe = iframe;
+  }
+  for (int i_observation = 0; i_observation < Nobservations_point;
+       i_observation++) {
+    int32_t i_point =
+        indices_frame_camintrinsics_camextrinsics.at(i_observation).x;
+    int32_t icam_intrinsics =
+        indices_frame_camintrinsics_camextrinsics.at(i_observation).y;
+    int32_t icam_extrinsics =
+        indices_frame_camintrinsics_camextrinsics.at(i_observation).z;
+
+    c_observations_point[i_observation].icam.intrinsics = icam_intrinsics;
+    c_observations_point[i_observation].icam.extrinsics = icam_extrinsics;
+    c_observations_point[i_observation].i_point = i_point;
+  }
+
+  int Ncameras_extrinsics = 0; // Seems to always be zero for single camera
+  int Nframes =
+      frames_rt_toref.size(); // Number of pictures of the object we've got
+  mrcal_observation_point_triangulated_t *observations_point_triangulated = NULL;
+  //     NULL;
+
+  if (!lensmodel_one_validate_args(&mrcal_lensmodel, intrinsics, false)) {
+    auto ret = std::make_unique<mrcal_result>();
+    ret->success = false;
+    return ret;
+  }
+
+  int Nstate = mrcal_num_states(
+      Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed,
+      Nobservations_board, problem_selections, &mrcal_lensmodel);
+
+  int Nmeasurements = mrcal_num_measurements(
+      Nobservations_board, Nobservations_point,
+      observations_point_triangulated,
+      0, // hard-coded to 0
+      calibration_object_width_n, calibration_object_height_n,
+      Ncameras_intrinsics, Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed,
+      problem_selections, &mrcal_lensmodel);
+
+  // OK, now we should have everything ready! Just some final setup and then
+  // call optimize
+
+  // Residuals
+  std::vector<double> b_packed_final(Nstate);
+  auto c_b_packed_final = b_packed_final.data();
+
+  std::vector<double> x_final(Nmeasurements);
+  auto c_x_final = x_final.data();
+
+  // Seeds
+  double *c_intrinsics = intrinsics.data();
+  mrcal_pose_t *c_extrinsics = extrinsics_rt_fromref;
+  mrcal_pose_t *c_frames = frames_rt_toref.data();
+  mrcal_point3_t *c_points = points;
+  mrcal_calobject_warp_t *c_calobject_warp = &calobject_warp;
+
+  // in
+  int *c_imagersizes = imagersize;
+  auto point_min_range = -1.0, point_max_range = -1.0;
+  mrcal_problem_constants_t problem_constants = {
+      .point_min_range = point_min_range, .point_max_range = point_max_range};
+  int verbose = 0;
+
+  auto stats = mrcal_optimize(
+      NULL, -1, c_x_final, Nmeasurements * sizeof(double), c_intrinsics,
+      c_extrinsics, c_frames, c_points, c_calobject_warp, Ncameras_intrinsics,
+      Ncameras_extrinsics, Nframes, Npoints, Npoints_fixed,
+      c_observations_board, c_observations_point, Nobservations_board,
+      Nobservations_point,
+      observations_point_triangulated, -1,
+      c_observations_board_pool, c_observations_point_pool, &mrcal_lensmodel, c_imagersizes,
+      problem_selections, &problem_constants, calibration_object_spacing,
+      calibration_object_width_n, calibration_object_height_n, verbose, false);
+
+  std::vector<double> residuals = {c_x_final, c_x_final + Nmeasurements};
+  return std::make_unique<mrcal_result>(
+      true, intrinsics, stats.rms_reproj_error__pixels, residuals,
+      calobject_warp, stats.Noutliers_board);
+}
+
+struct MrcalSolveOptions {
+  // If true, we solve for the intrinsics core. Applies only to those models
+  // that HAVE a core (fx,fy,cx,cy)
+  int do_optimize_intrinsics_core;
+
+  // If true, solve for the non-core lens parameters
+  int do_optimize_intrinsics_distortions;
+
+  // If true, solve for the geometry of the cameras
+  int do_optimize_extrinsics;
+
+  // If true, solve for the poses of the calibration object
+  int do_optimize_frames;
+
+  // If true, optimize the shape of the calibration object
+  int do_optimize_calobject_warp;
+
+  // If true, apply the regularization terms in the solver
+  int do_apply_regularization;
+
+  // Whether to try to find NEW outliers. The outliers given on
+  // input are respected regardless
+  int do_apply_outlier_rejection;
+};
+
+// lifted from mrcal-pywrap.c. Restrict a given selection to only valid options
+// License for mrcal-pywrap.c:
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+static mrcal_problem_selections_t
+construct_problem_selections(MrcalSolveOptions s, int Ncameras_intrinsics,
+                             int Ncameras_extrinsics, int Nframes,
+                             int Nobservations_board) {
+  // By default we optimize everything we can
+  if (s.do_optimize_intrinsics_core < 0)
+    s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0;
+  if (s.do_optimize_intrinsics_distortions < 0)
+    s.do_optimize_intrinsics_core = Ncameras_intrinsics > 0;
+  if (s.do_optimize_extrinsics < 0)
+    s.do_optimize_extrinsics = Ncameras_extrinsics > 0;
+  if (s.do_optimize_frames < 0)
+    s.do_optimize_frames = Nframes > 0;
+  if (s.do_optimize_calobject_warp < 0)
+    s.do_optimize_calobject_warp = Nobservations_board > 0;
+  return {
+      .do_optimize_intrinsics_core =
+          static_cast<bool>(s.do_optimize_intrinsics_core),
+      .do_optimize_intrinsics_distortions =
+          static_cast<bool>(s.do_optimize_intrinsics_distortions),
+      .do_optimize_extrinsics = static_cast<bool>(s.do_optimize_extrinsics),
+      .do_optimize_frames = static_cast<bool>(s.do_optimize_frames),
+      .do_optimize_calobject_warp =
+          static_cast<bool>(s.do_optimize_calobject_warp),
+      .do_apply_regularization = static_cast<bool>(s.do_apply_regularization),
+      .do_apply_outlier_rejection =
+          static_cast<bool>(s.do_apply_outlier_rejection),
+      // .do_apply_regularization_unity_cam01 = false
+  };
+}
+
+bool lensmodel_one_validate_args(mrcal_lensmodel_t *mrcal_lensmodel,
+                                 std::vector<double> intrinsics,
+                                 bool do_check_layout) {
+  int NlensParams = mrcal_lensmodel_num_params(mrcal_lensmodel);
+  int NlensParams_have = intrinsics.size();
+  if (NlensParams != NlensParams_have) {
+    BARF("intrinsics.shape[-1] MUST be %d. Instead got %d", NlensParams,
+         NlensParams_have);
+    return false;
+  }
+
+  return true;
+}
+
+mrcal_pose_t getSeedPose(const mrcal_point3_t *c_observations_board_pool,
+                         Size boardSize, Size imagerSize, double squareSize,
+                         double focal_len_guess) {
+  using std::vector, std::runtime_error;
+
+  if (!c_observations_board_pool) {
+    throw runtime_error("board was null");
+  }
+
+  double fx = focal_len_guess;
+  double fy = fx;
+  double cx = (imagerSize.width / 2.0) - 0.5;
+  double cy = (imagerSize.height / 2.0) - 0.5;
+
+  vector<Point3f> objectPoints;
+  vector<Point2d> imagePoints;
+
+  // Fill in object/image points
+  for (int i = 0; i < boardSize.height; i++) {
+    for (int j = 0; j < boardSize.width; j++) {
+      auto &corner = c_observations_board_pool[i * boardSize.width + j];
+      // weight<0 means ignored -- filter these out
+      if (corner.z >= 0) {
+        imagePoints.emplace_back(corner.x, corner.y);
+        objectPoints.push_back(Point3f(j * squareSize, i * squareSize, 0));
+      }
+    }
+  }
+
+  {
+    // convert from stereographic to pinhole to match python
+    std::vector<mrcal_point2_t> mrcal_imagepts(imagePoints.size());
+    std::transform(
+        imagePoints.begin(), imagePoints.end(), mrcal_imagepts.begin(),
+        [](const auto &pt) { return mrcal_point2_t{.x = pt.x, .y = pt.y}; });
+
+    mrcal_lensmodel_t model{.type = MRCAL_LENSMODEL_STEREOGRAPHIC};
+    std::vector<mrcal_point3_t> out(imagePoints.size());
+    const double intrinsics[] = {fx, fy, cx, cy};
+    bool ret = mrcal_unproject(out.data(), mrcal_imagepts.data(),
+                               mrcal_imagepts.size(), &model, intrinsics);
+    if (!ret) {
+      std::cerr << "couldn't unproject!" << std::endl;
+    }
+    model = {.type = MRCAL_LENSMODEL_PINHOLE};
+    mrcal_project(mrcal_imagepts.data(), NULL, NULL, out.data(), out.size(),
+                  &model, intrinsics);
+
+    std::transform(mrcal_imagepts.begin(), mrcal_imagepts.end(),
+                   imagePoints.begin(),
+                   [](const auto &pt) { return Point2d{pt.x, pt.y}; });
+  }
+
+  // Initial guess at intrinsics
+  Mat cameraMatrix = (Mat_<double>(3, 3) << fx, 0, cx, 0, fy, cy, 0, 0, 1);
+  Mat distCoeffs = Mat(4, 1, CV_64FC1, Scalar(0));
+
+  Mat_<double> rvec, tvec;
+  vector<Point3f> objectPoints3;
+  for (auto a : objectPoints)
+    objectPoints3.push_back(Point3f(a.x, a.y, 0));
+
+  // for (auto& o : objectPoints) std::cout << o << std::endl;
+  // for (auto& i : imagePoints) std::cout << i << std::endl;
+  // std::cout << "cam mat\n" << cameraMatrix << std::endl;
+  // std::cout << "distortion: " << distCoeffs << std::endl;
+
+  solvePnP(objectPoints3, imagePoints, cameraMatrix, distCoeffs, rvec, tvec,
+           false, SOLVEPNP_ITERATIVE);
+
+  return mrcal_pose_t{.r = {.x = rvec(0), .y = rvec(1), .z = rvec(2)},
+                      .t = {.x = tvec(0), .y = tvec(1), .z = tvec(2)}};
+}
+
+mrcal_result::~mrcal_result() {
+  // cholmod_l_free_sparse(&Jt, cctx.cc);
+  return;
+}
+
+// Code taken from mrcal, license:
+// Copyright (c) 2017-2023 California Institute of Technology ("Caltech"). U.S.
+// Government sponsorship acknowledged. All rights reserved.
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+std::unique_ptr<mrcal_result> mrcal_main(
+    // List, depth is ordered array observation[N frames, object_height,
+    // object_width] = [x,y, weight] weight<0 means ignored)
+    std::span<mrcal_point3_t> observations_board,
+    // RT transform from camera to object
+    std::span<mrcal_pose_t> frames_rt_toref,
+    // Chessboard size, in corners (not squares)
+    Size calobjectSize, double calibration_object_spacing,
+    // res, pixels
+    Size cameraRes, double focal_length_guess) {
+
+  std::unique_ptr<mrcal_result> result;
+
+  {
+    // stereographic initial guess for intrinsics
+    double cx = (cameraRes.width / 2.0) - 0.5;
+    double cy = (cameraRes.height / 2.0) - 0.5;
+    std::vector<double> intrinsics = {focal_length_guess, focal_length_guess,
+                                      cx, cy};
+
+    std::cout << "Initial solve (geometry only)" << std::endl;
+
+    mrcal_problem_selections_t options = construct_problem_selections(
+        {.do_optimize_intrinsics_core = false,
+         .do_optimize_intrinsics_distortions = false,
+         .do_optimize_extrinsics = false,
+         .do_optimize_frames = true,
+         .do_optimize_calobject_warp = false,
+         .do_apply_regularization = false,
+         .do_apply_outlier_rejection = false},
+        1, 0, frames_rt_toref.size(), frames_rt_toref.size());
+
+    mrcal_lensmodel_t mrcal_lensmodel;
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC;
+
+    // And run calibration. This should mutate frames_rt_toref in place
+    result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize,
+                             calibration_object_spacing, cameraRes, options,
+                             mrcal_lensmodel, intrinsics);
+  }
+
+  {
+    std::cout
+        << "Initial solve (geometry and LENSMODEL_STEREOGRAPHIC core only)"
+        << std::endl;
+    mrcal_problem_selections_t options = construct_problem_selections(
+        {.do_optimize_intrinsics_core = true,
+         .do_optimize_intrinsics_distortions = true,
+         .do_optimize_extrinsics = false,
+         .do_optimize_frames = true,
+         .do_optimize_calobject_warp = false,
+         .do_apply_regularization = false,
+         .do_apply_outlier_rejection = false},
+        1, 0, frames_rt_toref.size(), frames_rt_toref.size());
+
+    mrcal_lensmodel_t mrcal_lensmodel;
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC;
+
+    result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize,
+                             calibration_object_spacing, cameraRes, options,
+                             mrcal_lensmodel, result->intrinsics);
+  }
+
+  {
+    // Now we've got a seed, expand intrinsics to our target model
+    // see
+    // https://github.com/dkogan/mrcal/blob/33c3c50d5b7f991aca3a8e71ca52c5fffd153ef2/mrcal-calibrate-cameras#L533
+    mrcal_lensmodel_t mrcal_lensmodel;
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8;
+    // num distortion params
+    int nparams = mrcal_lensmodel_num_params(&mrcal_lensmodel);
+    std::vector<double> intrinsics(nparams);
+
+    // copy core in
+    std::copy(result->intrinsics.begin(), result->intrinsics.end(),
+              intrinsics.begin());
+
+    // and generate random distortion
+    std::random_device rd;
+    std::mt19937 gen(rd());
+    std::uniform_real_distribution<> dis(-0.5, 0.5);
+
+    int nDistortion = nparams - 4;
+    std::vector<double> seedDistortions(nDistortion);
+
+    for (int j = 0; j < seedDistortions.size(); j++) {
+      if (j < 5)
+        seedDistortions[j] = dis(gen) * 2.0 * 1e-6;
+      else
+        seedDistortions[j] = dis(gen) * 2.0 * 1e-9;
+    }
+
+    // copy distortion into our big intrinsics array
+    std::copy(seedDistortions.begin(), seedDistortions.end(),
+              intrinsics.begin() + result->intrinsics.size());
+
+    std::cout
+        << "Optimizing everything except board warp from seeded intrinsics"
+        << std::endl;
+    mrcal_problem_selections_t options = construct_problem_selections(
+        {.do_optimize_intrinsics_core = true,
+         .do_optimize_intrinsics_distortions = true,
+         .do_optimize_extrinsics = true,
+         .do_optimize_frames = true,
+         .do_optimize_calobject_warp = false,
+         .do_apply_regularization = true,
+         .do_apply_outlier_rejection = true},
+        1, 0, frames_rt_toref.size(), frames_rt_toref.size());
+
+    result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize,
+                             calibration_object_spacing, cameraRes, options,
+                             mrcal_lensmodel, intrinsics);
+  }
+
+  {
+    std::cout << "Final, full solve" << std::endl;
+    mrcal_problem_selections_t options = construct_problem_selections(
+        {.do_optimize_intrinsics_core = true,
+         .do_optimize_intrinsics_distortions = true,
+         .do_optimize_extrinsics = true,
+         .do_optimize_frames = true,
+         .do_optimize_calobject_warp = true,
+         .do_apply_regularization = true,
+         .do_apply_outlier_rejection = true},
+        1, 0, frames_rt_toref.size(), frames_rt_toref.size());
+
+    mrcal_lensmodel_t mrcal_lensmodel;
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8;
+
+    result = mrcal_calibrate(observations_board, frames_rt_toref, calobjectSize,
+                             calibration_object_spacing, cameraRes, options,
+                             mrcal_lensmodel, result->intrinsics);
+  }
+
+  return result;
+}
+
+bool undistort_mrcal(const cv::Mat *src, cv::Mat *dst, const cv::Mat *cameraMat,
+                     const cv::Mat *distCoeffs, CameraLensModel lensModel,
+                     // Extra stuff for splined stereographic models
+                     uint16_t order, uint16_t Nx, uint16_t Ny,
+                     uint16_t fov_x_deg) {
+  mrcal_lensmodel_t mrcal_lensmodel;
+  switch (lensModel) {
+  case CameraLensModel::LENSMODEL_OPENCV5:
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV5;
+    break;
+  case CameraLensModel::LENSMODEL_OPENCV8:
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_OPENCV8;
+    break;
+  case CameraLensModel::LENSMODEL_STEREOGRAPHIC:
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_STEREOGRAPHIC;
+    break;
+  case CameraLensModel::LENSMODEL_SPLINED_STEREOGRAPHIC:
+    mrcal_lensmodel.type = MRCAL_LENSMODEL_SPLINED_STEREOGRAPHIC;
+
+    /* Maximum degree of each 1D polynomial. This is almost certainly 2 */
+    /* (quadratic splines, C1 continuous) or 3 (cubic splines, C2 continuous) */
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.order = order;
+    /* The horizontal field of view. Not including fov_y. It's proportional with
+     * Ny and Nx */
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.fov_x_deg =
+        fov_x_deg;
+    /* We have a Nx by Ny grid of control points */
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Nx = Nx;
+    mrcal_lensmodel.LENSMODEL_SPLINED_STEREOGRAPHIC__config.Ny = Ny;
+    break;
+  default:
+    std::cerr << "Unknown lensmodel\n";
+    return false;
+  }
+
+  if (!(dst->cols == 2 && dst->cols == 2)) {
+    std::cerr << "Bad input array size\n";
+    return false;
+  }
+  if (!(dst->type() == CV_64FC2 && dst->type() == CV_64FC2)) {
+    std::cerr << "Bad input type -- need CV_64F\n";
+    return false;
+  }
+  if (!(dst->isContinuous() && dst->isContinuous())) {
+    std::cerr << "Bad input array -- need continuous\n";
+    return false;
+  }
+
+  // extract intrinsics core from opencv camera matrix
+  double fx = cameraMat->at<double>(0, 0);
+  double fy = cameraMat->at<double>(1, 1);
+  double cx = cameraMat->at<double>(0, 2);
+  double cy = cameraMat->at<double>(1, 2);
+
+  // Core, distortion coefficients concatenated
+  int NlensParams = mrcal_lensmodel_num_params(&mrcal_lensmodel);
+  std::vector<double> intrinsics(NlensParams);
+  intrinsics[0] = fx;
+  intrinsics[1] = fy;
+  intrinsics[2] = cx;
+  intrinsics[3] = cy;
+  for (size_t i = 0; i < distCoeffs->cols; i++) {
+    intrinsics[i + 4] = distCoeffs->at<double>(i);
+  }
+
+  // input points in the distorted image pixel coordinates
+  mrcal_point2_t *in = reinterpret_cast<mrcal_point2_t *>(dst->data);
+  // vec3 observation vectors defined up-to-length
+  std::vector<mrcal_point3_t> out(dst->rows);
+
+  mrcal_unproject(out.data(), in, dst->rows, &mrcal_lensmodel,
+                  intrinsics.data());
+
+  // The output is defined "up-to-length"
+  // Let's project through pinhole again
+
+  // Output points in pinhole pixel coordinates
+  mrcal_point2_t *pinhole_pts = reinterpret_cast<mrcal_point2_t *>(dst->data);
+
+  size_t bound = dst->rows;
+  for (size_t i = 0; i < bound; i++) {
+    // from mrcal-project-internal/pinhole model
+    mrcal_point3_t &p = out[i];
+
+    double z_recip = 1. / p.z;
+    double x = p.x * z_recip;
+    double y = p.y * z_recip;
+
+    pinhole_pts[i].x = x * fx + cx;
+    pinhole_pts[i].y = y * fy + cy;
+  }
+
+  return true;
+}

wpical/src/main/native/win/wpical.rc

@@ -0,0 +1 @@
+IDI_ICON1 ICON "wpical.ico"