Add Transform2d::Inverse() (#2407)

This is useful for undoing transformations. One application my FRC team
found was converting perspective n-point data from a "camera to target"
coordinate frame transformation to a "target to camera" coordinate frame
transformation.

wpilibc/src/main/native/cpp/geometry/Transform2d.cpp

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -24,6 +24,13 @@ Transform2d::Transform2d(Pose2d initial, Pose2d final) {
 Transform2d::Transform2d(Translation2d translation, Rotation2d rotation)
     : m_translation(translation), m_rotation(rotation) {}
 
+Transform2d Transform2d::Inverse() const {
+  // 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).
+  return Transform2d{(-Translation()).RotateBy(-Rotation()), -Rotation()};
+}
+
 bool Transform2d::operator==(const Transform2d& other) const {
   return m_translation == other.m_translation && m_rotation == other.m_rotation;
 }

wpilibc/src/main/native/include/frc/geometry/Transform2d.h

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -53,6 +53,13 @@ class Transform2d {
    */
   const Rotation2d& Rotation() const { return m_rotation; }
 
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  Transform2d Inverse() const;
+
   /**
    * Scales the transform by the scalar.
    *

wpilibc/src/test/native/cpp/geometry/Transform2dTest.cpp

@@ -0,0 +1,33 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include <cmath>
+
+#include "frc/geometry/Pose2d.h"
+#include "frc/geometry/Rotation2d.h"
+#include "frc/geometry/Transform2d.h"
+#include "frc/geometry/Translation2d.h"
+#include "gtest/gtest.h"
+
+using namespace frc;
+
+static constexpr double kEpsilon = 1E-9;
+
+TEST(Transform2dTest, Inverse) {
+  const Pose2d initial{1_m, 2_m, Rotation2d(45.0_deg)};
+  const Transform2d transform{Translation2d{5.0_m, 0.0_m}, Rotation2d(5.0_deg)};
+
+  auto transformed = initial + transform;
+  auto untransformed = transformed + transform.Inverse();
+
+  EXPECT_NEAR(initial.Translation().X().to<double>(),
+              untransformed.Translation().X().to<double>(), kEpsilon);
+  EXPECT_NEAR(initial.Translation().Y().to<double>(),
+              untransformed.Translation().Y().to<double>(), kEpsilon);
+  EXPECT_NEAR(initial.Rotation().Degrees().to<double>(),
+              untransformed.Rotation().Degrees().to<double>(), kEpsilon);
+}

wpilibj/src/main/java/edu/wpi/first/wpilibj/geometry/Transform2d.java

@@ -1,5 +1,5 @@
 /*----------------------------------------------------------------------------*/
-/* Copyright (c) 2019 FIRST. All Rights Reserved.                             */
+/* Copyright (c) 2019-2020 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.                                                               */
@@ -79,6 +79,19 @@ public class Transform2d {
     return m_rotation;
   }
 
+  /**
+   * Invert the transformation. This is useful for undoing a transformation.
+   *
+   * @return The inverted transformation.
+   */
+  public Transform2d inverse() {
+    // 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).
+    return new Transform2d(getTranslation().unaryMinus().rotateBy(getRotation().unaryMinus()),
+        getRotation().unaryMinus());
+  }
+
   @Override
   public String toString() {
     return String.format("Transform2d(%s, %s)", m_translation, m_rotation);

wpilibj/src/test/java/edu/wpi/first/wpilibj/geometry/Transform2dTest.java

@@ -0,0 +1,36 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2020 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 org.junit.jupiter.api.Test;
+
+import static org.junit.jupiter.api.Assertions.assertAll;
+import static org.junit.jupiter.api.Assertions.assertEquals;
+
+class Transform2dTest {
+  private static final double kEpsilon = 1E-9;
+
+  @Test
+  void testInverse() {
+    var initial = new Pose2d(new Translation2d(1.0, 2.0), Rotation2d.fromDegrees(45.0));
+    var transformation = new Transform2d(new Translation2d(5.0, 0.0),
+        Rotation2d.fromDegrees(5.0));
+
+    var transformed = initial.plus(transformation);
+    var untransformed = transformed.plus(transformation.inverse());
+
+    assertAll(
+        () -> assertEquals(initial.getTranslation().getX(), untransformed.getTranslation().getX(),
+                           kEpsilon),
+        () -> assertEquals(initial.getTranslation().getY(), untransformed.getTranslation().getY(),
+                           kEpsilon),
+        () -> assertEquals(initial.getRotation().getDegrees(),
+                           untransformed.getRotation().getDegrees(), kEpsilon)
+    );
+  }
+}