calibrations = currentCamera.getAllCalibrationData().stream()
+ .map(CameraCalibrationConfig.UICameraCalibrationConfig::new).collect(Collectors.toList());
+ tmp.put("calibration", calibrations);
+
return tmp;
}
diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
new file mode 100644
index 000000000..92c5a0219
--- /dev/null
+++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Pose2d.java
@@ -0,0 +1,271 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.io.IOException;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.SerializerProvider;
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+
+/**
+ * Represents a 2d pose containing translational and rotational elements.
+ */
+@JsonSerialize(using = Pose2d.PoseSerializer.class)
+@JsonDeserialize(using = Pose2d.PoseDeserializer.class)
+public class Pose2d {
+ private final Translation2d m_translation;
+ private final Rotation2d m_rotation;
+
+ /**
+ * Constructs a pose at the origin facing toward the positive X axis.
+ * (Translation2d{0, 0} and Rotation{0})
+ */
+ public Pose2d() {
+ m_translation = new Translation2d();
+ m_rotation = new Rotation2d();
+ }
+
+ /**
+ * Constructs a pose with the specified translation and rotation.
+ *
+ * @param translation The translational component of the pose.
+ * @param rotation The rotational component of the pose.
+ */
+ public Pose2d(Translation2d translation, Rotation2d rotation) {
+ m_translation = translation;
+ m_rotation = rotation;
+ }
+
+ /**
+ * Convenience constructors that takes in x and y values directly instead of
+ * having to construct a Translation2d.
+ *
+ * @param x The x component of the translational component of the pose.
+ * @param y The y component of the translational component of the pose.
+ * @param rotation The rotational component of the pose.
+ */
+ @SuppressWarnings("ParameterName")
+ public Pose2d(double x, double y, Rotation2d rotation) {
+ m_translation = new Translation2d(x, y);
+ m_rotation = rotation;
+ }
+
+ /**
+ * Transforms the pose by the given transformation and returns the new
+ * transformed pose.
+ *
+ * The matrix multiplication is as follows
+ * [x_new] [cos, -sin, 0][transform.x]
+ * [y_new] += [sin, cos, 0][transform.y]
+ * [t_new] [0, 0, 1][transform.t]
+ *
+ * @param other The transform to transform the pose by.
+ * @return The transformed pose.
+ */
+ public Pose2d plus(Transform2d other) {
+ return transformBy(other);
+ }
+
+ /**
+ * Returns the Transform2d that maps the one pose to another.
+ *
+ * @param other The initial pose of the transformation.
+ * @return The transform that maps the other pose to the current pose.
+ */
+ public Transform2d minus(Pose2d other) {
+ final var pose = this.relativeTo(other);
+ return new Transform2d(pose.getTranslation(), pose.getRotation());
+ }
+
+ /**
+ * Returns the translation component of the transformation.
+ *
+ * @return The translational component of the pose.
+ */
+ public Translation2d getTranslation() {
+ return m_translation;
+ }
+
+ /**
+ * Returns the rotational component of the transformation.
+ *
+ * @return The rotational component of the pose.
+ */
+ public Rotation2d getRotation() {
+ return m_rotation;
+ }
+
+ /**
+ * Transforms the pose by the given transformation and returns the new pose.
+ * See + operator for the matrix multiplication performed.
+ *
+ * @param other The transform to transform the pose by.
+ * @return The transformed pose.
+ */
+ public Pose2d transformBy(Transform2d other) {
+ return new Pose2d(m_translation.plus(other.getTranslation().rotateBy(m_rotation)),
+ m_rotation.plus(other.getRotation()));
+ }
+
+ /**
+ * Returns the other pose relative to the current pose.
+ *
+ *
This function can often be used for trajectory tracking or pose
+ * stabilization algorithms to get the error between the reference and the
+ * current pose.
+ *
+ * @param other The pose that is the origin of the new coordinate frame that
+ * the current pose will be converted into.
+ * @return The current pose relative to the new origin pose.
+ */
+ public Pose2d relativeTo(Pose2d other) {
+ var transform = new Transform2d(other, this);
+ return new Pose2d(transform.getTranslation(), transform.getRotation());
+ }
+
+ /**
+ * Obtain a new Pose2d from a (constant curvature) velocity.
+ *
+ *
See
+ * Controls Engineering in the FIRST Robotics Competition
+ * section on nonlinear pose estimation for derivation.
+ *
+ *
The twist is a change in pose in the robot's coordinate frame since the
+ * previous pose update. When the user runs exp() on the previous known
+ * field-relative pose with the argument being the twist, the user will
+ * receive the new field-relative pose.
+ *
+ *
"Exp" represents the pose exponential, which is solving a differential
+ * equation moving the pose forward in time.
+ *
+ * @param twist The change in pose in the robot's coordinate frame since the
+ * previous pose update. For example, if a non-holonomic robot moves forward
+ * 0.01 meters and changes angle by 0.5 degrees since the previous pose update,
+ * the twist would be Twist2d{0.01, 0.0, toRadians(0.5)}
+ * @return The new pose of the robot.
+ */
+ @SuppressWarnings("LocalVariableName")
+ public Pose2d exp(Twist2d twist) {
+ double dx = twist.dx;
+ double dy = twist.dy;
+ double dtheta = twist.dtheta;
+
+ double sinTheta = Math.sin(dtheta);
+ double cosTheta = Math.cos(dtheta);
+
+ double s;
+ double c;
+ if (Math.abs(dtheta) < 1E-9) {
+ s = 1.0 - 1.0 / 6.0 * dtheta * dtheta;
+ c = 0.5 * dtheta;
+ } else {
+ s = sinTheta / dtheta;
+ c = (1 - cosTheta) / dtheta;
+ }
+ var transform = new Transform2d(new Translation2d(dx * s - dy * c, dx * c + dy * s),
+ new Rotation2d(cosTheta, sinTheta));
+
+ return this.plus(transform);
+ }
+
+ /**
+ * Returns a Twist2d that maps this pose to the end pose. If c is the output
+ * of a.Log(b), then a.Exp(c) would yield b.
+ *
+ * @param end The end pose for the transformation.
+ * @return The twist that maps this to end.
+ */
+ public Twist2d log(Pose2d end) {
+ final var transform = end.relativeTo(this);
+ final var dtheta = transform.getRotation().getRadians();
+ final var halfDtheta = dtheta / 2.0;
+
+ final var cosMinusOne = transform.getRotation().getCos() - 1;
+
+ double halfThetaByTanOfHalfDtheta;
+ if (Math.abs(cosMinusOne) < 1E-9) {
+ halfThetaByTanOfHalfDtheta = 1.0 - 1.0 / 12.0 * dtheta * dtheta;
+ } else {
+ halfThetaByTanOfHalfDtheta = -(halfDtheta * transform.getRotation().getSin()) / cosMinusOne;
+ }
+
+ Translation2d translationPart = transform.getTranslation().rotateBy(
+ new Rotation2d(halfThetaByTanOfHalfDtheta, -halfDtheta)
+ ).times(Math.hypot(halfThetaByTanOfHalfDtheta, halfDtheta));
+
+ return new Twist2d(translationPart.getX(), translationPart.getY(), dtheta);
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Pose2d(%s, %s)", m_translation, m_rotation);
+ }
+
+ /**
+ * Checks equality between this Pose2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Pose2d) {
+ return ((Pose2d) obj).m_translation.equals(m_translation)
+ && ((Pose2d) obj).m_rotation.equals(m_rotation);
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_translation, m_rotation);
+ }
+
+ static class PoseSerializer extends StdSerializer {
+ PoseSerializer() {
+ super(Pose2d.class);
+ }
+
+ @Override
+ public void serialize(
+ Pose2d value, JsonGenerator jgen, SerializerProvider provider)
+ throws IOException, JsonProcessingException {
+
+ jgen.writeStartObject();
+ jgen.writeObjectField("translation", value.m_translation);
+ jgen.writeObjectField("rotation", value.m_rotation);
+ jgen.writeEndObject();
+ }
+ }
+
+ static class PoseDeserializer extends StdDeserializer {
+ PoseDeserializer() {
+ super(Pose2d.class);
+ }
+
+ @Override
+ public Pose2d deserialize(JsonParser jp, DeserializationContext ctxt)
+ throws IOException, JsonProcessingException {
+ JsonNode node = jp.getCodec().readTree(jp);
+
+ Translation2d translation =
+ jp.getCodec().treeToValue(node.get("translation"), Translation2d.class);
+ Rotation2d rotation = jp.getCodec().treeToValue(node.get("rotation"), Rotation2d.class);
+ return new Pose2d(translation, rotation);
+ }
+ }
+}
diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
new file mode 100644
index 000000000..40950056a
--- /dev/null
+++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Rotation2d.java
@@ -0,0 +1,251 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.io.IOException;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.SerializerProvider;
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+
+/**
+ * A rotation in a 2d coordinate frame represented a point on the unit circle
+ * (cosine and sine).
+ */
+@JsonSerialize(using = Rotation2d.RotationSerializer.class)
+@JsonDeserialize(using = Rotation2d.RotationDeserializer.class)
+public class Rotation2d {
+ private final double m_value;
+ private final double m_cos;
+ private final double m_sin;
+
+ /**
+ * Constructs a Rotation2d with a default angle of 0 degrees.
+ */
+ public Rotation2d() {
+ m_value = 0.0;
+ m_cos = 1.0;
+ m_sin = 0.0;
+ }
+
+ /**
+ * Constructs a Rotation2d with the given radian value.
+ * The x and y don't have to be normalized.
+ *
+ * @param value The value of the angle in radians.
+ */
+ public Rotation2d(double value) {
+ m_value = value;
+ m_cos = Math.cos(value);
+ m_sin = Math.sin(value);
+ }
+
+ /**
+ * Constructs a Rotation2d with the given x and y (cosine and sine)
+ * components.
+ *
+ * @param x The x component or cosine of the rotation.
+ * @param y The y component or sine of the rotation.
+ */
+ @SuppressWarnings("ParameterName")
+ public Rotation2d(double x, double y) {
+ double magnitude = Math.hypot(x, y);
+ if (magnitude > 1e-6) {
+ m_sin = y / magnitude;
+ m_cos = x / magnitude;
+ } else {
+ m_sin = 0.0;
+ m_cos = 1.0;
+ }
+ m_value = Math.atan2(m_sin, m_cos);
+ }
+
+ /**
+ * Constructs and returns a Rotation2d with the given degree value.
+ *
+ * @param degrees The value of the angle in degrees.
+ * @return The rotation object with the desired angle value.
+ */
+ public static Rotation2d fromDegrees(double degrees) {
+ return new Rotation2d(Math.toRadians(degrees));
+ }
+
+ /**
+ * Adds two rotations together, with the result being bounded between -pi and
+ * pi.
+ *
+ * For example, Rotation2d.fromDegrees(30) + Rotation2d.fromDegrees(60) =
+ * Rotation2d{-pi/2}
+ *
+ * @param other The rotation to add.
+ * @return The sum of the two rotations.
+ */
+ public Rotation2d plus(Rotation2d other) {
+ return rotateBy(other);
+ }
+
+ /**
+ * Subtracts the new rotation from the current rotation and returns the new
+ * rotation.
+ *
+ *
For example, Rotation2d.fromDegrees(10) - Rotation2d.fromDegrees(100) =
+ * Rotation2d{-pi/2}
+ *
+ * @param other The rotation to subtract.
+ * @return The difference between the two rotations.
+ */
+ public Rotation2d minus(Rotation2d other) {
+ return rotateBy(other.unaryMinus());
+ }
+
+ /**
+ * Takes the inverse of the current rotation. This is simply the negative of
+ * the current angular value.
+ *
+ * @return The inverse of the current rotation.
+ */
+ public Rotation2d unaryMinus() {
+ return new Rotation2d(-m_value);
+ }
+
+ /**
+ * Multiplies the current rotation by a scalar.
+ *
+ * @param scalar The scalar.
+ * @return The new scaled Rotation2d.
+ */
+ public Rotation2d times(double scalar) {
+ return new Rotation2d(m_value * scalar);
+ }
+
+ /**
+ * Adds the new rotation to the current rotation using a rotation matrix.
+ *
+ *
The matrix multiplication is as follows:
+ * [cos_new] [other.cos, -other.sin][cos]
+ * [sin_new] = [other.sin, other.cos][sin]
+ * value_new = atan2(cos_new, sin_new)
+ *
+ * @param other The rotation to rotate by.
+ * @return The new rotated Rotation2d.
+ */
+ public Rotation2d rotateBy(Rotation2d other) {
+ return new Rotation2d(
+ m_cos * other.m_cos - m_sin * other.m_sin,
+ m_cos * other.m_sin + m_sin * other.m_cos
+ );
+ }
+
+ /*
+ * Returns the radian value of the rotation.
+ *
+ * @return The radian value of the rotation.
+ */
+ public double getRadians() {
+ return m_value;
+ }
+
+ /**
+ * Returns the degree value of the rotation.
+ *
+ * @return The degree value of the rotation.
+ */
+ public double getDegrees() {
+ return Math.toDegrees(m_value);
+ }
+
+ /**
+ * Returns the cosine of the rotation.
+ *
+ * @return The cosine of the rotation.
+ */
+ public double getCos() {
+ return m_cos;
+ }
+
+ /**
+ * Returns the sine of the rotation.
+ *
+ * @return The sine of the rotation.
+ */
+ public double getSin() {
+ return m_sin;
+ }
+
+ /**
+ * Returns the tangent of the rotation.
+ *
+ * @return The tangent of the rotation.
+ */
+ public double getTan() {
+ return m_sin / m_cos;
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Rotation2d(Rads: %.2f, Deg: %.2f)", m_value, Math.toDegrees(m_value));
+ }
+
+ /**
+ * Checks equality between this Rotation2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Rotation2d) {
+ return Math.abs(((Rotation2d) obj).m_value - m_value) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_value);
+ }
+
+ static class RotationSerializer extends StdSerializer {
+ RotationSerializer() {
+ super(Rotation2d.class);
+ }
+
+ @Override
+ public void serialize(
+ Rotation2d value, JsonGenerator jgen, SerializerProvider provider)
+ throws IOException, JsonProcessingException {
+
+ jgen.writeStartObject();
+ jgen.writeNumberField("radians", value.m_value);
+ jgen.writeEndObject();
+ }
+ }
+
+ static class RotationDeserializer extends StdDeserializer {
+ RotationDeserializer() {
+ super(Rotation2d.class);
+ }
+
+ @Override
+ public Rotation2d deserialize(JsonParser jp, DeserializationContext ctxt)
+ throws IOException, JsonProcessingException {
+ JsonNode node = jp.getCodec().readTree(jp);
+ double radians = node.get("radians").numberValue().doubleValue();
+
+ return new Rotation2d(radians);
+ }
+ }
+}
\ No newline at end of file
diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
new file mode 100644
index 000000000..15f47851e
--- /dev/null
+++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java
@@ -0,0 +1,106 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.util.Objects;
+
+/**
+ * Represents a transformation for a Pose2d.
+ */
+public class Transform2d {
+ private final Translation2d m_translation;
+ private final Rotation2d m_rotation;
+
+ /**
+ * Constructs the transform that maps the initial pose to the final pose.
+ *
+ * @param initial The initial pose for the transformation.
+ * @param last The final pose for the transformation.
+ */
+ public Transform2d(Pose2d initial, Pose2d last) {
+ // We are rotating the difference between the translations
+ // using a clockwise rotation matrix. This transforms the global
+ // delta into a local delta (relative to the initial pose).
+ m_translation = last.getTranslation().minus(initial.getTranslation())
+ .rotateBy(initial.getRotation().unaryMinus());
+
+ m_rotation = last.getRotation().minus(initial.getRotation());
+ }
+
+ /**
+ * Constructs a transform with the given translation and rotation components.
+ *
+ * @param translation Translational component of the transform.
+ * @param rotation Rotational component of the transform.
+ */
+ public Transform2d(Translation2d translation, Rotation2d rotation) {
+ m_translation = translation;
+ m_rotation = rotation;
+ }
+
+ /**
+ * Constructs the identity transform -- maps an initial pose to itself.
+ */
+ public Transform2d() {
+ m_translation = new Translation2d();
+ m_rotation = new Rotation2d();
+ }
+
+ /**
+ * Scales the transform by the scalar.
+ *
+ * @param scalar The scalar.
+ * @return The scaled Transform2d.
+ */
+ public Transform2d times(double scalar) {
+ return new Transform2d(m_translation.times(scalar), m_rotation.times(scalar));
+ }
+
+ /**
+ * Returns the translation component of the transformation.
+ *
+ * @return The translational component of the transform.
+ */
+ public Translation2d getTranslation() {
+ return m_translation;
+ }
+
+ /**
+ * Returns the rotational component of the transformation.
+ *
+ * @return Reference to the rotational component of the transform.
+ */
+ public Rotation2d getRotation() {
+ return m_rotation;
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Transform2d(%s, %s)", m_translation, m_rotation);
+ }
+
+ /**
+ * Checks equality between this Transform2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Transform2d) {
+ return ((Transform2d) obj).m_translation.equals(m_translation)
+ && ((Transform2d) obj).m_rotation.equals(m_rotation);
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_translation, m_rotation);
+ }
+}
diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
new file mode 100644
index 000000000..9d3f55038
--- /dev/null
+++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Translation2d.java
@@ -0,0 +1,243 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.io.IOException;
+import java.util.Objects;
+
+import com.fasterxml.jackson.core.JsonGenerator;
+import com.fasterxml.jackson.core.JsonParser;
+import com.fasterxml.jackson.core.JsonProcessingException;
+import com.fasterxml.jackson.databind.DeserializationContext;
+import com.fasterxml.jackson.databind.JsonNode;
+import com.fasterxml.jackson.databind.SerializerProvider;
+import com.fasterxml.jackson.databind.annotation.JsonDeserialize;
+import com.fasterxml.jackson.databind.annotation.JsonSerialize;
+import com.fasterxml.jackson.databind.deser.std.StdDeserializer;
+import com.fasterxml.jackson.databind.ser.std.StdSerializer;
+
+/**
+ * Represents a translation in 2d space.
+ * This object can be used to represent a point or a vector.
+ *
+ * This assumes that you are using conventional mathematical axes.
+ * When the robot is placed on the origin, facing toward the X direction,
+ * moving forward increases the X, whereas moving to the left increases the Y.
+ */
+@JsonSerialize(using = Translation2d.TranslationSerializer.class)
+@JsonDeserialize(using = Translation2d.TranslationDeserializer.class)
+@SuppressWarnings({"ParameterName", "MemberName"})
+public class Translation2d {
+ private final double m_x;
+ private final double m_y;
+
+ /**
+ * Constructs a Translation2d with X and Y components equal to zero.
+ */
+ public Translation2d() {
+ this(0.0, 0.0);
+ }
+
+ /**
+ * Constructs a Translation2d with the X and Y components equal to the
+ * provided values.
+ *
+ * @param x The x component of the translation.
+ * @param y The y component of the translation.
+ */
+ public Translation2d(double x, double y) {
+ m_x = x;
+ m_y = y;
+ }
+
+ /**
+ * Calculates the distance between two translations in 2d space.
+ *
+ *
This function uses the pythagorean theorem to calculate the distance.
+ * distance = sqrt((x2 - x1)^2 + (y2 - y1)^2)
+ *
+ * @param other The translation to compute the distance to.
+ * @return The distance between the two translations.
+ */
+ public double getDistance(Translation2d other) {
+ return Math.hypot(other.m_x - m_x, other.m_y - m_y);
+ }
+
+ /**
+ * Returns the X component of the translation.
+ *
+ * @return The x component of the translation.
+ */
+ public double getX() {
+ return m_x;
+ }
+
+ /**
+ * Returns the Y component of the translation.
+ *
+ * @return The y component of the translation.
+ */
+ public double getY() {
+ return m_y;
+ }
+
+ /**
+ * Returns the norm, or distance from the origin to the translation.
+ *
+ * @return The norm of the translation.
+ */
+ public double getNorm() {
+ return Math.hypot(m_x, m_y);
+ }
+
+ /**
+ * Applies a rotation to the translation in 2d space.
+ *
+ *
This multiplies the translation vector by a counterclockwise rotation
+ * matrix of the given angle.
+ * [x_new] [other.cos, -other.sin][x]
+ * [y_new] = [other.sin, other.cos][y]
+ *
+ *
For example, rotating a Translation2d of {2, 0} by 90 degrees will return a
+ * Translation2d of {0, 2}.
+ *
+ * @param other The rotation to rotate the translation by.
+ * @return The new rotated translation.
+ */
+ public Translation2d rotateBy(Rotation2d other) {
+ return new Translation2d(
+ m_x * other.getCos() - m_y * other.getSin(),
+ m_x * other.getSin() + m_y * other.getCos()
+ );
+ }
+
+ /**
+ * Adds two translations in 2d space and returns the sum. This is similar to
+ * vector addition.
+ *
+ *
For example, Translation2d{1.0, 2.5} + Translation2d{2.0, 5.5} =
+ * Translation2d{3.0, 8.0}
+ *
+ * @param other The translation to add.
+ * @return The sum of the translations.
+ */
+ public Translation2d plus(Translation2d other) {
+ return new Translation2d(m_x + other.m_x, m_y + other.m_y);
+ }
+
+ /**
+ * Subtracts the other translation from the other translation and returns the
+ * difference.
+ *
+ *
For example, Translation2d{5.0, 4.0} - Translation2d{1.0, 2.0} =
+ * Translation2d{4.0, 2.0}
+ *
+ * @param other The translation to subtract.
+ * @return The difference between the two translations.
+ */
+ public Translation2d minus(Translation2d other) {
+ return new Translation2d(m_x - other.m_x, m_y - other.m_y);
+ }
+
+ /**
+ * Returns the inverse of the current translation. This is equivalent to
+ * rotating by 180 degrees, flipping the point over both axes, or simply
+ * negating both components of the translation.
+ *
+ * @return The inverse of the current translation.
+ */
+ public Translation2d unaryMinus() {
+ return new Translation2d(-m_x, -m_y);
+ }
+
+ /**
+ * Multiplies the translation by a scalar and returns the new translation.
+ *
+ *
For example, Translation2d{2.0, 2.5} * 2 = Translation2d{4.0, 5.0}
+ *
+ * @param scalar The scalar to multiply by.
+ * @return The scaled translation.
+ */
+ public Translation2d times(double scalar) {
+ return new Translation2d(m_x * scalar, m_y * scalar);
+ }
+
+ /**
+ * Divides the translation by a scalar and returns the new translation.
+ *
+ *
For example, Translation2d{2.0, 2.5} / 2 = Translation2d{1.0, 1.25}
+ *
+ * @param scalar The scalar to multiply by.
+ * @return The reference to the new mutated object.
+ */
+ public Translation2d div(double scalar) {
+ return new Translation2d(m_x / scalar, m_y / scalar);
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Translation2d(X: %.2f, Y: %.2f)", m_x, m_y);
+ }
+
+ public static Translation2d fromRotation2d(Rotation2d rotation) {
+ return new Translation2d(rotation.getCos(), rotation.getSin());
+ }
+
+ /**
+ * Checks equality between this Translation2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Translation2d) {
+ return Math.abs(((Translation2d) obj).m_x - m_x) < 1E-9
+ && Math.abs(((Translation2d) obj).m_y - m_y) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(m_x, m_y);
+ }
+
+ static class TranslationSerializer extends StdSerializer {
+ TranslationSerializer() {
+ super(Translation2d.class);
+ }
+
+ @Override
+ public void serialize(
+ Translation2d value, JsonGenerator jgen, SerializerProvider provider)
+ throws IOException, JsonProcessingException {
+
+ jgen.writeStartObject();
+ jgen.writeNumberField("x", value.m_x);
+ jgen.writeNumberField("y", value.m_y);
+ jgen.writeEndObject();
+ }
+ }
+
+ static class TranslationDeserializer extends StdDeserializer {
+ TranslationDeserializer() {
+ super(Translation2d.class);
+ }
+
+ @Override
+ public Translation2d deserialize(JsonParser jp, DeserializationContext ctxt)
+ throws IOException, JsonProcessingException {
+ JsonNode node = jp.getCodec().readTree(jp);
+ double xval = node.get("x").numberValue().doubleValue();
+ double yval = node.get("y").numberValue().doubleValue();
+
+ return new Translation2d(xval, yval);
+ }
+ }
+}
diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
new file mode 100644
index 000000000..24c8c63c0
--- /dev/null
+++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/geometry/Twist2d.java
@@ -0,0 +1,76 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.geometry;
+
+import java.util.Objects;
+
+/**
+ * A change in distance along arc since the last pose update. We can use ideas
+ * from differential calculus to create new Pose2ds from a Twist2d and vise
+ * versa.
+ *
+ * A Twist can be used to represent a difference between two poses.
+ */
+@SuppressWarnings("MemberName")
+public class Twist2d {
+ /**
+ * Linear "dx" component.
+ */
+ public double dx;
+
+ /**
+ * Linear "dy" component.
+ */
+ public double dy;
+
+ /**
+ * Angular "dtheta" component (radians).
+ */
+ public double dtheta;
+
+ public Twist2d() {
+ }
+
+ /**
+ * Constructs a Twist2d with the given values.
+ * @param dx Change in x direction relative to robot.
+ * @param dy Change in y direction relative to robot.
+ * @param dtheta Change in angle relative to robot.
+ */
+ public Twist2d(double dx, double dy, double dtheta) {
+ this.dx = dx;
+ this.dy = dy;
+ this.dtheta = dtheta;
+ }
+
+ @Override
+ public String toString() {
+ return String.format("Twist2d(dX: %.2f, dY: %.2f, dTheta: %.2f)", dx, dy, dtheta);
+ }
+
+ /**
+ * Checks equality between this Twist2d and another object.
+ *
+ * @param obj The other object.
+ * @return Whether the two objects are equal or not.
+ */
+ @Override
+ public boolean equals(Object obj) {
+ if (obj instanceof Twist2d) {
+ return Math.abs(((Twist2d) obj).dx - dx) < 1E-9
+ && Math.abs(((Twist2d) obj).dy - dy) < 1E-9
+ && Math.abs(((Twist2d) obj).dtheta - dtheta) < 1E-9;
+ }
+ return false;
+ }
+
+ @Override
+ public int hashCode() {
+ return Objects.hash(dx, dy, dtheta);
+ }
+}
diff --git a/chameleon-server/src/main/java/edu/wpi/first/wpilibj/util/Units.java b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/util/Units.java
new file mode 100644
index 000000000..0bdfdc7b6
--- /dev/null
+++ b/chameleon-server/src/main/java/edu/wpi/first/wpilibj/util/Units.java
@@ -0,0 +1,104 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2019 FIRST. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.util;
+
+/**
+ * Utility class that converts between commonly used units in FRC.
+ */
+public final class Units {
+ private static final double kInchesPerFoot = 12.0;
+ private static final double kMetersPerInch = 0.0254;
+ private static final double kSecondsPerMinute = 60;
+
+ /**
+ * Utility class, so constructor is private.
+ */
+ private Units() {
+ throw new UnsupportedOperationException("This is a utility class!");
+ }
+
+ /**
+ * Converts given meters to feet.
+ *
+ * @param meters The meters to convert to feet.
+ * @return Feet converted from meters.
+ */
+ public static double metersToFeet(double meters) {
+ return metersToInches(meters) / kInchesPerFoot;
+ }
+
+ /**
+ * Converts given feet to meters.
+ *
+ * @param feet The feet to convert to meters.
+ * @return Meters converted from feet.
+ */
+ public static double feetToMeters(double feet) {
+ return inchesToMeters(feet * kInchesPerFoot);
+ }
+
+ /**
+ * Converts given meters to inches.
+ *
+ * @param meters The meters to convert to inches.
+ * @return Inches converted from meters.
+ */
+ public static double metersToInches(double meters) {
+ return meters / kMetersPerInch;
+ }
+
+ /**
+ * Converts given inches to meters.
+ *
+ * @param inches The inches to convert to meters.
+ * @return Meters converted from inches.
+ */
+ public static double inchesToMeters(double inches) {
+ return inches * kMetersPerInch;
+ }
+
+ /**
+ * Converts given degrees to radians.
+ *
+ * @param degrees The degrees to convert to radians.
+ * @return Radians converted from degrees.
+ */
+ public static double degreesToRadians(double degrees) {
+ return Math.toRadians(degrees);
+ }
+
+ /**
+ * Converts given radians to degrees.
+ *
+ * @param radians The radians to convert to degrees.
+ * @return Degrees converted from radians.
+ */
+ public static double radiansToDegrees(double radians) {
+ return Math.toDegrees(radians);
+ }
+
+ /**
+ * Converts rotations per minute to radians per second.
+ *
+ * @param rpm The rotations per minute to convert to radians per second.
+ * @return Radians per second converted from rotations per minute.
+ */
+ public static double rotationsPerMinuteToRadiansPerSecond(double rpm) {
+ return rpm * Math.PI / (kSecondsPerMinute / 2);
+ }
+
+ /**
+ * Converts radians per second to rotations per minute.
+ *
+ * @param radiansPerSecond The radians per second to convert to from rotations per minute.
+ * @return Rotations per minute converted from radians per second.
+ */
+ public static double radiansPerSecondToRotationsPerMinute(double radiansPerSecond) {
+ return radiansPerSecond * (kSecondsPerMinute / 2) / Math.PI;
+ }
+}
diff --git a/chameleon-server/src/main/resources/web/img/chessboard.png b/chameleon-server/src/main/resources/web/img/chessboard.png
new file mode 100644
index 000000000..39bb399e8
Binary files /dev/null and b/chameleon-server/src/main/resources/web/img/chessboard.png differ
diff --git a/chameleon-server/src/test/java/com/chameleonvision/config/StaticCaptureTest.java b/chameleon-server/src/test/java/com/chameleonvision/config/StaticCaptureTest.java
index d8289cacc..1856e9f19 100644
--- a/chameleon-server/src/test/java/com/chameleonvision/config/StaticCaptureTest.java
+++ b/chameleon-server/src/test/java/com/chameleonvision/config/StaticCaptureTest.java
@@ -3,7 +3,7 @@ package com.chameleonvision.config;
import com.chameleonvision.util.ProgramDirectoryUtilities;
import com.chameleonvision.vision.camera.CameraStreamer;
import com.chameleonvision.vision.image.StaticImageCapture;
-import com.chameleonvision.vision.pipeline.impl.CVPipeline2d;
+import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
import edu.wpi.cscore.CameraServerCvJNI;
import edu.wpi.cscore.CameraServerJNI;
import edu.wpi.first.networktables.NetworkTableInstance;
@@ -58,7 +58,7 @@ class StaticCaptureTest {
@Test
void ImageProcessTest() throws InterruptedException {
ImageLoadTest();
- CVPipeline2d testPipeline = new CVPipeline2d();
+ StandardCVPipeline testPipeline = new StandardCVPipeline();
String testImage1 = "CargoSideStraightDark36in";
StaticImageCapture testCapture1 = loadedImages.get(testImage1);
diff --git a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/20in.png b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/20in.png
new file mode 100644
index 000000000..46cf8c92b
Binary files /dev/null and b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/20in.png differ
diff --git a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/30deg40in.png b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/30deg40in.png
new file mode 100644
index 000000000..068a1ad86
Binary files /dev/null and b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/30deg40in.png differ
diff --git a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/40deg20in.png b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/40deg20in.png
new file mode 100644
index 000000000..819998dc2
Binary files /dev/null and b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/40deg20in.png differ
diff --git a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/40in.png b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/40in.png
new file mode 100644
index 000000000..f329e121e
Binary files /dev/null and b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/40in.png differ
diff --git a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
new file mode 100644
index 000000000..04e8b3317
--- /dev/null
+++ b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/SolvePNPtest.java
@@ -0,0 +1,38 @@
+package com.chameleonvision.vision.pipeline;
+
+import com.chameleonvision.vision.image.StaticImageCapture;
+import com.chameleonvision.vision.pipeline.impl.StandardCVPipeline;
+import com.chameleonvision.vision.pipeline.impl.StandardCVPipelineSettings;
+import edu.wpi.cscore.CameraServerCvJNI;
+import edu.wpi.cscore.CameraServerJNI;
+import org.junit.jupiter.api.Test;
+
+import java.io.IOException;
+import java.nio.file.Path;
+
+public class SolvePNPtest {
+
+ private static final Path root = Path.of("src", "test", "java", "com", "chameleonvision", "vision", "pipeline");
+
+ @Test public void test20in() {
+
+ try {
+ forceLoad();
+ } catch (IOException e) {
+ return;
+ }
+
+ // mock up pipeline
+ var pipeline = new StandardCVPipeline();
+ var capture = new StaticImageCapture(Path.of(root.toString(), "20in.png"));
+ pipeline.initPipeline(capture);
+ var settings = new StandardCVPipelineSettings();
+
+ }
+
+ private void forceLoad() throws IOException {
+ CameraServerJNI.forceLoad();
+ CameraServerCvJNI.forceLoad();
+ }
+
+}
diff --git a/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/stream.png b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/stream.png
new file mode 100644
index 000000000..9bc26d701
Binary files /dev/null and b/chameleon-server/src/test/java/com/chameleonvision/vision/pipeline/stream.png differ