SCRIPT Move subprojects

tools/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

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

@@ -0,0 +1,17 @@
+// 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 <wpi/json.h>
+
+#include "cameracalibration.h"
+
+namespace fieldcalibration {
+int calibrate(std::string input_dir_path, wpi::json& output_json,
+              std::string camera_model_path, std::string ideal_map_path,
+              int pinned_tag_id, bool show_debug_window);
+}  // namespace fieldcalibration

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

@@ -0,0 +1,83 @@
+// 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 <map>
+
+#include <wpi/json.h>
+
+#include "tagpose.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 tag_id = static_cast<int>(tag.value().at("ID"));
+      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"));
+
+      tagMap.emplace(
+          tag_id, tag::Pose(tag_id, tagXPos, tagYPos, tagZPos, tagWQuat,
+                            tagXQuat, tagYQuat, tagZQuat, field_length_meters,
+                            field_width_meters));
+    }
+    fieldLength = field_length_meters;
+    fieldWidth = field_width_meters;
+  }
+
+  const tag::Pose& getTag(size_t tag) const { return tagMap.at(tag); }
+
+  int getNumTags() const { return tagMap.size(); }
+
+  bool hasTag(int tag) { return tagMap.find(tag) != tagMap.end(); }
+
+  wpi::json toJson() {
+    wpi::json json;
+    for (auto& [key, val] : tagMap) {
+      json["tags"].push_back(val.toJson());
+    }
+    json["field"]["length"] = fieldLength;
+    json["field"]["width"] = fieldWidth;
+    return json;
+  }
+
+  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;
+  }
+
+  void replaceTag(int tag_id, tag::Pose pose) {
+    tagMap.erase(tag_id);
+    tagMap.emplace(tag_id, pose);
+  }
+
+ private:
+  double fieldLength;
+  double fieldWidth;
+  std::map<int, tag::Pose> tagMap;
+};

tools/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 <wpi/json.h>
+
+#include "fieldmap.h"
+#include "tagpose.h"
+
+namespace fmap {
+wpi::json singleTag(int tag, const tag::Pose& tagpose);
+wpi::json convertfmap(const wpi::json& json);
+}  // namespace fmap

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

@@ -0,0 +1,24 @@
+// 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>
+#include <wpi/json.h>
+
+namespace tag {
+class Pose {
+ public:
+  Pose(int tag_id, double xpos, double ypos, double zpos, double w, double x,
+       double y, double z, double field_length_meters,
+       double field_width_meters);
+  int tagId;
+  double xPos, yPos, zPos, yawRot, rollRot, pitchRot;
+  Eigen::Quaterniond quaternion;
+  Eigen::Matrix3d rotationMatrix;
+  Eigen::Matrix4d transformMatrixFmap;
+  wpi::json toJson();
+};
+}  // namespace tag